fallback detection flag

koide3 · Jun 28, 2024 · 1455250 · 1455250
1 parent dafb56c
commit 1455250
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp
@@ -2,6 +2,7 @@
 // Copyright (c) 2021  Kenji Koide ([email protected])
 #pragma once
 
+#include <atomic>
 #include <gtsam_points/optimizers/incremental_fixed_lag_smoother_ext.hpp>
 
 namespace gtsam_points {
@@ -17,6 +18,7 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo
     const KeyTimestampMap& timestamps = KeyTimestampMap(),
     const gtsam::FactorIndices& factorsToRemove = gtsam::FactorIndices()) override;
 
+  bool fallbackHappened() const { return fallback_happend; }
   gtsam::Values calculateEstimate() const override;
 
   const gtsam::Value& calculateEstimate(gtsam::Key key) const;
@@ -61,6 +63,7 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo
 private:
   double current_stamp;
   mutable gtsam::Values values;
+  mutable std::atomic_bool fallback_happend;
   gtsam::NonlinearFactorGraph factors;
   std::unordered_map<gtsam::Key, std::vector<gtsam::NonlinearFactor::shared_ptr>> factor_map;
   mutable gtsam::FixedLagSmootherKeyTimestampMap stamps;

diff --git a/src/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext_with_fallback.cpp b/src/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext_with_fallback.cpp
@@ -16,6 +16,7 @@ namespace gtsam_points {
 
 IncrementalFixedLagSmootherExtWithFallback::IncrementalFixedLagSmootherExtWithFallback(double smootherLag, const ISAM2Params& parameters)
 : IncrementalFixedLagSmootherExt(smootherLag, parameters) {
+  fallback_happend = false;
   current_stamp = 0.0;
   smoother.reset(new IncrementalFixedLagSmootherExt(smootherLag, parameters));
 }
@@ -28,6 +29,7 @@ IncrementalFixedLagSmootherExtWithFallback::Result IncrementalFixedLagSmootherEx
   const KeyTimestampMap& timestamps,
   const gtsam::FactorIndices& factorsToRemove) {
   //
+  fallback_happend = false;
   factors.add(newFactors);
   for (const auto& factor : newFactors) {
     for (const auto key : factor->keys()) {
@@ -145,6 +147,7 @@ void IncrementalFixedLagSmootherExtWithFallback::update_fallback_state() {
 }
 
 void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
+  fallback_happend = true;
   std::cout << "falling back!!" << std::endl;
   std::cout << "smoother_lag:" << smoother->smootherLag() << std::endl;