fix(graph_msf): preserve IMU propagation across optimizer updates

graph_msf/include/graph_msf/core/GraphManager.h

@@ -62,7 +62,9 @@ class GraphManager {
 
   // Initialization Interface ---------------------------------------------------
   bool initImuIntegrators(double gravityValue);
-  bool initPoseVelocityBiasGraph(double timeStamp, const gtsam::Pose3& T_W_I0, const gtsam::Pose3& T_O_I0);
+  // imuAngularVelocity is the bias-uncorrected IMU-frame sample at timeStamp [rad/s].
+  bool initPoseVelocityBiasGraph(double timeStamp, const gtsam::Pose3& T_W_I0, const gtsam::Pose3& T_O_I0,
+                                const gtsam::Vector3& imuAngularVelocity);
 
   // IMU at the core -----------------------------------------------------------
   void addImuFactorAndGetState(SafeIntegratedNavState& returnPreIntegratedNavState,
@@ -193,9 +195,10 @@ class GraphManager {
   gtsam::NavState O_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0));
   Eigen::Isometry3d T_W_O_ = Eigen::Isometry3d::Identity();  // Current state pose, depending on whether propagated state jumps or not
   gtsam::Key propagatedStateKey_ = 0;                        // Current state key, always start with 0
-  double propagatedStateTime_ = 0.0;                         // Current state time
+  double propagatedImuSampleTime_ = 0.0;
+  double latestGraphStateKeyTime_ = 0.0;
   double lastOptimizedStateTime_ = 0.0;                      // Last optimized state time
-  gtsam::Vector3 currentAngularVelocity_ = gtsam::Vector3(0, 0, 0);
+  gtsam::Vector3 latestGraphStateKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0);
 
   // Optimizer(s)
   std::shared_ptr<OptimizerBase> rtOptimizerPtr_;

graph_msf/include/graph_msf/core/GraphManager.inl

@@ -21,7 +21,7 @@ void GraphManager::addUnaryFactorInImuFrame(const MEASUREMENT_TYPE& unaryMeasure
   std::string callingName = "GnssPositionUnaryFactor";
   if (!timeToKeyBufferPtr_->getClosestKeyAndTimestamp(closestGraphTime, closestKey, callingName, graphConfigPtr_->maxSearchDeviation_,
                                                       measurementTime)) {
-    if (propagatedStateTime_ - measurementTime < 0.0) {  // Factor is coming from the future, hence add it to the buffer and adding it later
+    if (propagatedImuSampleTime_ - measurementTime < 0.0) {  // Factor is coming from the future, hence add it to the buffer and adding it later
       // TODO: Add to buffer and return --> still add it until we are there
     } else {  // Otherwise do not add it
       REGULAR_COUT << RED_START << " Time deviation of " << typeid(FACTOR_TYPE).name() << " at key " << closestKey << " is "

graph_msf/include/graph_msf/interface/GraphMsf.h

@@ -145,7 +145,7 @@ class GraphMsf {
   //// Set Imu Attitude
   bool alignImu_(double& imuAttitudeRoll, double& imuAttitudePitch);
   //// Initialize the graph
-  void initGraph_(const double timeStamp_k);
+  void initGraph_(const double timeStamp_k, const Eigen::Vector3d& imuAngularVelocity);
   //// Updating the factor graph
   void optimizeGraph_();
 

graph_msf/src/lib/GraphManager.cpp

@@ -127,7 +127,8 @@ bool GraphManager::initImuIntegrators(const double gravityValue) {
   return true;
 }
 
-bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam::Pose3& T_W_I0, const gtsam::Pose3& T_O_I0) {
+bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam::Pose3& T_W_I0, const gtsam::Pose3& T_O_I0,
+                                             const gtsam::Vector3& imuAngularVelocity) {
   // Create Prior factor ----------------------------------------------------
   /// Prior factor noise
   auto priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
@@ -213,8 +214,10 @@ bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam
 
   // Update Current State ---------------------------------------------------
   const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
+  latestGraphStateKeyTime_ = timeStamp;
+  latestGraphStateKeyAngularVelocity_ = imuAngularVelocity;
   optimizedGraphState_.updateNavStateAndBias(propagatedStateKey_, timeStamp, gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0)),
-                                             gtsam::Vector3(0, 0, 0), *imuBiasPriorPtr_);
+                                             imuBiasPriorPtr_->correctGyroscope(imuAngularVelocity), *imuBiasPriorPtr_);
   O_imuPropagatedState_ = gtsam::NavState(T_O_I0, gtsam::Vector3(0, 0, 0));
   W_imuPropagatedState_ = gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0));
   T_W_O_ = (T_W_I0.inverse() * T_O_I0).matrix();
@@ -237,8 +240,7 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte
   // 1.1 Get last two measurements from buffer to determine dt
   TimeToImuMap imuMeas;
   imuBufferPtr->getLastTwoMeasurements(imuMeas);
-  propagatedStateTime_ = imuTimeK;
-  currentAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity;
+  propagatedImuSampleTime_ = imuTimeK;
 
   // 1.2 Update IMU Pre-integrator
   updateImuIntegrators_(imuMeas);
@@ -305,6 +307,8 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte
     // Get new key
     const gtsam::Key oldKey = propagatedStateKey_;
     const gtsam::Key newKey = newPropagatedStateKey_();
+    latestGraphStateKeyTime_ = imuTimeK;
+    latestGraphStateKeyAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity;
 
     // Add to time key buffer
     timeToKeyBufferPtr_->addToBuffer(imuTimeK, newKey);
@@ -372,15 +376,15 @@ bool GraphManager::getUnaryFactorGeneralKey(gtsam::Key& returnedKey, double& ret
   if (!timeToKeyBufferPtr_->getClosestKeyAndTimestamp(returnedGraphTime, returnedKey, unaryMeasurement.measurementName(),
                                                       graphConfigPtr_->maxSearchDeviation_, unaryMeasurement.timeK())) {
     // Measurement coming from the future
-    if (propagatedStateTime_ - unaryMeasurement.timeK() < 0.0) {  // Factor is coming from the future, hence add it to the buffer
+    if (propagatedImuSampleTime_ - unaryMeasurement.timeK() < 0.0) {  // Factor is coming from the future, hence add it to the buffer
       // Not too far in the future --> add to buffer and add later
-      if (unaryMeasurement.timeK() - propagatedStateTime_ < 4 * graphConfigPtr_->maxSearchDeviation_) {
+      if (unaryMeasurement.timeK() - propagatedImuSampleTime_ < 4 * graphConfigPtr_->maxSearchDeviation_) {
         // TODO: Add to buffer and return --> still add it until we are there
         return true;
       }
       // Too far in the future --> do not add it
       else {
-        std::cout << 1000 * (propagatedStateTime_ - unaryMeasurement.timeK()) << std::endl;
+        std::cout << 1000 * (propagatedImuSampleTime_ - unaryMeasurement.timeK()) << std::endl;
         std::cout << 1000 * (returnedGraphTime - unaryMeasurement.timeK()) << std::endl;
         REGULAR_COUT << RED_START << " Factor coming from the future, AND time deviation of " << typeid(unaryMeasurement).name()
                      << " at key " << returnedKey << " is " << 1000 * std::abs(returnedGraphTime - unaryMeasurement.timeK())
@@ -532,17 +536,17 @@ void GraphManager::updateGraph() {
   gtsam::Values newRtGraphValues, newBatchGraphValues;
   std::map<gtsam::Key, double> newRtGraphKeysTimestampsMap, newBatchGraphKeysTimestampsMap;
   gtsam::Key currentPropagatedKey;
-  gtsam::Vector3 currentAngularVelocity;
-  double currentPropagatedTime;
+  gtsam::Vector3 currentGraphStateKeyAngularVelocity;
+  double currentGraphStateKeyTime;
 
   // Mutex Block 1 -----------------
   {
     // Lock
     const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
     // Get current key and time
     currentPropagatedKey = propagatedStateKey_;
-    currentPropagatedTime = propagatedStateTime_;
-    currentAngularVelocity = currentAngularVelocity_;
+    currentGraphStateKeyTime = latestGraphStateKeyTime_;
+    currentGraphStateKeyAngularVelocity = latestGraphStateKeyAngularVelocity_;
     // Get copy of factors and values and empty buffers
     newRtGraphFactors = *rtFactorGraphBufferPtr_;
     newRtGraphValues = *rtGraphValuesBufferPtr_;
@@ -559,13 +563,8 @@ void GraphManager::updateGraph() {
       batchGraphKeysTimestampsMapBufferPtr_->clear();
     }
 
-    // Empty Buffer Pre-integrator --> everything missed during the update will
-    // be in here
-    if (graphConfigPtr_->usingBiasForPreIntegrationFlag_) {
-      imuBufferPreintegratorPtr_->resetIntegrationAndSetBias(optimizedGraphState_.imuBias());
-    } else {
-      imuBufferPreintegratorPtr_->resetIntegrationAndSetBias(gtsam::imuBias::ConstantBias());
-    }
+    // Catch-up starts at the graph key, including its unfinished IMU interval.
+    *imuBufferPreintegratorPtr_ = *imuStepPreintegratorPtr_;
   }  // end of locking
 
   // Log Update Duration
@@ -595,7 +594,7 @@ void GraphManager::updateGraph() {
     updateDurationEndTime_ = std::chrono::high_resolution_clock::now();
     // Duration in seconds
     std::chrono::duration<double> updateDuration = updateDurationEndTime_ - updateDurationStartTime_;
-    updateDurationContainer_[currentPropagatedTime] = updateDuration.count();
+    updateDurationContainer_[currentGraphStateKeyTime] = updateDuration.count();
   }
 
   // Return if optimization failed
@@ -777,7 +776,7 @@ void GraphManager::updateGraph() {
           // If active but added newly but never optimized
           else {
             // If too old but was never optimized --> remove or deactivate
-            double variableAge = framePairKeyMapIterator.second.computeVariableAge(currentPropagatedTime);
+            double variableAge = framePairKeyMapIterator.second.computeVariableAge(currentGraphStateKeyTime);
             if (variableAge > graphConfigPtr_->realTimeSmootherLag_) {
               REGULAR_COUT << YELLOW_START << "GMsf-GraphManager" << RED_START << " Fixed Frame Transformation between "
                            << framePairKeyMapIterator.first.first << " and " << framePairKeyMapIterator.first.second << " is too old ("
@@ -813,8 +812,8 @@ void GraphManager::updateGraph() {
     // 1. Optimized Graph State Status
     optimizedGraphState_.setIsOptimized();
     // Update Optimized Graph State
-    optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentPropagatedTime, resultNavState,
-                                               resultBias.correctGyroscope(currentAngularVelocity), resultBias);
+    optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphStateKeyTime, resultNavState,
+                                               resultBias.correctGyroscope(currentGraphStateKeyAngularVelocity), resultBias);
     optimizedGraphState_.updateReferenceFrameTransforms(resultReferenceFrameTransformations);
     optimizedGraphState_.updateReferenceFrameTransformsCovariance(resultReferenceFrameTransformationsCovariance);
     optimizedGraphState_.updateLandmarkTransforms(resultLandmarkTransformations);
@@ -844,12 +843,12 @@ void GraphManager::updateGraph() {
     // T_W_O_ = Eigen::Isometry3d((W_imuPropagatedState_.pose() * O_imuPropagatedState_.pose().inverse()).matrix());
 
     // Update the time of the last optimized state
-    lastOptimizedStateTime_ = currentPropagatedTime;
+    lastOptimizedStateTime_ = currentGraphStateKeyTime;
   }  // end of locking
 
   // Potentially log real-time state to container
   if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
-    realTimeReferenceFrameContainer_.emplace(currentPropagatedTime, resultReferenceFrameTransformations);
+    realTimeReferenceFrameContainer_.emplace(currentGraphStateKeyTime, resultReferenceFrameTransformations);
   }
 }
 

graph_msf/src/lib/GraphMsf.cpp

@@ -297,7 +297,7 @@ bool GraphMsf::addCoreImuMeasurementAndGetState(
   } else if (!initedGraphFlag_) {  // Case 4: IMU aligned, yaw and position initialized, valid measurement received, but graph not yet
                                    // initialized
     preIntegratedNavStatePtr_->updateLatestMeasurementTimestamp(imuTimeK);
-    initGraph_(imuTimeK);
+    initGraph_(imuTimeK, returnAddedImuMeasurements.tail<3>());
     returnPreIntegratedNavStatePtr = std::make_shared<SafeIntegratedNavState>(*preIntegratedNavStatePtr_);
     REGULAR_COUT << GREEN_START << " ...graph is initialized." << COLOR_END << std::endl;
     return true;
@@ -370,7 +370,7 @@ bool GraphMsf::alignImu_(double& imuAttitudeRoll, double& imuAttitudePitch) {
 }
 
 // Graph initialization for roll & pitch from starting attitude, assume zero yaw
-void GraphMsf::initGraph_(const double timeStamp_k) {
+void GraphMsf::initGraph_(const double timeStamp_k, const Eigen::Vector3d& imuAngularVelocity) {
   // Calculate initial attitude;
   const gtsam::Pose3& T_W_I0 = gtsam::Pose3(preIntegratedNavStatePtr_->getT_W_Ik().matrix());
   const gtsam::Pose3& T_O_I0 = gtsam::Pose3(preIntegratedNavStatePtr_->getT_O_Ik_gravityAligned().matrix());
@@ -382,7 +382,7 @@ void GraphMsf::initGraph_(const double timeStamp_k) {
   graphConfigPtr_->W_gravityVector_ = Eigen::Vector3d(0.0, 0.0, -graphConfigPtr_->gravityMagnitude_);
   graphMgrPtr_->initImuIntegrators(graphConfigPtr_->gravityMagnitude_);
   /// Initialize graph node
-  graphMgrPtr_->initPoseVelocityBiasGraph(timeStamp_k, T_W_I0, T_O_I0);
+  graphMgrPtr_->initPoseVelocityBiasGraph(timeStamp_k, T_W_I0, T_O_I0, imuAngularVelocity);
 
   // Read initial pose from graph for optimized pose
   gtsam::Pose3 T_W_I0_opt = graphMgrPtr_->getOptimizedGraphState().navState().pose();

graph_msf/src/lib/GraphMsfDualGraph.cpp

@@ -186,7 +186,7 @@ void GraphManager::activateGlobalGraph(const gtsam::Vector3& imuPosition, const
     {
       const std::lock_guard<std::mutex> operateOnGraphDataLock(operateOnGraphDataMutex_);
       currentPropagatedKey = propagatedStateKey_;
-      currentPropagatedTime = propagatedStateTime_;
+      currentPropagatedTime = propagatedImuSampleTime_;
       int lastKeyInSmoother = (--globalSmootherPtr_->timestamps().end())->first;
 
       std::cout << YELLOW_START << "GMsf-GraphManager" << GREEN_START << " Last Key in Smoother is: " << lastKeyInSmoother

graph_msf/src/lib/ImuBuffer.cpp

@@ -10,6 +10,7 @@ Please see the LICENSE file that has been included as part of this package.
 
 // CPP
 #include <iomanip>
+#include <iterator>
 
 // Workspace
 #include "graph_msf/core/ImuMeasurement.hpp"
@@ -67,18 +68,16 @@ Eigen::Matrix<double, 6, 1> ImuBuffer::addToImuBuffer(double ts, const Eigen::Ve
     if (ts > tLatestInBuffer_) {
       tLatestInBuffer_ = ts;
     }
-  }
-
-  // If IMU buffer is too large, remove oldest element
-  if (timeToImuBuffer_.size() > imuBufferLength_) {
-    timeToImuBuffer_.erase(timeToImuBuffer_.begin());
-  }
+    if (timeToImuBuffer_.size() > imuBufferLength_) {
+      timeToImuBuffer_.erase(timeToImuBuffer_.begin());
+    }
 
-  if (timeToImuBuffer_.size() > imuBufferLength_) {
-    std::ostringstream errorStream;
-    errorStream << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " IMU Buffer has grown too large. It contains "
-                << timeToImuBuffer_.size() << " measurements instead of " << imuBufferLength_ << ".";
-    throw std::runtime_error(errorStream.str());
+    if (timeToImuBuffer_.size() > imuBufferLength_) {
+      std::ostringstream errorStream;
+      errorStream << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " IMU Buffer has grown too large. It contains "
+                  << timeToImuBuffer_.size() << " measurements instead of " << imuBufferLength_ << ".";
+      throw std::runtime_error(errorStream.str());
+    }
   }
 
   return filteredImuMeas;
@@ -210,32 +209,26 @@ bool ImuBuffer::getIMUBufferIteratorsInInterval(const double tsStart, const doub
   return true;
 }
 
-// This function is better suitable for finding the closest IMU timestamp than the timeToKeyBuffer_ version, as there might be fewer keys
-// than IMU measurements
 bool ImuBuffer::getClosestImuMeasurement(double& returnedImuTimestamp, ImuMeasurement& returnedImuMeasurement,
                                          const double maxSearchDeviation, const double tK) {
-  std::_Rb_tree_iterator<std::pair<const double, ImuMeasurement>> upperIterator;
-  {
-    // Read from IMU buffer --> acquire mutex
-    const std::lock_guard<std::mutex> writeInBufferLock(writeInBufferMutex_);
-    upperIterator = timeToImuBuffer_.upper_bound(tK);
-  }
-
-  // Empty buffer
+  const std::lock_guard<std::mutex> writeInBufferLock(writeInBufferMutex_);
   if (timeToImuBuffer_.empty()) {
     std::cerr << YELLOW_START << "GMsf-ImuBuffer: Buffer is empty!" << COLOR_END << std::endl;
     return false;
   }
 
-  auto lowerIterator = upperIterator;
-  --lowerIterator;
-
-  // Keep key which is closer to tLidar
-  returnedImuTimestamp =
-      std::abs(tK - lowerIterator->first) < std::abs(upperIterator->first - tK) ? lowerIterator->first : upperIterator->first;
-  returnedImuMeasurement =
-      std::abs(tK - lowerIterator->first) < std::abs(upperIterator->first - tK) ? lowerIterator->second : upperIterator->second;
-  double timeDeviation = returnedImuTimestamp - tK;
+  auto closestIterator = timeToImuBuffer_.lower_bound(tK);
+  if (closestIterator == timeToImuBuffer_.end()) {
+    --closestIterator;
+  } else if (closestIterator != timeToImuBuffer_.begin()) {
+    const auto lowerIterator = std::prev(closestIterator);
+    if (tK - lowerIterator->first < closestIterator->first - tK) {
+      closestIterator = lowerIterator;
+    }
+  }
+  returnedImuTimestamp = closestIterator->first;
+  returnedImuMeasurement = closestIterator->second;
+  const double timeDeviation = returnedImuTimestamp - tK;
 
   if (verboseLevel_ >= 2) {
     std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Searched time step: " << std::setprecision(14) << tK << std::endl;