From 45d7b5c1dc444c0130492c35eba62c770d24eac6 Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 23 Sep 2026 19:57:08 +0200 Subject: [PATCH 1/4] fix(graph_msf): preserve IMU intervals across optimizer updates Co-authored-by: OpenAI Codex --- .../include/graph_msf/core/GraphManager.h | 3 +- graph_msf/src/lib/GraphManager.cpp | 30 +++-- graph_msf/src/lib/ImuBuffer.cpp | 53 ++++---- graph_msf/tests/test_graph_config.cpp | 114 ++++++++++++++++++ 4 files changed, 153 insertions(+), 47 deletions(-) diff --git a/graph_msf/include/graph_msf/core/GraphManager.h b/graph_msf/include/graph_msf/core/GraphManager.h index 1f76ee2a..acf7bd1f 100644 --- a/graph_msf/include/graph_msf/core/GraphManager.h +++ b/graph_msf/include/graph_msf/core/GraphManager.h @@ -194,8 +194,9 @@ class GraphManager { 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 propagatedStateKeyTime_ = 0.0; double lastOptimizedStateTime_ = 0.0; // Last optimized state time - gtsam::Vector3 currentAngularVelocity_ = gtsam::Vector3(0, 0, 0); + gtsam::Vector3 graphStateAngularVelocity_ = gtsam::Vector3(0, 0, 0); // Optimizer(s) std::shared_ptr rtOptimizerPtr_; diff --git a/graph_msf/src/lib/GraphManager.cpp b/graph_msf/src/lib/GraphManager.cpp index 747ea534..da38cb3f 100644 --- a/graph_msf/src/lib/GraphManager.cpp +++ b/graph_msf/src/lib/GraphManager.cpp @@ -213,6 +213,8 @@ bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam // Update Current State --------------------------------------------------- const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); + propagatedStateKeyTime_ = timeStamp; + graphStateAngularVelocity_ = imuBiasPriorPtr_->gyroscope(); optimizedGraphState_.updateNavStateAndBias(propagatedStateKey_, timeStamp, gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0)), gtsam::Vector3(0, 0, 0), *imuBiasPriorPtr_); O_imuPropagatedState_ = gtsam::NavState(T_O_I0, gtsam::Vector3(0, 0, 0)); @@ -238,7 +240,6 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte TimeToImuMap imuMeas; imuBufferPtr->getLastTwoMeasurements(imuMeas); propagatedStateTime_ = imuTimeK; - currentAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity; // 1.2 Update IMU Pre-integrator updateImuIntegrators_(imuMeas); @@ -305,6 +306,8 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte // Get new key const gtsam::Key oldKey = propagatedStateKey_; const gtsam::Key newKey = newPropagatedStateKey_(); + propagatedStateKeyTime_ = imuTimeK; + graphStateAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity; // Add to time key buffer timeToKeyBufferPtr_->addToBuffer(imuTimeK, newKey); @@ -533,7 +536,7 @@ void GraphManager::updateGraph() { std::map newRtGraphKeysTimestampsMap, newBatchGraphKeysTimestampsMap; gtsam::Key currentPropagatedKey; gtsam::Vector3 currentAngularVelocity; - double currentPropagatedTime; + double currentGraphStateTime; // Mutex Block 1 ----------------- { @@ -541,8 +544,8 @@ void GraphManager::updateGraph() { const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); // Get current key and time currentPropagatedKey = propagatedStateKey_; - currentPropagatedTime = propagatedStateTime_; - currentAngularVelocity = currentAngularVelocity_; + currentGraphStateTime = propagatedStateKeyTime_; + currentAngularVelocity = graphStateAngularVelocity_; // Get copy of factors and values and empty buffers newRtGraphFactors = *rtFactorGraphBufferPtr_; newRtGraphValues = *rtGraphValuesBufferPtr_; @@ -559,13 +562,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 +593,7 @@ void GraphManager::updateGraph() { updateDurationEndTime_ = std::chrono::high_resolution_clock::now(); // Duration in seconds std::chrono::duration updateDuration = updateDurationEndTime_ - updateDurationStartTime_; - updateDurationContainer_[currentPropagatedTime] = updateDuration.count(); + updateDurationContainer_[currentGraphStateTime] = updateDuration.count(); } // Return if optimization failed @@ -777,7 +775,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(currentGraphStateTime); 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,7 +811,7 @@ void GraphManager::updateGraph() { // 1. Optimized Graph State Status optimizedGraphState_.setIsOptimized(); // Update Optimized Graph State - optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentPropagatedTime, resultNavState, + optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphStateTime, resultNavState, resultBias.correctGyroscope(currentAngularVelocity), resultBias); optimizedGraphState_.updateReferenceFrameTransforms(resultReferenceFrameTransformations); optimizedGraphState_.updateReferenceFrameTransformsCovariance(resultReferenceFrameTransformationsCovariance); @@ -844,12 +842,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_ = currentGraphStateTime; } // end of locking // Potentially log real-time state to container if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) { - realTimeReferenceFrameContainer_.emplace(currentPropagatedTime, resultReferenceFrameTransformations); + realTimeReferenceFrameContainer_.emplace(currentGraphStateTime, resultReferenceFrameTransformations); } } diff --git a/graph_msf/src/lib/ImuBuffer.cpp b/graph_msf/src/lib/ImuBuffer.cpp index 1d296e5b..e2a127a9 100644 --- a/graph_msf/src/lib/ImuBuffer.cpp +++ b/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 +#include // Workspace #include "graph_msf/core/ImuMeasurement.hpp" @@ -67,18 +68,16 @@ Eigen::Matrix 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> upperIterator; - { - // Read from IMU buffer --> acquire mutex - const std::lock_guard writeInBufferLock(writeInBufferMutex_); - upperIterator = timeToImuBuffer_.upper_bound(tK); - } - - // Empty buffer + const std::lock_guard 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; diff --git a/graph_msf/tests/test_graph_config.cpp b/graph_msf/tests/test_graph_config.cpp index d1b6da15..0ab90c08 100644 --- a/graph_msf/tests/test_graph_config.cpp +++ b/graph_msf/tests/test_graph_config.cpp @@ -118,6 +118,115 @@ void sensorNoiseChangesActualStateCovariance() { "Gyroscope noise has no covariance effect"); } +void optimizerHandoffPreservesPartialImuInterval(int partial_steps, bool nonzero_bias) { + auto config = std::make_shared(); + config->useImuSignalLowPassFilter_ = false; + config->realTimeSmootherUseCholeskyFactorizationFlag_ = false; + if (nonzero_bias) { + config->accBiasPrior_ = Eigen::Vector3d(0.1, -0.2, 0.3); + config->gyroBiasPrior_ = Eigen::Vector3d(0.01, -0.02, 0.03); + } + graph_msf::GraphManager manager(config, "imu", "world"); + require(manager.initImuIntegrators(config->gravityMagnitude_), "IMU integrator initialization failed"); + require(manager.initPoseVelocityBiasGraph(1.0, gtsam::Pose3(), gtsam::Pose3()), "Prior graph initialization failed"); + auto buffer = std::make_shared(config); + const Eigen::Vector3d acceleration = Eigen::Vector3d(1.0, 0.0, config->gravityMagnitude_) + config->accBiasPrior_; + buffer->addToImuBuffer(0.99, acceleration, config->gyroBiasPrior_); + buffer->addToImuBuffer(1.0, acceleration, config->gyroBiasPrior_); + graph_msf::SafeIntegratedNavState state; + std::shared_ptr optimized_state; + int index = 0; + const auto add_imu = [&](bool create_state) { + const double timestamp = 1.0 + 0.01 * ++index; + buffer->addToImuBuffer(timestamp, acceleration, config->gyroBiasPrior_); + manager.addImuFactorAndGetState(state, optimized_state, buffer, timestamp, create_state); + }; + for (int step = 1; step <= 10 + partial_steps; ++step) { + add_imu(step == 10); + } + + manager.updateGraph(); + + require(std::abs(manager.getOptimizedGraphState().ts() - 1.1) < 1e-12, + "Optimized timestamp does not match its graph key"); + const auto check_state = [&]() { + const double elapsed = 0.01 * index; + require(std::abs(state.getT_W_Ik().translation().x() - 0.5 * elapsed * elapsed) < 1e-7, + "Optimizer handoff lost position integration"); + require(std::abs(state.getI_v_W_I().x() - elapsed) < 1e-7, + "Optimizer handoff lost velocity integration"); + }; + for (int repeat = 0; repeat < 2; ++repeat) { + add_imu(false); + check_state(); + manager.updateGraph(); + } + add_imu(true); + const double next_key_time = 1.0 + 0.01 * index; + add_imu(false); + manager.updateGraph(); + require(std::abs(manager.getOptimizedGraphState().ts() - next_key_time) < 1e-12, + "Optimizer timestamp did not advance with the graph key"); + add_imu(false); + check_state(); + + const Eigen::Vector3d graph_angular_velocity = config->gyroBiasPrior_ + Eigen::Vector3d(0.0, 0.0, 0.1); + for (int step = 1; step <= 2; ++step) { + const double timestamp = 1.0 + 0.01 * ++index; + const Eigen::Vector3d angular_velocity = config->gyroBiasPrior_ + Eigen::Vector3d(0.0, 0.0, 0.1 * step); + buffer->addToImuBuffer(timestamp, acceleration, angular_velocity); + manager.addImuFactorAndGetState(state, optimized_state, buffer, timestamp, step == 1); + } + manager.updateGraph(); + const auto& optimized = manager.getOptimizedGraphState(); + require((optimized.angularVelocityCorrected() + optimized.imuBias().gyroscope()).isApprox(graph_angular_velocity, 1e-12), + "Optimized angular velocity does not match its graph key"); +} + +void closestImuLookupHandlesBufferBoundaries() { + auto config = std::make_shared(); + config->useImuSignalLowPassFilter_ = false; + config->imuBufferLength_ = 3; + graph_msf::ImuBuffer buffer(config); + double timestamp = -1.0; + graph_msf::ImuMeasurement measurement; + require(!buffer.getClosestImuMeasurement(timestamp, measurement, 1.0, 1.0), "Empty IMU lookup must fail"); + + const auto add = [&](double time) { + buffer.addToImuBuffer(time, Eigen::Vector3d::Constant(time), Eigen::Vector3d::Constant(-time)); + }; + const auto expect = [&](double query, double deviation, double expected) { + require(buffer.getClosestImuMeasurement(timestamp, measurement, deviation, query), "Closest IMU lookup failed"); + require(timestamp == expected && measurement.timestamp == expected, "Closest IMU timestamp is incorrect"); + require(measurement.acceleration.isApprox(Eigen::Vector3d::Constant(expected)), "Closest IMU acceleration is incorrect"); + require(measurement.angularVelocity.isApprox(Eigen::Vector3d::Constant(-expected)), "Closest IMU angular velocity is incorrect"); + }; + + add(1.0); + expect(1.0, 0.0, 1.0); + expect(0.75, 0.25, 1.0); + expect(1.25, 0.25, 1.0); + require(!buffer.getClosestImuMeasurement(timestamp, measurement, 0.125, 0.75), "Early lookup outside tolerance must fail"); + require(!buffer.getClosestImuMeasurement(timestamp, measurement, 0.125, 1.25), "Late lookup outside tolerance must fail"); + + add(2.0); + add(3.0); + expect(1.0, 0.0, 1.0); + expect(2.0, 0.0, 2.0); + expect(3.0, 0.0, 3.0); + expect(0.75, 0.25, 1.0); + expect(3.25, 0.25, 3.0); + expect(1.25, 0.25, 1.0); + expect(1.75, 0.25, 2.0); + expect(1.5, 0.5, 2.0); + require(!buffer.getClosestImuMeasurement(timestamp, measurement, 0.125, 1.25), "Interior lookup outside tolerance must fail"); + + add(4.0); + expect(2.0, 0.0, 2.0); + expect(4.0, 0.0, 4.0); + require(!buffer.getClosestImuMeasurement(timestamp, measurement, 0.0, 1.0), "Evicted IMU measurement must not be returned"); +} + void stationaryPropagationUsesInitializedGravity(bool estimate_gravity) { auto config = std::make_shared(); config->imuRate_ = 10.0; @@ -202,6 +311,11 @@ int main() { rejectsInvalidStateAndOptimizationCounts(); rejectsInMotionInitialization(); sensorNoiseChangesActualStateCovariance(); + for (const bool nonzero_bias : {false, true}) { + optimizerHandoffPreservesPartialImuInterval(0, nonzero_bias); + optimizerHandoffPreservesPartialImuInterval(7, nonzero_bias); + } + closestImuLookupHandlesBufferBoundaries(); stationaryPropagationUsesInitializedGravity(false); stationaryPropagationUsesInitializedGravity(true); disabledMarginalWindowHandlesSingleState(); From 61305d98730b24ee74a4f2abf64e00c08781755a Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 23 Sep 2026 20:04:13 +0200 Subject: [PATCH 2/4] refactor(graph_msf): distinguish IMU sample and graph-key times Co-authored-by: OpenAI Codex --- .../include/graph_msf/core/GraphManager.h | 6 ++-- .../include/graph_msf/core/GraphManager.inl | 2 +- graph_msf/src/lib/GraphManager.cpp | 36 +++++++++---------- graph_msf/src/lib/GraphMsfDualGraph.cpp | 2 +- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/graph_msf/include/graph_msf/core/GraphManager.h b/graph_msf/include/graph_msf/core/GraphManager.h index acf7bd1f..b7a8aaed 100644 --- a/graph_msf/include/graph_msf/core/GraphManager.h +++ b/graph_msf/include/graph_msf/core/GraphManager.h @@ -193,10 +193,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 propagatedStateKeyTime_ = 0.0; + double propagatedImuSampleTime_ = 0.0; + double latestGraphKeyTime_ = 0.0; double lastOptimizedStateTime_ = 0.0; // Last optimized state time - gtsam::Vector3 graphStateAngularVelocity_ = gtsam::Vector3(0, 0, 0); + gtsam::Vector3 latestGraphKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0); // Optimizer(s) std::shared_ptr rtOptimizerPtr_; diff --git a/graph_msf/include/graph_msf/core/GraphManager.inl b/graph_msf/include/graph_msf/core/GraphManager.inl index 04568c17..04780706 100644 --- a/graph_msf/include/graph_msf/core/GraphManager.inl +++ b/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 " diff --git a/graph_msf/src/lib/GraphManager.cpp b/graph_msf/src/lib/GraphManager.cpp index da38cb3f..6165515f 100644 --- a/graph_msf/src/lib/GraphManager.cpp +++ b/graph_msf/src/lib/GraphManager.cpp @@ -213,8 +213,8 @@ bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam // Update Current State --------------------------------------------------- const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); - propagatedStateKeyTime_ = timeStamp; - graphStateAngularVelocity_ = imuBiasPriorPtr_->gyroscope(); + latestGraphKeyTime_ = timeStamp; + latestGraphKeyAngularVelocity_ = imuBiasPriorPtr_->gyroscope(); optimizedGraphState_.updateNavStateAndBias(propagatedStateKey_, timeStamp, gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0)), gtsam::Vector3(0, 0, 0), *imuBiasPriorPtr_); O_imuPropagatedState_ = gtsam::NavState(T_O_I0, gtsam::Vector3(0, 0, 0)); @@ -239,7 +239,7 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte // 1.1 Get last two measurements from buffer to determine dt TimeToImuMap imuMeas; imuBufferPtr->getLastTwoMeasurements(imuMeas); - propagatedStateTime_ = imuTimeK; + propagatedImuSampleTime_ = imuTimeK; // 1.2 Update IMU Pre-integrator updateImuIntegrators_(imuMeas); @@ -306,8 +306,8 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte // Get new key const gtsam::Key oldKey = propagatedStateKey_; const gtsam::Key newKey = newPropagatedStateKey_(); - propagatedStateKeyTime_ = imuTimeK; - graphStateAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity; + latestGraphKeyTime_ = imuTimeK; + latestGraphKeyAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity; // Add to time key buffer timeToKeyBufferPtr_->addToBuffer(imuTimeK, newKey); @@ -375,15 +375,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()) @@ -535,8 +535,8 @@ void GraphManager::updateGraph() { gtsam::Values newRtGraphValues, newBatchGraphValues; std::map newRtGraphKeysTimestampsMap, newBatchGraphKeysTimestampsMap; gtsam::Key currentPropagatedKey; - gtsam::Vector3 currentAngularVelocity; - double currentGraphStateTime; + gtsam::Vector3 currentGraphKeyAngularVelocity; + double currentGraphKeyTime; // Mutex Block 1 ----------------- { @@ -544,8 +544,8 @@ void GraphManager::updateGraph() { const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); // Get current key and time currentPropagatedKey = propagatedStateKey_; - currentGraphStateTime = propagatedStateKeyTime_; - currentAngularVelocity = graphStateAngularVelocity_; + currentGraphKeyTime = latestGraphKeyTime_; + currentGraphKeyAngularVelocity = latestGraphKeyAngularVelocity_; // Get copy of factors and values and empty buffers newRtGraphFactors = *rtFactorGraphBufferPtr_; newRtGraphValues = *rtGraphValuesBufferPtr_; @@ -593,7 +593,7 @@ void GraphManager::updateGraph() { updateDurationEndTime_ = std::chrono::high_resolution_clock::now(); // Duration in seconds std::chrono::duration updateDuration = updateDurationEndTime_ - updateDurationStartTime_; - updateDurationContainer_[currentGraphStateTime] = updateDuration.count(); + updateDurationContainer_[currentGraphKeyTime] = updateDuration.count(); } // Return if optimization failed @@ -775,7 +775,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(currentGraphStateTime); + double variableAge = framePairKeyMapIterator.second.computeVariableAge(currentGraphKeyTime); 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 (" @@ -811,8 +811,8 @@ void GraphManager::updateGraph() { // 1. Optimized Graph State Status optimizedGraphState_.setIsOptimized(); // Update Optimized Graph State - optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphStateTime, resultNavState, - resultBias.correctGyroscope(currentAngularVelocity), resultBias); + optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphKeyTime, resultNavState, + resultBias.correctGyroscope(currentGraphKeyAngularVelocity), resultBias); optimizedGraphState_.updateReferenceFrameTransforms(resultReferenceFrameTransformations); optimizedGraphState_.updateReferenceFrameTransformsCovariance(resultReferenceFrameTransformationsCovariance); optimizedGraphState_.updateLandmarkTransforms(resultLandmarkTransformations); @@ -842,12 +842,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_ = currentGraphStateTime; + lastOptimizedStateTime_ = currentGraphKeyTime; } // end of locking // Potentially log real-time state to container if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) { - realTimeReferenceFrameContainer_.emplace(currentGraphStateTime, resultReferenceFrameTransformations); + realTimeReferenceFrameContainer_.emplace(currentGraphKeyTime, resultReferenceFrameTransformations); } } diff --git a/graph_msf/src/lib/GraphMsfDualGraph.cpp b/graph_msf/src/lib/GraphMsfDualGraph.cpp index 443acd7a..285eae31 100644 --- a/graph_msf/src/lib/GraphMsfDualGraph.cpp +++ b/graph_msf/src/lib/GraphMsfDualGraph.cpp @@ -186,7 +186,7 @@ void GraphManager::activateGlobalGraph(const gtsam::Vector3& imuPosition, const { const std::lock_guard 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 From 756635ee904e1ba2cdba7382bea6d577e9b551a1 Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 23 Sep 2026 20:06:38 +0200 Subject: [PATCH 3/4] refactor(graph_msf): name navigation state key metadata explicitly Co-authored-by: OpenAI Codex --- .../include/graph_msf/core/GraphManager.h | 4 +-- graph_msf/src/lib/GraphManager.cpp | 28 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/graph_msf/include/graph_msf/core/GraphManager.h b/graph_msf/include/graph_msf/core/GraphManager.h index b7a8aaed..87764b22 100644 --- a/graph_msf/include/graph_msf/core/GraphManager.h +++ b/graph_msf/include/graph_msf/core/GraphManager.h @@ -194,9 +194,9 @@ class GraphManager { 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 propagatedImuSampleTime_ = 0.0; - double latestGraphKeyTime_ = 0.0; + double latestGraphStateKeyTime_ = 0.0; double lastOptimizedStateTime_ = 0.0; // Last optimized state time - gtsam::Vector3 latestGraphKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0); + gtsam::Vector3 latestGraphStateKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0); // Optimizer(s) std::shared_ptr rtOptimizerPtr_; diff --git a/graph_msf/src/lib/GraphManager.cpp b/graph_msf/src/lib/GraphManager.cpp index 6165515f..235efa1f 100644 --- a/graph_msf/src/lib/GraphManager.cpp +++ b/graph_msf/src/lib/GraphManager.cpp @@ -213,8 +213,8 @@ bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam // Update Current State --------------------------------------------------- const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); - latestGraphKeyTime_ = timeStamp; - latestGraphKeyAngularVelocity_ = imuBiasPriorPtr_->gyroscope(); + latestGraphStateKeyTime_ = timeStamp; + latestGraphStateKeyAngularVelocity_ = imuBiasPriorPtr_->gyroscope(); optimizedGraphState_.updateNavStateAndBias(propagatedStateKey_, timeStamp, gtsam::NavState(T_W_I0, gtsam::Vector3(0, 0, 0)), gtsam::Vector3(0, 0, 0), *imuBiasPriorPtr_); O_imuPropagatedState_ = gtsam::NavState(T_O_I0, gtsam::Vector3(0, 0, 0)); @@ -306,8 +306,8 @@ void GraphManager::addImuFactorAndGetState(SafeIntegratedNavState& returnPreInte // Get new key const gtsam::Key oldKey = propagatedStateKey_; const gtsam::Key newKey = newPropagatedStateKey_(); - latestGraphKeyTime_ = imuTimeK; - latestGraphKeyAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity; + latestGraphStateKeyTime_ = imuTimeK; + latestGraphStateKeyAngularVelocity_ = imuMeas.rbegin()->second.angularVelocity; // Add to time key buffer timeToKeyBufferPtr_->addToBuffer(imuTimeK, newKey); @@ -535,8 +535,8 @@ void GraphManager::updateGraph() { gtsam::Values newRtGraphValues, newBatchGraphValues; std::map newRtGraphKeysTimestampsMap, newBatchGraphKeysTimestampsMap; gtsam::Key currentPropagatedKey; - gtsam::Vector3 currentGraphKeyAngularVelocity; - double currentGraphKeyTime; + gtsam::Vector3 currentGraphStateKeyAngularVelocity; + double currentGraphStateKeyTime; // Mutex Block 1 ----------------- { @@ -544,8 +544,8 @@ void GraphManager::updateGraph() { const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); // Get current key and time currentPropagatedKey = propagatedStateKey_; - currentGraphKeyTime = latestGraphKeyTime_; - currentGraphKeyAngularVelocity = latestGraphKeyAngularVelocity_; + currentGraphStateKeyTime = latestGraphStateKeyTime_; + currentGraphStateKeyAngularVelocity = latestGraphStateKeyAngularVelocity_; // Get copy of factors and values and empty buffers newRtGraphFactors = *rtFactorGraphBufferPtr_; newRtGraphValues = *rtGraphValuesBufferPtr_; @@ -593,7 +593,7 @@ void GraphManager::updateGraph() { updateDurationEndTime_ = std::chrono::high_resolution_clock::now(); // Duration in seconds std::chrono::duration updateDuration = updateDurationEndTime_ - updateDurationStartTime_; - updateDurationContainer_[currentGraphKeyTime] = updateDuration.count(); + updateDurationContainer_[currentGraphStateKeyTime] = updateDuration.count(); } // Return if optimization failed @@ -775,7 +775,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(currentGraphKeyTime); + 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 (" @@ -811,8 +811,8 @@ void GraphManager::updateGraph() { // 1. Optimized Graph State Status optimizedGraphState_.setIsOptimized(); // Update Optimized Graph State - optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphKeyTime, resultNavState, - resultBias.correctGyroscope(currentGraphKeyAngularVelocity), resultBias); + optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphStateKeyTime, resultNavState, + resultBias.correctGyroscope(currentGraphStateKeyAngularVelocity), resultBias); optimizedGraphState_.updateReferenceFrameTransforms(resultReferenceFrameTransformations); optimizedGraphState_.updateReferenceFrameTransformsCovariance(resultReferenceFrameTransformationsCovariance); optimizedGraphState_.updateLandmarkTransforms(resultLandmarkTransformations); @@ -842,12 +842,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_ = currentGraphKeyTime; + lastOptimizedStateTime_ = currentGraphStateKeyTime; } // end of locking // Potentially log real-time state to container if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) { - realTimeReferenceFrameContainer_.emplace(currentGraphKeyTime, resultReferenceFrameTransformations); + realTimeReferenceFrameContainer_.emplace(currentGraphStateKeyTime, resultReferenceFrameTransformations); } } From 24240703a10cfa983f2b4b9fbf89cbafa0f8d6db Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 23 Sep 2026 20:11:11 +0200 Subject: [PATCH 4/4] fix(graph_msf): initialize angular velocity from the IMU sample Co-authored-by: OpenAI Codex --- .../include/graph_msf/core/GraphManager.h | 4 ++- .../include/graph_msf/interface/GraphMsf.h | 2 +- graph_msf/src/lib/GraphManager.cpp | 7 +++--- graph_msf/src/lib/GraphMsf.cpp | 6 ++--- graph_msf/tests/test_graph_config.cpp | 25 +++++++++++++++++-- 5 files changed, 34 insertions(+), 10 deletions(-) diff --git a/graph_msf/include/graph_msf/core/GraphManager.h b/graph_msf/include/graph_msf/core/GraphManager.h index 87764b22..849048c8 100644 --- a/graph_msf/include/graph_msf/core/GraphManager.h +++ b/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, diff --git a/graph_msf/include/graph_msf/interface/GraphMsf.h b/graph_msf/include/graph_msf/interface/GraphMsf.h index d6232d4a..5aad5afc 100644 --- a/graph_msf/include/graph_msf/interface/GraphMsf.h +++ b/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_(); diff --git a/graph_msf/src/lib/GraphManager.cpp b/graph_msf/src/lib/GraphManager.cpp index 235efa1f..17902922 100644 --- a/graph_msf/src/lib/GraphManager.cpp +++ b/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( @@ -214,9 +215,9 @@ bool GraphManager::initPoseVelocityBiasGraph(const double timeStamp, const gtsam // Update Current State --------------------------------------------------- const std::lock_guard operateOnGraphDataLock(operateOnGraphDataMutex_); latestGraphStateKeyTime_ = timeStamp; - latestGraphStateKeyAngularVelocity_ = imuBiasPriorPtr_->gyroscope(); + 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(); diff --git a/graph_msf/src/lib/GraphMsf.cpp b/graph_msf/src/lib/GraphMsf.cpp index 79fd69b8..f17e7578 100644 --- a/graph_msf/src/lib/GraphMsf.cpp +++ b/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(*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(); diff --git a/graph_msf/tests/test_graph_config.cpp b/graph_msf/tests/test_graph_config.cpp index 0ab90c08..81f1a4dc 100644 --- a/graph_msf/tests/test_graph_config.cpp +++ b/graph_msf/tests/test_graph_config.cpp @@ -91,7 +91,8 @@ Covariances integratedCovariances(double acc_noise, double gyro_noise) { config->gyroNoiseDensity_ = gyro_noise; graph_msf::GraphManager manager(config, "imu", "world"); require(manager.initImuIntegrators(config->gravityMagnitude_), "IMU integrator initialization failed"); - require(manager.initPoseVelocityBiasGraph(1.0, gtsam::Pose3(), gtsam::Pose3()), "Prior graph initialization failed"); + require(manager.initPoseVelocityBiasGraph(1.0, gtsam::Pose3(), gtsam::Pose3(), config->gyroBiasPrior_), + "Prior graph initialization failed"); auto buffer = std::make_shared(config); const Eigen::Vector3d acceleration(0.0, 0.0, config->gravityMagnitude_); buffer->addToImuBuffer(0.99, acceleration, Eigen::Vector3d::Zero()); @@ -118,6 +119,24 @@ void sensorNoiseChangesActualStateCovariance() { "Gyroscope noise has no covariance effect"); } +void initialGraphStateUsesMeasuredAngularVelocity() { + auto config = std::make_shared(); + config->gyroBiasPrior_ = Eigen::Vector3d(0.01, -0.02, 0.03); + const Eigen::Vector3d measured_angular_velocity(0.04, 0.05, -0.06); + const Eigen::Vector3d expected = measured_angular_velocity - config->gyroBiasPrior_; + graph_msf::GraphManager manager(config, "imu", "world"); + require(manager.initImuIntegrators(config->gravityMagnitude_), "IMU integrator initialization failed"); + require(manager.initPoseVelocityBiasGraph(1.0, gtsam::Pose3(), gtsam::Pose3(), measured_angular_velocity), + "Prior graph initialization failed"); + + require(manager.getOptimizedGraphState().angularVelocityCorrected().isApprox(expected, 1e-12), + "Initial angular velocity does not use the measured sample"); + manager.updateGraph(); + const auto& optimized = manager.getOptimizedGraphState(); + require((optimized.angularVelocityCorrected() + optimized.imuBias().gyroscope()).isApprox(measured_angular_velocity, 1e-12), + "Initial graph-key angular velocity was replaced by the bias prior"); +} + void optimizerHandoffPreservesPartialImuInterval(int partial_steps, bool nonzero_bias) { auto config = std::make_shared(); config->useImuSignalLowPassFilter_ = false; @@ -128,7 +147,8 @@ void optimizerHandoffPreservesPartialImuInterval(int partial_steps, bool nonzero } graph_msf::GraphManager manager(config, "imu", "world"); require(manager.initImuIntegrators(config->gravityMagnitude_), "IMU integrator initialization failed"); - require(manager.initPoseVelocityBiasGraph(1.0, gtsam::Pose3(), gtsam::Pose3()), "Prior graph initialization failed"); + require(manager.initPoseVelocityBiasGraph(1.0, gtsam::Pose3(), gtsam::Pose3(), config->gyroBiasPrior_), + "Prior graph initialization failed"); auto buffer = std::make_shared(config); const Eigen::Vector3d acceleration = Eigen::Vector3d(1.0, 0.0, config->gravityMagnitude_) + config->accBiasPrior_; buffer->addToImuBuffer(0.99, acceleration, config->gyroBiasPrior_); @@ -311,6 +331,7 @@ int main() { rejectsInvalidStateAndOptimizationCounts(); rejectsInMotionInitialization(); sensorNoiseChangesActualStateCovariance(); + initialGraphStateUsesMeasuredAngularVelocity(); for (const bool nonzero_bias : {false, true}) { optimizerHandoffPreservesPartialImuInterval(0, nonzero_bias); optimizerHandoffPreservesPartialImuInterval(7, nonzero_bias);