Add EqVIO EquivariantFilter

gtsam/base/ProductLieGroup-inl.h

@@ -38,22 +38,38 @@ template <typename G, typename H>
 ProductLieGroup<G, H> ProductLieGroup<G, H>::retract(const TangentVector& v,
                                                      ChartJacobian H1,
                                                      ChartJacobian H2) const {
-  if (H1 || H2) {
-    throw std::runtime_error(
-        "ProductLieGroup::retract derivatives not implemented yet");
-  }
   const size_t firstDimension = firstDim();
   const size_t secondDimension = secondDim();
+  const size_t productDimension =
+      combinedDimension(firstDimension, secondDimension);
   if (static_cast<size_t>(v.size()) !=
-      combinedDimension(firstDimension, secondDimension)) {
+      productDimension) {
     throw std::invalid_argument(
         "ProductLieGroup::retract tangent dimension does not match product "
         "dimension");
   }
-  G g =
-      traits<G>::Retract(this->first, tangentSegment<G>(v, 0, firstDimension));
+  Jacobian1 D_g_first;
+  Jacobian1 D_g_second;
+  Jacobian2 D_h_first;
+  Jacobian2 D_h_second;
+  G g = traits<G>::Retract(this->first, tangentSegment<G>(v, 0, firstDimension),
+                           H1 ? &D_g_first : nullptr,
+                           H2 ? &D_g_second : nullptr);
   H h = traits<H>::Retract(
-      this->second, tangentSegment<H>(v, firstDimension, secondDimension));
+      this->second, tangentSegment<H>(v, firstDimension, secondDimension),
+      H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr);
+  if (H1) {
+    *H1 = zeroJacobian(productDimension);
+    H1->block(0, 0, firstDimension, firstDimension) = D_g_first;
+    H1->block(firstDimension, firstDimension, secondDimension,
+              secondDimension) = D_h_first;
+  }
+  if (H2) {
+    *H2 = zeroJacobian(productDimension);
+    H2->block(0, 0, firstDimension, firstDimension) = D_g_second;
+    H2->block(firstDimension, firstDimension, secondDimension,
+              secondDimension) = D_h_second;
+  }
   return ProductLieGroup(g, h);
 }
 
@@ -62,16 +78,33 @@ typename ProductLieGroup<G, H>::TangentVector
 ProductLieGroup<G, H>::localCoordinates(const ProductLieGroup& g,
                                         ChartJacobian H1,
                                         ChartJacobian H2) const {
-  if (H1 || H2) {
-    throw std::runtime_error(
-        "ProductLieGroup::localCoordinates derivatives not implemented yet");
-  }
   checkMatchingDimensions(g, "localCoordinates");
   const size_t firstDimension = firstDim();
   const size_t secondDimension = secondDim();
-  typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
-  typename traits<H>::TangentVector v2 =
-      traits<H>::Local(this->second, g.second);
+  const size_t productDimension =
+      combinedDimension(firstDimension, secondDimension);
+  Jacobian1 D_g_first;
+  Jacobian1 D_g_second;
+  Jacobian2 D_h_first;
+  Jacobian2 D_h_second;
+  typename traits<G>::TangentVector v1 =
+      traits<G>::Local(this->first, g.first, H1 ? &D_g_first : nullptr,
+                       H2 ? &D_g_second : nullptr);
+  typename traits<H>::TangentVector v2 = traits<H>::Local(
+      this->second, g.second, H1 ? &D_h_first : nullptr,
+      H2 ? &D_h_second : nullptr);
+  if (H1) {
+    *H1 = zeroJacobian(productDimension);
+    H1->block(0, 0, firstDimension, firstDimension) = D_g_first;
+    H1->block(firstDimension, firstDimension, secondDimension,
+              secondDimension) = D_h_first;
+  }
+  if (H2) {
+    *H2 = zeroJacobian(productDimension);
+    H2->block(0, 0, firstDimension, firstDimension) = D_g_second;
+    H2->block(firstDimension, firstDimension, secondDimension,
+              secondDimension) = D_h_second;
+  }
   return makeTangentVector(v1, v2, firstDimension, secondDimension);
 }
 
@@ -469,16 +502,18 @@ template <typename G, int N, typename Derived>
 Derived PowerLieGroupBase<G, N, Derived>::retract(const TangentVector& v,
                                                   ChartJacobian H1,
                                                   ChartJacobian H2) const {
-  if (H1 || H2) {
-    throw std::runtime_error(
-        "PowerLieGroup::retract derivatives not implemented yet");
-  }
   const size_t count = componentCount();
   checkDynamicTangentSize(v, count, "retract");
+  JacobianStorage firstJacobians = makeJacobianStorage(count);
+  JacobianStorage secondJacobians = makeJacobianStorage(count);
   Derived result = makeResult(count);
   for (size_t i = 0; i < count; ++i) {
-    result[i] = traits<G>::Retract(derived()[i], tangentSegment(v, i));
+    result[i] = traits<G>::Retract(derived()[i], tangentSegment(v, i),
+                                   H1 ? &firstJacobians[i] : nullptr,
+                                   H2 ? &secondJacobians[i] : nullptr);
   }
+  fillJacobianBlocks(H1, firstJacobians, count);
+  fillJacobianBlocks(H2, secondJacobians, count);
   return result;
 }
 
@@ -487,15 +522,19 @@ typename PowerLieGroupBase<G, N, Derived>::TangentVector
 PowerLieGroupBase<G, N, Derived>::localCoordinates(const Derived& g,
                                                    ChartJacobian H1,
                                                    ChartJacobian H2) const {
-  if (H1 || H2) {
-    throw std::runtime_error(
-        "PowerLieGroup::localCoordinates derivatives not implemented yet");
-  }
   checkMatchingCounts(g, "localCoordinates");
+  const size_t count = componentCount();
+  JacobianStorage firstJacobians = makeJacobianStorage(count);
+  JacobianStorage secondJacobians = makeJacobianStorage(count);
   TangentVector v = zeroTangent(componentCount());
-  for (size_t i = 0; i < componentCount(); ++i) {
-    assignTangentSegment(v, i, traits<G>::Local(derived()[i], g[i]));
+  for (size_t i = 0; i < count; ++i) {
+    assignTangentSegment(
+        v, i,
+        traits<G>::Local(derived()[i], g[i], H1 ? &firstJacobians[i] : nullptr,
+                         H2 ? &secondJacobians[i] : nullptr));
   }
+  fillJacobianBlocks(H1, firstJacobians, count);
+  fillJacobianBlocks(H2, secondJacobians, count);
   return v;
 }
 

gtsam/navigation/EquivariantFilter.h

@@ -66,10 +66,45 @@ class EquivariantFilter : public ManifoldEKF<M> {
   M xi_ref_;  // Origin (reference) state on the manifold
   typename Symmetry::Orbit act_on_ref_;  // Orbit of the reference state
   MatrixMG Dphi0_;           // Differential of state action at identity
-  MatrixGM InnovationLift_;  // Innovation lift matrix ((Dphi0)^+)
+ MatrixGM InnovationLift_;  // Innovation lift matrix ((Dphi0)^+)
 
   G g_;  // Group element estimate
 
+ protected:
+  /// @return Current reference manifold state.
+  const M& referenceState() const { return xi_ref_; }
+
+  /// Recompute Dphi0 and innovation lift matrix from current reference state.
+  void recomputeActionDifferentials() {
+    act_on_ref_ = typename Symmetry::Orbit(xi_ref_);
+    const G identity_at_g = traits<G>::Compose(g_.inverse(), g_);
+    act_on_ref_(identity_at_g, &Dphi0_);
+    InnovationLift_ = Dphi0_.completeOrthogonalDecomposition().pseudoInverse();
+  }
+
+  /// Reset reference, covariance, and group estimate; sync manifold state.
+  void resetReferenceAndGroup(const M& xi_ref, const CovarianceM& P,
+                              const G& g) {
+    xi_ref_ = xi_ref;
+    g_ = g;
+    recomputeActionDifferentials();
+    if constexpr (DimM == Eigen::Dynamic) {
+      this->n_ = traits<M>::GetDimension(xi_ref_);
+      this->I_ = MatrixM::Identity(this->n_, this->n_);
+    }
+    this->P_ = P;
+    this->X_ = act_on_ref_(g_);
+  }
+
+  /// Set group estimate and update manifold state through the action.
+  void setGroupEstimateAndSyncState(const G& g) {
+    g_ = g;
+    this->X_ = act_on_ref_(g_);
+  }
+
+  /// Set error covariance directly.
+  void setErrorCovariance(const CovarianceM& P) { this->P_ = P; }
+
  public:
   /**
    * @brief Initialize the Equivariant Filter.
@@ -82,7 +117,8 @@ class EquivariantFilter : public ManifoldEKF<M> {
                     const G& X0 = traits<G>::Identity())
       : Base(xi_ref, Sigma), xi_ref_(xi_ref), act_on_ref_(xi_ref), g_(X0) {
     // Compute differential of action phi at identity (Dphi0)
-    act_on_ref_(traits<G>::Identity(), &Dphi0_);
+    const G identity_at_g = traits<G>::Compose(g_.inverse(), g_);
+    act_on_ref_(identity_at_g, &Dphi0_);
 
     // Precompute the Innovation Lift matrix (pseudo-inverse of Dphi0)
     InnovationLift_ = Dphi0_.completeOrthogonalDecomposition().pseudoInverse();
@@ -284,6 +320,23 @@ class EquivariantFilter : public ManifoldEKF<M> {
     this->validateInputs(prediction, H, z, R);
     update<Vector>(prediction, H, z, R);
   }
+
+  /// Vector measurement update using a custom innovation lift delta_x=f(delta_xi).
+  template <typename InnovationLiftFn>
+  void updateWithVector(const gtsam::Vector& prediction, const Matrix& H,
+                        const gtsam::Vector& z, const Matrix& R,
+                        InnovationLiftFn&& innovationLift) {
+    this->validateInputs(prediction, H, z, R);
+
+    const gtsam::Vector innovation = traits<Vector>::Local(z, prediction);
+    const Matrix K = this->KalmanGain(H, R);
+    const TangentM delta_xi = -K * innovation;
+    const TangentG delta_x = innovationLift(delta_xi);
+
+    g_ = traits<G>::Compose(traits<G>::Expmap(delta_x), g_);
+    this->X_ = act_on_ref_(g_);
+    this->JosephUpdate(K, H, R);
+  }
 };
 
 }  // namespace gtsam

gtsam/navigation/doc/ImuFactor.ipynb

@@ -29,7 +29,7 @@
         "Instead of connecting five variables (`Pose_i`, `Vel_i`, `Pose_j`, `Vel_j`, `Bias_i`), the `ImuFactor2` connects three:\n",
         "\n",
         "1. `NavState` at time $i$ (`NavState` combines pose and velocity)\n",
-        "2. `NavState` at time $j`\n",
+        "2. `NavState` at time $j$\n",
         "3. IMU Bias at time $i$ (`imuBias::ConstantBias`)\n",
         "\n",
         "### Modeling Bias\n",
@@ -48,7 +48,13 @@
         ]
       },
       "source": [
-        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
+        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
+        "Atlanta, Georgia 30332-0415\n",
+        "All Rights Reserved\n",
+        "\n",
+        "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
+        "\n",
+        "See LICENSE for the license information"
       ]
     },
     {
@@ -330,4 +336,4 @@
   },
   "nbformat": 4,
   "nbformat_minor": 2
-}
+}

gtsam_unstable/CMakeLists.txt

@@ -5,6 +5,7 @@ project(gtsam_unstable LANGUAGES CXX)
 set (gtsam_unstable_subdirs
     base
     geometry
+    navigation
     linear
     discrete
     dynamics
@@ -69,6 +70,7 @@ install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_IN
 set(gtsam_unstable_srcs
     ${base_srcs}
     ${geometry_srcs}
+    ${navigation_srcs}
     ${linear_srcs}
     ${discrete_srcs}
     ${dynamics_srcs}

gtsam_unstable/examples/CMakeLists.txt

@@ -1,4 +1,5 @@
 set (excluded_examples "")
+list(APPEND excluded_examples "EqVIOFilterRosbagReplay.cpp")
 
 # Add examples to exclude if GTSAM_USE_BOOST_FEATURES is not set
 if (NOT GTSAM_USE_BOOST_FEATURES) 
@@ -10,3 +11,24 @@ endif()
 
 
 gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
+
+option(GTSAM_UNSTABLE_BUILD_EQVIO_ROSBAG_EXAMPLE
+  "Build EqVIO rosbag replay example (requires ROS1 rosbag + cv_bridge)" OFF)
+
+if(GTSAM_UNSTABLE_BUILD_EQVIO_ROSBAG_EXAMPLE)
+  find_package(OpenCV REQUIRED)
+  find_package(rosbag REQUIRED)
+  find_package(cv_bridge REQUIRED)
+
+  add_executable(EqVIOFilterRosbagReplay EqVIOFilterRosbagReplay.cpp)
+  target_link_libraries(EqVIOFilterRosbagReplay
+    gtsam_unstable
+    ${OpenCV_LIBS}
+    ${rosbag_LIBRARIES}
+    ${cv_bridge_LIBRARIES})
+  target_include_directories(EqVIOFilterRosbagReplay PRIVATE
+    ${OpenCV_INCLUDE_DIRS}
+    ${rosbag_INCLUDE_DIRS}
+    ${cv_bridge_INCLUDE_DIRS})
+  list(APPEND examples_list EqVIOFilterRosbagReplay)
+endif()