From 32b8effde2d845336ad1b4814ddb55ef292b7dd2 Mon Sep 17 00:00:00 2001
From: mkielo3 <mkielo3@gatech.edu>
Date: Fri, 16 May 2025 00:12:09 -0400
Subject: [PATCH] fixed IncrementalFixedLagSmooth

---
 gtsam/geometry/Gal3.cpp                           | 15 +++++++++++++++
 gtsam/geometry/Gal3.h                             | 14 --------------
 gtsam/navigation/LieGroupEKF.h                    |  3 +++
 .../nonlinear/IncrementalFixedLagSmoother.h       | 10 +++++-----
 4 files changed, 23 insertions(+), 19 deletions(-)

diff --git a/gtsam/geometry/Gal3.cpp b/gtsam/geometry/Gal3.cpp
index bc694e3b8..081ab2659 100644
--- a/gtsam/geometry/Gal3.cpp
+++ b/gtsam/geometry/Gal3.cpp
@@ -40,6 +40,21 @@ namespace gtsam {
 //------------------------------------------------------------------------------
 namespace { // Anonymous namespace for internal linkage
   constexpr double kSmallAngleThreshold = 1e-10;
+
+  // The type of the Lie algebra (matrix representation)
+  using LieAlgebra = Matrix5;
+
+  // Helper functions for accessing tangent vector components
+  Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
+  Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
+  Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
+  Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
+  // Const versions
+  Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
+  Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
+  Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
+  Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
+
 } // end anonymous namespace
 
 //------------------------------------------------------------------------------
diff --git a/gtsam/geometry/Gal3.h b/gtsam/geometry/Gal3.h
index 0b1a0ea79..2234562b5 100644
--- a/gtsam/geometry/Gal3.h
+++ b/gtsam/geometry/Gal3.h
@@ -167,20 +167,6 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
   /// @name Lie Group Static Functions
   /// @{
 
-  /// The type of the Lie algebra (matrix representation)
-  using LieAlgebra = Matrix5;
-
-  // Helper functions for accessing tangent vector components
-  static Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
-  static Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
-  static Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
-  static Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
-  // Const versions
-  static Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
-  static Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
-  static Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
-  static Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
-
   /// Exponential map at identity: tangent vector xi -> manifold element g
   static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {});
 
diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h
index 007ed3ad0..a5ea73fba 100644
--- a/gtsam/navigation/LieGroupEKF.h
+++ b/gtsam/navigation/LieGroupEKF.h
@@ -26,6 +26,9 @@
 
 #include <gtsam/navigation/ManifoldEKF.h> // Include the base class
 #include <gtsam/base/Lie.h> // Include for Lie group traits and operations
+// #include "gtsam/base/Matrix.h"
+#include "gtsam/base/lieProxies.h"
+
 
 #include <Eigen/Dense>
 #include <type_traits>
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
index e6b8dbb01..f58b4205d 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@@ -11,11 +11,11 @@
 
 #pragma once
 
-// #ifdef _MSC_VER
-// #pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory")
-// #else
-// #warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory"
-// #endif
+#ifdef _MSC_VER
+#pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory")
+#else
+#warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory"
+#endif
 
 
 #include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>