From be9c4dc7c698ff6005c9ae1f9202045338eec710 Mon Sep 17 00:00:00 2001
From: "k.koide" <k.koide@aist.go.jp>
Date: Thu, 12 Dec 2024 10:08:16 +0900
Subject: [PATCH] make fix variable types customizable

---
 ...ental_fixed_lag_smoother_with_fallback.hpp |  4 +++
 ...l_fixed_lag_smoother_ext_with_fallback.cpp | 30 +++++++++++++++----
 2 files changed, 28 insertions(+), 6 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
index 79dcb2e..51acf0c 100644
--- a/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp
+++ b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp
@@ -12,6 +12,8 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo
   IncrementalFixedLagSmootherExtWithFallback(double smootherLag, const ISAM2Params& parameters);
   ~IncrementalFixedLagSmootherExtWithFallback() override;
 
+  void set_fix_variable_types(const std::vector<std::pair<char, int>>& var_types) { fix_variable_types = var_types; }
+
   Result update(
     const gtsam::NonlinearFactorGraph& newFactors = gtsam::NonlinearFactorGraph(),
     const gtsam::Values& newTheta = gtsam::Values(),  //
@@ -69,5 +71,7 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo
   mutable gtsam::FixedLagSmootherKeyTimestampMap stamps;
 
   mutable std::unique_ptr<IncrementalFixedLagSmootherExt> smoother;
+
+  std::vector<std::pair<char, int>> fix_variable_types;  // (chr, type)  type: 0=Pose3, 1=Point3
 };
 }  // namespace gtsam_points
\ No newline at end of file
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
index 1dfdb30..5da2e4d 100644
--- 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,7 +16,8 @@
 namespace gtsam_points {
 
 IncrementalFixedLagSmootherExtWithFallback::IncrementalFixedLagSmootherExtWithFallback(double smootherLag, const ISAM2Params& parameters)
-: IncrementalFixedLagSmootherExt(smootherLag, parameters) {
+: IncrementalFixedLagSmootherExt(smootherLag, parameters),
+  fix_variable_types({{'x', 0}}) {
   fallback_happend = false;
   current_stamp = 0.0;
   smoother.reset(new IncrementalFixedLagSmootherExt(smootherLag, parameters));
@@ -273,11 +274,28 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
     const int dim = value.value.dim();
     new_factors.emplace_shared<gtsam_points::LinearDampingFactor>(value.key, dim, 1e6);
 
-    if (symbol.chr() == 'x') {
-      new_factors.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(
-        value.key,
-        gtsam::Pose3(value.value.cast<gtsam::Pose3>()),
-        gtsam::noiseModel::Isotropic::Precision(6, 1e6));
+    for (const auto var_type : fix_variable_types) {
+      if (symbol.chr() == var_type.first) {
+        std::cout << "fixing " << symbol << std::endl;
+        switch (var_type.second) {
+          case 0:
+            new_factors.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(
+              value.key,
+              value.value.cast<gtsam::Pose3>(),
+              gtsam::noiseModel::Isotropic::Precision(6, 1e6));
+            break;
+          case 1:
+            new_factors.emplace_shared<gtsam::PriorFactor<gtsam::Vector3>>(
+              value.key,
+              value.value.cast<gtsam::Vector3>(),
+              gtsam::noiseModel::Isotropic::Precision(3, 1e6));
+            break;
+          default:
+            std::cerr << "error: unknown variable type!! (chr=" << static_cast<int>(var_type.first) << " type=" << var_type.second << ")"
+                      << std::endl;
+            break;
+        }
+      }
     }
   }