My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
ExcavatorEstimator.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
14
15// Workspace
19
20namespace excavator_se {
21
22ExcavatorEstimator::ExcavatorEstimator(std::shared_ptr<ros::NodeHandle> privateNodePtr) : holistic_fusion::HolisticFusionRos(privateNodePtr) {
23 REGULAR_COUT << " ExcavatorEstimator-Constructor called." << COLOR_END << std::endl;
24
25 // Configurations ----------------------------
26 // Static Transforms
27 staticTransformsPtr_ = std::make_shared<ExcavatorStaticTransforms>(privateNodePtr);
28
29 // GNSS Handler
30 gnssHandlerPtr_ = std::make_shared<holistic_fusion::GnssHandler>();
31
32 // Setup
34}
35
37 REGULAR_COUT << GREEN_START << " ExcavatorEstimator-Setup called." << COLOR_END << std::endl;
38
39 // Read parameters ----------------------------
41
42 // Super class
44
45 // Publishers ----------------------------
47
48 // Subscribers ----------------------------
50
51 // Messages ----------------------------
53
54 // Static Transforms
55 staticTransformsPtr_->findTransformations();
56}
57
59 // Status
60 REGULAR_COUT << GREEN_START << " Initializing Publishers..." << COLOR_END << std::endl;
61
62 // Paths
63 pubMeasWorldGnssLPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measGnssL_path_world_gnssL", ROS_QUEUE_SIZE);
64 pubMeasWorldGnssRPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measGnssR_path_world_gnssR", ROS_QUEUE_SIZE);
65 pubMeasMapLioPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measLiDAR_path_map_imu", ROS_QUEUE_SIZE);
66}
67
69 // LiDAR Odometry
71 subLidarOdometry_ = privateNode.subscribe<nav_msgs::Odometry>(
72 "/lidar_odometry_topic", ROS_QUEUE_SIZE, &ExcavatorEstimator::lidarOdometryCallback_, this, ros::TransportHints().tcpNoDelay());
73 std::cout << YELLOW_START << "ExcavatorEstimator" << COLOR_END << " Initialized LiDAR Odometry subscriber." << std::endl;
74 }
75
76 // GNSS
78 subGnssLeft_.subscribe(privateNode, "/gnss_topic_1", ROS_QUEUE_SIZE);
79 subGnssRight_.subscribe(privateNode, "/gnss_topic_2", ROS_QUEUE_SIZE);
81 new message_filters::Synchronizer<GnssExactSyncPolicy>(GnssExactSyncPolicy(ROS_QUEUE_SIZE), subGnssLeft_, subGnssRight_));
82 gnssExactSyncPtr_->registerCallback(boost::bind(&ExcavatorEstimator::gnssCallback_, this, _1, _2));
83 std::cout << YELLOW_START << "FactorGraphFiltering" << COLOR_END << " Initialized Gnss subscriber (for both Gnss topics)." << std::endl;
84 }
85}
86
88 // Status
89 REGULAR_COUT << GREEN_START << " Initializing Messages..." << COLOR_END << std::endl;
90
91 // Paths
92 measGnss_worldGnssLPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
93 measGnss_worldGnssRPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
94 measLio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); // Preintegrated Estimate of LiDAR Frame
95}
96
97void ExcavatorEstimator::lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& odomLidarPtr) {
98 // Static members
99 static int lidarOdometryCallbackCounter__ = -1;
100 static Eigen::Isometry3d lio_T_M_LKm1__;
101 static double lidarOdometryTimeKm1__ = 0;
102
103 // Counter
104 ++lidarOdometryCallbackCounter__;
105
106 Eigen::Isometry3d lio_T_M_Lk;
107 holistic_fusion::odomMsgToEigen(*odomLidarPtr, lio_T_M_Lk.matrix());
108
109 // Transform to IMU frame
110 double lidarOdometryTimeK = odomLidarPtr->header.stamp.toSec();
111
112 // Measurement
113 // Frame Names
114 const std::string& sensorFrameName = dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getLioOdometryFrame();
115 const std::string& fixedFrameName = odomLidarPtr->header.frame_id;
116 // Measurement
118 "LioUnary6D", int(lioOdometryRate_), sensorFrameName, sensorFrameName + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
119 lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, fixedFrameName, staticTransformsPtr_->getWorldFrame(),
121
122 if (lidarOdometryCallbackCounter__ <= 2) {
123 return;
124 } else if (areYawAndPositionInited()) { // Already initialized --> unary factor
125 this->addUnaryPose3AbsoluteMeasurement(unary6DMeasurement);
126 } else if (!(useLeftGnssFlag_ || useRightGnssFlag_) || secondsSinceStart() > 15) { // Initializing
127 REGULAR_COUT << GREEN_START << " LiDAR odometry callback is setting global yaw, as it was not set so far." << COLOR_END << std::endl;
128 this->initHeadingAndPosition(unary6DMeasurement);
129 }
130
131 // Wrap up iteration
132 lio_T_M_LKm1__ = lio_T_M_Lk;
133 lidarOdometryTimeKm1__ = lidarOdometryTimeK;
134
135 // Visualization ----------------------------
136 // Add to path message
137 addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id, odomLidarPtr->header.stamp, (lio_T_M_Lk.matrix()).block<3, 1>(0, 3),
138 graphConfigPtr_->imuBufferLength_ * 4);
139
140 // Publish Path
142}
143
144void ExcavatorEstimator::gnssCallback_(const sensor_msgs::NavSatFix::ConstPtr& leftGnssMsgPtr,
145 const sensor_msgs::NavSatFix::ConstPtr& rightGnssMsgPtr) {
146 // Static method variables
147 static Eigen::Vector3d accumulatedLeftCoordinates__(0.0, 0.0, 0.0);
148 static Eigen::Vector3d accumulatedRightCoordinates__(0.0, 0.0, 0.0);
149 // static Eigen::Vector3d W_t_W_GnssL_km1__, W_t_W_GnssR_km1__;
150 static int gnssCallbackCounter__ = 0;
151
152 // Start
153 ++gnssCallbackCounter__;
154 Eigen::Vector3d leftGnssCoord = Eigen::Vector3d(leftGnssMsgPtr->latitude, leftGnssMsgPtr->longitude, leftGnssMsgPtr->altitude);
155 Eigen::Vector3d rightGnssCoord = Eigen::Vector3d(rightGnssMsgPtr->latitude, rightGnssMsgPtr->longitude, rightGnssMsgPtr->altitude);
156 Eigen::Vector3d leftGnssCovarianceXYZ(leftGnssMsgPtr->position_covariance[0], leftGnssMsgPtr->position_covariance[4],
157 leftGnssMsgPtr->position_covariance[8]);
158 Eigen::Vector3d rightGnssCovarianceXYZ(rightGnssMsgPtr->position_covariance[0], rightGnssMsgPtr->position_covariance[4],
159 rightGnssMsgPtr->position_covariance[8]);
160
161 // Workflow
162 if (gnssCallbackCounter__ <= NUM_GNSS_CALLBACKS_UNTIL_START) { // Accumulate measurements at the beginning
163 // Wait until measurements got accumulated
164 accumulatedLeftCoordinates__ += leftGnssCoord;
165 accumulatedRightCoordinates__ += rightGnssCoord;
166 if (!(gnssCallbackCounter__ % 10)) {
167 REGULAR_COUT << " NOT ENOUGH Gnss MESSAGES ARRIVED!" << std::endl;
168 }
169 return;
170 } else if (gnssCallbackCounter__ ==
171 NUM_GNSS_CALLBACKS_UNTIL_START + 1) { // Initialize the GNSS handler with the initial position and initialize the graph
172 Eigen::Vector3d meanLeftCoordinates = accumulatedLeftCoordinates__ / NUM_GNSS_CALLBACKS_UNTIL_START;
173 Eigen::Vector3d meanRightCoordinates = accumulatedRightCoordinates__ / NUM_GNSS_CALLBACKS_UNTIL_START;
174 gnssHandlerPtr_->initHandler(meanLeftCoordinates, meanRightCoordinates);
175 }
176
177 // Convert to cartesian coordinates --> Needed for every ramaining step
178 Eigen::Vector3d W_t_W_GnssL, W_t_W_GnssR;
179 gnssHandlerPtr_->convertNavSatToPositions(leftGnssCoord, rightGnssCoord, W_t_W_GnssL, W_t_W_GnssR);
180 double yaw_W_C = gnssHandlerPtr_->computeYaw(W_t_W_GnssL, W_t_W_GnssR);
181
182 std::string fixedFrameName = staticTransformsPtr_->getWorldFrame(); // + "_ENU";
183
184 // State Machine
185 if (!areYawAndPositionInited() && areRollAndPitchInited()) { // Try to initialize yaw and position if not done already
186 const std::string& cabinFrame = dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getCabinFrame();
187 const std::string& leftGnssFrame = dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getLeftGnssFrame();
188 const std::string& worldFrame = staticTransformsPtr_->getWorldFrame();
190 "InitYaw", int(gnssRate_), cabinFrame, cabinFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
191 leftGnssMsgPtr->header.stamp.toSec(), 1.0, yaw_W_C, Eigen::Matrix<double, 1, 1>::Ones(), worldFrame, worldFrame);
193 "InitPosition", int(gnssRate_), leftGnssFrame, leftGnssFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
194 leftGnssMsgPtr->header.stamp.toSec(), 1.0, W_t_W_GnssL, Eigen::Vector3d::Ones(), worldFrame, worldFrame);
195 if (this->initYawAndPosition(yaw_W_S1, W_t_W_S2)) {
196 REGULAR_COUT << " Set yaw and position successfully." << std::endl;
197 }
198 } else { // Already initialized --> add to graph
199 // Measurement type 1: GNSS Yaw ----------------------------------------------------
200 if (useGnssYawFlag_) {
201 // Sensor Frame Name
202 const std::string& sensorFrameName = dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getCabinFrame();
203 // Create yaw measurement and add it
204 Eigen::Matrix<double, 1, 1> gnssHeadingUnaryNoise(std::max(5.0 * leftGnssCovarianceXYZ.maxCoeff(), gnssHeadingUnaryNoise_));
206 "GnssYaw", int(gnssRate_), sensorFrameName, sensorFrameName + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
207 leftGnssMsgPtr->header.stamp.toSec(), 1.0, yaw_W_C, gnssHeadingUnaryNoise, fixedFrameName, staticTransformsPtr_->getWorldFrame());
208 this->addUnaryYawAbsoluteMeasurement(meas_yaw_W_C);
209 }
210 // Measurement type 2: GNSS Left Position ----------------------------------------------------
211 if (useLeftGnssFlag_) {
212 // Sensor Frame Name
213 const std::string& sensorFrameName = dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getLeftGnssFrame();
214 // Create left position measurement and add it
215 leftGnssCovarianceXYZ = Eigen::Vector3d(std::max(leftGnssCovarianceXYZ(0), gnssPositionUnaryNoise_),
216 std::max(leftGnssCovarianceXYZ(1), gnssPositionUnaryNoise_),
217 std::max(leftGnssCovarianceXYZ(2), gnssPositionUnaryNoise_));
219 "GnssLeftPosition", int(gnssRate_), sensorFrameName, sensorFrameName + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
220 leftGnssMsgPtr->header.stamp.toSec(), 1.0, W_t_W_GnssL, leftGnssCovarianceXYZ, fixedFrameName,
221 staticTransformsPtr_->getWorldFrame());
222 this->addUnaryPosition3AbsoluteMeasurement(meas_W_t_W_GnssL);
223 }
224 // Measurement type 3: GNSS Right Position ----------------------------------------------------
225 if (useRightGnssFlag_) {
226 // Sensor Frame Name
227 const std::string& sensorFrameName = dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getRightGnssFrame();
228 // Create right position measurement and add it
229 rightGnssCovarianceXYZ = Eigen::Vector3d(std::max(rightGnssCovarianceXYZ(0), gnssPositionUnaryNoise_),
230 std::max(rightGnssCovarianceXYZ(1), gnssPositionUnaryNoise_),
231 std::max(rightGnssCovarianceXYZ(2), gnssPositionUnaryNoise_));
233 "GnssRightPosition", int(gnssRate_), sensorFrameName, sensorFrameName + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
234 rightGnssMsgPtr->header.stamp.toSec(), 1.0, W_t_W_GnssR, rightGnssCovarianceXYZ, fixedFrameName,
235 staticTransformsPtr_->getWorldFrame());
236 this->addUnaryPosition3AbsoluteMeasurement(meas_W_t_W_GnssR);
237 }
238 // Visualizations ------------------------------------------------------------
239 // Left GNSS
240 if (useLeftGnssFlag_) {
241 addToPathMsg(measGnss_worldGnssLPathPtr_, staticTransformsPtr_->getWorldFrame(), leftGnssMsgPtr->header.stamp, W_t_W_GnssL,
242 graphConfigPtr_->imuBufferLength_ * 4);
244 }
245 // Right GNSS
246 if (useRightGnssFlag_) {
247 addToPathMsg(measGnss_worldGnssRPathPtr_, staticTransformsPtr_->getWorldFrame(), rightGnssMsgPtr->header.stamp, W_t_W_GnssR,
248 graphConfigPtr_->imuBufferLength_ * 4);
250 }
251 }
252}
253
255 const std::shared_ptr<holistic_fusion::SafeIntegratedNavState>& preIntegratedNavStatePtr,
256 const std::shared_ptr<holistic_fusion::SafeNavStateWithCovarianceAndBias>& optimizedStateWithCovarianceAndBiasPtr) {
257 // Lookup I->B, also influenced by rotation of cabin
258 static tf::StampedTransform transform_I_B;
259 tfListener_.waitForTransform(staticTransformsPtr_->getImuFrame(),
260 dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getBaseLinkFrame(), ros::Time(0),
261 ros::Duration(0.1));
262 tfListener_.lookupTransform(staticTransformsPtr_->getImuFrame(),
263 dynamic_cast<ExcavatorStaticTransforms*>(staticTransformsPtr_.get())->getBaseLinkFrame(), ros::Time(0),
264 transform_I_B);
265 // Update Imu->Base transformation
266 holistic_fusion::tfToIsometry3(transform_I_B, staticTransformsPtr_->lv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(),
267 staticTransformsPtr_->getBaseLinkFrame()));
268 // Updaate Base->Imu transformation
269 staticTransformsPtr_->lv_T_frame1_frame2(staticTransformsPtr_->getBaseLinkFrame(), staticTransformsPtr_->getImuFrame()) =
270 staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), staticTransformsPtr_->getBaseLinkFrame()).inverse();
271
272 // Publish state
273 holistic_fusion::HolisticFusionRos::publishState(preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr);
274}
275
276} // namespace excavator_se
#define NUM_GNSS_CALLBACKS_UNTIL_START
#define REGULAR_COUT
#define COLOR_END
#define GREEN_START
#define ROS_QUEUE_SIZE
#define YELLOW_START
void readParams(const ros::NodeHandle &privateNode) override
message_filters::Subscriber< sensor_msgs::NavSatFix > subGnssLeft_
Eigen::Matrix< double, 6, 1 > lioSe3AlignmentRandomWalk_
Eigen::Matrix< double, 6, 1 > lioPoseUnaryNoise_
message_filters::sync_policies::ExactTime< sensor_msgs::NavSatFix, sensor_msgs::NavSatFix > GnssExactSyncPolicy
void initializeSubscribers(ros::NodeHandle &privateNode) override
virtual void publishState(const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > &preIntegratedNavStatePtr, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr) override
nav_msgs::PathPtr measGnss_worldGnssRPathPtr_
message_filters::Subscriber< sensor_msgs::NavSatFix > subGnssRight_
void gnssCallback_(const sensor_msgs::NavSatFix::ConstPtr &leftGnssPtr, const sensor_msgs::NavSatFix::ConstPtr &rightGnssPtr)
nav_msgs::PathPtr measGnss_worldGnssLPathPtr_
void initializePublishers(ros::NodeHandle &privateNode) override
Eigen::Matrix< double, 6, 1 > initialSe3AlignmentStdDev_
ExcavatorEstimator(std::shared_ptr< ros::NodeHandle > privateNodePtr)
void lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr &lidar_odom_ptr)
boost::shared_ptr< message_filters::Synchronizer< GnssExactSyncPolicy > > gnssExactSyncPtr_
void initializeMessages(ros::NodeHandle &privateNode) override
std::shared_ptr< holistic_fusion::GnssHandler > gnssHandlerPtr_
void addUnaryYawAbsoluteMeasurement(const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S) override
void addUnaryPosition3AbsoluteMeasurement(UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_t_R_S) override
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 setup(const std::shared_ptr< StaticTransforms > staticTransformsPtr)
static void addToPathMsg(const nav_msgs::PathPtr &pathPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Vector3d &t, int maxBufferLength)
HolisticFusionRos(const std::shared_ptr< ros::NodeHandle > &privateNodePtr)
virtual void publishState(const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > &integratedNavStatePtr, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr)
std::shared_ptr< GraphConfig > graphConfigPtr_
bool initYawAndPosition(const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S1, const UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &M_t_M_S2)
std::shared_ptr< StaticTransforms > staticTransformsPtr_
bool initHeadingAndPosition(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &T_M_S)
const std::string & getBaseLinkFrame()
Class to represent x-dimensional absolute unary measurement in the graph.
void odomMsgToEigen(const nav_msgs::Odometry &odomLidar, Eigen::Matrix4d &T)
void tfToIsometry3(const tf::Transform &tf_T, Eigen::Isometry3d &T)
static RobustNorm None()
Definition Measurement.h:33