My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
SmbEstimator.cpp
Go to the documentation of this file.
1/*
2Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
3All rights reserved.
4This file is released under the "BSD-3-Clause License".
5Please see the LICENSE file that has been included as part of this package.
6 */
7
8// Implementation
10
11// Project
13
14// Workspace
19
20namespace smb_se {
21
22SmbEstimator::SmbEstimator(std::shared_ptr<ros::NodeHandle> privateNodePtr) : holistic_fusion::HolisticFusionRos(privateNodePtr) {
23 REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl;
24
25 // Configurations ----------------------------
26 // Static transforms
27 staticTransformsPtr_ = std::make_shared<SmbStaticTransforms>(privateNodePtr);
28
29 // Set up
31}
32
34 REGULAR_COUT << GREEN_START << " SmbEstimator-Setup called." << COLOR_END << std::endl;
35
36 // Read parameters ----------------------------
38
39 // Super class
40 HolisticFusionRos::setup(staticTransformsPtr_);
41
42 // Publishers ----------------------------
44
45 // Subscribers ----------------------------
47
48 // Messages ----------------------------
50
51 // Services ----------------------------
53
54 // Transforms ----------------------------
55 staticTransformsPtr_->findTransformations();
56
57 // Wrap up ----------------------------
58 REGULAR_COUT << GREEN_START << " Set up successfully." << COLOR_END << std::endl;
59}
60
62 // Paths
63 pubMeasMapLioPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measLiDAR_path_map_imu", ROS_QUEUE_SIZE);
64 pubMeasMapVioPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measVIO_path_map_imu", ROS_QUEUE_SIZE);
65}
66
68 // LiDAR Odometry
70 subLioOdometry_ = privateNode.subscribe<nav_msgs::Odometry>(
71 "/lidar_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::lidarOdometryCallback_, this, ros::TransportHints().tcpNoDelay());
72 REGULAR_COUT << COLOR_END << " Initialized LiDAR Odometry subscriber with topic: " << subLioOdometry_.getTopic() << std::endl;
73 }
74
75 // Wheel Odometry Between
77 subWheelOdometryBetween_ = privateNode.subscribe<nav_msgs::Odometry>(
78 "/wheel_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::wheelOdometryPoseCallback_, this, ros::TransportHints().tcpNoDelay());
79 REGULAR_COUT << COLOR_END << " Initialized Wheel Odometry subscriber with topic: " << subWheelOdometryBetween_.getTopic() << std::endl;
80 }
81
82 // Wheel Linear Velocities
84 subWheelLinearVelocities_ = privateNode.subscribe<std_msgs::Float64MultiArray>(
85 "/wheel_velocities_topic", ROS_QUEUE_SIZE, &SmbEstimator::wheelLinearVelocitiesCallback_, this, ros::TransportHints().tcpNoDelay());
86 REGULAR_COUT << COLOR_END << " Initialized Wheel Linear Velocities subscriber with topic: " << subWheelLinearVelocities_.getTopic()
87 << std::endl;
88 }
89
90 // VIO
92 subVioOdometry_ = privateNode.subscribe<nav_msgs::Odometry>("/vio_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::vioOdometryCallback_,
93 this, ros::TransportHints().tcpNoDelay());
94 REGULAR_COUT << COLOR_END << " Initialized VIO Odometry subscriber with topic: " << subVioOdometry_.getTopic() << std::endl;
95 }
96}
97
99 // Path
100 measLio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
101 measVio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
102}
103
105 // Nothing for now
106}
107
108void SmbEstimator::imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr) {
109 // Check whether any of the measurements is available, otherwise do pure imu integration
112 // Initialization
113 REGULAR_COUT << RED_START << " IMU callback is setting global yaw and position, as no other odometry is available. Initializing..."
114 << COLOR_END << std::endl;
116 REGULAR_COUT << RED_START << " ...initialized yaw and position to identity." << COLOR_END << std::endl;
117 // Pretend that we received first measurement --> In order to allow for optimization
119 }
120
121 // Super class
123}
124
125void SmbEstimator::lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& odomLidarPtr) {
126 // Static members
127 static int lidarOdometryCallbackCounter__ = -1;
128
129 // Counter
130 ++lidarOdometryCallbackCounter__;
131
132 Eigen::Isometry3d lio_T_M_Lk;
133 holistic_fusion::odomMsgToEigen(*odomLidarPtr, lio_T_M_Lk.matrix());
134
135 // Transform to IMU frame
136 double lidarOdometryTimeK = odomLidarPtr->header.stamp.toSec();
137
138 // Frame Name
139 const std::string& lioOdometryFrame = dynamic_cast<SmbStaticTransforms*>(staticTransformsPtr_.get())->getLioOdometryFrame(); // alias
140
141 // Measurement
143 "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId,
144 holistic_fusion::RobustNorm::None(), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id,
146
147 // Add measurement or initialize
148 if (lidarOdometryCallbackCounter__ <= 2) {
149 return;
150 } else if (areYawAndPositionInited()) { // Already initialized --> unary factor
151 this->addUnaryPose3AbsoluteMeasurement(unary6DMeasurement);
152 } else { // Initializing
153 REGULAR_COUT << GREEN_START << " LiDAR odometry callback is setting global yaw, as it was not set so far." << COLOR_END << std::endl;
154 this->initHeadingAndPosition(unary6DMeasurement);
155 }
156
157 // Visualization ----------------------------
158 // Add to path message
159 addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id, odomLidarPtr->header.stamp,
160 (lio_T_M_Lk * staticTransformsPtr_
161 ->rv_T_frame1_frame2(dynamic_cast<SmbStaticTransforms*>(staticTransformsPtr_.get())->getLioOdometryFrame(),
162 staticTransformsPtr_->getImuFrame())
163 .matrix())
164 .block<3, 1>(0, 3),
165 graphConfigPtr_->imuBufferLength_ * 4);
166
167 // Publish Path
169}
170
171void SmbEstimator::wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr& wheelOdometryKPtr) {
172 if (!areRollAndPitchInited()) {
173 return;
174 }
175
176 // Counter
178
179 // Eigen Type
180 Eigen::Isometry3d T_O_Bw_k;
181 holistic_fusion::odomMsgToEigen(*wheelOdometryKPtr, T_O_Bw_k.matrix());
182 double wheelOdometryTimeK = wheelOdometryKPtr->header.stamp.toSec();
183
184 // At start
186 T_O_Bw_km1_ = T_O_Bw_k;
187 wheelOdometryTimeKm1_ = wheelOdometryTimeK;
188 return;
189 }
190
191 // Frame name
192 const std::string& wheelOdometryFrame =
194
195 // State Machine
197 // If no LIO, then initialize the graph here
198 if (!useLioOdometryFlag_) {
199 REGULAR_COUT << GREEN_START << " Wheel odometry callback is setting global yaw and position, as lio is all set to false." << COLOR_END
200 << std::endl;
202 REGULAR_COUT << " Initialized yaw and position to identity in the wheel odometry callback, as lio and vio are all set to false."
203 << std::endl;
204 }
205 } else {
206 // Only add every 5th measurement
207 int measurementRate = static_cast<int>(wheelOdometryBetweenRate_ / 5);
208 // Check
210 // Compute Delta
211 Eigen::Isometry3d T_Bkm1_Bk = T_O_Bw_km1_.inverse() * T_O_Bw_k;
212 // Create measurement
214 "Wheel_odometry_6D", measurementRate, wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId,
216 // Add to graph
217 this->addBinaryPose3Measurement(delta6DMeasurement);
218
219 // Prepare for next iteration
220 T_O_Bw_km1_ = T_O_Bw_k;
221 wheelOdometryTimeKm1_ = wheelOdometryTimeK;
222 }
223 }
224}
225
226void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr& wheelsSpeedsPtr) {
227 if (!areRollAndPitchInited()) {
228 return;
229 }
230
231 // Get the 3 values
232 const double timeK = wheelsSpeedsPtr->data[0];
233 const double leftWheelSpeedRps = wheelsSpeedsPtr->data[1];
234 const double rightWheelSpeedRps = wheelsSpeedsPtr->data[2];
235 const double leftWheelSpeedMs = leftWheelSpeedRps * wheelRadiusMeter_;
236 const double rightWheelSpeedMs = rightWheelSpeedRps * wheelRadiusMeter_;
237
238 // Print
239 if (graphConfigPtr_->verboseLevel_ > 1) {
240 REGULAR_COUT << " Wheel linear velocities callback: " << std::setprecision(14) << timeK << " " << leftWheelSpeedMs << " [m/s] "
241 << rightWheelSpeedMs << " [m/s]" << std::endl;
242 }
243
244 // Frame name
245 const std::string& wheelLinearVelocityLeftFrame =
247 const std::string& wheelLinearVelocityRightFrame =
249
250 // State Machine
252 // If no LIO, then initialize the graph here
254 REGULAR_COUT << GREEN_START << " Wheel linear velocities callback is setting global yaw and position, as lio is all set to false."
255 << COLOR_END << std::endl;
258 << " Initialized yaw and position to identity in the wheel linear velocities callback, as lio and vio are all set to false."
259 << std::endl;
260 }
261 } else {
262 // Left Wheel
263 holistic_fusion::UnaryMeasurementXD<Eigen::Vector3d, 3> leftWheelLinearVelocityMeasurement(
264 "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame,
265 wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::Tukey(1.0), timeK, 1.0,
266 Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_);
267 this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement);
268
269 // Right Wheel
270 holistic_fusion::UnaryMeasurementXD<Eigen::Vector3d, 3> rightWheelLinearVelocityMeasurement(
271 "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame,
272 wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::Tukey(1.0), timeK, 1.0,
273 Eigen::Vector3d(rightWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_);
274 this->addUnaryVelocity3LocalMeasurement(rightWheelLinearVelocityMeasurement);
275 }
276}
277
278void SmbEstimator::vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr& vioOdomPtr) {
279 std::cout << "VIO odometry not yet stable enough for usage, disable flag." << std::endl;
280
281 // Extract
282 Eigen::Isometry3d vio_T_M_Ck;
283 holistic_fusion::odomMsgToEigen(*vioOdomPtr, vio_T_M_Ck.matrix());
284
285 // Visualization ----------------------------
286 // Add to path message
287 addToPathMsg(measVio_mapImuPathPtr_, vioOdomPtr->header.frame_id, vioOdomPtr->header.stamp,
288 (vio_T_M_Ck * staticTransformsPtr_
289 ->rv_T_frame1_frame2(dynamic_cast<SmbStaticTransforms*>(staticTransformsPtr_.get())->getVioOdometryFrame(),
290 staticTransformsPtr_->getImuFrame())
291 .matrix())
292 .block<3, 1>(0, 3),
293 graphConfigPtr_->imuBufferLength_ * 4 * 10);
294
295 // Publish Path
297}
298
299} // namespace smb_se
#define REGULAR_COUT
#define COLOR_END
#define GREEN_START
#define RED_START
#define ROS_QUEUE_SIZE
void addBinaryPose3Measurement(const BinaryMeasurementXD< Eigen::Isometry3d, 6 > &F_T_F_S) override
Binary Measurements.
void addUnaryPose3AbsoluteMeasurement(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &R_T_R_S, const bool addToOnlineSmootherFlag=true) override
Unary Measurements / Absolute Measurements: In reference frame --> systematic drift.
void addUnaryVelocity3LocalMeasurement(UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_F_S) override
static void addToPathMsg(const nav_msgs::PathPtr &pathPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Vector3d &t, int maxBufferLength)
virtual void imuCallback(const sensor_msgs::Imu::ConstPtr &imuPtr)
HolisticFusionRos(const std::shared_ptr< ros::NodeHandle > &privateNodePtr)
std::shared_ptr< GraphConfig > graphConfigPtr_
std::shared_ptr< StaticTransforms > staticTransformsPtr_
bool initHeadingAndPosition(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &T_M_S)
Class to represent x-dimensional absolute unary measurement in the graph.
Class to represent x-dimensional unary measurement in the graph.
void lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr &lidarOdomPtr)
void initializeSubscribers(ros::NodeHandle &privateNode) override
ros::Subscriber subVioOdometry_
Eigen::Matrix< double, 6, 1 > initialSe3AlignmentNoise_
void initializeMessages(ros::NodeHandle &privateNode) override
double wheelLinearVelocitiesRate_
nav_msgs::PathPtr measVio_mapImuPathPtr_
void initializePublishers(ros::NodeHandle &privateNode) override
Eigen::Matrix< double, 6, 1 > lioPoseUnaryNoise_
double wheelOdometryBetweenRate_
void wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr &wheelOdomPtr)
Eigen::Matrix< double, 6, 1 > lioSe3AlignmentRandomWalk_
ros::Subscriber subLioOdometry_
ros::Subscriber subWheelOdometryBetween_
void readParams(const ros::NodeHandle &privateNode) override
Eigen::Matrix< double, 3, 1 > wheelLinearVelocitiesNoise_
void initializeServices(ros::NodeHandle &privateNode) override
void vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr &vioOdomPtr)
SmbEstimator(std::shared_ptr< ros::NodeHandle > privateNodePtr)
ros::Publisher pubMeasMapVioPath_
ros::Publisher pubMeasMapLioPath_
Eigen::Matrix< double, 6, 1 > wheelPoseBetweenNoise_
void wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr &wheelsSpeedsPtr)
ros::Subscriber subWheelLinearVelocities_
Eigen::Isometry3d T_O_Bw_km1_
void imuCallback(const sensor_msgs::Imu::ConstPtr &imuPtr) override
nav_msgs::PathPtr measLio_mapImuPathPtr_
const std::string & getWheelLinearVelocityRightFrame()
const std::string & getWheelLinearVelocityLeftFrame()
const std::string & getWheelOdometryBetweenFrame()
const std::string & getVioOdometryFrame()
const std::string & getLioOdometryFrame()
void odomMsgToEigen(const nav_msgs::Odometry &odomLidar, Eigen::Matrix4d &T)
static RobustNorm Tukey(const double &robustNormConstant)
Definition Measurement.h:36
static RobustNorm None()
Definition Measurement.h:33