My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
GraphManager.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#define MIN_ITERATIONS_BEFORE_REMOVING_STATIC_TRANSFORM 200
9
10// C++
11#include <algorithm>
12#include <chrono>
13#include <cmath>
14#include <filesystem>
15#include <iomanip>
16#include <string>
17#include <utility>
18
19// IO
20#include <gtsam/slam/dataset.h>
21
22// Factors
23#include <gtsam/navigation/CombinedImuFactor.h>
24#include <gtsam/nonlinear/ExpressionFactorGraph.h>
25
26// Workspace
30// ISAM2
33// LM
36
37#define REGULAR_COUT std::cout << YELLOW_START << "GMSF-GraphManager" << COLOR_END
38
39namespace holistic_fusion {
40
41namespace {
42
43// Returns Earth rate in rad/s in a gravity-aligned, z-up frame. The full vector requires ENU alignment.
44gtsam::Vector3 earthRotationRateInWorld(double latitudeDeg, bool worldFrameNorthAligned) {
45 if (!std::isfinite(latitudeDeg) || std::fabs(latitudeDeg) > 90.0) {
46 throw std::runtime_error("GraphManager: noise_params.latitudeDeg must be finite and within [-90, 90], got " +
47 std::to_string(latitudeDeg));
48 }
49 constexpr double earthRotationRate = 7.2921159e-05; // [rad/s]
50 const double latitudeRad = latitudeDeg * M_PI / 180.0;
51 gtsam::Vector3 W_omega_IW(0.0, 0.0, earthRotationRate * std::sin(latitudeRad)); // vertical component, valid for any yaw
52 if (worldFrameNorthAligned) {
53 W_omega_IW.y() = earthRotationRate * std::cos(latitudeRad); // horizontal component, only if y points north (ENU)
54 }
55 return W_omega_IW;
56}
57
58} // namespace
59
60// Public --------------------------------------------------------------------
61GraphManager::GraphManager(std::shared_ptr<GraphConfig> graphConfigPtr, std::string imuFrame, std::string worldFrame)
62 : graphConfigPtr_(std::move(graphConfigPtr)), imuFrame_(std::move(imuFrame)), worldFrame_(std::move(worldFrame)) {
63 // Buffer
64 rtFactorGraphBufferPtr_ = std::make_shared<gtsam::NonlinearFactorGraph>();
65 batchFactorGraphBufferPtr_ = std::make_shared<gtsam::NonlinearFactorGraph>();
66 rtGraphValuesBufferPtr_ = std::make_shared<gtsam::Values>();
67 batchGraphValuesBufferPtr_ = std::make_shared<gtsam::Values>();
68 rtGraphKeysTimestampsMapBufferPtr_ = std::make_shared<std::map<gtsam::Key, double>>();
69 batchGraphKeysTimestampsMapBufferPtr_ = std::make_shared<std::map<gtsam::Key, double>>();
70
71 // Keys
72 timeToKeyBufferPtr_ = std::make_shared<TimeGraphKeyBuffer>(graphConfigPtr_->imuBufferLength_, graphConfigPtr_->verboseLevel_);
73
74 // Optimizers
75 // A. Real-time Optimizer
76 if (graphConfigPtr_->realTimeSmootherUseIsamFlag_) {
77 rtOptimizerPtr_ = std::make_shared<OptimizerIsam2FixedLag>(graphConfigPtr_);
78 } else {
79 rtOptimizerPtr_ = std::make_shared<OptimizerLMFixedLag>(graphConfigPtr_);
80 }
81 // B. Batch Optimizer
82 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
83 if (graphConfigPtr_->slowBatchSmootherUseIsamFlag_) {
84 batchOptimizerPtr_ = std::make_shared<OptimizerIsam2Batch>(graphConfigPtr_);
85 } else {
86 batchOptimizerPtr_ = std::make_shared<OptimizerLMBatch>(graphConfigPtr_);
87 }
88 }
89}
90
91// Initialization Interface ---------------------------------------------------
92bool GraphManager::initImuIntegrators(const double gravityValue) {
93 // Gravity direction definition
94 imuParamsPtr_ = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(gravityValue); // ROS convention
95
96 // Set noise and bias parameters
98 imuParamsPtr_->setAccelerometerCovariance(
99 gtsam::Matrix33::Identity() * std::pow(graphConfigPtr_->accNoiseDensity_, 2));
100 imuParamsPtr_->setIntegrationCovariance(gtsam::Matrix33::Identity(3, 3) *
101 std::pow(graphConfigPtr_->integrationNoiseDensity_, 2)); // error committed in integrating
102 // position from velocities
104 imuParamsPtr_->setGyroscopeCovariance(
105 gtsam::Matrix33::Identity() * std::pow(graphConfigPtr_->gyroNoiseDensity_, 2));
109 if (graphConfigPtr_->earthRotationCompensationFlag_) {
110 const gtsam::Vector3 W_omega_IW =
111 earthRotationRateInWorld(graphConfigPtr_->latitudeDeg_, graphConfigPtr_->worldFrameNorthAlignedFlag_);
112 imuParamsPtr_->setOmegaCoriolis(W_omega_IW);
113 REGULAR_COUT << " Earth rotation compensation enabled, latitude " << graphConfigPtr_->latitudeDeg_
114 << " deg, world frame north-aligned: " << (graphConfigPtr_->worldFrameNorthAlignedFlag_ ? "yes" : "no")
115 << ", W_omega_IW [rad/s]: " << W_omega_IW.transpose() << std::endl;
116 }
118 imuParamsPtr_->setBiasAccCovariance(gtsam::Matrix33::Identity(3, 3) * std::pow(graphConfigPtr_->accBiasRandomWalkNoiseDensity_, 2));
119 imuParamsPtr_->setBiasOmegaCovariance(gtsam::Matrix33::Identity(3, 3) * std::pow(graphConfigPtr_->gyroBiasRandomWalkNoiseDensity_, 2));
120
121 // Use previously defined prior for gyro
122 imuBiasPriorPtr_ = std::make_shared<gtsam::imuBias::ConstantBias>(graphConfigPtr_->accBiasPrior_, graphConfigPtr_->gyroBiasPrior_);
123
124 // Init Pre-integrators
125 imuStepPreintegratorPtr_ = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(imuParamsPtr_, *imuBiasPriorPtr_);
126 imuBufferPreintegratorPtr_ = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(imuParamsPtr_, *imuBiasPriorPtr_);
127 imuParamsPtr_->print("HolisticFusion-IMU PreIntegration Parameters:");
128 return true;
129}
130
131bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam::Pose3& T_W_I0, const gtsam::Pose3& T_O_I0,
132 const gtsam::Vector3& imuAngularVelocity) {
133 // Create Prior factor ----------------------------------------------------
135 auto priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
136 (gtsam::Vector(6) << graphConfigPtr_->initialOrientationStdDev_, graphConfigPtr_->initialOrientationStdDev_,
137 graphConfigPtr_->initialOrientationStdDev_, graphConfigPtr_->initialPositionStdDev_,
138 graphConfigPtr_->initialPositionStdDev_, graphConfigPtr_->initialPositionStdDev_)
139 .finished()); // rad,rad,rad,m, m, m
140 auto priorVelocityNoise = gtsam::noiseModel::Isotropic::Sigma(3, graphConfigPtr_->initialVelocityStdDev_); // m/s
141 auto priorBiasNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << graphConfigPtr_->initialAccBiasStdDev_, // m/s^2
142 graphConfigPtr_->initialAccBiasStdDev_, // m/s^2
143 graphConfigPtr_->initialAccBiasStdDev_, // m/s^2
144 graphConfigPtr_->initialGyroBiasStdDev_, // rad/s
145 graphConfigPtr_->initialGyroBiasStdDev_, // rad/s
146 graphConfigPtr_->initialGyroBiasStdDev_) // rad/s
147 .finished()); // acc, acc, acc, gyro, gyro, gyro
148
149 // Pre-allocate
150 gtsam::NonlinearFactorGraph newRtGraphFactors, newBatchGraphFactors;
151 gtsam::Values newRtGraphValues, newBatchGraphValues;
152 std::shared_ptr<std::map<gtsam::Key, double>> priorKeyTimestampMapPtr = std::make_shared<std::map<gtsam::Key, double>>();
153
154 // Looking up from IMU buffer --> acquire mutex (otherwise values for key might not be set)
155 {
156 const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
157
158 // Initial estimate
159 gtsam::Values valuesEstimate;
160 REGULAR_COUT << " Initial Pose of imu in world frame: " << T_W_I0 << std::endl;
161 valuesEstimate.insert(gtsam::symbol_shorthand::X(propagatedStateKey_), T_W_I0);
162 REGULAR_COUT << " Initial velocity assumed to be: " << gtsam::Vector3(0, 0, 0) << std::endl;
163 valuesEstimate.insert(gtsam::symbol_shorthand::V(propagatedStateKey_), gtsam::Vector3(0, 0, 0));
164 REGULAR_COUT << " Initial bias set to: " << *imuBiasPriorPtr_ << std::endl;
165 valuesEstimate.insert(gtsam::symbol_shorthand::B(propagatedStateKey_), *imuBiasPriorPtr_);
167 rtGraphValuesBufferPtr_->insert(valuesEstimate);
168 batchGraphValuesBufferPtr_->insert(valuesEstimate); // TODO: Check whether velocity can be removed from batch
169 writeValueKeysToKeyTimeStampMap_(valuesEstimate, timeStamp, priorKeyTimestampMapPtr);
170
171 // Initialize graph -------------------------------------------------
172 rtFactorGraphBufferPtr_->resize(0);
174 rtFactorGraphBufferPtr_->emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(
175 gtsam::symbol_shorthand::X(propagatedStateKey_), T_W_I0,
176 priorPoseNoise); // POSE - PriorFactor format is (key,value,matrix) value
177 batchFactorGraphBufferPtr_->emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(
178 gtsam::symbol_shorthand::X(propagatedStateKey_), T_W_I0,
179 priorPoseNoise); // POSE - PriorFactor format is (key,value,matrix) value
180 // is same type as type of PriorFactor
181 rtFactorGraphBufferPtr_->emplace_shared<gtsam::PriorFactor<gtsam::Vector3>>(gtsam::symbol_shorthand::V(propagatedStateKey_),
182 gtsam::Vector3(0, 0, 0),
183 priorVelocityNoise); // VELOCITY
184 batchFactorGraphBufferPtr_->emplace_shared<gtsam::PriorFactor<gtsam::Vector3>>(gtsam::symbol_shorthand::V(propagatedStateKey_),
185 gtsam::Vector3(0, 0, 0),
186 priorVelocityNoise); // VELOCITY
187 rtFactorGraphBufferPtr_->emplace_shared<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(
188 gtsam::symbol_shorthand::B(propagatedStateKey_), *imuBiasPriorPtr_,
189 priorBiasNoise); // BIAS
190 batchFactorGraphBufferPtr_->emplace_shared<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(
191 gtsam::symbol_shorthand::B(propagatedStateKey_), *imuBiasPriorPtr_,
192 priorBiasNoise); // BIAS
193
194 // Add to Time Key Buffer
195 timeToKeyBufferPtr_->addToBuffer(timeStamp, propagatedStateKey_);
196
197 // Copy over
198 // Rt
199 newRtGraphFactors = *rtFactorGraphBufferPtr_;
200 newRtGraphValues = *rtGraphValuesBufferPtr_;
201 rtFactorGraphBufferPtr_->resize(0);
203 // Batch
204 newBatchGraphFactors = *batchFactorGraphBufferPtr_;
205 newBatchGraphValues = *batchGraphValuesBufferPtr_;
208 }
209
211 // Rt Smoother
212 addFactorsToRtSmootherAndOptimize(newRtGraphFactors, newRtGraphValues, *priorKeyTimestampMapPtr, graphConfigPtr_, 0);
213 // Batch Smoother
214 addFactorsToBatchSmootherAndOptimize(newBatchGraphFactors, newBatchGraphValues, *priorKeyTimestampMapPtr, graphConfigPtr_);
215
216 // Update Current State ---------------------------------------------------
217 const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
218 latestGraphStateKeyTime_ = timeStamp;
219 latestGraphStateKeyAngularVelocity_ = imuAngularVelocity;
220 optimizedGraphState_.updateNavStateAndBias(propagatedStateKey_, timeStamp, gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0)),
221 imuBiasPriorPtr_->correctGyroscope(imuAngularVelocity), *imuBiasPriorPtr_);
222 O_imuPropagatedState_ = gtsam::NavState(T_O_I0, gtsam::Vector3(0, 0, 0));
223 W_imuPropagatedState_ = gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0));
224 T_W_O_ = (T_W_I0.inverse() * T_O_I0).matrix();
225 return true;
226}
227
228// IMU at the core --------------------------------------------------------------
230 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& newOptimizedNavStatePtr,
231 const std::shared_ptr<ImuBuffer>& imuBufferPtr, const double imuTimeK, bool createNewStateFlag) {
232 // Logging of latency
233 if (graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_) {
234 latencyStartTime_ = std::chrono::high_resolution_clock::now();
235 }
236
237 // Looking up from IMU buffer --> acquire mutex (otherwise values for key might not be set)
238 const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
239
240 // Part 1 (ALWAYS): Propagate state and imu measurement to pre-integrator -----------------------
241 // 1.1 Get last two measurements from buffer to determine dt
242 TimeToImuMap imuMeas;
243 imuBufferPtr->getLastTwoMeasurements(imuMeas);
244 propagatedImuSampleTime_ = imuTimeK;
245
246 // 1.2 Update IMU Pre-integrator
247 updateImuIntegrators_(imuMeas);
248
249 // 1.3 Predict propagated state via forward integration
250 // Previous pose
251 gtsam::Pose3 T_W_I_beforeInt = W_imuPropagatedState_.pose();
252 if (graphConfigPtr_->usingBiasForPreIntegrationFlag_) {
254 imuBufferPtr->integrateNavStateFromTimestamp(imuMeas.begin()->first, imuMeas.rbegin()->first, W_imuPropagatedState_,
255 optimizedGraphState_.imuBias(), graphConfigPtr_->W_gravityVector_);
256 } else {
258 imuBufferPtr->integrateNavStateFromTimestamp(imuMeas.begin()->first, imuMeas.rbegin()->first, W_imuPropagatedState_,
259 gtsam::imuBias::ConstantBias(), graphConfigPtr_->W_gravityVector_);
260 }
261
262 // 1.4 Propagate to state in odom
263 // Delta pose
264 const gtsam::Pose3 T_W_I_afterInt = W_imuPropagatedState_.pose();
265 const gtsam::Pose3 T_Ikm1_Ik = T_W_I_beforeInt.between(T_W_I_afterInt);
266 // Pose of IMU in Odom
267 gtsam::Pose3 T_O_I = O_imuPropagatedState_.pose() * T_Ikm1_Ik;
268 // Get Gravity Alignment right also in Odom frame
269 const Eigen::Matrix3d& R_W_I = T_W_I_afterInt.rotation().matrix();
270 Eigen::Matrix3d R_O_I = T_O_I.rotation().matrix();
271 R_O_I.row(2) = R_W_I.row(2); // Copy gravity direction
272 R_O_I.row(1) = R_O_I.row(2).cross(R_O_I.row(0)).normalized();
273 R_O_I.row(0) = R_O_I.row(1).cross(R_O_I.row(2)).normalized();
274 T_O_I = gtsam::Pose3(gtsam::Rot3(R_O_I), T_O_I.translation());
275 // Velocity
276 gtsam::Vector3 O_v_O_I = T_O_I.rotation() * W_imuPropagatedState_.bodyVelocity();
277 // Update the NavState
278 O_imuPropagatedState_ = gtsam::NavState(T_O_I.rotation(), T_O_I.translation(), O_v_O_I);
279
280 // 1.5 Fill in optimized state container
281 if (optimizedGraphState_.isOptimized()) {
282 newOptimizedNavStatePtr = std::make_shared<SafeNavStateWithCovarianceAndBias>(optimizedGraphState_);
283 } else {
284 newOptimizedNavStatePtr = nullptr;
285 }
286
287 // 1.6 Transformation between World and Odom
288 T_W_O_ = Eigen::Isometry3d((W_imuPropagatedState_.pose() * O_imuPropagatedState_.pose().inverse()).matrix());
289
290 // 1.7 Return pre-integrated state
291 gtsam::NavState& T_O_Ik_nav = O_imuPropagatedState_; // Alias
292 // Assign poses and velocities
293 returnPreIntegratedNavState.update(T_W_O_, Eigen::Isometry3d(T_O_Ik_nav.pose().matrix()), T_O_Ik_nav.bodyVelocity(),
294 optimizedGraphState_.imuBias().correctGyroscope(imuMeas.rbegin()->second.angularVelocity), imuTimeK,
295 false);
296
297 // Check whether new state can be created (in case fixed lag smoother has a short window)
298 if (createNewStateFlag && imuTimeK - lastOptimizedStateTime_ > graphConfigPtr_->realTimeSmootherLag_ - WORST_CASE_OPTIMIZATION_TIME &&
300 createNewStateFlag = false;
302 << " The current measurement would fall outside of the real-time smoother lag, hence skipping the creation of a new"
303 << "IMU measurement. Not creating new state." << COLOR_END << std::endl;
304 }
305
306 // Part 2 (OPTIONAL): Create new state and add IMU factor to graph ------------------------------
307 if (createNewStateFlag) {
308 // Get new key
309 const gtsam::Key oldKey = propagatedStateKey_;
310 const gtsam::Key newKey = newPropagatedStateKey_();
311 latestGraphStateKeyTime_ = imuTimeK;
312 latestGraphStateKeyAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity;
313
314 // Add to time key buffer
315 timeToKeyBufferPtr_->addToBuffer(imuTimeK, newKey);
316
317 // Add IMU Factor to graph
318 gtsam::CombinedImuFactor imuFactor(gtsam::symbol_shorthand::X(oldKey), gtsam::symbol_shorthand::V(oldKey),
319 gtsam::symbol_shorthand::X(newKey), gtsam::symbol_shorthand::V(newKey),
320 gtsam::symbol_shorthand::B(oldKey), gtsam::symbol_shorthand::B(newKey), *imuStepPreintegratorPtr_);
322
323 // Add IMU values
324 gtsam::Values valuesEstimate;
325 valuesEstimate.insert(gtsam::symbol_shorthand::X(newKey), W_imuPropagatedState_.pose());
326 valuesEstimate.insert(gtsam::symbol_shorthand::V(newKey), W_imuPropagatedState_.velocity());
327 valuesEstimate.insert(gtsam::symbol_shorthand::B(newKey), optimizedGraphState_.imuBias());
328 rtGraphValuesBufferPtr_->insert(valuesEstimate);
329 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
330 batchGraphValuesBufferPtr_->insert(valuesEstimate);
331 }
332
333 // Add timestamps
335 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
337 }
338
339 // After adding this factor we can again empty the step integrator
340 // Reset IMU Step Pre-integration
341 if (graphConfigPtr_->usingBiasForPreIntegrationFlag_) {
342 imuStepPreintegratorPtr_->resetIntegrationAndSetBias(optimizedGraphState_.imuBias());
343 } else {
344 imuStepPreintegratorPtr_->resetIntegrationAndSetBias(gtsam::imuBias::ConstantBias());
345 }
346 } // End of create new state
347
348 // Potentially log real-time state to container
349 if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
352 }
353
354 // Logging of latency
355 if (graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_) {
356 latencyEndTime_ = std::chrono::high_resolution_clock::now();
357 // Duration in seconds
358 std::chrono::duration<double> latencyDuration = latencyEndTime_ - latencyStartTime_;
359 latencyContainer_[imuTimeK] = latencyDuration.count();
360 }
361}
362
363// Set T_W_F
364bool GraphManager::setInitialWorldFrameToFixedFrameTransform(const Eigen::Isometry3d& T_W_F, const std::string& fixedFrame) {
365 gtsamDynamicExpressionKeys_.get<gtsam::Pose3>()
366 .setInitialGuessForFramePair(worldFrame_, fixedFrame, gtsam::Pose3(T_W_F.matrix()));
367 std::cout << " Set world to fixed frame transform T_" << worldFrame_ << "_" << fixedFrame << " to: " << T_W_F.matrix()
368 << std::endl;
369 return true;
370}
371
372gtsam::Pose3 GraphManager::getInitialWorldFrameToFixedFrameTransform(const std::string& fixedFrame) {
373 gtsam::Pose3 T_W_F = gtsam::Pose3::Identity();
374 gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().getInitialGuessForFramePair(worldFrame_, fixedFrame, T_W_F);
375 return T_W_F;
376}
377
378// Unary factors ----------------------------------------------------------------
379// Key Lookup
380bool GraphManager::hasReferenceFrameKeyframe(const std::string& worldFrame, const std::string& fixedFrame) {
381 auto& referenceFrameKeys = gtsamDynamicExpressionKeys_.get<gtsam::Pose3>();
382 std::lock_guard<std::mutex> modifyGraphKeysLock(referenceFrameKeys.mutex());
383 return referenceFrameKeys.isFramePairInDictionary(worldFrame, fixedFrame);
384}
385
386bool GraphManager::getUnaryFactorGeneralKey(gtsam::Key& returnedKey, double& returnedGraphTime, const UnaryMeasurement& unaryMeasurement) {
387 // Find the closest key in existing graph
388 // Case 1: Can't add immediately
389 if (!timeToKeyBufferPtr_->getClosestKeyAndTimestamp(returnedGraphTime, returnedKey, unaryMeasurement.measurementName(),
390 graphConfigPtr_->maxSearchDeviation_, unaryMeasurement.timeK())) {
391 // Measurement coming from the future
392 if (propagatedImuSampleTime_ - unaryMeasurement.timeK() < 0.0) { // Factor is coming from the future, hence add it to the buffer
393 // Not too far in the future --> add to buffer and add later
394 if (unaryMeasurement.timeK() - propagatedImuSampleTime_ < 4 * graphConfigPtr_->maxSearchDeviation_) {
395 // TODO: Add to buffer and return --> still add it until we are there
396 return true;
397 }
398 // Too far in the future --> do not add it
399 else {
400 std::cout << 1000 * (propagatedImuSampleTime_ - unaryMeasurement.timeK()) << std::endl;
401 std::cout << 1000 * (returnedGraphTime - unaryMeasurement.timeK()) << std::endl;
402 REGULAR_COUT << RED_START << " Factor coming from the future, AND time deviation of " << typeid(unaryMeasurement).name()
403 << " at key " << returnedKey << " is " << 1000 * std::abs(returnedGraphTime - unaryMeasurement.timeK())
404 << " ms, being larger than admissible deviation of " << 4 * 1000 * graphConfigPtr_->maxSearchDeviation_
405 << " ms. Not adding to graph." << COLOR_END << std::endl;
406 return false;
407 }
408 }
409 // Measurement coming from the past, but could not find suitable key
410 else { // Otherwise do not add it
411 REGULAR_COUT << RED_START << " Time deviation of " << typeid(unaryMeasurement).name() << " at key " << returnedKey << " is "
412 << 1000 * std::abs(returnedGraphTime - unaryMeasurement.timeK()) << " ms, being larger than admissible deviation of "
413 << 1000 * graphConfigPtr_->maxSearchDeviation_ << " ms. Not adding to graph." << COLOR_END << std::endl;
414 return false;
415 }
416 }
417 // Case 2: Can add immediately
418 else {
419 return true;
420 }
421}
422
423// Robust Aware Between factors ------------------------------------------------------------------------------------------------------
424gtsam::Key GraphManager::addPoseBetweenFactor(const gtsam::Pose3& deltaPose, const Eigen::Matrix<double, 6, 1>& poseBetweenNoiseDensity,
425 const double timeKm1, const double timeK, const double rate,
426 const RobustNormEnum& robustNormEnum, const double robustNormConstant) {
427 // Find corresponding keys in graph
428 const double maxLidarTimestampDistance = (1.0 / rate) + (2.0 * graphConfigPtr_->maxSearchDeviation_);
429 gtsam::Key closestKeyKm1, closestKeyK;
430 double keyTimeStampDistance{0.0};
431
432 if (!findGraphKeys_(closestKeyKm1, closestKeyK, keyTimeStampDistance, maxLidarTimestampDistance, timeKm1, timeK, "pose between")) {
433 REGULAR_COUT << RED_START << " Current propagated key: " << propagatedStateKey_ << " , PoseBetween factor not added between keys "
434 << closestKeyKm1 << " and " << closestKeyK << COLOR_END << std::endl;
435 return closestKeyK;
436 }
437
438 // Scale delta pose according to timeStampDistance
439 const double scale = keyTimeStampDistance / (timeK - timeKm1);
440 if (graphConfigPtr_->verboseLevel_ > 3) {
441 REGULAR_COUT << " Scaling factor in tangent space for pose between delta pose: " << scale << std::endl;
442 }
443 gtsam::Pose3 scaledDeltaPose = gtsam::Pose3::Expmap(scale * gtsam::Pose3::Logmap(deltaPose));
444
445 // Create noise model
446 assert(poseBetweenNoiseDensity.size() == 6);
447 auto noise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(poseBetweenNoiseDensity))); // rad,rad,rad,m,m,m
448 gtsam::noiseModel::Robust::shared_ptr robustErrorFunction;
449 // Pick Robust Error Function
450 switch (robustNormEnum) {
452 robustErrorFunction = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(robustNormConstant), noise);
453 break;
455 robustErrorFunction = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Cauchy::Create(robustNormConstant), noise);
456 break;
458 robustErrorFunction = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Tukey::Create(robustNormConstant), noise);
459 break;
461 robustErrorFunction =
462 gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::GemanMcClure::Create(robustNormConstant), noise);
463 break;
464 }
465
466 // Create pose between factor and add it
467 gtsam::BetweenFactor<gtsam::Pose3> poseBetweenFactor;
468 if (robustNormEnum == RobustNormEnum::None) {
469 poseBetweenFactor = gtsam::BetweenFactor<gtsam::Pose3>(gtsam::symbol_shorthand::X(closestKeyKm1),
470 gtsam::symbol_shorthand::X(closestKeyK), scaledDeltaPose, noise);
471 } else {
472 poseBetweenFactor = gtsam::BetweenFactor<gtsam::Pose3>(gtsam::symbol_shorthand::X(closestKeyKm1),
473 gtsam::symbol_shorthand::X(closestKeyK), scaledDeltaPose, robustErrorFunction);
474 }
475
476 // Write to graph
478
479 // Print summary
480 if (graphConfigPtr_->verboseLevel_ > 3) {
481 REGULAR_COUT << " Current propagated key: " << propagatedStateKey_ << ", " << YELLOW_START << " PoseBetween factor added between key "
482 << closestKeyKm1 << " and key " << closestKeyK << COLOR_END << std::endl;
483 }
484
485 return closestKeyK;
486}
487
488gtsam::NavState GraphManager::calculateNavStateAtGeneralKey(bool& computeSuccessfulFlag,
489 const std::shared_ptr<holistic_fusion::OptimizerBase> optimizerPtr,
490 const gtsam::Key& generalKey, const char* callingFunctionName) {
491 // Nav State (pose and velocity)
492 gtsam::Pose3 resultPose;
493 gtsam::Vector3 resultVelocity;
494 try {
495 resultPose = optimizerPtr->calculateEstimatedPose3(gtsam::symbol_shorthand::X(generalKey)); // auto result = mainGraphPtr_->estimate();
496 resultVelocity = optimizerPtr->calculateEstimatedVelocity3(gtsam::symbol_shorthand::V(generalKey));
497 computeSuccessfulFlag = true;
498 } catch (const std::out_of_range& outOfRangeExeception) {
499 REGULAR_COUT << "Out of Range exeception while optimizing graph: " << outOfRangeExeception.what() << '\n';
501 << " This happens if the measurement delay is larger than the graph-smootherLag, i.e. the optimized graph instances are "
502 "not connected. Increase the lag in this case."
503 << COLOR_END << std::endl;
504 REGULAR_COUT << RED_START << " CalculateNavStateAtKey called by " << callingFunctionName << COLOR_END << std::endl;
505 computeSuccessfulFlag = false;
506 }
507 return gtsam::NavState(resultPose, resultVelocity);
508}
509
510Eigen::Matrix<double, 6, 6> GraphManager::calculatePoseCovarianceAtKeyInWorldFrame(std::shared_ptr<holistic_fusion::OptimizerBase> graphPtr,
511 const gtsam::Key& graphKey,
512 const char* callingFunctionName,
513 std::optional<const gtsam::NavState> optionalNavState) {
514 // Pose Covariance in Tangent Space
515 std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
516 gtsam::Matrix66 resultPoseCovarianceBodyFrame = graphPtr->calculateMarginalCovarianceMatrixAtKey(graphKey);
517 std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
518 auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
519 if (!optionalNavState.has_value() && duration > 400) {
520 std::cout << RED_START << "Time to calculate marginal covariance: " << duration << " microseconds, which is too much." << std::endl;
521 }
522 // Transform covariance from I_S_I in body frame, to W_S_W in world frame
523 // Get NavState
524 gtsam::NavState navState;
525 if (optionalNavState.has_value()) {
526 navState = optionalNavState.value();
527 } else {
528 gtsam::Pose3 resultPose = graphPtr->calculateEstimatedPose3(graphKey); // auto result = mainGraphPtr_->estimate();
529 gtsam::Point3 resultVelocity = gtsam::Point3::Zero(); // Assume Zero Velocity, but adjoint below is still fine to map to world
530 navState = gtsam::NavState(resultPose, resultVelocity);
531 }
532 // Compute adjoint matrix
533 gtsam::Matrix66 adjointMatrix = navState.pose().AdjointMap();
534 // Transform covariance
535 gtsam::Matrix66 resultPoseCovarianceWorldFrame = adjointMatrix * resultPoseCovarianceBodyFrame * adjointMatrix.transpose();
536 // Return
537 return resultPoseCovarianceWorldFrame;
538}
539
541 // At compile time get the symbols for the 6D and 3D states
542 constexpr int numDynamic6DStates = countNDStates<6>();
543 constexpr int numDynamic3DStates = countNDStates<3>();
544 constexpr std::array<char, numDynamic6DStates> dim6StateSymbols = getSymbolArrayForNDStates<6, numDynamic6DStates>();
545 constexpr std::array<char, numDynamic3DStates> dim3StateSymbols = getSymbolArrayForNDStates<3, numDynamic3DStates>();
546
547 // Method variables
548 gtsam::NonlinearFactorGraph newRtGraphFactors, newBatchGraphFactors;
549 gtsam::Values newRtGraphValues, newBatchGraphValues;
550 std::map<gtsam::Key, double> newRtGraphKeysTimestampsMap, newBatchGraphKeysTimestampsMap;
551 gtsam::Key currentPropagatedKey;
552 gtsam::Vector3 currentGraphStateKeyAngularVelocity;
553 double currentGraphStateKeyTime;
554
555 // Mutex Block 1 -----------------
556 {
557 // Lock
558 const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
559 // Get current key and time
560 currentPropagatedKey = propagatedStateKey_;
561 currentGraphStateKeyTime = latestGraphStateKeyTime_;
562 currentGraphStateKeyAngularVelocity = latestGraphStateKeyAngularVelocity_;
563 // Get copy of factors and values and empty buffers
564 newRtGraphFactors = *rtFactorGraphBufferPtr_;
565 newRtGraphValues = *rtGraphValuesBufferPtr_;
566 newRtGraphKeysTimestampsMap = *rtGraphKeysTimestampsMapBufferPtr_;
567 rtFactorGraphBufferPtr_->resize(0);
570 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
571 newBatchGraphFactors = *batchFactorGraphBufferPtr_;
572 newBatchGraphValues = *batchGraphValuesBufferPtr_;
573 newBatchGraphKeysTimestampsMap = *batchGraphKeysTimestampsMapBufferPtr_;
577 }
578
579 // Catch-up starts at the graph key, including its unfinished IMU interval.
581 } // end of locking
582
583 // Log Update Duration
584 if (graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_) {
585 updateDurationStartTime_ = std::chrono::high_resolution_clock::now();
586 }
587
588 // Graph Update (time consuming) -------------------------------------------
589 bool successfulOptimizationFlag = true;
590 // Rt Smoother
591 if (newRtGraphFactors.size() > 0) {
592 successfulOptimizationFlag = addFactorsToRtSmootherAndOptimize(newRtGraphFactors, newRtGraphValues, newRtGraphKeysTimestampsMap,
593 graphConfigPtr_, graphConfigPtr_->additionalOptimizationIterations_);
594 } else if (graphConfigPtr_->verboseLevel_ > 3) {
595 REGULAR_COUT << " No factors present in rt smoother, not optimizing." << std::endl;
596 }
597 // Batch Smoother
598 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_ && newBatchGraphFactors.size() > 0) {
599 successfulOptimizationFlag =
600 addFactorsToBatchSmootherAndOptimize(newBatchGraphFactors, newBatchGraphValues, newBatchGraphKeysTimestampsMap, graphConfigPtr_);
601 } else if (graphConfigPtr_->verboseLevel_ > 3) {
602 REGULAR_COUT << " No factors present in batch smoother, not optimizing." << std::endl;
603 }
604
605 // Log Update Duration
606 if (graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_) {
607 updateDurationEndTime_ = std::chrono::high_resolution_clock::now();
608 // Duration in seconds
609 std::chrono::duration<double> updateDuration = updateDurationEndTime_ - updateDurationStartTime_;
610 updateDurationContainer_[currentGraphStateKeyTime] = updateDuration.count();
611 }
612
613 // Return if optimization failed
614 if (!successfulOptimizationFlag) {
615 REGULAR_COUT << RED_START << " Graph optimization failed. " << COLOR_END << std::endl;
616 return;
617 }
618
619 // Compute entire results ----------------------------------------------
620 // A. NavState ------------------------------
621 gtsam::NavState resultNavState =
622 calculateNavStateAtGeneralKey(successfulOptimizationFlag, rtOptimizerPtr_, currentPropagatedKey, __func__);
623 rtOptimizerPtr_->addLatestPoseBelief(resultNavState.pose());
624 // B. Bias ------------------------------
625 gtsam::imuBias::ConstantBias resultBias = rtOptimizerPtr_->calculateEstimatedBias(gtsam::symbol_shorthand::B(currentPropagatedKey));
626 rtOptimizerPtr_->addLatestImuBiasBelief(resultBias);
627 // gtsam::Matrix resultAccBiasCov =
628 // rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsam::symbol_shorthand::B(currentPropagatedKey)); C. Compute & Transform
629 // Covariances ------------------------------ Pose Covariance in World Frame
630 gtsam::Matrix66 resultPoseCovarianceWorldFrame =
631 calculatePoseCovarianceAtKeyInWorldFrame(rtOptimizerPtr_, gtsam::symbol_shorthand::X(currentPropagatedKey), __func__, resultNavState);
632 // Velocity Covariance
633 gtsam::Matrix33 resultVelocityCovariance =
634 rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsam::symbol_shorthand::V(currentPropagatedKey));
635
636 // D. Reference Frame Transformations ------------------------------
637 // Containers
638 TransformsDictionary<Eigen::Isometry3d> resultReferenceFrameTransformations(Eigen::Isometry3d::Identity());
639 TransformsDictionary<Eigen::Matrix<double, 6, 6>> resultReferenceFrameTransformationsCovariance(gtsam::Z_6x6);
640 TransformsDictionary<Eigen::Isometry3d> resultLandmarkTransformations(Eigen::Isometry3d::Identity());
641 TransformsDictionary<Eigen::Matrix<double, 6, 6>> resultLandmarkTransformationsCovariance(gtsam::Z_6x6);
642
643 // Actual computation of fixed-frame transformations
644 if (graphConfigPtr_->optimizeReferenceFramePosesWrtWorldFlag_) {
645 // Mutex because we are changing the dynamically allocated graphKeys
646 std::lock_guard<std::mutex> modifyGraphKeysLock(gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().mutex());
647
648 // Iterate through all dynamically allocated variables (holistic, calibration, landmarks) --------------------------------
649 for (auto& framePairKeyMapIterator : gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().getTransformsMap()) {
650 // Frame Pair
651 const std::pair<std::string, std::string>& framePair = framePairKeyMapIterator.first; // alias
652
653 // Get Variable Key
654 const gtsam::Key& gtsamKey = framePairKeyMapIterator.second.key(); // alias
655
656 // Check whether it is landmark state
657 bool isLandmarkState = false;
658 if (framePairKeyMapIterator.second.getVariableTypeEnum() == DynamicVariableTypeEnum::Landmark) {
659 isLandmarkState = true;
660 }
661
662 // State Category Character
663 const char stateCategory = gtsam::Symbol(gtsamKey).chr();
664
665 // Try to compute results and uncertainties (if variable is still active)
666 if (framePairKeyMapIterator.second.isVariableActive()) {
667 // Case 1: All worked ------------------------------------------
668 try { // Obtain estimate and covariance from the extrinsic transformations
669 gtsam::Pose3 T_frame1_frame2;
670 gtsam::Matrix66 T_frame1_frame2_covariance = gtsam::Z_6x6;
671
672 // 6D Transformations
673 if (isCharInCharArray<numDynamic6DStates>(stateCategory, dim6StateSymbols)) {
674 T_frame1_frame2 = rtOptimizerPtr_->calculateEstimatedPose3(gtsamKey);
675 T_frame1_frame2_covariance = rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsamKey);
676 // Add current belief back to the map
677 framePairKeyMapIterator.second.updateLatestEstimate(T_frame1_frame2, T_frame1_frame2_covariance);
678 // Update the initial guess for the offline graph
679 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
680 gtsam::Values valuesEstimate;
681 valuesEstimate.insert(gtsamKey, T_frame1_frame2);
682 std::ignore = batchOptimizerPtr_->updateExistingValues(valuesEstimate);
683 }
684 }
685 // 3D Position3 Vectors
686 else if (isCharInCharArray<numDynamic3DStates>(stateCategory, dim3StateSymbols)) {
687 gtsam::Point3 point3Estimate = rtOptimizerPtr_->calculateEstimatedPoint3(gtsamKey);
688 T_frame1_frame2 = gtsam::Pose3(gtsam::Rot3::Identity(), point3Estimate);
689 T_frame1_frame2_covariance.block<3, 3>(3, 3) = rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsamKey);
690 // Add current belief back to the map
691 framePairKeyMapIterator.second.updateLatestEstimate(T_frame1_frame2, T_frame1_frame2_covariance);
692 // Update the initial guess for the offline graph
693 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
694 gtsam::Values valuesEstimate;
695 valuesEstimate.insert(gtsamKey, point3Estimate);
696 std::ignore = batchOptimizerPtr_->updateExistingValues(valuesEstimate);
697 }
698 }
699 // Only these two types are allowed for dynamic states for now
700 else {
701 throw std::runtime_error("Key is neither a pose nor a displacement key.");
702 }
703
704 // Transform quantities to world frame (instead of keyframe)
705 // For correction first have to transform T_W_F to T_F_W, apply correction, and then transform back to T_W_F
706 gtsam::Pose3 T_frame2_frame1_corrected = T_frame1_frame2.inverse();
707 T_frame2_frame1_corrected =
708 gtsam::Pose3(T_frame2_frame1_corrected.rotation(),
709 T_frame2_frame1_corrected.translation() + framePairKeyMapIterator.second.getReferenceFrameKeyframePosition());
710 gtsam::Pose3 T_frame1_frame2_corrected = T_frame2_frame1_corrected.inverse();
711 // Write to container
712 // 1: For Landmark States
713 if (isLandmarkState) {
714 // Write to landmark container
715 resultLandmarkTransformations.set_T_frame1_frame2(framePairKeyMapIterator.first.first, framePairKeyMapIterator.first.second,
716 Eigen::Isometry3d(T_frame1_frame2_corrected.matrix()));
717 resultLandmarkTransformationsCovariance.set_T_frame1_frame2(framePairKeyMapIterator.first.first,
718 framePairKeyMapIterator.first.second, T_frame1_frame2_covariance);
719 }
720 // 2: For Reference Frame States
721 else {
722 // Write to reference frame container
723 resultReferenceFrameTransformations.set_T_frame1_frame2(framePairKeyMapIterator.first.first,
724 framePairKeyMapIterator.first.second,
725 Eigen::Isometry3d(T_frame1_frame2_corrected.matrix()));
726 resultReferenceFrameTransformationsCovariance.set_T_frame1_frame2(
727 framePairKeyMapIterator.first.first, framePairKeyMapIterator.first.second, T_frame1_frame2_covariance);
728 // Potentially fill in existing pairs for logging
729 if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
731 realTimeReferenceFrameNamePairs_.insert(framePair);
732 REGULAR_COUT << " Added frame pair: " << framePair.first << " to " << framePair.second << " to real-time logging."
733 << std::endl;
734 }
735 }
736 }
737
738 // Mark that this key has at least been optimized once
739 if (framePairKeyMapIterator.second.getNumberStepsOptimized() == 0) {
741 REGULAR_COUT << " Fixed-frame Transformation between " << framePairKeyMapIterator.first.first << " and "
742 << framePairKeyMapIterator.first.second << " optimized for the first time." << COLOR_END << std::endl;
743 }
745 REGULAR_COUT << GREEN_START << " Result, RPY (deg): " << T_frame1_frame2.rotation().rpy().transpose() * (180.0 / M_PI)
746 << ", t (x, y, z): " << T_frame1_frame2.translation().transpose() << COLOR_END << std::endl;
747 }
748 }
749 // Increase Counter
750 framePairKeyMapIterator.second.incrementNumberStepsOptimized();
751
752 // Check health status of transformation --> Delete if diverged too much --> only necessary for global fixed frames
753 if (framePairKeyMapIterator.first.second == worldFrame_ &&
754 framePairKeyMapIterator.second.getNumberStepsOptimized() > MIN_ITERATIONS_BEFORE_REMOVING_STATIC_TRANSFORM) {
755 const gtsam::Pose3& T_frame1_frame2_initial =
756 framePairKeyMapIterator.second.getApproximateTransformationBeforeOptimization(); // alias
757 const double errorTangentSpace = gtsam::Pose3::Logmap(T_frame1_frame2_initial.between(T_frame1_frame2)).norm();
758
759 // Check error in tangent space
760 if (errorTangentSpace > graphConfigPtr_->referenceFramePosesResetThreshold_) {
761 REGULAR_COUT << RED_START << "Error in tangent space: " << errorTangentSpace << std::endl;
762 REGULAR_COUT << YELLOW_START << "GMsf-GraphManager" << RED_START << " Fixed Frame Transformation between "
763 << framePairKeyMapIterator.first.first << " and " << framePairKeyMapIterator.first.second
764 << " diverged too much. Removing from optimization and adding again freshly at next possibility." << COLOR_END
765 << std::endl;
766 // Remove state from state dictionary
767 gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().removeOrDeactivateTransform(framePairKeyMapIterator.first.first,
768 framePairKeyMapIterator.first.second);
769 break;
770 }
771 }
772 } // end: try statement
773 // Case 2: Result computation failed -----------------------------------------
774 catch (const std::out_of_range& exception) {
775 // Only remove/deactivate measurements that are still active and have been optimized before
776 if (framePairKeyMapIterator.second.getNumberStepsOptimized() > 0) {
777 REGULAR_COUT << RED_START << " OutOfRange-Exception while querying the active transformation and/or covariance at key "
778 << gtsam::Symbol(gtsamKey) << ", for frame pair " << framePairKeyMapIterator.first.first << ","
779 << framePairKeyMapIterator.first.second << std::endl
780 << " This happens if the requested variable is outside of the smoother window (e.g. because corresponding "
781 "measurement stopped). Hence, we keep this estimate unchanged and remove/deactivate it from the state "
782 "dictionary."
783 << COLOR_END << std::endl;
784 // Remove state from state dictionary
785 gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().removeOrDeactivateTransform(framePairKeyMapIterator.first.first,
786 framePairKeyMapIterator.first.second);
787 return;
788 }
789 // If active but added newly but never optimized
790 else {
791 // If too old but was never optimized --> remove or deactivate
792 double variableAge = framePairKeyMapIterator.second.computeVariableAge(currentGraphStateKeyTime);
793 if (variableAge > graphConfigPtr_->realTimeSmootherLag_) {
794 REGULAR_COUT << YELLOW_START << "GMsf-GraphManager" << RED_START << " Fixed Frame Transformation between "
795 << framePairKeyMapIterator.first.first << " and " << framePairKeyMapIterator.first.second << " is too old ("
796 << variableAge
797 << ")s and was never optimized. Removing from optimization and adding again freshly at next "
798 "possibility."
799 << COLOR_END << std::endl;
800 // Remove state from state dictionary
801 gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().removeOrDeactivateTransform(framePairKeyMapIterator.first.first,
802 framePairKeyMapIterator.first.second);
803 }
804 // Otherwise keep for now and potentially print out
805 else if (graphConfigPtr_->verboseLevel_ >= 1) {
807 << YELLOW_START << " Tried to query the transformation and/or covariance for frame pair "
808 << framePairKeyMapIterator.first.first << " to " << framePairKeyMapIterator.first.second
809 << ", at key: " << gtsam::Symbol(gtsamKey)
810 << ". Not yet available, as it was not yet optimized. Waiting for next optimization iteration until publishing it. "
811 "Current state key: "
812 << currentPropagatedKey << COLOR_END << std::endl;
813 }
814 }
815 } // catch statement
816 } // end: if active statement
817 } // for loop over all transforms
818 }
819
820 // Mutex block 2 ------------------
821 {
822 // Lock
823 const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
824
825 // 1. Optimized Graph State Status
826 optimizedGraphState_.setIsOptimized();
827 // Update Optimized Graph State
828 optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphStateKeyTime, resultNavState,
829 resultBias.correctGyroscope(currentGraphStateKeyAngularVelocity), resultBias);
830 optimizedGraphState_.updateReferenceFrameTransforms(resultReferenceFrameTransformations);
831 optimizedGraphState_.updateReferenceFrameTransformsCovariance(resultReferenceFrameTransformationsCovariance);
832 optimizedGraphState_.updateLandmarkTransforms(resultLandmarkTransformations);
833 optimizedGraphState_.updateLandmarkTransformsCovariance(resultLandmarkTransformationsCovariance);
834 optimizedGraphState_.updateCovariances(resultPoseCovarianceWorldFrame, resultVelocityCovariance);
835
836 // 2. State in World: Predict from solution to obtain refined propagated state
837 if (graphConfigPtr_->usingBiasForPreIntegrationFlag_) {
838 W_imuPropagatedState_ = imuBufferPreintegratorPtr_->predict(resultNavState, optimizedGraphState_.imuBias());
839 } else {
840 W_imuPropagatedState_ = imuBufferPreintegratorPtr_->predict(resultNavState, gtsam::imuBias::ConstantBias());
841 }
842
843 // 3. State in Odom
844 // Correct rotation only for roll and pitch, keep integrated yaw
845 // gtsam::Rot3 R_O_I_rp_corrected =
846 // gtsam::Rot3::Ypr(O_imuPropagatedState_.pose().rotation().yaw(), W_imuPropagatedState_.pose().rotation().pitch(),
847 // W_imuPropagatedState_.pose().rotation().roll());
848 // TODO: Apply gravity correction
849 // gtsam::Rot3 R_O_I_rp_corrected = O_imuPropagatedState_.pose().rotation();
850 // // Rotate corrected velocity to odom frame
851 // gtsam::Vector3 O_v_O_I = R_O_I_rp_corrected * W_imuPropagatedState_.bodyVelocity();
852 // Update the NavState
853 // O_imuPropagatedState_ = gtsam::NavState(R_O_I_rp_corrected, O_imuPropagatedState_.pose().translation(), O_v_O_I);
854
855 // // 4. Transformation between World and Odom
856 // T_W_O_ = Eigen::Isometry3d((W_imuPropagatedState_.pose() * O_imuPropagatedState_.pose().inverse()).matrix());
857
858 // Update the time of the last optimized state
859 lastOptimizedStateTime_ = currentGraphStateKeyTime;
860 } // end of locking
861
862 // Potentially log real-time state to container
863 if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
864 realTimeReferenceFrameContainer_.emplace(currentGraphStateKeyTime, resultReferenceFrameTransformations);
865 }
866}
867
868bool GraphManager::optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag) {
869 if (!runSlowBatchSmootherOptimization_(maxIterations)) {
870 return false;
871 }
872 saveOptimizedValuesToFile(batchOptimizerPtr_->getAllOptimizedStates(), batchOptimizerPtr_->getFullKeyTimestampMap(), savePath,
873 saveCovarianceFlag);
874 return true;
875}
876
877std::optional<BatchOptimizationResult> GraphManager::optimizeSlowBatchSmoother(int maxIterations, const bool computePoseCovarianceFlag) {
878 if (!runSlowBatchSmootherOptimization_(maxIterations)) {
879 return std::nullopt;
880 }
881 return extractBatchOptimizationResult_(computePoseCovarianceFlag);
882}
883
884// Logging of the real-time navigation states
885bool GraphManager::logRealTimeNavStates(const std::string& savePath, const std::string& timeString) {
886 // Note enabled
887 if (!graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
888 REGULAR_COUT << RED_START << " Logging of real-time states is disabled. " << COLOR_END << std::endl;
889 return false;
890 }
891 // Enabled
892 else {
893 // Main container: map
894 std::map<std::string, std::ofstream> fileStreams;
895
896 // Create directory if it does not exist
897 if (!std::filesystem::exists(savePath + timeString)) {
898 std::filesystem::create_directories(savePath + timeString);
899 }
900
901 // Create File Streams
902 // CSV
903 FileLogger::createPose3CsvFileStream(fileStreams, savePath, "rt_world_x_state_6D_pose", timeString, false);
904 FileLogger::createPose3CsvFileStream(fileStreams, savePath, "rt_odom_x_state_6D_pose", timeString, false);
905 // TUM
906 FileLogger::createPose3TumFileStream(fileStreams, savePath, "rt_world_x_state_6D_pose_tum", timeString);
907 FileLogger::createPose3TumFileStream(fileStreams, savePath, "rt_odom_x_state_6D_pose_tum", timeString);
908
909 // Iterate through all states and write them to file
910 // A. World Pose
911 for (const auto& statePair : realTimeWorldPoseContainer_) {
912 // Write to files
913 // CSV
914 FileLogger::writePose3ToCsvFile(fileStreams, statePair.second, "rt_world_x_state_6D_pose", statePair.first, false);
915 // TUM
916 FileLogger::writePose3ToTumFile(fileStreams, statePair.second, "rt_world_x_state_6D_pose_tum", statePair.first);
917 }
918 // B. Odom Pose
919 for (const auto& statePair : realTimeOdomPoseContainer_) {
920 // Write to files
921 // CSV
922 FileLogger::writePose3ToCsvFile(fileStreams, statePair.second, "rt_odom_x_state_6D_pose", statePair.first, false);
923 // TUM
924 FileLogger::writePose3ToTumFile(fileStreams, statePair.second, "rt_odom_x_state_6D_pose_tum", statePair.first);
925 }
926 return true;
927 }
928}
929
930// Logging of real-time reference frame states
931bool GraphManager::logRealTimeReferenceFrameStates(const std::string& savePath, const std::string& timeString) {
932 // Note enabled
933 if (!graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
934 REGULAR_COUT << RED_START << " Logging of real-time states is disabled. " << COLOR_END << std::endl;
935 return false;
936 }
937 // Enabled
938 else {
939 // Main container: map
940 std::map<std::string, std::ofstream> fileStreams;
941
942 // Create directory if it does not exist
943 if (!std::filesystem::exists(savePath + timeString)) {
944 std::filesystem::create_directories(savePath + timeString);
945 }
946
947 // State Category String
948 std::string stateCategoryString = "rt_R_6D_transform";
949
950 // Go through all reference frame states that have been logged
951 for (const std::pair<std::string, std::string>& framePair : realTimeReferenceFrameNamePairs_) {
952 // Frame Information
953 std::string frameInformation = "_" + framePair.first + "_to_" + framePair.second;
954 // Replace / with underscore
955 std::replace(frameInformation.begin(), frameInformation.end(), '/', '_');
956 // Create File Streams
957 // CSV
958 FileLogger::createPose3CsvFileStream(fileStreams, savePath, stateCategoryString + frameInformation, timeString, false);
959 // TUM
960 FileLogger::createPose3TumFileStream(fileStreams, savePath, stateCategoryString + frameInformation + "_tum", timeString);
961 }
962
963 // Iterate through all timestamps and corresponding transform maps
964 for (const auto& mapPair : realTimeReferenceFrameContainer_) {
965 const double& timeStamp = mapPair.first; // alias
966 const TransformsDictionary<Eigen::Isometry3d>& referenceFrameTransforms = mapPair.second;
967 // Iterate through all transforms for this time stamp
968 for (const auto& framePairTransform : referenceFrameTransforms.getTransformsMap()) {
969 // Frame Pair
970 const std::pair<std::string, std::string>& framePair = framePairTransform.first; // alias
971 // Transform
972 const Eigen::Isometry3d& transform = framePairTransform.second; // alias
973 // Frame Information
974 std::string frameInformation = "_" + framePair.first + "_to_" + framePair.second;
975 // Replace / with underscore
976 std::replace(frameInformation.begin(), frameInformation.end(), '/', '_');
977 // Write to files
978 // CSV
979 FileLogger::writePose3ToCsvFile(fileStreams, gtsam::Pose3(transform.matrix()), stateCategoryString + frameInformation, timeStamp,
980 false);
981 // TUM
982 FileLogger::writePose3ToTumFile(fileStreams, gtsam::Pose3(transform.matrix()), stateCategoryString + frameInformation + "_tum",
983 timeStamp);
984 }
985 }
986 return true;
987 }
988}
989
990// Log Latency and Update Duration to Memory
991bool GraphManager::logLatencyAndUpdateDuration(const std::string& savePath, const std::string& timeString) {
992 // Note enabled
993 if (!graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_) {
994 REGULAR_COUT << RED_START << " Logging of latency and update duration is disabled. " << COLOR_END << std::endl;
995 return false;
996 }
997 // Enabled
998 else {
999 // Main container: map
1000 std::map<std::string, std::ofstream> fileStreams;
1001
1002 // Create directory if it does not exist
1003 if (!std::filesystem::exists(savePath + timeString)) {
1004 std::filesystem::create_directories(savePath + timeString);
1005 }
1006
1007 // Create File Streams
1008 // CSV
1009 FileLogger::createDoubleCsvFileStream(fileStreams, savePath, "latency", timeString);
1010 FileLogger::createDoubleCsvFileStream(fileStreams, savePath, "update_duration", timeString);
1011
1012 // Iterate through all timestamps and corresponding latency
1013 for (const auto& latencyPair : latencyContainer_) {
1014 const double& timeStamp = latencyPair.first; // alias
1015 const double& latency = latencyPair.second; // alias
1016 // Write to files
1017 // CSV
1018 FileLogger::writeDoubleToCsvFile(fileStreams, latency, "latency", timeStamp);
1019 }
1020
1021 // Iterate through all timestamps and corresponding update duration
1022 for (const auto& updateDurationPair : updateDurationContainer_) {
1023 const double& timeStamp = updateDurationPair.first; // alias
1024 const double& updateDuration = updateDurationPair.second; // alias
1025 // Write to files
1026 // CSV
1027 FileLogger::writeDoubleToCsvFile(fileStreams, updateDuration, "update_duration", timeStamp);
1028 }
1029
1030 return true;
1031 }
1032}
1033
1034// Save optimized values to file
1035void GraphManager::saveOptimizedValuesToFile(const gtsam::Values& optimizedValues, const std::map<gtsam::Key, double>& keyTimestampMap,
1036 const std::string& savePath, const bool saveCovarianceFlag) {
1037 // At compile time get the symbols for the 6D and 3D states
1038 constexpr int num6DStates = countNDStates<6>();
1039 constexpr int num3DStates = countNDStates<3>();
1040 constexpr std::array<char, num6DStates> dim6StateSymbols = getSymbolArrayForNDStates<6, num6DStates>();
1041 constexpr std::array<char, num3DStates> dim3StateSymbols = getSymbolArrayForNDStates<3, num3DStates>();
1042
1043 // Map to hold file streams, keyed by category
1044 std::map<std::string, std::ofstream> fileStreams;
1045
1046 // Get current time as string for file name
1047 std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
1048 std::tm now_tm = *std::localtime(&now_time_t);
1049 // String of time without line breaks: year_month_day_hour_min_sec
1050 std::ostringstream oss;
1051 oss << std::put_time(&now_tm, "%Y_%m_%d_%H_%M_%S");
1052 // Convert stream to string
1053 std::string timeString = oss.str();
1054 // Remove any line breaks
1055 timeString.erase(std::remove(timeString.begin(), timeString.end(), '\n'), timeString.end());
1056 // Replace spaces with underscores
1057 std::replace(timeString.begin(), timeString.end(), ' ', '_');
1058
1059 // Create directory if it does not exist
1060 if (!std::filesystem::exists(savePath + timeString)) {
1061 std::filesystem::create_directories(savePath + timeString);
1062 }
1063
1064 // Save real-time states
1065 if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
1066 // Real-time states
1067 std::ignore = logRealTimeNavStates(savePath, timeString);
1068 // Real-time Reference Frame States
1069 std::ignore = logRealTimeReferenceFrameStates(savePath, timeString);
1070 }
1071
1072 // Save Latency and Update Duration
1073 if (graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_) {
1074 std::ignore = logLatencyAndUpdateDuration(savePath, timeString);
1075 }
1076
1077 // Save optimized states
1078 // A. 6D SE(3) states -----------------------------------------------------------
1079 for (const auto& keyPosePair : optimizedValues.extract<gtsam::Pose3>()) {
1080 // Read out information
1081 const gtsam::Key& graphKey = keyPosePair.first;
1082 gtsam::Pose3 pose = keyPosePair.second;
1083 const gtsam::Symbol stateSymbol(graphKey);
1084 const char stateCategory = stateSymbol.chr();
1085 const double timeStamp = keyTimestampMap.at(graphKey);
1086
1087 // Additional strings
1088 std::string stateCategoryString = "";
1089 std::string frameInformation = "";
1090
1091 // State Category Capital
1092 const std::string stateCategoryCapital(1, static_cast<char>(std::toupper(stateCategory)));
1093
1094 // A.A Creation of the identifier for the file -----------------------------------
1095 // Case 1: Navigation State Pose
1096 if (stateCategory == 'x') {
1097 stateCategoryString = stateCategoryCapital + "_state_6D_pose";
1098 }
1099 // Case 2: Frame Transform
1100 else if (isCharInCharArray<num6DStates>(stateCategory, dim6StateSymbols)) {
1101 // State Category
1102 stateCategoryString = stateCategoryCapital + "_6D_transform";
1103 // Get Frame Pair
1104 std::pair<std::string, std::string> framePair;
1105 if (gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().getFramePairFromGtsamKey(framePair, graphKey)) {
1106 frameInformation = "_" + framePair.first + "_to_" + framePair.second;
1107 } else {
1108 REGULAR_COUT << RED_START << " Could not find frame pair for key: " << stateSymbol << COLOR_END << std::endl;
1109 throw std::runtime_error("Could not find frame pair for key.");
1110 }
1111 }
1112 // Otherwise: Undefined --> throw error
1113 else {
1114 throw std::logic_error(stateCategoryString + " is an unknown 6D state category. Check the admissible GTSAM symbols in the config.");
1115 }
1116 // Put together the identifier
1117 std::string transformIdentifier = stateCategoryString + frameInformation;
1118 // Replace / with underscore
1119 std::replace(transformIdentifier.begin(), transformIdentifier.end(), '/', '_');
1120
1121 // Check for the keyframe position
1122 Eigen::Vector3d keyframePosition = Eigen::Vector3d::Zero();
1123 gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().getKeyframePositionFromGtsamKey(keyframePosition, graphKey);
1124
1125 // A.B Write to file -----------------------------------------------------------
1126 // Check if we already have a file stream for this category --> if not, create one
1127 // CSV file for Pose3
1128 fileLogger_.createPose3CsvFileStream(fileStreams, savePath, transformIdentifier, timeString, saveCovarianceFlag);
1129 // TUM file for Pose3
1130 fileLogger_.createPose3TumFileStream(fileStreams, savePath, transformIdentifier + "_tum", timeString);
1131
1132 // Compute Covariance of Pose
1133 Eigen::Matrix<double, 6, 6> poseCovarianceInWorldRos = Eigen::Matrix<double, 6, 6>::Zero();
1134 if (saveCovarianceFlag) {
1135 gtsam::Matrix66 poseCovarianceInWorldGtsam = calculatePoseCovarianceAtKeyInWorldFrame(batchOptimizerPtr_, graphKey, __func__);
1136 // Convert to ROS Format
1137 poseCovarianceInWorldRos = convertCovarianceGtsamConventionToRosConvention(poseCovarianceInWorldGtsam);
1138 }
1139
1140 // If keyframe position is not zero, move the frame location to capture the random walk
1141 if (keyframePosition.norm() > 1e-6) {
1142 const gtsam::Pose3& T_W_M = pose; // alias
1143 gtsam::Pose3 T_M_W_corrected = T_W_M.inverse();
1144 T_M_W_corrected = gtsam::Pose3(T_M_W_corrected.rotation(),
1145 T_M_W_corrected.translation() + keyframePosition); // Remove the effect of the keyframe position
1146 gtsam::Pose3 T_W_M_corrected = T_M_W_corrected.inverse();
1147 pose = T_W_M_corrected;
1148 REGULAR_COUT << " Corrected frame position for " << transformIdentifier << " (" << gtsam::Symbol(graphKey) << ") by "
1149 << keyframePosition.transpose() << std::endl;
1150 }
1151
1152 // Write the values to the appropriate file
1153 // CSV file for Pose3
1154 fileLogger_.writePose3ToCsvFile(fileStreams, pose, transformIdentifier, timeStamp, saveCovarianceFlag, poseCovarianceInWorldRos);
1155 // TUM file for Pose3
1156 fileLogger_.writePose3ToTumFile(fileStreams, pose, transformIdentifier + "_tum", timeStamp);
1157 } // end of for loop over all pose states
1158
1159 // B. 3D R(3) states (e.g. velocity, calibration displacement, landmarks) -----------------------------------------------------------
1160 for (const auto& keyVectorPair : optimizedValues.extract<gtsam::Point3>()) {
1161 // Read out information
1162 const gtsam::Key& key = keyVectorPair.first;
1163 const gtsam::Vector& vector = keyVectorPair.second;
1164 const gtsam::Symbol symbol(key);
1165 const char stateCategory = symbol.chr();
1166 const double timeStamp = keyTimestampMap.at(key);
1167
1168 // Additional strings
1169 std::string stateCategoryString = "";
1170 std::string frameInformation = "";
1171
1172 // State Category Non-Capital (as it is a vector instead of a matrix)
1173 const std::string stateCategoryNonCapital(1, stateCategory);
1174
1175 // B.A Creation of the identifier for the file -----------------------------------
1176 // Case 1: Navigation State Velocity
1177 if (stateCategory == 'v') {
1178 stateCategoryString = stateCategoryNonCapital + "_state_3D_velocity";
1179 }
1180 // Case 2: Point3 (e.g. calibration displacement, landmarks), TODO: no landmarks are saved for now
1181 else if (isCharInCharArray<num3DStates>(stateCategory, dim3StateSymbols)) {
1182 // State Category
1183 stateCategoryString = stateCategoryNonCapital + "_3D_vector";
1184 // Skip landmarks for now
1185 if (stateCategory == 'l') {
1186 continue;
1187 }
1188
1189 // Get Frame Pair
1190 std::pair<std::string, std::string> framePair;
1191 if (gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().getFramePairFromGtsamKey(framePair, key)) {
1192 frameInformation = "_" + framePair.first + "_to_" + framePair.second;
1193 } else {
1194 REGULAR_COUT << RED_START << " Could not find frame pair for key: " << symbol.chr() << COLOR_END << std::endl;
1195 }
1196 }
1197 // Otherwise: Undefined --> throw error
1198 else {
1199 throw std::logic_error(stateCategoryString + " is an unknown 3D state category. Check the admissible GTSAM symbols in the config.");
1200 }
1201 // Put together the identifier
1202 std::string transformIdentifier = stateCategoryString + frameInformation;
1203
1204 // B.B Write to file -----------------------------------------------------------
1205 // Check if we already have a file stream for this category --> if not, create one
1206 fileLogger_.createPoint3CsvFileStream(fileStreams, savePath, transformIdentifier, timeString);
1207
1208 // Write the values to the appropriate file
1209 fileLogger_.writePoint3ToCsvFile(fileStreams, vector, transformIdentifier, timeStamp);
1210 } // end of for loop over all vector states
1211
1212 // C. Bias states (accelerometer and gyroscope) -----------------------------------------------------------
1213 for (const auto& keyBiasPair : optimizedValues.extract<gtsam::imuBias::ConstantBias>()) {
1214 // Read out information
1215 const gtsam::Key& key = keyBiasPair.first;
1216 const gtsam::imuBias::ConstantBias& bias = keyBiasPair.second;
1217 const gtsam::Symbol symbol(key);
1218 const char stateCategory = symbol.chr();
1219 const double timeStamp = keyTimestampMap.at(key);
1220
1221 // Assert that the symbol is a bias
1222 assert(stateCategory == 'b');
1223
1224 // Capitalized state category
1225 const std::string stateCategoryCapital(1, static_cast<char>(std::toupper(stateCategory)));
1226 ;
1227
1228 // C.A Creation of the identifier for the file -----------------------------------
1229 std::string stateCategoryIdentifier = stateCategoryCapital + "_imu_bias";
1230
1231 // C.B Write to file -----------------------------------------------------------
1232 // Check if we already have a file stream for this category --> if not, create one
1233 fileLogger_.createImuBiasCsvFileStream(fileStreams, savePath, stateCategoryIdentifier, timeString);
1234
1235 // Write the values to the appropriate file
1236 fileLogger_.writeImuBiasToCsvFile(fileStreams, bias, stateCategoryIdentifier, timeStamp);
1237 }
1238
1239 // Close all file streams
1240 for (auto& pair : fileStreams) {
1241 pair.second.close();
1242 }
1243
1244 // Print
1245 REGULAR_COUT << GREEN_START << " Saved all optimized states to files." << COLOR_END << std::endl;
1246}
1247
1248// Save optimized Graph to Common Open source G2o format
1249void GraphManager::saveOptimizedGraphToG2o(const OptimizerBase& optimizedGraph, const gtsam::Values& optimizedValues,
1250 const std::string& saveFileName) {
1251 // Safe optimized states
1252 gtsam::writeG2o(optimizedGraph.getNonlinearFactorGraph(), optimizedValues, saveFileName);
1253}
1254
1255// Calculate State at Key
1256gtsam::NavState GraphManager::calculateStateAtGeneralKey(bool& computeSuccessfulFlag, const gtsam::Key& generalKey) {
1257 return calculateNavStateAtGeneralKey(computeSuccessfulFlag, rtOptimizerPtr_, generalKey, __func__);
1258}
1259
1260// Private --------------------------------------------------------------------
1261
1263 if (!graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
1264 return false;
1265 }
1266 std::chrono::time_point<std::chrono::high_resolution_clock> startOptimizationTime = std::chrono::high_resolution_clock::now();
1267 batchOptimizerPtr_->optimize(maxIterations);
1268 std::chrono::time_point<std::chrono::high_resolution_clock> endOptimizationTime = std::chrono::high_resolution_clock::now();
1269 double optimizationDuration =
1270 std::chrono::duration_cast<std::chrono::milliseconds>(endOptimizationTime - startOptimizationTime).count();
1271 std::cout << "Optimization took " << optimizationDuration / 1000 << " seconds." << std::endl;
1272 std::cout << "Number of optimization variables: " << batchOptimizerPtr_->getAllOptimizedStates().size() << std::endl;
1273 return true;
1274}
1275
1277 const gtsam::Values& optimizedStateValues = batchOptimizerPtr_->getAllOptimizedStates();
1278 const std::map<gtsam::Key, double>& keyTimestampMap = batchOptimizerPtr_->getFullKeyTimestampMap();
1279
1281 // X, V and B of one navigation state always share the same key index.
1282 for (const auto& [poseKey, T_W_Ik] : optimizedStateValues.extract<gtsam::Pose3>()) {
1283 const gtsam::Symbol poseSymbol(poseKey);
1284 if (poseSymbol.chr() != 'x') {
1285 continue;
1286 }
1287 const gtsam::Key index = poseSymbol.index();
1288 const gtsam::imuBias::ConstantBias& bias =
1289 optimizedStateValues.at<gtsam::imuBias::ConstantBias>(gtsam::symbol_shorthand::B(index));
1290
1291 BatchOptimizedNavState navState;
1292 navState.timeK = keyTimestampMap.at(poseKey);
1293 navState.T_W_Ik = Eigen::Isometry3d(T_W_Ik.matrix());
1294 navState.W_v_W_I = optimizedStateValues.at<gtsam::Vector3>(gtsam::symbol_shorthand::V(index));
1295 navState.accelerometerBias = bias.accelerometer();
1296 navState.gyroscopeBias = bias.gyroscope();
1297 if (computePoseCovarianceFlag) {
1298 navState.poseCovarianceInWorld =
1300 }
1301 result.navStates.push_back(std::move(navState));
1302 }
1303
1304 std::sort(result.navStates.begin(), result.navStates.end(),
1305 [](const BatchOptimizedNavState& a, const BatchOptimizedNavState& b) { return a.timeK < b.timeK; });
1306 return result;
1307}
1308
1309// Update of the two IMU pre-integrators
1311 if (imuMeas.size() < 2) {
1312 REGULAR_COUT << " Received less than 2 IMU messages --- No Preintegration done." << std::endl;
1313 return;
1314 } else if (imuMeas.size() > 2) {
1315 REGULAR_COUT << RED_START << "Currently only supporting two IMU messages for pre-integration." << COLOR_END << std::endl;
1316 throw std::runtime_error("Terminating.");
1317 }
1318
1319 // Start integrating with imu_meas.begin()+1 meas to calculate dt,
1320 // imu_meas.begin() meas was integrated before
1321 auto currItr = imuMeas.begin();
1322 auto prevItr = currItr++;
1323
1324 // Calculate dt and integrate IMU measurements for both pre-integrators
1325 for (; currItr != imuMeas.end(); ++currItr, ++prevItr) {
1326 double dt = currItr->first - prevItr->first;
1327 imuStepPreintegratorPtr_->integrateMeasurement(currItr->second.acceleration, // acc
1328 currItr->second.angularVelocity, // gyro
1329 dt); // delta t
1330 imuBufferPreintegratorPtr_->integrateMeasurement(currItr->second.acceleration, // acc
1331 currItr->second.angularVelocity, // gyro
1332 dt); // delta t
1333 }
1334}
1335
1336// Returns true if the factors/values were added without any problems
1337// Otherwise returns false (e.g. if exception occurs while adding
1338bool GraphManager::addFactorsToRtSmootherAndOptimize(const gtsam::NonlinearFactorGraph& newRtGraphFactors,
1339 const gtsam::Values& newRtGraphValues,
1340 const std::map<gtsam::Key, double>& newRtGraphKeysTimestampsMap,
1341 const std::shared_ptr<GraphConfig>& graphConfigPtr, const int additionalIterations) {
1342 // Timing
1343 std::chrono::time_point<std::chrono::high_resolution_clock> startLoopTime = std::chrono::high_resolution_clock::now();
1344 std::chrono::time_point<std::chrono::high_resolution_clock> endLoopTime;
1345
1346 // Lock for optimization (as shared rtOptimizer is optimized)
1347 const std::lock_guard<std::mutex> optimization(optimizationRunningMutex_);
1348
1349 // Perform update of the real-time smoother, including optimization
1350 bool successFlag = rtOptimizerPtr_->update(newRtGraphFactors, newRtGraphValues, newRtGraphKeysTimestampsMap);
1351 // Additional iterations
1352 for (size_t itr = 0; itr < additionalIterations; ++itr) {
1353 successFlag = successFlag && rtOptimizerPtr_->update();
1354 }
1355
1356 // Logging
1357 if (graphConfigPtr->verboseLevel_ >= VerbosityLevels::kOptimizationEffort2) {
1358 endLoopTime = std::chrono::high_resolution_clock::now();
1359 REGULAR_COUT << GREEN_START << " Whole optimization loop took "
1360 << std::chrono::duration_cast<std::chrono::milliseconds>(endLoopTime - startLoopTime).count() << " milliseconds."
1361 << COLOR_END << std::endl;
1362 }
1363
1364 return successFlag;
1365}
1366
1367// Batch Smoother
1368bool GraphManager::addFactorsToBatchSmootherAndOptimize(const gtsam::NonlinearFactorGraph& newBatchGraphFactors,
1369 const gtsam::Values& newBatchGraphValues,
1370 const std::map<gtsam::Key, double>& newBatchGraphKeysTimestampsMap,
1371 const std::shared_ptr<GraphConfig>& graphConfigPtr) {
1372 // Lock for optimization (as shared rtOptimizer is optimized)
1373 const std::lock_guard<std::mutex> optimization(optimizationRunningMutex_);
1374
1375 // Add Factors and States to Batch Optimization (if desired) without running optimization
1376 bool successFlag = false;
1377 if (graphConfigPtr->useAdditionalSlowBatchSmootherFlag_) {
1378 successFlag = batchOptimizerPtr_->update(newBatchGraphFactors, newBatchGraphValues, newBatchGraphKeysTimestampsMap);
1379 }
1380
1381 // Return
1382 return successFlag;
1383}
1384
1385// Find closest keys in graph
1386bool GraphManager::findGraphKeys_(gtsam::Key& closestKeyKm1, gtsam::Key& closestKeyK, double& keyTimeStampDistance,
1387 const double maxTimestampDistance, const double timeKm1, const double timeK, const std::string& name) {
1388 // Find closest lidar keys in existing graph
1389 double closestGraphTimeKm1, closestGraphTimeK;
1390 {
1391 // Looking up from IMU buffer --> acquire mutex (otherwise values for key
1392 // might not be set)
1393 const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
1394 bool success =
1395 timeToKeyBufferPtr_->getClosestKeyAndTimestamp(closestGraphTimeKm1, closestKeyKm1, name + " km1", maxTimestampDistance, timeKm1);
1396 success =
1397 success && timeToKeyBufferPtr_->getClosestKeyAndTimestamp(closestGraphTimeK, closestKeyK, name + " k", maxTimestampDistance, timeK);
1398 if (!success) {
1399 REGULAR_COUT << RED_START << " Could not find closest keys for " << name << COLOR_END << std::endl;
1400 return false;
1401 }
1402 }
1403
1404 // Check
1405 if (closestGraphTimeKm1 > closestGraphTimeK) {
1406 REGULAR_COUT << RED_START << " Time at time step k-1 must be smaller than time at time step k." << COLOR_END << std::endl;
1407 return false;
1408 }
1409
1410 keyTimeStampDistance = std::abs(closestGraphTimeK - closestGraphTimeKm1);
1411 if (keyTimeStampDistance > maxTimestampDistance) {
1412 REGULAR_COUT << " Distance of " << name << " timestamps is too big. Found timestamp difference is "
1413 << closestGraphTimeK - closestGraphTimeKm1 << " which is larger than the maximum admissible distance of "
1414 << maxTimestampDistance << ". Still adding constraints to graph." << COLOR_END << std::endl;
1415 }
1416 return true;
1417}
1418
1419void GraphManager::writeValueKeysToKeyTimeStampMap_(const gtsam::Values& values, const double measurementTime,
1420 std::shared_ptr<std::map<gtsam::Key, double>> keyTimestampMapPtr) {
1421 for (const auto& value : values) {
1422 writeKeyToKeyTimeStampMap_(value.key, measurementTime, keyTimestampMapPtr);
1423 }
1424}
1425
1426} // namespace holistic_fusion
#define REGULAR_COUT
#define MIN_ITERATIONS_BEFORE_REMOVING_STATIC_TRANSFORM
#define COLOR_END
#define GREEN_START
#define RED_START
#define YELLOW_START
static void writePose3ToCsvFile(std::map< std::string, std::ofstream > &fileStreams, const gtsam::Pose3 &pose, const std::string &transformIdentifier, const double timeStamp, const bool saveCovarianceFlag, const std::optional< Eigen::Matrix< double, 6, 6 > > &optionalPoseCovarianceInWorldRos=std::nullopt)
static void writeDoubleToCsvFile(std::map< std::string, std::ofstream > &fileStreams, const double value, const std::string &transformIdentifier, const double timeStamp)
static void writePose3ToTumFile(std::map< std::string, std::ofstream > &fileStreams, const gtsam::Pose3 &pose, const std::string &transformIdentifier, const double timeStamp)
static void createPose3CsvFileStream(std::map< std::string, std::ofstream > &fileStreams, const std::string &savePath, const std::string &transformIdentifier, const std::string &timeString, const bool saveCovarianceFlag)
static void createDoubleCsvFileStream(std::map< std::string, std::ofstream > &fileStreams, const std::string &savePath, const std::string &transformIdentifier, const std::string &timeString)
static void createPose3TumFileStream(std::map< std::string, std::ofstream > &fileStreams, const std::string &savePath, const std::string &transformIdentifier, const std::string &timeString)
const uint64_t newPropagatedStateKey_()
Generate new key.
std::shared_ptr< TimeGraphKeyBuffer > timeToKeyBufferPtr_
gtsam::NavState O_imuPropagatedState_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements > imuStepPreintegratorPtr_
Step Preintegrator.
void saveOptimizedValuesToFile(const gtsam::Values &optimizedValues, const std::map< gtsam::Key, double > &keyTimestampMap, const std::string &savePath, const bool saveCovarianceFlag)
std::set< std::pair< std::string, std::string > > realTimeReferenceFrameNamePairs_
std::map< double, gtsam::Pose3 > realTimeOdomPoseContainer_
bool logLatencyAndUpdateDuration(const std::string &savePath, const std::string &timeString)
void addImuFactorAndGetState(SafeIntegratedNavState &returnPreIntegratedNavState, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &newOptimizedNavStatePtr, const std::shared_ptr< ImuBuffer > &imuBufferPtr, double imuTimeK, bool createNewStateFlag)
bool setInitialWorldFrameToFixedFrameTransform(const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
gtsam::Pose3 getInitialWorldFrameToFixedFrameTransform(const std::string &fixedFrame)
bool addFactorsToBatchSmootherAndOptimize(const gtsam::NonlinearFactorGraph &newBatchGraphFactors, const gtsam::Values &newBatchGraphValues, const std::map< gtsam::Key, double > &newBatchGraphKeysTimestampsMap, const std::shared_ptr< GraphConfig > &graphConfigPtr)
bool runSlowBatchSmootherOptimization_(int maxIterations)
bool getUnaryFactorGeneralKey(gtsam::Key &returnedKey, double &returnedGraphTime, const UnaryMeasurement &unaryMeasurement)
bool hasReferenceFrameKeyframe(const std::string &worldFrame, const std::string &fixedFrame)
std::map< double, double > updateDurationContainer_
gtsam::NavState calculateStateAtGeneralKey(bool &computeSuccessfulFlag, const gtsam::Key &generalKey)
bool logRealTimeNavStates(const std::string &savePath, const std::string &timeString)
std::shared_ptr< holistic_fusion::GraphConfig > graphConfigPtr_
Config.
std::map< double, double > latencyContainer_
std::shared_ptr< gtsam::NonlinearFactorGraph > batchFactorGraphBufferPtr_
bool initImuIntegrators(double gravityValue)
gtsam::Vector3 latestGraphStateKeyAngularVelocity_
static gtsam::NavState calculateNavStateAtGeneralKey(bool &computeSuccessfulFlag, std::shared_ptr< holistic_fusion::OptimizerBase > graphPtr, const gtsam::Key &generalKey, const char *callingFunctionName)
std::shared_ptr< OptimizerBase > batchOptimizerPtr_
void writeValueKeysToKeyTimeStampMap_(const gtsam::Values &values, double measurementTime, std::shared_ptr< std::map< gtsam::Key, double > > keyTimestampMapPtr)
std::map< double, TransformsDictionary< Eigen::Isometry3d > > realTimeReferenceFrameContainer_
bool logRealTimeReferenceFrameStates(const std::string &savePath, const std::string &timeString)
void updateImuIntegrators_(const TimeToImuMap &imuMeas)
Update IMU integrators.
BatchOptimizationResult extractBatchOptimizationResult_(const bool computePoseCovarianceFlag)
std::mutex operateOnGraphDataMutex_
Mutex.
std::chrono::time_point< std::chrono::high_resolution_clock > latencyEndTime_
GraphManager(std::shared_ptr< GraphConfig > graphConfigPtr, std::string imuFrame, std::string worldFrame)
std::chrono::time_point< std::chrono::high_resolution_clock > updateDurationEndTime_
bool initPoseVelocityBiasGraph(double timeStamp, const gtsam::Pose3 &T_W_I0, const gtsam::Pose3 &T_O_I0, const gtsam::Vector3 &imuAngularVelocity)
DynamicDictionaryContainer gtsamDynamicExpressionKeys_
std::shared_ptr< OptimizerBase > rtOptimizerPtr_
bool addFactorToRtAndBatchGraph_(const gtsam::NoiseModelFactor *noiseModelFactorPtr, const bool addToOnlineSmootherFlag=true)
std::chrono::time_point< std::chrono::high_resolution_clock > latencyStartTime_
bool findGraphKeys_(gtsam::Key &closestKeyKm1, gtsam::Key &closestKeyK, double &keyTimeStampDistance, double maxTimestampDistance, double timeKm1, double timeK, const std::string &name)
Find graph keys for timestamps.
std::map< double, gtsam::Pose3 > realTimeWorldPoseContainer_
holistic_fusion::GraphState optimizedGraphState_
std::shared_ptr< gtsam::NonlinearFactorGraph > rtFactorGraphBufferPtr_
Data buffer.
std::shared_ptr< std::map< gtsam::Key, double > > rtGraphKeysTimestampsMapBufferPtr_
std::shared_ptr< gtsam::Values > batchGraphValuesBufferPtr_
bool addFactorsToRtSmootherAndOptimize(const gtsam::NonlinearFactorGraph &newRtGraphFactors, const gtsam::Values &newRtGraphValues, const std::map< gtsam::Key, double > &newRtGraphKeysTimestampsMap, const std::shared_ptr< GraphConfig > &graphConfigPtr, const int additionalIterations)
gtsam::NavState W_imuPropagatedState_
Propagated state (at IMU frequency).
static Eigen::Matrix< double, 6, 6 > calculatePoseCovarianceAtKeyInWorldFrame(std::shared_ptr< holistic_fusion::OptimizerBase > graphPtr, const gtsam::Key &graphKey, const char *callingFunctionName, std::optional< const gtsam::NavState > optionalNavState=std::nullopt)
gtsam::Key addPoseBetweenFactor(const gtsam::Pose3 &deltaPose, const Eigen::Matrix< double, 6, 1 > &poseBetweenNoiseDensity, double lidarTimeKm1, double lidarTimeK, double rate, const RobustNormEnum &robustNormEnum, const double robustNormConstant)
std::shared_ptr< gtsam::imuBias::ConstantBias > imuBiasPriorPtr_
std::shared_ptr< std::map< gtsam::Key, double > > batchGraphKeysTimestampsMapBufferPtr_
static void saveOptimizedGraphToG2o(const OptimizerBase &optimizedGraph, const gtsam::Values &optimizedValues, const std::string &saveFileName)
bool addFactorSafelyToRtAndBatchGraph_(const gtsam::NoiseModelFactor *noiseModelFactorPtr, double measurementTimestamp)
std::chrono::time_point< std::chrono::high_resolution_clock > updateDurationStartTime_
std::shared_ptr< gtsam::Values > rtGraphValuesBufferPtr_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements > imuBufferPreintegratorPtr_
Buffer Preintegrator.
void writeKeyToKeyTimeStampMap_(const gtsam::Key &key, double measurementTime, std::shared_ptr< std::map< gtsam::Key, double > > keyTimestampMapPtr)
Associate timestamp to each 'value key', e.g. for graph key 0, value keys (x0,v0,b0) need to be assoc...
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements::Params > imuParamsPtr_
bool optimizeSlowBatchSmoother(int maxIterations, const std::string &savePath, const bool saveCovarianceFlag)
const std::string & measurementName() const
Names.
Definition Measurement.h:82
virtual const gtsam::NonlinearFactorGraph & getNonlinearFactorGraph() const =0
void update(const Eigen::Isometry3d &T_W_O, const Eigen::Isometry3d &T_O_Ik_gravityAligned, const Eigen::Vector3d &I_v_W_I, const Eigen::Vector3d &I_w_W_I, double timeK, bool odomNotJump)
Definition NavState.cpp:67
void set_T_frame1_frame2(const std::string &frame1, const std::string &frame2, const TRANSFORM_TYPE T_frame1_frame2)
std::map< std::pair< std::string, std::string >, TRANSFORM_TYPE > & getTransformsMap()
Class to represent a unary measurement in the graph.
std::map< double, holistic_fusion::ImuMeasurement, std::less< double >, Eigen::aligned_allocator< std::pair< const double, holistic_fusion::ImuMeasurement > > > TimeToImuMap
Definition ImuBuffer.hpp:35
@ kFunctioningAndStateMachine1
Definition GraphConfig.h:17
Eigen::Matrix< double, 6, 6 > convertCovarianceGtsamConventionToRosConvention(const Eigen::Matrix< double, 6, 6 > &covGtsam)
Convert a 6x6 covariance matrix from ROS convention to GTSAM convention.
Definition conversions.h:18
constexpr std::array< char, numStates > getSymbolArrayForNDStates()
bool isCharInCharArray(const char c, std::array< char, ARRAY_SIZE > charArray)
constexpr int countNDStates()
std::vector< BatchOptimizedNavState > navStates