Invariant EKF

examples/GEKF_Rot3Example.cpp

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file IEKF_Rot3Example.cpp
+ * @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
+ * and a single magnetometer update.
+ * @date April 25, 2025
+ * @authors Scott Baker, Matt Kielo, Frank Dellaert
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/navigation/LieGroupEKF.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ ---
+static constexpr double k = 0.5;
+Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
+  // φ = Logmap(R), Dφ = ∂φ/∂δR
+  Matrix3 D_phi;
+  Vector3 phi = Rot3::Logmap(X, D_phi);
+  // zero out yaw
+  phi[2] = 0.0;
+  D_phi.row(2).setZero();
+
+  if (H) *H = -k * D_phi;  // ∂(–kφ)/∂δR
+  return -k * phi;         // xi ∈ 𝔰𝔬(3)
+}
+
+// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
+static const Vector3 m_world(0, 0, -1);
+Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
+  Vector3 z = X.inverse().rotate(m_world);
+  if (H) *H = -skewSymmetric(z);
+  return z;
+}
+
+int main() {
+  // Initial estimate (identity) and covariance
+  const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
+  const Matrix3 P0 = Matrix3::Identity() * 0.1;
+  LieGroupEKF<Rot3> ekf(R0, P0);
+
+  // Timestep, process noise, measurement noise
+  double dt = 0.1;
+  Matrix3 Q = Matrix3::Identity() * 0.01;
+  Matrix3 Rm = Matrix3::Identity() * 0.05;
+
+  cout << "=== Init ===\nR:\n"
+       << ekf.state().matrix() << "\nP:\n"
+       << ekf.covariance() << "\n\n";
+
+  // Predict using state‐dependent f
+  ekf.predict(dynamicsSO3, dt, Q);
+  cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
+
+  // Magnetometer measurement = body‐frame reading of m_world
+  Vector3 z = h_mag(R0);
+  ekf.update(h_mag, z, Rm);
+  cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n";
+
+  return 0;
+}

examples/IEKF_NavstateExample.cpp

@@ -0,0 +1,95 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file IEKF_NavstateExample.cpp
+ * @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
+ * @date April 25, 2025
+ * @authors Scott Baker, Matt Kielo, Frank Dellaert
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/navigation/InvariantEKF.h>
+#include <gtsam/navigation/NavState.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+/**
+ * @brief Left-invariant dynamics for NavState.
+ * @param imu  6×1 vector [a; ω]: linear acceleration and angular velocity.
+ * @return     9×1 tangent: [ω; 0₃; a].
+ */
+Vector9 dynamics(const Vector6& imu) {
+  auto a = imu.head<3>();
+  auto w = imu.tail<3>();
+  Vector9 xi;
+  xi << w, Vector3::Zero(), a;
+  return xi;
+}
+
+/**
+ * @brief GPS measurement model: returns position and its Jacobian.
+ * @param X    Current state.
+ * @param H    Optional 3×9 Jacobian w.r.t. X.
+ * @return     3×1 position vector.
+ */
+Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
+  return X.position(H);
+}
+
+int main() {
+  // Initial state & covariances
+  NavState X0;  // R=I, v=0, t=0
+  Matrix9 P0 = Matrix9::Identity() * 0.1;
+  InvariantEKF<NavState> ekf(X0, P0);
+
+  // Noise & timestep
+  double dt = 1.0;
+  Matrix9 Q = Matrix9::Identity() * 0.01;
+  Matrix3 R = Matrix3::Identity() * 0.5;
+
+  // Two IMU samples [a; ω]
+  Vector6 imu1;
+  imu1 << 0.1, 0, 0, 0, 0.2, 0;
+  Vector6 imu2;
+  imu2 << 0, 0.3, 0, 0.4, 0, 0;
+
+  // Two GPS fixes
+  Vector3 z1;
+  z1 << 0.3, 0, 0;
+  Vector3 z2;
+  z2 << 0.6, 0, 0;
+
+  cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
+       << "\n\n";
+
+  // --- first predict/update ---
+  ekf.predict(dynamics(imu1), dt, Q);
+  cout << "--- After predict 1 ---\nX: " << ekf.state()
+       << "\nP: " << ekf.covariance() << "\n\n";
+  ekf.update(h_gps, z1, R);
+  cout << "--- After update 1 ---\nX: " << ekf.state()
+       << "\nP: " << ekf.covariance() << "\n\n";
+
+  // --- second predict/update ---
+  ekf.predict(dynamics(imu2), dt, Q);
+  cout << "--- After predict 2 ---\nX: " << ekf.state()
+       << "\nP: " << ekf.covariance() << "\n\n";
+  ekf.update(h_gps, z2, R);
+  cout << "--- After update 2 ---\nX: " << ekf.state()
+       << "\nP: " << ekf.covariance() << "\n";
+
+  return 0;
+}

examples/IEKF_SE2Example.cpp

@@ -0,0 +1,119 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file IEKF_SE2Example.cpp
+ * @brief A left invariant Extended Kalman Filter example using a Lie group
+ * odometry as the prediction stage on SE(2) and
+ *
+ * This example uses the templated InvariantEKF class to estimate the state of
+ * an object using odometry / GPS measurements The prediction stage of the
+ * InvariantEKF uses a Lie Group element to propagate the stage in a discrete
+ * InvariantEKF. For most cases, U = exp(u^ * dt) if u is a control vector that
+ * is constant over the interval dt. However, if u is not constant over dt,
+ * other approaches are needed to find the value of U. This approach simply
+ * takes a Lie group element U, which can be found in various different ways.
+ *
+ * This data was compared to a left invariant EKF on SE(2) using identical
+ * measurements and noise from the source of the InEKF plugin
+ * https://inekf.readthedocs.io/en/latest/ Based on the paper "An Introduction
+ * to the Invariant Extended Kalman Filter" by Easton R. Potokar, Randal W.
+ * Beard, and Joshua G. Mangelson
+ *
+ * @date Apr 25, 2025
+ * @authors Scott Baker, Matt Kielo, Frank Dellaert
+ */
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/navigation/InvariantEKF.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a 2D GPS measurement function that returns the predicted measurement h
+// and Jacobian H. The predicted measurement h is the translation of the state
+// X.
+Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) {
+  return X.translation(H);
+}
+
+// Define a InvariantEKF class that uses the Pose2 Lie group as the state and
+// the Vector2 measurement type.
+int main() {
+  // // Initialize the filter's state, covariance, and time interval values.
+  Pose2 X0(0.0, 0.0, 0.0);
+  Matrix3 P0 = Matrix3::Identity() * 0.1;
+
+  // Create the filter with the initial state, covariance, and measurement
+  // function.
+  InvariantEKF<Pose2> ekf(X0, P0);
+
+  // Define the process covariance and measurement covariance matrices Q and R.
+  Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
+  Matrix2 R = I_2x2 * 0.01;
+
+  // Define odometry movements.
+  // U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.
+  // U2: Move 1 unit in X, 1 unit in Y, 0 radians in theta.
+  Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0);
+
+  // Create GPS measurements.
+  // z1: Measure robot at (1, 0)
+  // z2: Measure robot at (1, 1)
+  Vector2 z1, z2;
+  z1 << 1.0, 0.0;
+  z2 << 1.0, 1.0;
+
+  // Define a transformation matrix to convert the covariance into (theta, x, y)
+  // form. The paper and data mentioned above uses a (theta, x, y) notation,
+  // whereas GTSAM uses (x, y, theta). The transformation matrix is used to
+  // directly compare results of the covariance matrix.
+  Matrix3 TransformP;
+  TransformP << 0, 0, 1, 1, 0, 0, 0, 1, 0;
+
+  // Propagating/updating the filter
+  // Initial state and covariance
+  cout << "\nInitialization:\n";
+  cout << "X0: " << ekf.state() << endl;
+  cout << "P0: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // First prediction stage
+  ekf.predict(U1, Q);
+  cout << "\nFirst Prediction:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // First update stage
+  ekf.update(h_gps, z1, R);
+  cout << "\nFirst Update:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // Second prediction stage
+  ekf.predict(U2, Q);
+  cout << "\nSecond Prediction:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // Second update stage
+  ekf.update(h_gps, z2, R);
+  cout << "\nSecond Update:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  return 0;
+}

examples/ISAM2_City10000.cpp

@@ -27,8 +27,6 @@
 #include <gtsam/slam/dataset.h>
 #include <time.h>
 
-#include <boost/algorithm/string/classification.hpp>
-#include <boost/algorithm/string/split.hpp>
 #include <fstream>
 #include <string>
 #include <vector>

gtsam/base/Lie.h

@@ -277,14 +277,18 @@ inline Class expmap_default(const Class& t, const Vector& d) {
 template<typename T>
 class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
 public:
+  // Concept marker: allows checking IsLieGroup<T>::value in templates
+  static constexpr bool value =
+    std::is_base_of<lie_group_tag, typename traits<T>::structure_category>::value;
+
   typedef typename traits<T>::structure_category structure_category_tag;
   typedef typename traits<T>::ManifoldType ManifoldType;
   typedef typename traits<T>::TangentVector TangentVector;
   typedef typename traits<T>::ChartJacobian ChartJacobian;
 
   GTSAM_CONCEPT_USAGE(IsLieGroup) {
     static_assert(
-        (std::is_base_of<lie_group_tag, structure_category_tag>::value),
+        value,
         "This type's trait does not assert it is a Lie group (or derived)");
 
     // group operations with Jacobians

gtsam/base/Manifold.h

@@ -138,10 +138,13 @@ class IsManifold {
   static const int dim = traits<T>::dimension;
   typedef typename traits<T>::ManifoldType ManifoldType;
   typedef typename traits<T>::TangentVector TangentVector;
+  // Concept marker: allows checking IsManifold<T>::value in templates
+  static constexpr bool value =
+    std::is_base_of<manifold_tag, structure_category_tag>::value;
 
   GTSAM_CONCEPT_USAGE(IsManifold) {
     static_assert(
-        (std::is_base_of<manifold_tag, structure_category_tag>::value),
+        value,
         "This type's structure_category trait does not assert it as a manifold (or derived)");
     static_assert(TangentVector::SizeAtCompileTime == dim);