My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
HolisticFusion.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// C++
9#include <chrono>
10#include <cmath>
11#include <stdexcept>
12
13// Implementation
15
16// Workspace
19
20namespace holistic_fusion {
21
22// Public -----------------------------------------------------------
25 REGULAR_COUT << GREEN_START << " HolisticFusion-Constructor called." << COLOR_END << std::endl;
26}
27
30 // Properly stop the optimizeGraphThread_ before destruction to avoid std::terminate.
32 if (optimizeGraphThread_.joinable()) {
34 }
35}
36
37void HolisticFusion::setup(const std::shared_ptr<GraphConfig> graphConfigPtr, const std::shared_ptr<StaticTransforms> staticTransformsPtr) {
38 REGULAR_COUT << GREEN_START << " HolisticFusion-Setup called." << COLOR_END << std::endl;
39
40 // Check if setup has been called before
41 if (graphConfigPtr == nullptr || staticTransformsPtr == nullptr) {
42 throw std::runtime_error("setup() of inheriting classes did not has not been called before.");
43 } else {
44 graphConfigPtr_ = graphConfigPtr;
45 staticTransformsPtr_ = staticTransformsPtr;
46 }
47
48 if (!graphConfigPtr_->staticAtStartup_) {
49 graphConfigPtr_->gyroBiasPrior_.setZero();
50 throw std::logic_error("HolisticFusion: initialization_params.static_at_startup=false: in-motion initialization is not implemented.");
51 }
52
53 if (graphConfigPtr_->additionalOptimizationIterations_ < 0) {
54 throw std::invalid_argument("HolisticFusion: additionalOptimizationIterations must be nonnegative.");
55 }
56 if (graphConfigPtr_->createStateEveryNthImuMeasurement_ < 1) {
57 throw std::invalid_argument("HolisticFusion: createStateEveryNthImuMeasurement must be positive.");
58 }
59
60 // Imu Buffer
61 // Initialize IMU buffer
62 coreImuBufferPtr_ = std::make_shared<holistic_fusion::ImuBuffer>(graphConfigPtr_);
63
64 // Graph Manager
65 std::cout << "HolisticFusion: Creating GraphManager." << std::endl;
67 std::make_shared<GraphManager>(graphConfigPtr_, staticTransformsPtr_->getImuFrame(), staticTransformsPtr_->getWorldFrame());
68 std::cout << "HolisticFusion: GraphManager created." << std::endl;
69
72 REGULAR_COUT << " Initialized thread for optimizing the graph in parallel." << std::endl;
73}
74
75// Trigger functions -----------------------
76bool HolisticFusion::optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag) {
77 return graphMgrPtr_->optimizeSlowBatchSmoother(maxIterations, savePath, saveCovarianceFlag);
78}
79
80std::optional<BatchOptimizationResult> HolisticFusion::optimizeSlowBatchSmoother(int maxIterations, const bool computePoseCovarianceFlag) {
81 return graphMgrPtr_->optimizeSlowBatchSmoother(maxIterations, computePoseCovarianceFlag);
82}
83
84bool HolisticFusion::logRealTimeNavStates(const std::string& savePath) {
85 // String of time without line breaks: year_month_day_hour_min_sec
86 std::ostringstream oss;
87 std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
88 std::tm now_tm = *std::localtime(&now_time_t);
89 oss << std::put_time(&now_tm, "%Y_%m_%d_%H_%M_%S");
90 // Convert stream to string
91 std::string timeString = oss.str();
92 // Return
93 return graphMgrPtr_->logRealTimeNavStates(savePath, timeString);
94}
95
96// Getter functions -----------------------
100
104
106 return initedGraphFlag_;
107}
108
109// Initialization -----------------------
111 const std::lock_guard<std::mutex> initYawAndPositionLock(initYawAndPositionMutex_);
112 if (!canInitYawAndPosition_()) {
113 return false;
114 }
115
116 const gtsam::Pose3 T_W_Smeas = initialT_W_fixedFrame_(T_M_S) * gtsam::Pose3(T_M_S.unaryMeasurement().matrix());
117 const gtsam::Rot3 R_W_Iest(preIntegratedNavStatePtr_->getT_W_Ik().rotation());
118 const gtsam::Rot3 R_I_S(
119 staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), T_M_S.sensorFrameName()).rotation());
120
121 // Removing the twist of the offset about the z-axis of the world leaves a rotation about a horizontal axis, i.e. zero heading error
122 const gtsam::Quaternion q_W_Wmeas = (R_W_Iest * R_I_S * T_W_Smeas.rotation().inverse()).toQuaternion();
123 const gtsam::Rot3 R_W_I = gtsam::Rot3::Rz(-2.0 * std::atan2(q_W_Wmeas.z(), q_W_Wmeas.w())) * R_W_Iest;
124
125 setInitialOrientationAndPosition_(R_W_I, T_W_Smeas.translation(), T_M_S.sensorFrameName());
126 return true;
127}
128
131 if (yaw_W_S1.fixedFrameName() != yaw_W_S1.worldFrameName()) {
132 throw std::invalid_argument("HolisticFusion::initYawAndPosition: the yaw of " + yaw_W_S1.measurementName() + " is in frame " +
133 yaw_W_S1.fixedFrameName() + ", but it must be in the world frame " + yaw_W_S1.worldFrameName() +
134 ". Use initHeadingAndPosition() for an orientation in a fixed frame.");
135 }
136 const std::lock_guard<std::mutex> initYawAndPositionLock(initYawAndPositionMutex_);
137 if (!canInitYawAndPosition_()) {
138 return false;
139 }
140
141 const gtsam::Rot3 R_W_Iest(preIntegratedNavStatePtr_->getT_W_Ik().rotation());
142 const gtsam::Rot3 R_I_S1(
143 staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), yaw_W_S1.sensorFrameName()).rotation());
144 // A rotation about the z-axis of the world adds its angle to the Euler yaw
145 const gtsam::Rot3 R_W_I = gtsam::Rot3::Rz(yaw_W_S1.unaryMeasurement() - (R_W_Iest * R_I_S1).yaw()) * R_W_Iest;
146
147 const Eigen::Vector3d W_t_W_S2 = initialT_W_fixedFrame_(M_t_M_S2).transformFrom(gtsam::Point3(M_t_M_S2.unaryMeasurement()));
148 setInitialOrientationAndPosition_(R_W_I, W_t_W_S2, M_t_M_S2.sensorFrameName());
149 return true;
150}
151
153 const std::lock_guard<std::mutex> initYawAndPositionLock(initYawAndPositionMutex_);
154 if (!canInitYawAndPosition_()) {
155 return false;
156 }
158 REGULAR_COUT << GREEN_START << " Initialized the heading and the position of " << staticTransformsPtr_->getInitializationFrame()
159 << " to identity." << COLOR_END << std::endl;
160 return true;
161}
162
164 if (!alignedImuFlag_) {
165 REGULAR_COUT << RED_START << " Tried to set initial yaw, but initial attitude is not yet set." << COLOR_END << std::endl;
166 return false;
167 }
169 REGULAR_COUT << RED_START << " Tried to set initial yaw, but it has been set before." << COLOR_END << std::endl;
170 return false;
171 }
172 return true;
173}
174
176 // Without optimized fixed frames, measurements in M are taken as measurements in the world
177 if (measurement.fixedFrameName() == measurement.worldFrameName() || !graphConfigPtr_->optimizeReferenceFramePosesWrtWorldFlag_) {
178 return gtsam::Pose3::Identity();
179 }
180 return graphMgrPtr_->getInitialWorldFrameToFixedFrameTransform(measurement.fixedFrameName());
181}
182
183void HolisticFusion::setInitialOrientationAndPosition_(const gtsam::Rot3& R_W_I, const Eigen::Vector3d& W_t_W_S,
184 const std::string& sensorFrame) {
185 preIntegratedNavStatePtr_->updateOrientationInWorld(R_W_I.matrix(), graphConfigPtr_->odomNotJumpAtStartFlag_);
186 const Eigen::Vector3d W_t_W_I = W_t_W_Frame1_to_W_t_W_Frame2_(W_t_W_S, sensorFrame, staticTransformsPtr_->getImuFrame(), R_W_I.matrix());
187 preIntegratedNavStatePtr_->updatePositionInWorld(W_t_W_I, graphConfigPtr_->odomNotJumpAtStartFlag_);
189
191 << " Initial pose of imu frame in world frame has been set to RPY (deg): " << R_W_I.rpy().transpose() * (180.0 / M_PI)
192 << ", t (m): " << W_t_W_I.transpose() << "." << COLOR_END << std::endl;
193}
194
195bool HolisticFusion::initWorldFrameToFixedFrameTransform(const Eigen::Isometry3d& T_W_F, const std::string& fixedFrame) {
196 // Set the initial transform
197 std::cout << "Setting initial transform from world frame to " << fixedFrame << " frame: " << T_W_F.matrix() << std::endl;
198 // Actually set the transform
199 return graphMgrPtr_->setInitialWorldFrameToFixedFrameTransform(T_W_F, fixedFrame);
200}
201
202// Adders --------------------------------
205 const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, double imuTimeK,
206 std::shared_ptr<SafeIntegratedNavState>& returnPreIntegratedNavStatePtr,
207 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& returnOptimizedStateWithCovarianceAndBiasPtr,
208 Eigen::Matrix<double, 6, 1>& returnAddedImuMeasurements) {
209 // Setup -------------------------
210 // Increase counter
212
213 // Adapt time stamp
214 imuTimeK += graphConfigPtr_->imuTimeOffset_;
215
216 // First Iteration
217 if (preIntegratedNavStatePtr_ == nullptr) {
218 preIntegratedNavStatePtr_ = std::make_shared<SafeIntegratedNavState>();
219 preIntegratedNavStatePtr_->updateLatestMeasurementTimestamp(imuTimeK);
220 }
221
222 // Filter out imu messages with same time stamp
223 if (std::abs(imuTimeK - preIntegratedNavStatePtr_->getTimeK()) < 1e-8 && imuCallbackCounter_ > 1) {
224 REGULAR_COUT << RED_START << " Imu time " << std::setprecision(14) << imuTimeK << " was repeated." << COLOR_END << std::endl;
225 return false;
226 }
227
228 // Potentially convert to m/s^2
229 Eigen::Vector3d linearAccInMps2 = linearAcc;
230 if (graphConfigPtr_->isImuAccInG_) {
231 constexpr double gravityConst = 9.80665;
232 linearAccInMps2 *= gravityConst;
233 }
234
235 // Check the norm and spit out warning in case the acceleration is either too small or too big
236 const double accNorm = linearAccInMps2.norm();
237 if (accNorm < 2) {
238 REGULAR_COUT << RED_START << " IMU linear acceleration is too small: " << linearAccInMps2.transpose() << COLOR_END << std::endl;
239 REGULAR_COUT << RED_START << " Check whether isImuAccInG_ is not mistakenly set to false." << COLOR_END << std::endl;
240 } else if (accNorm > 90) {
241 REGULAR_COUT << RED_START << " IMU linear acceleration is too big: " << linearAccInMps2.transpose() << COLOR_END << std::endl;
242 REGULAR_COUT << RED_START << " Check whether isImuAccInG_ is not mistakenly set to true." << COLOR_END << std::endl;
243 }
244
245 // Clamp linear acceleration to 60 m/s^2 to avoid outliers
246 // constexpr double maxAcc = 60.0;
247 // if (accNorm > maxAcc) {
248 // // Iterate through each element and scale
249 // for (int i = 0; i < 3; ++i) {
250 // // If element is too big, clamp
251 // if (std::abs(linearAccInMps2(i)) > maxAcc) {
252 // linearAccInMps2(i) = (linearAccInMps2(i) > 0 ? maxAcc : -maxAcc);
253 // }
254 // }
255 // REGULAR_COUT << YELLOW_START << " Clamped IMU linear acceleration to: " << linearAccInMps2.transpose() << COLOR_END << std::endl;
256 // }
257
258 // Add measurement to buffer
259 returnAddedImuMeasurements = coreImuBufferPtr_->addToImuBuffer(imuTimeK, linearAccInMps2, angularVel);
260
261 // Locking
262 const std::lock_guard<std::mutex> initYawAndPositionLock(initYawAndPositionMutex_);
263
264 // State Machine in form of if-else statements -----------------
265 if (!alignedImuFlag_) { // Case 1: IMU not aligned
266 // Try to align
267 double imuAttitudeRoll, imuAttitudePitch = 0.0;
268 if (!alignImu_(imuAttitudeRoll, imuAttitudePitch)) { // Case 1.1: IMU alignment failed --> try again next time
269 // Print only once per second
270 if (imuCallbackCounter_ % int(graphConfigPtr_->imuRate_) == 0) {
271 REGULAR_COUT << " NOT ENOUGH IMU MESSAGES TO INITIALIZE POSE. WAITING FOR MORE..." << std::endl;
272 }
273 return false;
274 } else { // Case 1.2: IMU alignment succeeded --> continue next call iteration
275 Eigen::Matrix3d R_W_I0_attitude = gtsam::Rot3::Ypr(0.0, imuAttitudePitch, imuAttitudeRoll).matrix();
276 gtsam::Rot3 R_W_Init0_attitude(
277 R_W_I0_attitude *
278 staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), staticTransformsPtr_->getInitializationFrame())
279 .rotation()
280 .matrix());
281 // Set yaw of base frame to zero
282 // Set yaw of base frame to zero
283 R_W_Init0_attitude = gtsam::Rot3::Ypr(0.0, R_W_Init0_attitude.pitch(), R_W_Init0_attitude.roll());
284 R_W_I0_attitude =
285 R_W_Init0_attitude.matrix() *
286 staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getInitializationFrame(), staticTransformsPtr_->getImuFrame())
287 .rotation()
288 .matrix();
289 Eigen::Isometry3d T_O_Ik_attitude = Eigen::Isometry3d::Identity();
290 T_O_Ik_attitude.matrix().block<3, 3>(0, 0) = R_W_I0_attitude;
291 Eigen::Vector3d O_t_O_Ik =
292 Eigen::Vector3d(0, 0, 0) -
293 R_W_I0_attitude *
294 staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), staticTransformsPtr_->getInitializationFrame())
295 .translation();
296 T_O_Ik_attitude.matrix().block<3, 1>(0, 3) = O_t_O_Ik;
297 REGULAR_COUT << " Setting zero position of " << staticTransformsPtr_->getInitializationFrame()
298 << ", hence iniital position of IMU is: " << O_t_O_Ik.transpose() << std::endl;
299 Eigen::Vector3d zeroPVeloctiy = Eigen::Vector3d(0, 0, 0);
300 preIntegratedNavStatePtr_ = std::make_shared<SafeIntegratedNavState>(T_O_Ik_attitude, zeroPVeloctiy, zeroPVeloctiy, imuTimeK);
301 REGULAR_COUT << GREEN_START << " IMU aligned. Initial pre-integrated state in odom frame: "
302 << preIntegratedNavStatePtr_->getT_O_Ik_gravityAligned().matrix() << COLOR_END << std::endl;
303 alignedImuFlag_ = true;
304 return false;
305 }
306 } else if (!areYawAndPositionInited()) { // Case 2: IMU aligned, but yaw and position not initialized, waiting for external
307 // initialization, meanwhile publishing initial roll and pitch
308 // Printing every second
309 if (imuCallbackCounter_ % int(graphConfigPtr_->imuRate_) == 0) {
310 REGULAR_COUT << " IMU callback waiting for initialization of global yaw and initial position." << std::endl;
311 }
312 // Publish state with correct roll and pitch, nothing has changed compared to Case 1.2
313 preIntegratedNavStatePtr_->updateLatestMeasurementTimestamp(imuTimeK);
314 returnPreIntegratedNavStatePtr = std::make_shared<SafeIntegratedNavState>(*preIntegratedNavStatePtr_);
315 return true;
316 } else if (!validFirstMeasurementReceivedFlag_) { // Case 3: No valid measurement received yet
317 if (imuCallbackCounter_ % int(graphConfigPtr_->imuRate_) == 0) {
318 REGULAR_COUT << RED_START << " IMU callback waiting for first valid measurement before initializing graph." << COLOR_END << std::endl;
319 }
320 preIntegratedNavStatePtr_->updateLatestMeasurementTimestamp(imuTimeK);
321 returnPreIntegratedNavStatePtr = std::make_shared<SafeIntegratedNavState>(*preIntegratedNavStatePtr_);
322 return true;
323 } else if (!initedGraphFlag_) { // Case 4: IMU aligned, yaw and position initialized, valid measurement received, but graph not yet
324 // initialized
325 preIntegratedNavStatePtr_->updateLatestMeasurementTimestamp(imuTimeK);
326 initGraph_(imuTimeK, returnAddedImuMeasurements.tail<3>());
327 returnPreIntegratedNavStatePtr = std::make_shared<SafeIntegratedNavState>(*preIntegratedNavStatePtr_);
328 REGULAR_COUT << GREEN_START << " ...graph is initialized." << COLOR_END << std::endl;
329 return true;
330 }
331
332 // Case 5: Normal operation, meaning predicting the next state via integration -------------
333 // Only create state every n-th measurements (or at first successful iteration)
334 bool createNewStateFlag = imuCallbackCounter_ % graphConfigPtr_->createStateEveryNthImuMeasurement_ == 0 || !normalOperationFlag_;
335 // Add IMU factor and return propagated & optimized state
336 graphMgrPtr_->addImuFactorAndGetState(*preIntegratedNavStatePtr_, returnOptimizedStateWithCovarianceAndBiasPtr, coreImuBufferPtr_,
337 imuTimeK, createNewStateFlag);
338 returnPreIntegratedNavStatePtr = std::make_shared<SafeIntegratedNavState>(*preIntegratedNavStatePtr_);
339
340 // Set to normal operation
343 }
344
345 // Return
346 return true;
347}
348
349// Ambiguous Measurements -----------------------
350bool HolisticFusion::addZeroMotionFactor(double timeKm1, double timeK, double noiseDensity) {
351 static_cast<void>(graphMgrPtr_->addPoseBetweenFactor(gtsam::Pose3::Identity(), noiseDensity * Eigen::Matrix<double, 6, 1>::Ones(),
352 timeKm1, timeK, 10, RobustNormEnum::None, 0.0));
353 graphMgrPtr_->addUnaryClassicFactor<gtsam::Vector3, 3, gtsam::PriorFactor<gtsam::Vector3>, gtsam::symbol_shorthand::V>(
354 gtsam::Vector3::Zero(), noiseDensity * Eigen::Matrix<double, 3, 1>::Ones(), timeK);
355
356 return true;
357}
358
359bool HolisticFusion::addZeroVelocityFactor(double timeK, double noiseDensity) {
360 graphMgrPtr_->addUnaryClassicFactor<gtsam::Vector3, 3, gtsam::PriorFactor<gtsam::Vector3>, gtsam::symbol_shorthand::V>(
361 gtsam::Vector3::Zero(), noiseDensity * Eigen::Matrix<double, 3, 1>::Ones(), timeK);
362
363 return true;
364}
365
366// Private ---------------------------------------------------------------
367
369bool HolisticFusion::alignImu_(double& imuAttitudeRoll, double& imuAttitudePitch) {
370 gtsam::Rot3 R_W_I_rollPitch;
371 static int alignImuCounter__ = -1;
372 ++alignImuCounter__;
373 double estimatedGravityMagnitude;
374 if (coreImuBufferPtr_->estimateAttitudeFromImu(R_W_I_rollPitch, estimatedGravityMagnitude, graphMgrPtr_->getInitGyrBiasReference())) {
375 imuAttitudeRoll = R_W_I_rollPitch.roll();
376 imuAttitudePitch = R_W_I_rollPitch.pitch();
377 if (graphConfigPtr_->estimateGravityFromImuFlag_) {
378 graphConfigPtr_->gravityMagnitude_ = estimatedGravityMagnitude;
379 REGULAR_COUT << " Attitude of IMU is initialized. Determined Gravity Magnitude: " << estimatedGravityMagnitude << std::endl;
380 } else {
381 REGULAR_COUT << " Estimated gravity magnitude from IMU is: " << estimatedGravityMagnitude << std::endl;
382 REGULAR_COUT << " This gravity is not used, because estimateGravityFromImu is set to false. Gravity set to "
383 << graphConfigPtr_->gravityMagnitude_ << "." << std::endl;
384 gtsam::Vector3 gravityVector = gtsam::Vector3(0, 0, graphConfigPtr_->gravityMagnitude_);
385 gtsam::Vector3 estimatedGravityVector = gtsam::Vector3(0, 0, estimatedGravityMagnitude);
386 gtsam::Vector3 gravityVectorError = estimatedGravityVector - gravityVector;
387 gtsam::Vector3 gravityVectorErrorInImuFrame = R_W_I_rollPitch.inverse().rotate(gravityVectorError);
388 graphMgrPtr_->getInitAccBiasReference() = gravityVectorErrorInImuFrame;
389 std::cout << YELLOW_START << "GMsf" << COLOR_END << " Gravity error in IMU frame is: " << gravityVectorErrorInImuFrame.transpose()
390 << std::endl;
391 }
392 return true;
393 } else {
394 return false;
395 }
396}
397
398// Graph initialization for roll & pitch from starting attitude, assume zero yaw
399void HolisticFusion::initGraph_(const double timeStamp_k, const Eigen::Vector3d& imuAngularVelocity) {
400 // Calculate initial attitude;
401 const gtsam::Pose3& T_W_I0 = gtsam::Pose3(preIntegratedNavStatePtr_->getT_W_Ik().matrix());
402 const gtsam::Pose3& T_O_I0 = gtsam::Pose3(preIntegratedNavStatePtr_->getT_O_Ik_gravityAligned().matrix());
403 // Print
404 REGULAR_COUT << GREEN_START << " Total initial IMU attitude is RPY (deg): " << T_W_I0.rotation().rpy().transpose() * (180.0 / M_PI)
405 << COLOR_END << std::endl;
406
407 // Gravity
408 graphConfigPtr_->W_gravityVector_ = Eigen::Vector3d(0.0, 0.0, -graphConfigPtr_->gravityMagnitude_);
409 graphMgrPtr_->initImuIntegrators(graphConfigPtr_->gravityMagnitude_);
411 graphMgrPtr_->initPoseVelocityBiasGraph(timeStamp_k, T_W_I0, T_O_I0, imuAngularVelocity);
412
413 // Read initial pose from graph for optimized pose
414 gtsam::Pose3 T_W_I0_opt = graphMgrPtr_->getOptimizedGraphState().navState().pose();
416 << " INITIAL POSE of IMU in world frame after first optimization, x,y,z (m): " << T_W_I0_opt.translation().transpose()
417 << ", RPY (deg): " << T_W_I0_opt.rotation().rpy().transpose() * (180.0 / M_PI) << COLOR_END << std::endl;
418 REGULAR_COUT << GREEN_START << " INITIAL position of IMU in odom frame after first optimization, x,y,z (m): "
419 << preIntegratedNavStatePtr_->getT_O_Ik_gravityAligned().translation().transpose() << std::endl;
420 REGULAR_COUT << " Factor graph key of very first node: " << graphMgrPtr_->getPropagatedStateKey() << std::endl;
421
422 // Set flag
423 initedGraphFlag_ = true;
424}
425
427 // While loop
428 REGULAR_COUT << " Thread for updating graph is ready." << std::endl;
429 double lastOptimizedTime = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
430 double currentTime = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
431 bool optimizedAtLeastOnce = false;
432 while (!stopOptimizeGraphThread_) {
433 bool optimizeGraphFlag = false;
434 // Mutex for optimizeGraph Flag
435 {
436 // Lock
437 const std::lock_guard<std::mutex> optimizeGraphLock(optimizeGraphMutex_);
438 currentTime = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
439 // Optimize at most at the rate of maxOptimizationFrequency but at least every second
440 if ((optimizeGraphFlag_ && ((currentTime - lastOptimizedTime) > (1.0 / graphConfigPtr_->maxOptimizationFrequency_))) ||
441 ((currentTime - lastOptimizedTime) > (1.0 / graphConfigPtr_->minOptimizationFrequency_) && optimizedAtLeastOnce)) {
442 optimizeGraphFlag = true;
443 lastOptimizedTime = currentTime;
444 optimizedAtLeastOnce = true;
445 optimizeGraphFlag_ = false;
446 }
447 }
448
449 // Optimize
450 if (optimizeGraphFlag) {
451 graphMgrPtr_->updateGraph();
452 } // else just sleep for a short amount of time before polling again
453 else {
454 std::this_thread::sleep_for(std::chrono::milliseconds(1));
455 }
456 }
457}
458
460Eigen::Vector3d HolisticFusion::W_t_W_Frame1_to_W_t_W_Frame2_(const Eigen::Vector3d& W_t_W_frame1, const std::string& frame1,
461 const std::string& frame2, const Eigen::Matrix3d& R_W_frame2) {
462 // Static transforms
463 const Eigen::Isometry3d& T_frame2_frame1 = staticTransformsPtr_->rv_T_frame1_frame2(frame2, frame1);
464 const Eigen::Vector3d& frame1_t_frame1_frame2 = staticTransformsPtr_->rv_T_frame1_frame2(frame1, frame2).translation();
465
467 Eigen::Matrix3d R_W_frame1 = R_W_frame2 * T_frame2_frame1.rotation();
468
470 Eigen::Vector3d W_t_frame1_frame2 = R_W_frame1 * frame1_t_frame1_frame2;
471
473 return W_t_W_frame1 + W_t_frame1_frame2;
474}
475
479
480// Check whether measurement violated covariance, if yes, add to set and print once, if not, remove from set and print that returned
481bool HolisticFusion::checkAndPrintCovarianceViolation_(const std::string& measurementName, const bool violatedFlag) {
482 if (violatedFlag) {
484 measurementsWithViolatedCovariance_.insert(measurementName);
485 REGULAR_COUT << RED_START << " " << measurementName << " covariance violated. Not adding factor until not violated anymore."
486 << COLOR_END << std::endl;
487 }
488 return true;
489 } else {
491 measurementsWithViolatedCovariance_.erase(measurementName);
492 REGULAR_COUT << GREEN_START << " " << measurementName << " covariance not violated anymore." << COLOR_END << std::endl;
493 }
494 return false;
495 }
496}
497
498} // namespace holistic_fusion
#define REGULAR_COUT
#define COLOR_END
#define GREEN_START
#define RED_START
#define YELLOW_START
Eigen::Vector3d W_t_W_Frame1_to_W_t_W_Frame2_(const Eigen::Vector3d &W_t_W_frame1, const std::string &frame1, const std::string &frame2, const Eigen::Matrix3d &R_W_frame2)
Utility functions / Geometric transformation to IMU in world frame.
bool addZeroMotionFactor(double timeKm1, double timeK, double noiseDensity)
void setup(const std::shared_ptr< GraphConfig > graphConfigPtr, const std::shared_ptr< StaticTransforms > staticTransformsPtr)
std::shared_ptr< GraphConfig > graphConfigPtr_
std::shared_ptr< SafeIntegratedNavState > preIntegratedNavStatePtr_
State Containers.
bool initYawAndPosition(const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S1, const UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &M_t_M_S2)
std::atomic< bool > stopOptimizeGraphThread_
void setInitialOrientationAndPosition_(const gtsam::Rot3 &R_W_I, const Eigen::Vector3d &W_t_W_S, const std::string &sensorFrame)
bool initWorldFrameToFixedFrameTransform(const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
bool logRealTimeNavStates(const std::string &savePath)
std::shared_ptr< holistic_fusion::ImuBuffer > coreImuBufferPtr_
gtsam::Pose3 initialT_W_fixedFrame_(const UnaryMeasurementAbsolute &measurement)
bool alignedImuFlag_
Flags / Initialization.
bool addZeroVelocityFactor(double timeK, double noiseDensity)
bool addCoreImuMeasurementAndGetState(const Eigen::Vector3d &linearAcc, const Eigen::Vector3d &angularVel, const double imuTimeK, std::shared_ptr< SafeIntegratedNavState > &returnPreIntegratedNavStatePtr, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &returnOptimizedStateWithCovarianceAndBiasPtr, Eigen::Matrix< double, 6, 1 > &returnAddedImuMeasurements)
Main: IMU --------------------—.
bool alignImu_(double &imuAttitudeRoll, double &imuAttitudePitch)
Worker functions / Set Imu Attitude.
virtual ~HolisticFusion()
Destructor --------—.
std::shared_ptr< GraphManager > graphMgrPtr_
bool canInitYawAndPosition_() const
Initialization / Whether the yaw and the position can be initialized now. Prints the reason if not.
bool optimizeSlowBatchSmoother(int maxIterations, const std::string &savePath, const bool saveCovarianceFlag)
std::shared_ptr< StaticTransforms > staticTransformsPtr_
bool checkAndPrintCovarianceViolation_(const std::string &measurementName, const bool violatedFlag)
void initGraph_(const double timeStamp_k, const Eigen::Vector3d &imuAngularVelocity)
bool initHeadingAndPosition(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &T_M_S)
HolisticFusion()
Constructor --------—.
std::unordered_set< std::string > measurementsWithViolatedCovariance_
const std::string & sensorFrameName() const
Definition Measurement.h:83
const std::string & measurementName() const
Names.
Definition Measurement.h:82
Class to represent an absolute unary measurement in the graph. Exact dimension is not known.
Class to represent x-dimensional absolute unary measurement in the graph.
const MEASUREMENT_TYPE & unaryMeasurement() const