My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
Position3Estimator.cpp
Go to the documentation of this file.
1/*
2Copyright 2024 by Julian Nubert & Timon Mathis, Robotic Systems Lab & Autonomous 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// C++
12#include <boost/filesystem.hpp>
13
14// Project
17
18// Workspace
22
23namespace position3_se {
24
25Position3Estimator::Position3Estimator(std::shared_ptr<ros::NodeHandle> privateNodePtr) : holistic_fusion::HolisticFusionRos(privateNodePtr) {
26 REGULAR_COUT << GREEN_START << " Position3Estimator-Constructor called." << COLOR_END << std::endl;
27
28 // Configurations ----------------------------
29 // Static Transforms
30 staticTransformsPtr_ = std::make_shared<Position3StaticTransforms>(
32
33 // GNSS Handler
34 gnssHandlerPtr_ = std::make_shared<holistic_fusion::GnssHandler>();
35
36 // Alignment Handler
37 trajectoryAlignmentHandler_ = std::make_shared<holistic_fusion::TrajectoryAlignmentHandler>();
38
39 // Setup
41}
42
43//---------------------------------------------------------------
45 REGULAR_COUT << GREEN_START << " Position3Estimator-Setup called." << COLOR_END << std::endl;
46
47 // Read parameters ----------------------------
49
50 // Super class
51 HolisticFusionRos::setup(staticTransformsPtr_);
52
53 // Publishers ----------------------------
55
56 // Subscribers ----------------------------
58
59 // Messages ----------------------------
61
62 // Services ----------------------------
64
65 // Static Transforms
66 staticTransformsPtr_->findTransformations();
67
68 // Wrap up ----------------------------
69 REGULAR_COUT << GREEN_START << " Set up successfully." << COLOR_END << std::endl;
70}
71
72//---------------------------------------------------------------
74 // Status
75 REGULAR_COUT << GREEN_START << " Initializing Publishers..." << COLOR_END << std::endl;
76
77 // Paths
79 pubMeasWorldPrismPositionPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measPosition_path_world_prism", ROS_QUEUE_SIZE);
80 }
82 pubMeasWorldGnssPositionPath_ = privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measPosition_path_world_gnss", ROS_QUEUE_SIZE);
83 }
86 privateNode.advertise<nav_msgs::Path>("/holistic_fusion/measPosition_path_world_gnss_offline_pose", ROS_QUEUE_SIZE);
87 }
88}
89
91 // Prism Position
93 subPrismPosition_ = privateNode.subscribe<geometry_msgs::PointStamped>(
94 "/prism_position_topic", ROS_QUEUE_SIZE, &Position3Estimator::prismPositionCallback_, this, ros::TransportHints().tcpNoDelay());
95 }
96 // GNSS
98 subGnssPosition_ = privateNode.subscribe<sensor_msgs::NavSatFix>(
99 "/gnss_position_topic", ROS_QUEUE_SIZE, &Position3Estimator::gnssPositionCallback_, this, ros::TransportHints().tcpNoDelay());
100 }
101 // GNSS Offline Pose
104 privateNode.subscribe<nav_msgs::Odometry>("/gnss_offline_pose_topic", ROS_QUEUE_SIZE, &Position3Estimator::gnssOfflinePoseCallback_,
105 this, ros::TransportHints().tcpNoDelay());
106 }
107
108 // Log
109 std::cout << YELLOW_START << "FactorGraphFiltering" << COLOR_END << " Initialized Position subscriber (on position_topic)." << std::endl;
110}
111
112//---------------------------------------------------------------
114 // Status
115 REGULAR_COUT << GREEN_START << " Initializing Messages..." << COLOR_END << std::endl;
116
117 // Paths
118 measPosition_worldPrismPositionPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
119 measPosition_worldGnssPositionPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
120 measPosition_worldGnssOfflinePosePathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path);
121}
122
123// --------------------------------------------------------------
125 // Trigger Prism Unary
128 privateNode.advertiseService("/atn_position3_fuser_node/togglePrismUnary", &Position3Estimator::srvTogglePrismUnaryCallback_, this);
129 }
130 // Trigger GNSS Unary
133 privateNode.advertiseService("/atn_position3_fuser_node/toggleGnssUnary", &Position3Estimator::srvToggleGnssUnaryCallback_, this);
134 }
135 // Trigger GNSS Offline Pose Unary
137 srvToggleGnssOfflinePoseUnary_ = privateNode.advertiseService("/atn_position3_fuser_node/toggleGnssOfflinePoseUnary",
139 }
140}
141
142// Overwritten service for offline optimization
143bool Position3Estimator::srvOfflineSmootherOptimizeCallback(holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request& req,
144 holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response& res) {
145 std::cout << "Position3Estimator: srvOfflineSmootherOptimizeCallback (derived class) called." << std::endl;
146 // Call parent class and create most of the files already
148
149 // File streams
150 std::map<std::string, std::ofstream> fileStreams;
151
152 // Write T_totalStation_totalStationOld_ to file ----------------------------
153 // Get time string by finding latest directory in optimizationResultLoggingPath
155 if (timeString.empty()) {
156 REGULAR_COUT << " Could not find latest directory in " << optimizationResultLoggingPath << std::endl;
157 return false;
158 } else {
159 REGULAR_COUT << " Found latest directory in " << optimizationResultLoggingPath << ": " << timeString << std::endl;
160 }
161 // Create file stream
162 std::string transformIdentifier = "R_6D_transform_" + totalStationReferenceFrame_ + "_to_" + totalStationReferenceFrame_ + "Old";
163 holistic_fusion::createPose3CsvFileStream(fileStreams, optimizationResultLoggingPath, transformIdentifier, timeString, false);
164 // Add to file
165 holistic_fusion::writePose3ToCsvFile(fileStreams, T_totalStation_totalStationOld_, transformIdentifier, ros::Time::now().toSec(), false);
166 REGULAR_COUT << " Wrote T_totalStation_totalStationOld to file." << std::endl;
167
168 // Depending on whether GNSS or Prism was used for initialization, write the corresponding T_W_R identity to file ------------------------
170 // Write T_totalStation_totalStationOld_ to file
171 std::string transformIdentifier = "R_6D_transform_" + staticTransformsPtr_->getWorldFrame() + "_to_" + gnssReferenceFrame_;
172 holistic_fusion::createPose3CsvFileStream(fileStreams, optimizationResultLoggingPath, transformIdentifier, timeString, false);
173 // Add to file
174 holistic_fusion::writePose3ToCsvFile(fileStreams, Eigen::Isometry3d::Identity(), transformIdentifier, ros::Time::now().toSec(), false);
175 REGULAR_COUT << " Wrote T_W_gnssReferenceFrame to file." << std::endl;
176 } else {
177 // Write T_totalStation_enu_ to file
178 std::string transformIdentifier = "R_6D_transform_" + staticTransformsPtr_->getWorldFrame() + "_to_" + totalStationReferenceFrame_;
179 holistic_fusion::createPose3CsvFileStream(fileStreams, optimizationResultLoggingPath, transformIdentifier, timeString, false);
180 // Add to file
181 holistic_fusion::writePose3ToCsvFile(fileStreams, T_totalStation_enu_, transformIdentifier, ros::Time::now().toSec(), false);
182 REGULAR_COUT << " Wrote T_W_totalStationReferenceFrame to file." << std::endl;
183 }
184
185 // Close all file streams
186 for (auto& pair : fileStreams) {
187 pair.second.close();
188 }
189 // Return
190 return success;
191}
192
193//---------------------------------------------------------------
194void Position3Estimator::prismPositionCallback_(const geometry_msgs::PointStamped::ConstPtr& leicaPositionPtr) {
195 // Counter
197
198 // Fixed Frame
199 std::string fixedFrame;
201 fixedFrame = staticTransformsPtr_->getWorldFrame();
202 } else {
203 fixedFrame = leicaPositionPtr->header.frame_id;
204 }
205
206 // Translate to Eigen
207 Eigen::Vector3d positionMeas = Eigen::Vector3d(leicaPositionPtr->point.x, leicaPositionPtr->point.y, leicaPositionPtr->point.z);
209
210 // Case: First call
212 REGULAR_COUT << " First prism position measurement received. Setting initial position to " << positionMeas.transpose() << std::endl;
213 // Set initial position
214 initialPrismPosition_ = positionMeas;
215 // Set Reference Frame
216 totalStationReferenceFrame_ = leicaPositionPtr->header.frame_id;
217 }
218
219 // Case: Not moved enough --> check if moved enough in meanwhile, only relevant if not initialized by GNSS
221 if ((positionMeas - initialPrismPosition_).norm() > PRISM_MOVED_ENOUGH_THRESHOLD) {
223 REGULAR_COUT << " Prism moved enough from initial position of " << initialPrismPosition_.transpose() << " to "
224 << positionMeas.transpose() << std::endl;
225 }
226 }
227
228 // Flag
229 bool addToGraphFlag = true;
230
231 // Case: Graph is not initialized by this callback, but instead by GNSS
233 trajectoryAlignmentHandler_->addR3Position(positionMeas, leicaPositionPtr->header.stamp.toSec());
234 // In radians
235 double yaw{0};
236 if (!(trajectoryAlignmentHandler_->alignTrajectories(yaw, T_totalStation_enu_))) {
237 if (prismPositionCallbackCounter_ % 10 == 0) {
238 REGULAR_COUT << " Graph was initialized by GNSS, hence trying to align. Trajectory alignment not ready. Waiting for more motion."
239 << std::endl;
240 }
241 addToGraphFlag = false;
242 } else {
244 // Only keep yaw of the orientation part of the transformation
245 REGULAR_COUT << GREEN_START << " Total Station to Old Total Station Transformation: " << COLOR_END
246 << T_totalStation_totalStationOld_.matrix() << std::endl;
248 REGULAR_COUT << GREEN_START << " Total Station to Old Total Station Transformation after removing roll and pitch: " << COLOR_END
249 << T_totalStation_totalStationOld_.matrix() << std::endl;
250 // Prepare for using this transformation in the graph
252 return; // Do not add unary measurement this round
253 }
254 }
255
256 // State Machine
258 // Try to initialize yaw and position if not done already
259 const std::string& baseFrame = staticTransformsPtr_->getBaseLinkFrame();
260 const std::string& prismFrame = dynamic_cast<Position3StaticTransforms*>(staticTransformsPtr_.get())->getPrismPositionMeasFrame();
261 const std::string& worldFrame = staticTransformsPtr_->getWorldFrame();
264 leicaPositionPtr->header.stamp.toSec(), 1.0, 0.0, Eigen::Matrix<double, 1, 1>::Ones(), worldFrame, worldFrame);
266 "InitPosition", int(prismPositionRate_), prismFrame, prismFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
267 leicaPositionPtr->header.stamp.toSec(), 1.0, positionMeas, Eigen::Vector3d::Ones(), worldFrame, worldFrame);
268 if (this->initYawAndPosition(yaw_W_S1, W_t_W_S2)) {
269 std::cout << "-------------------" << std::endl;
270 REGULAR_COUT << RED_START << " Prism callback is setting global yaw." << COLOR_END << std::endl;
271 std::cout << "-------------------" << std::endl;
272 REGULAR_COUT << " Set yaw and position successfully." << std::endl;
273 } else {
274 REGULAR_COUT << " Could not set yaw and position." << std::endl;
275 }
276 }
277 // Else if active
278 else if (usePrismPositionUnaryFlag_ && addToGraphFlag) {
279 const std::string& positionMeasFrame =
281 // Consider whether graph was initialized by GNSS or not ----------------------------
282 // if (initializedByGnssFlag_) {
283 // fixedFrame = leicaPositionPtr->header.frame_id;
284 // } else {
285 // fixedFrame = staticTransformsPtr_->getWorldFrame();
286 // }
287 // Potentially transform measurement to start ENU frame (if world origin was set to coincide with GNSS)
289 positionMeas = T_totalStation_totalStationOld_ * positionMeas;
290 }
291
292 // Already initialized --> add position measurement to graph ----------------------------
294 "LeicaPosition", int(prismPositionRate_), positionMeasFrame, positionMeasFrame + sensorFrameCorrectedNameId,
295 holistic_fusion::RobustNorm::None(), leicaPositionPtr->header.stamp.toSec(), POS_COVARIANCE_VIOLATION_THRESHOLD, positionMeas,
296 positionCovarianceXYZ, fixedFrame, staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, prismSe3AlignmentRandomWalk_);
297 this->addUnaryPosition3AbsoluteMeasurement(meas_W_t_W_P);
298 }
299 // Else if not active
300 else if ((prismPositionCallbackCounter_ % 10) == 0 && addToGraphFlag) {
301 REGULAR_COUT << " Prism unary measurement is turned off." << std::endl;
302 }
303
304 // Visualizations
305 // Fixed Frame
306 std::string msgFixedFrame = fixedFrame;
307 if (msgFixedFrame != staticTransformsPtr_->getWorldFrame()) {
308 msgFixedFrame += referenceFrameAlignedNameId;
309 }
310 // Add to Path
312 addToPathMsg(measPosition_worldPrismPositionPathPtr_, msgFixedFrame, leicaPositionPtr->header.stamp, positionMeas,
313 graphConfigPtr_->imuBufferLength_ * 4);
315 }
316}
317
318//---------------------------------------------------------------
319void Position3Estimator::gnssPositionCallback_(const sensor_msgs::NavSatFix::ConstPtr& gnssPositionPtr) {
320 throw std::runtime_error("Position3Estimator: outdated in terms of logic.");
321
322 // Counter
324
325 // Translate to Eigen
326 Eigen::Vector3d gnssCoord = Eigen::Vector3d(gnssPositionPtr->latitude, gnssPositionPtr->longitude, gnssPositionPtr->altitude);
327 Eigen::Vector3d estStdDevXYZ(sqrt(gnssPositionPtr->position_covariance[0]), sqrt(gnssPositionPtr->position_covariance[4]),
328 sqrt(gnssPositionPtr->position_covariance[8]));
329
330 // Initialize GNSS Handler
331 if (gnssPositionCallbackCounter_ < NUM_GNSS_CALLBACKS_UNTIL_START) { // Accumulate measurements
332 // Wait until measurements got accumulated
333 accumulatedGnssCoordinates_ += gnssCoord;
334 if ((gnssPositionCallbackCounter_ % 10) == 0) {
335 REGULAR_COUT << " NOT ENOUGH GNSS MESSAGES ARRIVED!" << std::endl;
336 }
337 return;
338 } else if (gnssPositionCallbackCounter_ == NUM_GNSS_CALLBACKS_UNTIL_START) { // Initialize GNSS Handler
340 REGULAR_COUT << " GNSS Handler initialized." << std::endl;
341 return;
342 }
343
344 // Convert to Cartesian Coordinates
345 Eigen::Vector3d W_t_W_Gnss;
346 gnssHandlerPtr_->convertNavSatToPosition(gnssCoord, W_t_W_Gnss);
347 // std::string fixedFrame = staticTransformsPtr_->getWorldFrame();
348 std::string fixedFrame = gnssPositionPtr->header.frame_id;
349
350 // Regular Operation
351 // Initialize if needed and no prism is used
353 // Try to initialize yaw and position if not done already
354 const std::string& baseFrame = staticTransformsPtr_->getBaseLinkFrame();
355 const std::string& gnssFrame = dynamic_cast<Position3StaticTransforms*>(staticTransformsPtr_.get())->getGnssPositionMeasFrame();
356 const std::string& worldFrame = staticTransformsPtr_->getWorldFrame();
358 "InitYaw", int(gnssPositionRate_), baseFrame, baseFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
359 gnssPositionPtr->header.stamp.toSec(), 1.0, 0.0, Eigen::Matrix<double, 1, 1>::Ones(), worldFrame, worldFrame);
361 "InitPosition", int(gnssPositionRate_), gnssFrame, gnssFrame + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
362 gnssPositionPtr->header.stamp.toSec(), 1.0, W_t_W_Gnss, Eigen::Vector3d::Ones(), worldFrame, worldFrame);
363 if (this->initYawAndPosition(yaw_W_S1, W_t_W_S2)) {
364 REGULAR_COUT << " GNSS set yaw and position successfully, as there is no prism." << std::endl;
365 } else {
366 REGULAR_COUT << " Could not set yaw and position." << std::endl;
367 }
368 }
369 // Else if inited or prism is used
371 const std::string& positionMeasFrame = dynamic_cast<Position3StaticTransforms*>(staticTransformsPtr_.get())->getGnssPositionMeasFrame();
372 // Already initialized --> add position measurement to graph
374 "GnssPosition", int(gnssPositionRate_), positionMeasFrame, positionMeasFrame + sensorFrameCorrectedNameId,
375 holistic_fusion::RobustNorm::None(), gnssPositionPtr->header.stamp.toSec(), POS_COVARIANCE_VIOLATION_THRESHOLD, W_t_W_Gnss, estStdDevXYZ,
377 this->addUnaryPosition3AbsoluteMeasurement(meas_W_t_W_P);
378 }
379 // Status
380 else if ((gnssPositionCallbackCounter_ % 10) == 0) {
381 REGULAR_COUT << " GNSS unary measurement is turned off or roll/pitch not yet inited." << std::endl;
382 }
383
384 // Adjust frame name
385 if (fixedFrame != staticTransformsPtr_->getWorldFrame()) {
386 fixedFrame += referenceFrameAlignedNameId;
387 }
388
389 // Visualizations
390 addToPathMsg(measPosition_worldGnssPositionPathPtr_, fixedFrame, gnssPositionPtr->header.stamp, W_t_W_Gnss,
391 graphConfigPtr_->imuBufferLength_ * 4);
392 // Publish Path
394}
395
396//---------------------------------------------------------------
397void Position3Estimator::gnssOfflinePoseCallback_(const nav_msgs::Odometry::ConstPtr& gnssOfflinePosePtr) {
398 // Counter
400
401 // Fixed Frame
402 std::string fixedFrame;
404 fixedFrame = staticTransformsPtr_->getWorldFrame();
405 } else {
406 fixedFrame = gnssOfflinePosePtr->header.frame_id;
407 }
408
409 // Case: First call
411 REGULAR_COUT << " First GNSS offline pose measurement received." << std::endl;
412 // Set Reference Frame
413 gnssReferenceFrame_ = gnssOfflinePosePtr->header.frame_id;
414 }
415
416 // Prepare Data
417 Eigen::Isometry3d T_ENU_Gk = Eigen::Isometry3d::Identity();
418 holistic_fusion::odomMsgToEigen(*gnssOfflinePosePtr, T_ENU_Gk.matrix());
419 double gnssUnaryTimeK = gnssOfflinePosePtr->header.stamp.toSec();
420
421 // If initialized by gnss --> make sure we can do the alignment with prism
423 trajectoryAlignmentHandler_->addSe3Position(T_ENU_Gk.translation(), gnssOfflinePosePtr->header.stamp.toSec());
424 }
425
426 // Get Uncertainty from message
427 auto estGnssOfflinePoseMeasUnaryNoiseList = gnssOfflinePosePtr->pose.covariance;
428 Eigen::Matrix<double, 6, 1> estGnssOfflinePoseMeasUnaryNoise;
429 estGnssOfflinePoseMeasUnaryNoise << sqrt(estGnssOfflinePoseMeasUnaryNoiseList[0]), sqrt(estGnssOfflinePoseMeasUnaryNoiseList[7]),
430 sqrt(estGnssOfflinePoseMeasUnaryNoiseList[14]), sqrt(estGnssOfflinePoseMeasUnaryNoiseList[21]),
431 sqrt(estGnssOfflinePoseMeasUnaryNoiseList[28]), sqrt(estGnssOfflinePoseMeasUnaryNoiseList[35]);
432
433 // Measurement
434 const std::string& gnssFrameName =
437 "Lidar_unary_6D", int(gnssOfflinePoseRate_), gnssFrameName, gnssFrameName + sensorFrameCorrectedNameId, holistic_fusion::RobustNorm::None(),
438 gnssUnaryTimeK, 1.0, T_ENU_Gk, gnssOfflinePoseMeasUnaryNoise_, fixedFrame, staticTransformsPtr_->getWorldFrame(),
440
441 // Wait until enough measurements have arrived
443 return;
444 }
445 // Initialize if needed and no prism is used
446 else if (!areYawAndPositionInited() && areRollAndPitchInited() && initializeUsingGnssFlag_) { // Initializing if no GNSS
447 std::cout << "-------------------" << std::endl;
448 REGULAR_COUT << RED_START << " GNSS offline odometry callback is setting global yaw." << COLOR_END << std::endl;
449 std::cout << "-------------------" << std::endl;
450 this->initHeadingAndPosition(unary6DMeasurement);
451 }
452 // Otherwise just add measurement
453 else if (areYawAndPositionInited()) { // Already initialized --> unary factor
454 // If prism has not moved enough, do not add GNSS measurements
455 bool addToOnlineSmootherFlag = true;
457 if ((gnssOfflinePoseCallbackCounter_ % 100) == 0) {
458 REGULAR_COUT << " PRISM HAS NOT MOVED ENOUGH YET! Not adding GNSS measurements to online graph." << std::endl;
459 }
460 addToOnlineSmootherFlag = false;
461 }
462 this->addUnaryPose3AbsoluteMeasurement(unary6DMeasurement, addToOnlineSmootherFlag);
463 }
464
465 // Visualization ----------------------------
466 // Add to path message
467 addToPathMsg(measPosition_worldGnssOfflinePosePathPtr_, fixedFrame, gnssOfflinePosePtr->header.stamp, T_ENU_Gk.translation(),
468 graphConfigPtr_->imuBufferLength_ * 4);
469
470 // Publish Path
472}
473
474//---------------------------------------------------------------
475bool Position3Estimator::srvTogglePrismUnaryCallback_(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
476 // Toggle Flag
478
479 // Response
480 res.success = true;
481 res.message = "Prism unary measurement toggled " + std::string(usePrismPositionUnaryFlag_ ? "on" : "off") + ".";
482
483 return true;
484}
485
486//---------------------------------------------------------------
487bool Position3Estimator::srvToggleGnssUnaryCallback_(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
488 // Toggle Flag
490
491 // Response
492 res.success = true;
493 res.message = "GNSS unary measurement toggled " + std::string(useGnssPositionUnaryFlag_ ? "on" : "off") + ".";
494
495 return true;
496}
497
498//---------------------------------------------------------------
499bool Position3Estimator::srvToggleGnssOfflinePoseUnaryCallback_(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
500 // Toggle Flag
502
503 // Response
504 res.success = true;
505 res.message = "GNSS offline pose unary measurement toggled " + std::string(useGnssOfflinePoseUnaryFlag_ ? "on" : "off") + ".";
506
507 return true;
508}
509
510} // namespace position3_se
#define NUM_GNSS_CALLBACKS_UNTIL_START
#define REGULAR_COUT
#define COLOR_END
#define GREEN_START
#define RED_START
#define ROS_QUEUE_SIZE
#define YELLOW_START
#define POS_COVARIANCE_VIOLATION_THRESHOLD
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.
virtual bool srvOfflineSmootherOptimizeCallback(holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &req, holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &res)
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)
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)
Class to represent x-dimensional absolute unary measurement in the graph.
Eigen::Isometry3d T_totalStation_totalStationOld_
Position3Estimator(std::shared_ptr< ros::NodeHandle > privateNodePtr)
nav_msgs::PathPtr measPosition_worldGnssPositionPathPtr_
bool srvOfflineSmootherOptimizeCallback(holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &req, holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &res) override
void initializeMessages(ros::NodeHandle &privateNode) override
ros::ServiceServer srvToggleGnssPositionUnary_
std::shared_ptr< holistic_fusion::GnssHandler > gnssHandlerPtr_
void initializeServices(ros::NodeHandle &privateNode) override
void gnssPositionCallback_(const sensor_msgs::NavSatFix::ConstPtr &gnssPositionPtr)
Eigen::Matrix< double, 6, 1 > gnssSe3AlignmentRandomWalk_
void initializePublishers(ros::NodeHandle &privateNode) override
void readParams(const ros::NodeHandle &privateNode) override
void gnssOfflinePoseCallback_(const nav_msgs::Odometry::ConstPtr &gnssOfflinePosePtr)
Eigen::Matrix< double, 6, 1 > gnssOfflinePoseMeasUnaryNoise_
void prismPositionCallback_(const geometry_msgs::PointStamped::ConstPtr &leicaPositionPtr)
Eigen::Matrix< double, 6, 1 > initialSe3AlignmentNoise_
ros::ServiceServer srvToggleGnssOfflinePoseUnary_
ros::ServiceServer srvTogglePrismPositionUnary_
std::shared_ptr< holistic_fusion::TrajectoryAlignmentHandler > trajectoryAlignmentHandler_
nav_msgs::PathPtr measPosition_worldGnssOfflinePosePathPtr_
Eigen::Matrix< double, 6, 1 > prismSe3AlignmentRandomWalk_
static constexpr bool constexprUseGnssPositionUnaryFlag_
static constexpr bool constexprUsePrismPositionUnaryFlag_
static constexpr double PRISM_MOVED_ENOUGH_THRESHOLD
nav_msgs::PathPtr measPosition_worldPrismPositionPathPtr_
bool srvToggleGnssOfflinePoseUnaryCallback_(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
void initializeSubscribers(ros::NodeHandle &privateNode) override
static constexpr bool constexprUseGnssOfflinePoseUnaryFlag_
bool srvToggleGnssUnaryCallback_(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
bool srvTogglePrismUnaryCallback_(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
void createPose3CsvFileStream(std::map< std::string, std::ofstream > &fileStreams, const std::string &savePath, const std::string &transformIdentifier, const std::string &timeString, const bool saveCovarianceFlag)
void odomMsgToEigen(const nav_msgs::Odometry &odomLidar, Eigen::Matrix4d &T)
std::string getLatestSubdirectory(const std::string &directoryPath)
void inPlaceRemoveRollPitch(Eigen::Isometry3d &T)
void writePose3ToCsvFile(std::map< std::string, std::ofstream > &fileStreams, const Eigen::Isometry3d pose, const std::string &transformIdentifier, const double timeStamp, const bool saveCovarianceFlag, const std::optional< Eigen::Matrix< double, 6, 6 > > &optionalPoseCovarianceInWorldRos=std::nullopt)
static RobustNorm None()
Definition Measurement.h:33