From c403e4af5435f2b4bcc7a43b4a18aa85fadd8769 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Tue, 1 Apr 2025 17:43:52 -0400 Subject: [PATCH 01/54] Add files via upload A Left Invariant IEKF on SE(2). --- examples/SE2LIEKF_NoFactors.cpp | 124 ++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 examples/SE2LIEKF_NoFactors.cpp diff --git a/examples/SE2LIEKF_NoFactors.cpp b/examples/SE2LIEKF_NoFactors.cpp new file mode 100644 index 000000000..51693b7ee --- /dev/null +++ b/examples/SE2LIEKF_NoFactors.cpp @@ -0,0 +1,124 @@ + +/* ---------------------------------------------------------------------------- + + * 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 SE2LIEKF_NoFactors.cpp + * + * A simple left invariant EKF operating in SE(2) using an odometry and GPS measurements. + * No factors are used here. + * 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 April 1, 2025 + * @author Scott Baker + */ + + + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#define PI 3.14159265358 + +// Create an adjoint class +// Function for the prediction stage. +// Note that U is the exponential map of some control vector (u*dt) + void predict(Pose2& X, Matrix& P, Vector3& u, Matrix& Q) + { + //Pose2 U = Pose2::Expmap(u*dt); + Pose2 U(u(0), u(1), u(2)); + Matrix Adj = U.inverse().AdjointMap(); + X = X.compose(U); + P = Adj * P * Adj.transpose() + Q; + } + + // Update stage that uses GPS measurements in a left invariant update way. + void update(Pose2& X, Matrix& P, Point2& z, Matrix& H, Matrix& R) + { + // find residual R^T * (z - zhat) + Point2 zhat = X.translation(); + Point2 e_world = z-zhat; + Point2 residual = X.rotation().unrotate(e_world); + Matrix S = H*P*H.transpose() + R; + Matrix K = P*H.transpose()*S.inverse(); + Vector3 delta = K*residual; + X = X * Pose2::Expmap(delta); + P = (Matrix3::Identity() - K*H) * P; + } + + + +int main() { + + // Define movements and measurements + const double dt = 1.0; + + Vector3 u1(1.0, 1.0, 0.5), u2(1.0,1.0,0.0); // odometry + Point2 z1(1.0, 0.0), z2(1.0, 1.0); // gps + + + + // Set up noise models (uses simple GPS) + + // Odometry Process + Vector3 pModel(0.05, 0.05, 0.001); // covariance of the odometry process (rad) + Matrix Q = pModel.asDiagonal(); // Q covariance matrix + + // GPS process + Vector2 rModel(0.01, 0.01); + Matrix R = rModel.asDiagonal(); + Matrix H(2,3); + H << 1,0,0, + 0,1,0; + + // A matrix that transforms the P matrix into a form of [theta, x, y] for comparison to InEKF's structure. + Matrix3 TransformP; + TransformP << 0, 0, 1, + 1,0,0, + 0,1,0; + +// Initialize + Pose2 X(0, 0, 0); + Matrix P = Matrix3::Identity()*0.1; + + cout << "Initial X\n" << X << endl; + cout << "Initial P\n" << TransformP * P * TransformP.transpose() << endl; + + // First Prediction + predict(X, P, u1, dt, Q); + cout << "Predicted X1\n" << X << endl; + cout << "Predicted P1\n" << TransformP * P * TransformP.transpose() << endl; + + // First Update + update(X, P, z1, H, R); + cout << "Updated X1\n" << X << endl; + cout << "Updated P1\n" << TransformP * P * TransformP.transpose() << endl; + + // Second Prediction + predict(X, P, u2, dt, Q); + cout << "Predicted X2\n" << X << endl; + cout << "Predicted P2\n" << TransformP * P * TransformP.transpose() << endl; + + // Second Update + update(X, P, z2, H, R); + cout << "Updated X2\n" << X << endl; + cout << "Updated P2\n" << TransformP * P * TransformP.transpose() << endl; + + return 0; + } + From aafeabe1e3c63734f94e7d863dadc8e53cc05445 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 21:10:39 -0400 Subject: [PATCH 02/54] Add files via upload --- examples/LIEKF_SE2_SimpleGPSExample.cpp | 74 +++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 examples/LIEKF_SE2_SimpleGPSExample.cpp diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp new file mode 100644 index 000000000..ae6396be2 --- /dev/null +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -0,0 +1,74 @@ +// +// Created by Scott on 4/18/2025. +// +#include +#include +#include + +using namespace std; +using namespace gtsam; + + // Measurement Processor + Vector2 h_gps(const Pose2& X, + OptionalJacobian<2,3> H = {}) { + return X.translation(H); + } + int main() { + static const int dim = traits::dimension; + + // Initialization + Pose2 X0(0.0, 0.0, 0.0); + Matrix3 P0 = Matrix3::Identity() * 0.1; + double dt = 1.0; + + // Define GPS measurements + Matrix23 H; + h_gps(X0, H); + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; + + std::function)> measurement_function = h_gps; + LIEKF ekf(X0, P0, measurement_function); + + // Define Covariances + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); + + // Define odometry movements + Pose2 U1(1.0,1.0,0.5), U2(1.0,1.0,0.0); + + // Define a transformation matrix to convert the covariance into (theta, x, y) form. + Matrix3 TransformP; + TransformP << 0, 0, 1, + 1,0,0, + 0,1,0; + + // Predict / update stages + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.getState() << endl; + cout << "P0: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + + ekf.predict(U1, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.predict(U2, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + return 0; + } \ No newline at end of file From 693a08990e1c3c686476726858d4e3c01f75bb58 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 21:11:26 -0400 Subject: [PATCH 03/54] Add files via upload --- examples/LIEKF_NavstateExample.cpp | 88 ++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 examples/LIEKF_NavstateExample.cpp diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp new file mode 100644 index 000000000..042140cd3 --- /dev/null +++ b/examples/LIEKF_NavstateExample.cpp @@ -0,0 +1,88 @@ +// +// Created by Scott on 4/18/2025. +// +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// define dynamics +Vector9 dynamics(const NavState& X, const Vector6& imu, + OptionalJacobian<9, 9> H = {}) { + const auto a = imu.head<3>(); + const auto w = imu.tail<3>(); + Vector9 result; + result << w, Z_3x1, a; + if (H) { + *H = Matrix::Zero(9, 9); + } + return result; +} + +// define measurement processor + Vector3 h_gps(const NavState& X, + OptionalJacobian<3,9> H = {}) { + if (H) *H << Z_3x3, Z_3x3, X.R(); + return X.t(); + } + + int main() { + // Initialization + NavState X0; + Matrix9 P0 = Matrix9::Identity() * 0.1; + double dt = 1.0; + + // Create measurement function h_func + GeneralLIEKF::MeasurementFunction h_func = + [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; + + // Create dynamics + GeneralLIEKF::Dynamics dynamics_func = dynamics; + + // Initialize filter + GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + + // Covariances + Matrix9 Q = Matrix9::Identity() * 0.1; + Matrix3 R = Matrix3::Identity()*0.01; + + // IMU measurements + Vector6 imu1, imu2; + imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + // GPS measurements + Vector3 z1, z2; + z1 << 0.0, 0.0, 0.0; + z2 << 0.0, 0.0, 0.0; + + // Predict / update stages + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.getState() << endl; + cout << "P0: " << ekf.getCovariance() << endl; + + + ekf.predict(imu1, dt, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + ekf.predict(imu2, dt, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << ekf.getCovariance() << endl; + + return 0; + } \ No newline at end of file From c521dbc45946a0c94267d2b337b4056a9e1e7009 Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 21:12:19 -0400 Subject: [PATCH 04/54] Add files via upload --- gtsam/nonlinear/LIEKF.h | 176 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 176 insertions(+) create mode 100644 gtsam/nonlinear/LIEKF.h diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h new file mode 100644 index 000000000..4e510f33b --- /dev/null +++ b/gtsam/nonlinear/LIEKF.h @@ -0,0 +1,176 @@ +/* ---------------------------------------------------------------------------- + +* 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 LIEKF.h + * @brief Base and classes for Left Invariant Extended Kalman Filters + * + * Templates are implemented for a Left Invariant Extended Kalman Filter operating on Lie Groups. + * + * + * @date April 24, 2025 + * @author Scott Baker + * @author Matt Kielo + */ + +#pragma once +#include +#include +#include +#include +#include +namespace gtsam { + +/** +* @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) +* +* This class provides the prediction and update structure based on control inputs and a measurement function. +* +* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) +* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) +*/ + +template +class LIEKF { + +public: + static constexpr int n = traits::dimension; ///< Dimension of the state. + static constexpr int m = traits::dimension; ///< Dimension of the measurement. + + using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. + using MatrixN = Eigen::Matrix; ///< Typedef for the identity matrix. + + /** + * @brief Construct with a measurement function + * @param X0 Initial State + * @param P0 Initial Covariance + * @param h Measurement function + */ + LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ + : X(X0), P(P0), h_(h) {} + + + /** + * @brief Get current state estimate. + * @return Const reference to the state estiamte. + */ + const LieGroup& getState() const { return X; } + + /** + * @brief Get current covariance estimate. + * @return Const reference to the covariance estimate. + */ + const Matrix& getCovariance() const { return P; } + + /** + * @brief Prediction stage with a Lie group element U. + * @param U Lie group control input + * @param Q Process noise covariance matrix. + */ + void predict(const LieGroup& U, const Matrix& Q) { + LieGroup::Jacobian A; + X = X.compose(U, A); + P = A * P * A.transpose() + Q; + } + + /** + * @brief Prediction stage with a control vector u and a time interval dt. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + void predict(const Vector& u, double dt, const Matrix& Q) { + predict(LieGroup::Expmap(u*dt), Q);d + } + + /** + * @brief Update stage using a measurement and measurement covariance. + * @param z Measurement + * @param R Measurement noise covariance matrix. + */ + void update(const Measurement& z, const Matrix& R) { + Matrix H(m, n); + Vector y = h_(X, H)-z; + Matrix S = H * P * H.transpose() + R; + Matrix K = P * H.transpose() * S.inverse(); + X = X.expmap(-K * y); + P = (I_n - K * H) * P; // move Identity to be a constant. + } + + protected: + LieGroup X; ///< Current state estimate. + Matrix P; ///< Current covariance estimate. + + private: + MeasurementFunction h_; ///< Measurement function + static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + +}; + +/** +* @brief Create the static identity matrix I_n of size nxn for use in the update stage. +* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) +* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) +*/ +template +const typename LIEKF::MatrixN LIEKF::I_n + = typename LIEKF::MatrixN::Identity(); + +/** + * @brief General Left Invariant Extended Kalman Filter with dynamics function. + * + * This class extends the LIEKF class to include a dynamics function f. The dynamics maps + * a state and control vector to a tangent vector xi. + * + * @tparam LieGroup The Lie group type for the state. + * @tparam Measurement The type of the measurement. + * @tparam _p The dimension of the control vector. + */ +template +class GeneralLIEKF:public LIEKF { + public: + using Control = Eigen::Matrix; ///< Typedef for the control vector. + using TangentVector = typename traits::TangentVector; ///< Typedef for the tangent vector. + using Dynamics = std::function)>; + + + /** + * @brief Construct with general dynamics + * @param X0 Initial State + * @param P0 Initial Covariance + * @param f Dynamics function that depends on state and control vector + * @param h Measurement function + */ + GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, MeasurementFunction& +h) : LIEKF(X0, P0, h), f_(f) {} + + /** + * @brief Prediction stage with a dynamics function that calculates the tangent vector xi in the tangent space. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + void predict(const Control& u, double dt, const Matrix& Q) { + LieGroup::Jacobian H; + const TangentVector xi = f_(X, u, H); + LieGroup U = LieGroup::Expmap(xi * dt); + auto A = U.inverse().AdjointMap() * H; + X = X.compose(U); + P = A * P * A.transpose() + Q; + } + private: + Dynamics f_; ///< Dynamics function. +}; + +}// ends namespace + + From 136a7cdfd02a6dc07692703744f764d8fd7830ff Mon Sep 17 00:00:00 2001 From: scottiyio Date: Thu, 24 Apr 2025 22:06:46 -0400 Subject: [PATCH 05/54] Delete examples/SE2LIEKF_NoFactors.cpp --- examples/SE2LIEKF_NoFactors.cpp | 124 -------------------------------- 1 file changed, 124 deletions(-) delete mode 100644 examples/SE2LIEKF_NoFactors.cpp diff --git a/examples/SE2LIEKF_NoFactors.cpp b/examples/SE2LIEKF_NoFactors.cpp deleted file mode 100644 index 51693b7ee..000000000 --- a/examples/SE2LIEKF_NoFactors.cpp +++ /dev/null @@ -1,124 +0,0 @@ - -/* ---------------------------------------------------------------------------- - - * 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 SE2LIEKF_NoFactors.cpp - * - * A simple left invariant EKF operating in SE(2) using an odometry and GPS measurements. - * No factors are used here. - * 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 April 1, 2025 - * @author Scott Baker - */ - - - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#define PI 3.14159265358 - -// Create an adjoint class -// Function for the prediction stage. -// Note that U is the exponential map of some control vector (u*dt) - void predict(Pose2& X, Matrix& P, Vector3& u, Matrix& Q) - { - //Pose2 U = Pose2::Expmap(u*dt); - Pose2 U(u(0), u(1), u(2)); - Matrix Adj = U.inverse().AdjointMap(); - X = X.compose(U); - P = Adj * P * Adj.transpose() + Q; - } - - // Update stage that uses GPS measurements in a left invariant update way. - void update(Pose2& X, Matrix& P, Point2& z, Matrix& H, Matrix& R) - { - // find residual R^T * (z - zhat) - Point2 zhat = X.translation(); - Point2 e_world = z-zhat; - Point2 residual = X.rotation().unrotate(e_world); - Matrix S = H*P*H.transpose() + R; - Matrix K = P*H.transpose()*S.inverse(); - Vector3 delta = K*residual; - X = X * Pose2::Expmap(delta); - P = (Matrix3::Identity() - K*H) * P; - } - - - -int main() { - - // Define movements and measurements - const double dt = 1.0; - - Vector3 u1(1.0, 1.0, 0.5), u2(1.0,1.0,0.0); // odometry - Point2 z1(1.0, 0.0), z2(1.0, 1.0); // gps - - - - // Set up noise models (uses simple GPS) - - // Odometry Process - Vector3 pModel(0.05, 0.05, 0.001); // covariance of the odometry process (rad) - Matrix Q = pModel.asDiagonal(); // Q covariance matrix - - // GPS process - Vector2 rModel(0.01, 0.01); - Matrix R = rModel.asDiagonal(); - Matrix H(2,3); - H << 1,0,0, - 0,1,0; - - // A matrix that transforms the P matrix into a form of [theta, x, y] for comparison to InEKF's structure. - Matrix3 TransformP; - TransformP << 0, 0, 1, - 1,0,0, - 0,1,0; - -// Initialize - Pose2 X(0, 0, 0); - Matrix P = Matrix3::Identity()*0.1; - - cout << "Initial X\n" << X << endl; - cout << "Initial P\n" << TransformP * P * TransformP.transpose() << endl; - - // First Prediction - predict(X, P, u1, dt, Q); - cout << "Predicted X1\n" << X << endl; - cout << "Predicted P1\n" << TransformP * P * TransformP.transpose() << endl; - - // First Update - update(X, P, z1, H, R); - cout << "Updated X1\n" << X << endl; - cout << "Updated P1\n" << TransformP * P * TransformP.transpose() << endl; - - // Second Prediction - predict(X, P, u2, dt, Q); - cout << "Predicted X2\n" << X << endl; - cout << "Predicted P2\n" << TransformP * P * TransformP.transpose() << endl; - - // Second Update - update(X, P, z2, H, R); - cout << "Updated X2\n" << X << endl; - cout << "Updated P2\n" << TransformP * P * TransformP.transpose() << endl; - - return 0; - } - From 12d422341d66aac2c386443c8b8cfe41b3f2ba9d Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 14:43:19 -0400 Subject: [PATCH 06/54] clang-formatted, changed getFoobar() to foobar() --- gtsam/nonlinear/LIEKF.h | 192 ++++++++++++++++++++++------------------ 1 file changed, 104 insertions(+), 88 deletions(-) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 4e510f33b..b3520b06b 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -13,7 +13,8 @@ * @file LIEKF.h * @brief Base and classes for Left Invariant Extended Kalman Filters * - * Templates are implemented for a Left Invariant Extended Kalman Filter operating on Lie Groups. + * Templates are implemented for a Left Invariant Extended Kalman Filter + * operating on Lie Groups. * * * @date April 24, 2025 @@ -22,59 +23,66 @@ */ #pragma once -#include #include -#include #include +#include + #include +#include namespace gtsam { /** -* @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) -* -* This class provides the prediction and update structure based on control inputs and a measurement function. -* -* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) -* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) -*/ + * @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) + * + * This class provides the prediction and update structure based on control + * inputs and a measurement function. + * + * @tparam LieGroup Lie group used for state representation (e.g., Pose2, + * Pose3, NavState) + * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement + * for 3D position) + */ template class LIEKF { + public: + static constexpr int n = + traits::dimension; ///< Dimension of the state. + static constexpr int m = + traits::dimension; ///< Dimension of the measurement. -public: - static constexpr int n = traits::dimension; ///< Dimension of the state. - static constexpr int m = traits::dimension; ///< Dimension of the measurement. - - using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. - using MatrixN = Eigen::Matrix; ///< Typedef for the identity matrix. + using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. + using MatrixN = + Eigen::Matrix; ///< Typedef for the identity matrix. /** - * @brief Construct with a measurement function - * @param X0 Initial State - * @param P0 Initial Covariance - * @param h Measurement function - */ - LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ + * @brief Construct with a measurement function + * @param X0 Initial State + * @param P0 Initial Covariance + * @param h Measurement function + */ + LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ : X(X0), P(P0), h_(h) {} + /** + * @brief Get current state estimate. + * @return Const reference to the state estiamte. + */ + const LieGroup& state() const { return X; } /** - * @brief Get current state estimate. - * @return Const reference to the state estiamte. - */ - const LieGroup& getState() const { return X; } + * @brief Get current covariance estimate. + * @return Const reference to the covariance estimate. + */ + const Matrix& covariance() const { return P; } /** - * @brief Get current covariance estimate. - * @return Const reference to the covariance estimate. - */ - const Matrix& getCovariance() const { return P; } - - /** - * @brief Prediction stage with a Lie group element U. - * @param U Lie group control input - * @param Q Process noise covariance matrix. - */ + * @brief Prediction stage with a Lie group element U. + * @param U Lie group control input + * @param Q Process noise covariance matrix. + */ void predict(const LieGroup& U, const Matrix& Q) { LieGroup::Jacobian A; X = X.compose(U, A); @@ -82,83 +90,92 @@ public: } /** - * @brief Prediction stage with a control vector u and a time interval dt. - * @param u Control vector element - * @param dt Time interval - * @param Q Process noise covariance matrix. - */ - void predict(const Vector& u, double dt, const Matrix& Q) { - predict(LieGroup::Expmap(u*dt), Q);d + * @brief Prediction stage with a control vector u and a time interval dt. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + void predict(const Vector& u, double dt, const Matrix& Q) { + predict(LieGroup::Expmap(u * dt), Q); } /** - * @brief Update stage using a measurement and measurement covariance. - * @param z Measurement - * @param R Measurement noise covariance matrix. - */ + * @brief Update stage using a measurement and measurement covariance. + * @param z Measurement + * @param R Measurement noise covariance matrix. + */ void update(const Measurement& z, const Matrix& R) { Matrix H(m, n); - Vector y = h_(X, H)-z; + Vector y = h_(X, H) - z; Matrix S = H * P * H.transpose() + R; Matrix K = P * H.transpose() * S.inverse(); X = X.expmap(-K * y); - P = (I_n - K * H) * P; // move Identity to be a constant. + P = (I_n - K * H) * P; // move Identity to be a constant. } - protected: - LieGroup X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. - - private: - MeasurementFunction h_; ///< Measurement function - static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + protected: + LieGroup X; ///< Current state estimate. + Matrix P; ///< Current covariance estimate. + private: + MeasurementFunction h_; ///< Measurement function + static const MatrixN + I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. }; /** -* @brief Create the static identity matrix I_n of size nxn for use in the update stage. -* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) -* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) -*/ + * @brief Create the static identity matrix I_n of size nxn for use in the + * update stage. + * @tparam LieGroup Lie group used for state representation (e.g., Pose2, + * Pose3, NavState) + * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement + * for 3D position) + */ template -const typename LIEKF::MatrixN LIEKF::I_n - = typename LIEKF::MatrixN::Identity(); +const typename LIEKF::MatrixN + LIEKF::I_n = + typename LIEKF::MatrixN::Identity(); /** * @brief General Left Invariant Extended Kalman Filter with dynamics function. * - * This class extends the LIEKF class to include a dynamics function f. The dynamics maps - * a state and control vector to a tangent vector xi. + * This class extends the LIEKF class to include a dynamics function f. The + * dynamics maps a state and control vector to a tangent vector xi. * * @tparam LieGroup The Lie group type for the state. * @tparam Measurement The type of the measurement. * @tparam _p The dimension of the control vector. */ template -class GeneralLIEKF:public LIEKF { +class GeneralLIEKF : public LIEKF { public: - using Control = Eigen::Matrix; ///< Typedef for the control vector. - using TangentVector = typename traits::TangentVector; ///< Typedef for the tangent vector. - using Dynamics = std::function)>; - + using Control = + Eigen::Matrix; ///< Typedef for the control vector. + using TangentVector = + typename traits::TangentVector; ///< Typedef for the tangent + ///< vector. + using Dynamics = std::function)>; /** - * @brief Construct with general dynamics - * @param X0 Initial State - * @param P0 Initial Covariance - * @param f Dynamics function that depends on state and control vector - * @param h Measurement function - */ - GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, MeasurementFunction& -h) : LIEKF(X0, P0, h), f_(f) {} + * @brief Construct with general dynamics + * @param X0 Initial State + * @param P0 Initial Covariance + * @param f Dynamics function that depends on state and control vector + * @param h Measurement function + */ + GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, + MeasurementFunction& h) + : LIEKF(X0, P0, h), f_(f) {} /** - * @brief Prediction stage with a dynamics function that calculates the tangent vector xi in the tangent space. - * @param u Control vector element - * @param dt Time interval - * @param Q Process noise covariance matrix. - */ + * @brief Prediction stage with a dynamics function that calculates the + * tangent vector xi in the tangent space. + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ void predict(const Control& u, double dt, const Matrix& Q) { LieGroup::Jacobian H; const TangentVector xi = f_(X, u, H); @@ -167,10 +184,9 @@ h) : LIEKF(X0, P0, h), f_(f) {} X = X.compose(U); P = A * P * A.transpose() + Q; } - private: - Dynamics f_; ///< Dynamics function. + + private: + Dynamics f_; ///< Dynamics function. }; -}// ends namespace - - +} // namespace gtsam From 8aed7316a52bca940a70d691a288131d034ae37c Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 15:02:30 -0400 Subject: [PATCH 07/54] Added documentation and format changes. - Added comments - Clang-formatted --- examples/LIEKF_NavstateExample.cpp | 149 ++++++++++++++++++----------- 1 file changed, 92 insertions(+), 57 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 042140cd3..6c6d594a7 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -1,14 +1,43 @@ -// -// Created by Scott on 4/18/2025. -// -#include +/* ---------------------------------------------------------------------------- + + * 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 LIEKF_NavstateExample.cpp + * @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF + * on NavState using IMU/GPS measurements. + * + * This example uses the templated GeneralLIEKF class to estimate the state of + * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses + * a generic dynamics function to predict the state. This simulates a navigation + * state of (pose, velocity, position) + * + * @date Apr 25, 2025 + * @author Scott Baker + * @author Matt Kielo + * @author Frank Dellaert + */ #include +#include + #include using namespace std; using namespace gtsam; -// define dynamics +// Define a dynamics function. +// The dynamics function for NavState returns a result vector of +// size 9 of [angular_velocity, 0, 0, 0, linear_acceleration] as well as +// a Jacobian of the dynamics function with respect to the state X. +// Since this is a left invariant EKF, the error dynamics do not rely on the +// state Vector9 dynamics(const NavState& X, const Vector6& imu, OptionalJacobian<9, 9> H = {}) { const auto a = imu.head<3>(); @@ -21,68 +50,74 @@ Vector9 dynamics(const NavState& X, const Vector6& imu, return result; } -// define measurement processor - Vector3 h_gps(const NavState& X, - OptionalJacobian<3,9> H = {}) { - if (H) *H << Z_3x3, Z_3x3, X.R(); - return X.t(); - } +// define a GPS measurement processor. The GPS measurement processor returns +// the expected measurement h(x) = translation of X with a Jacobian H used in +// the update stage of the LIEKF. +Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { + if (H) *H << Z_3x3, Z_3x3, X.R(); + return X.t(); +} - int main() { - // Initialization - NavState X0; - Matrix9 P0 = Matrix9::Identity() * 0.1; - double dt = 1.0; +int main() { + // Initialize the filter's state, covariance, and time interval values. + NavState X0; + Matrix9 P0 = Matrix9::Identity() * 0.1; + double dt = 1.0; - // Create measurement function h_func - GeneralLIEKF::MeasurementFunction h_func = - [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; + // Create the measurement function h_func that wraps h_gps + GeneralLIEKF::MeasurementFunction h_func = + [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; - // Create dynamics - GeneralLIEKF::Dynamics dynamics_func = dynamics; + // Create the dynamics function dynamics_func + GeneralLIEKF::Dynamics dynamics_func = dynamics; - // Initialize filter - GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + // Create the filter with the initial state, covariance, and dynamics and + // measurement functions. + GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); - // Covariances - Matrix9 Q = Matrix9::Identity() * 0.1; - Matrix3 R = Matrix3::Identity()*0.01; + // Create the process covariance and measurement covariance matrices Q and R. + Matrix9 Q = Matrix9::Identity() * 0.01; + Matrix3 R = Matrix3::Identity() * 0.5; - // IMU measurements - Vector6 imu1, imu2; - imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + // Create the IMU measurements of the form (linear_acceleration, + // angular_velocity) + Vector6 imu1, imu2; + imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - // GPS measurements - Vector3 z1, z2; - z1 << 0.0, 0.0, 0.0; - z2 << 0.0, 0.0, 0.0; + // Create the GPS measurements of the form (px, py, pz) + Vector3 z1, z2; + z1 << 0.0, 0.0, 0.0; + z2 << 0.0, 0.0, 0.0; - // Predict / update stages - cout << "\nInitialization:\n"; - cout << "X0: " << ekf.getState() << endl; - cout << "P0: " << ekf.getCovariance() << endl; + // Run the predict and update stages, and print their results. + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.state() << endl; + cout << "P0: " << ekf.covariance() << endl; + // First prediction stage + ekf.predict(imu1, dt, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.predict(imu1, dt, Q); - cout << "\nFirst Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; + // First update stage + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.update(z1, R); - cout << "\nFirst Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; + // Second prediction stage + ekf.predict(imu2, dt, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.predict(imu2, dt, Q); - cout << "\nSecond Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; + // Second update stage + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << ekf.covariance() << endl; - ekf.update(z2, R); - cout << "\nSecond Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << ekf.getCovariance() << endl; - - return 0; - } \ No newline at end of file + return 0; +} \ No newline at end of file From 6ff04399f5e59796d5a0a378adf606900423d4eb Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 15:31:52 -0400 Subject: [PATCH 08/54] Added authors --- gtsam/nonlinear/LIEKF.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index b3520b06b..598a6300a 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -20,6 +20,7 @@ * @date April 24, 2025 * @author Scott Baker * @author Matt Kielo + * @author Frank Dellaert */ #pragma once From f857001931cead6d0e174df6bb2067adc4eec68d Mon Sep 17 00:00:00 2001 From: scottiyio Date: Fri, 25 Apr 2025 15:32:31 -0400 Subject: [PATCH 09/54] Added documentation & clang formatting --- examples/LIEKF_SE2_SimpleGPSExample.cpp | 164 ++++++++++++++++-------- 1 file changed, 107 insertions(+), 57 deletions(-) diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index ae6396be2..c2d0acdb6 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -1,74 +1,124 @@ -// -// Created by Scott on 4/18/2025. -// -#include +/* ---------------------------------------------------------------------------- + + * 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 LIEKF_SE2_SimpleGPSExample.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 LIEKF class to estimate the state of + * an object using odometry / GPS measurements The prediction stage of the LIEKF + * uses a Lie Group element to propagate the stage in a discrete LIEKF. 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 + * @author Scott Baker + * @author Matt Kielo + * @author Frank Dellaert + */ #include +#include + #include using namespace std; using namespace gtsam; - // Measurement Processor - Vector2 h_gps(const Pose2& X, - OptionalJacobian<2,3> H = {}) { - return X.translation(H); - } - int main() { - static const int dim = traits::dimension; +// 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 LIEKF 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; + double dt = 1.0; - // Initialization - Pose2 X0(0.0, 0.0, 0.0); - Matrix3 P0 = Matrix3::Identity() * 0.1; - double dt = 1.0; + // Create the function measurement_function that wraps h_gps. + std::function)> + measurement_function = h_gps; + // Create the filter with the initial state, covariance, and measurement + // function. + LIEKF ekf(X0, P0, measurement_function); - // Define GPS measurements - Matrix23 H; - h_gps(X0, H); - Vector2 z1, z2; - z1 << 1.0, 0.0; - z2 << 1.0, 1.0; + // Define the process covariance and measurement covariance matrices Q and R. + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); - std::function)> measurement_function = h_gps; - LIEKF ekf(X0, P0, measurement_function); + // 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); - // Define Covariances - Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); - Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); + // 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 odometry movements - Pose2 U1(1.0,1.0,0.5), U2(1.0,1.0,0.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; - // Define a transformation matrix to convert the covariance into (theta, x, y) form. - 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; - // Predict / update stages - cout << "\nInitialization:\n"; - cout << "X0: " << ekf.getState() << endl; - cout << "P0: " << TransformP * ekf.getCovariance() * 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(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; - ekf.predict(U1, Q); - cout << "\nFirst Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * 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; - ekf.update(z1, R); - cout << "\nFirst Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + // Second update stage + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.state() << endl; + cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() + << endl; - ekf.predict(U2, Q); - cout << "\nSecond Prediction:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; - - ekf.update(z2, R); - cout << "\nSecond Update:\n"; - cout << "X: " << ekf.getState() << endl; - cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; - - return 0; - } \ No newline at end of file + return 0; +} \ No newline at end of file From 0d832a65995eff723fc65ae76edd138b93c71966 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 11:14:57 -0400 Subject: [PATCH 11/54] no_boost --- examples/ISAM2_City10000.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index 2c210ab44..b756fb1e1 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -27,8 +27,6 @@ #include #include -#include -#include #include #include #include From b1c32cb6f157af11533f62e26d27cc3e9bb1edc0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 11:47:46 -0400 Subject: [PATCH 12/54] Templated predict/update --- examples/LIEKF_NavstateExample.cpp | 21 ++-- examples/LIEKF_SE2_SimpleGPSExample.cpp | 10 +- gtsam/nonlinear/LIEKF.h | 128 ++++++++---------------- 3 files changed, 51 insertions(+), 108 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 6c6d594a7..22639acb1 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,10 +11,10 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF + * @brief A left invariant Extended Kalman Filter example using the LIEKF * on NavState using IMU/GPS measurements. * - * This example uses the templated GeneralLIEKF class to estimate the state of + * This example uses the templated LIEKF class to estimate the state of * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses * a generic dynamics function to predict the state. This simulates a navigation * state of (pose, velocity, position) @@ -64,16 +64,9 @@ int main() { Matrix9 P0 = Matrix9::Identity() * 0.1; double dt = 1.0; - // Create the measurement function h_func that wraps h_gps - GeneralLIEKF::MeasurementFunction h_func = - [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; - - // Create the dynamics function dynamics_func - GeneralLIEKF::Dynamics dynamics_func = dynamics; - // Create the filter with the initial state, covariance, and dynamics and // measurement functions. - GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + LIEKF ekf(X0, P0); // Create the process covariance and measurement covariance matrices Q and R. Matrix9 Q = Matrix9::Identity() * 0.01; @@ -96,25 +89,25 @@ int main() { cout << "P0: " << ekf.covariance() << endl; // First prediction stage - ekf.predict(imu1, dt, Q); + ekf.predict<6>(dynamics, imu1, dt, Q); cout << "\nFirst Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; // First update stage - ekf.update(z1, R); + ekf.update(h_gps, z1, R); cout << "\nFirst Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; // Second prediction stage - ekf.predict(imu2, dt, Q); + ekf.predict<6>(dynamics, imu2, dt, Q); cout << "\nSecond Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; // Second update stage - ekf.update(z2, R); + ekf.update(h_gps, z2, R); cout << "\nSecond Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index c2d0acdb6..1f0a01212 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -53,14 +53,10 @@ 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; - double dt = 1.0; - // Create the function measurement_function that wraps h_gps. - std::function)> - measurement_function = h_gps; // Create the filter with the initial state, covariance, and measurement // function. - LIEKF ekf(X0, P0, measurement_function); + LIEKF ekf(X0, P0); // Define the process covariance and measurement covariance matrices Q and R. Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); @@ -100,7 +96,7 @@ int main() { << endl; // First update stage - ekf.update(z1, R); + ekf.update(h_gps, z1, R); cout << "\nFirst Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() @@ -114,7 +110,7 @@ int main() { << endl; // Second update stage - ekf.update(z2, R); + ekf.update(h_gps, z2, R); cout << "\nSecond Update:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose() diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 598a6300a..93fe5c8b5 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -38,23 +38,17 @@ namespace gtsam { * This class provides the prediction and update structure based on control * inputs and a measurement function. * - * @tparam LieGroup Lie group used for state representation (e.g., Pose2, + * @tparam G Lie group used for state representation (e.g., Pose2, * Pose3, NavState) * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement * for 3D position) */ -template +template class LIEKF { public: - static constexpr int n = - traits::dimension; ///< Dimension of the state. - static constexpr int m = - traits::dimension; ///< Dimension of the measurement. + static constexpr int n = traits::dimension; ///< Dimension of the state. - using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. using MatrixN = Eigen::Matrix; ///< Typedef for the identity matrix. @@ -64,14 +58,13 @@ class LIEKF { * @param P0 Initial Covariance * @param h Measurement function */ - LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ - : X(X0), P(P0), h_(h) {} + LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} /** * @brief Get current state estimate. - * @return Const reference to the state estiamte. + * @return Const reference to the state estimate. */ - const LieGroup& state() const { return X; } + const G& state() const { return X; } /** * @brief Get current covariance estimate. @@ -84,8 +77,8 @@ class LIEKF { * @param U Lie group control input * @param Q Process noise covariance matrix. */ - void predict(const LieGroup& U, const Matrix& Q) { - LieGroup::Jacobian A; + void predict(const G& U, const Matrix& Q) { + typename G::Jacobian A; X = X.compose(U, A); P = A * P * A.transpose() + Q; } @@ -97,17 +90,41 @@ class LIEKF { * @param Q Process noise covariance matrix. */ void predict(const Vector& u, double dt, const Matrix& Q) { - predict(LieGroup::Expmap(u * dt), Q); + predict(G::Expmap(u * dt), Q); + } + + /** + * @brief Prediction stage with a dynamics function that calculates the + * tangent vector xi in the tangent space. + * @tparam _p The dimension of the control vector. + * @tparam Dynamics : (G, VectorP, OptionalJacobian) -> TangentVector + * @param f Dynamics function that depends on state and control vector + * @param u Control vector element + * @param dt Time interval + * @param Q Process noise covariance matrix. + */ + template + void predict(Dynamics&& f, const Eigen::Matrix& u, double dt, + const Matrix& Q) { + typename G::Jacobian F; + const typename G::TangentVector xi = f(X, u, F); + G U = G::Expmap(xi * dt); + auto A = U.inverse().AdjointMap() * F; + X = X.compose(U); + P = A * P * A.transpose() + Q; } /** * @brief Update stage using a measurement and measurement covariance. + * @tparam Measurement + * @tparam Prediction : (G, OptionalJacobian) -> Measurement * @param z Measurement * @param R Measurement noise covariance matrix. */ - void update(const Measurement& z, const Matrix& R) { - Matrix H(m, n); - Vector y = h_(X, H) - z; + template + void update(Prediction&& h, const Measurement& z, const Matrix& R) { + Eigen::Matrix::dimension, n> H; + Vector y = h(X, H) - z; Matrix S = H * P * H.transpose() + R; Matrix K = P * H.transpose() * S.inverse(); X = X.expmap(-K * y); @@ -115,79 +132,16 @@ class LIEKF { } protected: - LieGroup X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. + G X; ///< Current state estimate. + Matrix P; ///< Current covariance estimate. private: - MeasurementFunction h_; ///< Measurement function static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. }; -/** - * @brief Create the static identity matrix I_n of size nxn for use in the - * update stage. - * @tparam LieGroup Lie group used for state representation (e.g., Pose2, - * Pose3, NavState) - * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement - * for 3D position) - */ -template -const typename LIEKF::MatrixN - LIEKF::I_n = - typename LIEKF::MatrixN::Identity(); - -/** - * @brief General Left Invariant Extended Kalman Filter with dynamics function. - * - * This class extends the LIEKF class to include a dynamics function f. The - * dynamics maps a state and control vector to a tangent vector xi. - * - * @tparam LieGroup The Lie group type for the state. - * @tparam Measurement The type of the measurement. - * @tparam _p The dimension of the control vector. - */ -template -class GeneralLIEKF : public LIEKF { - public: - using Control = - Eigen::Matrix; ///< Typedef for the control vector. - using TangentVector = - typename traits::TangentVector; ///< Typedef for the tangent - ///< vector. - using Dynamics = std::function)>; - - /** - * @brief Construct with general dynamics - * @param X0 Initial State - * @param P0 Initial Covariance - * @param f Dynamics function that depends on state and control vector - * @param h Measurement function - */ - GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, - MeasurementFunction& h) - : LIEKF(X0, P0, h), f_(f) {} - - /** - * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi in the tangent space. - * @param u Control vector element - * @param dt Time interval - * @param Q Process noise covariance matrix. - */ - void predict(const Control& u, double dt, const Matrix& Q) { - LieGroup::Jacobian H; - const TangentVector xi = f_(X, u, H); - LieGroup U = LieGroup::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * H; - X = X.compose(U); - P = A * P * A.transpose() + Q; - } - - private: - Dynamics f_; ///< Dynamics function. -}; +/// Create the static identity matrix I_n of size nxn for use in update +template +const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); } // namespace gtsam From 935246549459b34f805fe9f97d1151a69660d375 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:14:12 -0400 Subject: [PATCH 13/54] More symmetric templating --- examples/LIEKF_NavstateExample.cpp | 4 ++-- gtsam/nonlinear/LIEKF.h | 24 +++++++++++++----------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 22639acb1..a99e91398 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -89,7 +89,7 @@ int main() { cout << "P0: " << ekf.covariance() << endl; // First prediction stage - ekf.predict<6>(dynamics, imu1, dt, Q); + ekf.predict(dynamics, imu1, dt, Q); cout << "\nFirst Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; @@ -101,7 +101,7 @@ int main() { cout << "P: " << ekf.covariance() << endl; // Second prediction stage - ekf.predict<6>(dynamics, imu2, dt, Q); + ekf.predict(dynamics, imu2, dt, Q); cout << "\nSecond Prediction:\n"; cout << "X: " << ekf.state() << endl; cout << "P: " << ekf.covariance() << endl; diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 93fe5c8b5..3d71337fa 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -84,10 +84,13 @@ class LIEKF { } /** - * @brief Prediction stage with a control vector u and a time interval dt. + * @brief Prediction stage with a tangent vector xi and a time interval dt. * @param u Control vector element * @param dt Time interval * @param Q Process noise covariance matrix. + * + * @note Use this if your dynamics does not depend on the state, e.g. + * predict(f(u), dt, q) where u is a control input */ void predict(const Vector& u, double dt, const Matrix& Q) { predict(G::Expmap(u * dt), Q); @@ -95,28 +98,27 @@ class LIEKF { /** * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi in the tangent space. - * @tparam _p The dimension of the control vector. - * @tparam Dynamics : (G, VectorP, OptionalJacobian) -> TangentVector - * @param f Dynamics function that depends on state and control vector - * @param u Control vector element + * tangent vector xi that *depends on the state*. + * @tparam Control The control input type + * @tparam Dynamics : (G, Control, OptionalJacobian) -> TangentVector + * @param f Dynamics function that depends on state and control input + * @param u Control input * @param dt Time interval * @param Q Process noise covariance matrix. */ - template - void predict(Dynamics&& f, const Eigen::Matrix& u, double dt, - const Matrix& Q) { + template + void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; const typename G::TangentVector xi = f(X, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f X = X.compose(U); P = A * P * A.transpose() + Q; } /** * @brief Update stage using a measurement and measurement covariance. - * @tparam Measurement + * @tparam Measurement The measurement output type * @tparam Prediction : (G, OptionalJacobian) -> Measurement * @param z Measurement * @param R Measurement noise covariance matrix. From f0e35aecea7edf61f74a4f110b9c85ce418d7160 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:24:00 -0400 Subject: [PATCH 14/54] Comments with o4-mini --- gtsam/nonlinear/LIEKF.h | 166 ++++++++++++++++++++++------------------ 1 file changed, 90 insertions(+), 76 deletions(-) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 3d71337fa..840d004a7 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- -* GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -10,82 +10,82 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF.h - * @brief Base and classes for Left Invariant Extended Kalman Filters + * @file LIEKF.h + * @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation * - * Templates are implemented for a Left Invariant Extended Kalman Filter - * operating on Lie Groups. + * This file defines the LIEKF class template for performing prediction and + * update steps of an Extended Kalman Filter on states residing in a Lie group. + * The class supports state evolution via group composition and dynamics + * functions, along with measurement updates using tangent-space corrections. * - * - * @date April 24, 2025 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #pragma once + #include #include #include #include -#include + + namespace gtsam { /** - * @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) + * @class LIEKF + * @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G * - * This class provides the prediction and update structure based on control - * inputs and a measurement function. + * @tparam G Lie group type providing: + * - static int dimension = tangent dimension + * - using TangentVector = Eigen::Vector... + * - using Jacobian = Eigen::Matrix... + * - methods: Expmap(), expmap(), compose(), inverse().AdjointMap() * - * @tparam G Lie group used for state representation (e.g., Pose2, - * Pose3, NavState) - * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement - * for 3D position) + * This filter maintains a state X in the group G and covariance P in the + * tangent space. Prediction steps are performed via group composition or a + * user-supplied dynamics function. Updates apply a measurement function h + * returning both predicted measurement and its Jacobian H, and correct state + * using the left-invariant error in the tangent space. */ - template class LIEKF { public: - static constexpr int n = traits::dimension; ///< Dimension of the state. + /// Tangent-space dimension + static constexpr int n = traits::dimension; - using MatrixN = - Eigen::Matrix; ///< Typedef for the identity matrix. + /// Square matrix of size n for covariance and Jacobians + using MatrixN = Eigen::Matrix; + + /// Constructor: initialize with state and covariance + LIEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} + + /// @return current state estimate + const G& state() const { return X_; } + + /// @return current covariance estimate + const Matrix& covariance() const { return P_; } /** - * @brief Construct with a measurement function - * @param X0 Initial State - * @param P0 Initial Covariance - * @param h Measurement function - */ - LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} - - /** - * @brief Get current state estimate. - * @return Const reference to the state estimate. - */ - const G& state() const { return X; } - - /** - * @brief Get current covariance estimate. - * @return Const reference to the covariance estimate. - */ - const Matrix& covariance() const { return P; } - - /** - * @brief Prediction stage with a Lie group element U. - * @param U Lie group control input - * @param Q Process noise covariance matrix. + * Predict step via group composition: + * X_{k+1} = X_k * U + * P_{k+1} = A P_k A^T + Q + * where A = Ad_{U^{-1}}. i.e., d(X.compose(U))/dX evaluated at X_k. + * + * @param U Lie group increment (e.g., Expmap of control * dt) + * @param Q process noise covariance in tangent space */ void predict(const G& U, const Matrix& Q) { typename G::Jacobian A; - X = X.compose(U, A); - P = A * P * A.transpose() + Q; + X_ = X_.compose(U, A); + P_ = A * P_ * A.transpose() + Q; } /** - * @brief Prediction stage with a tangent vector xi and a time interval dt. - * @param u Control vector element + * Predict step via tangent control vector: + * U = Expmap(u * dt) + * @param u tangent control vector * @param dt Time interval * @param Q Process noise covariance matrix. * @@ -97,52 +97,66 @@ class LIEKF { } /** - * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi that *depends on the state*. - * @tparam Control The control input type - * @tparam Dynamics : (G, Control, OptionalJacobian) -> TangentVector - * @param f Dynamics function that depends on state and control input - * @param u Control input - * @param dt Time interval - * @param Q Process noise covariance matrix. + * Predict step with state-dependent dynamics: + * xi = f(X, u, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @tparam Control control input type + * @tparam Dynamics signature: G f(const G&, const Control&, + * OptionalJacobian&) + * + * @param f dynamics functor depending on state and control + * @param u control input + * @param dt time step + * @param Q process noise covariance */ template void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; - const typename G::TangentVector xi = f(X, u, F); + auto xi = f(X_, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f - X = X.compose(U); - P = A * P * A.transpose() + Q; + auto A = U.inverse().AdjointMap() * F; + X_ = X_.compose(U); + P_ = A * P_ * A.transpose() + Q; } /** - * @brief Update stage using a measurement and measurement covariance. - * @tparam Measurement The measurement output type - * @tparam Prediction : (G, OptionalJacobian) -> Measurement - * @param z Measurement - * @param R Measurement noise covariance matrix. + * Measurement update: + * z_pred, H = h(X) + * K = P H^T (H P H^T + R)^{-1} + * X <- Expmap(-K (z_pred - z)) * X + * P <- (I - K H) P + * + * @tparam Measurement measurement type (e.g., Vector) + * @tparam Prediction functor signature: Measurement h(const G&, + * OptionalJacobian&) + * + * @param h measurement model returning predicted z and Jacobian H + * @param z observed measurement + * @param R measurement noise covariance */ template void update(Prediction&& h, const Measurement& z, const Matrix& R) { Eigen::Matrix::dimension, n> H; - Vector y = h(X, H) - z; - Matrix S = H * P * H.transpose() + R; - Matrix K = P * H.transpose() * S.inverse(); - X = X.expmap(-K * y); - P = (I_n - K * H) * P; // move Identity to be a constant. + auto z_pred = h(X_, H); + auto y = z_pred - z; + Matrix S = H * P_ * H.transpose() + R; + Matrix K = P_ * H.transpose() * S.inverse(); + X_ = X_.expmap(-K * y); + P_ = (I_n - K * H) * P_; } protected: - G X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. + G X_; ///< group state estimate + Matrix P_; ///< covariance in tangent space private: - static const MatrixN - I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + /// Identity matrix of size n + static const MatrixN I_n; }; -/// Create the static identity matrix I_n of size nxn for use in update +// Define static identity I_n template const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); From f63255be5b81a9ef44423c80935f3adff3a56977 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:30:41 -0400 Subject: [PATCH 15/54] Move to nav --- gtsam/{nonlinear => navigation}/LIEKF.h | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/{nonlinear => navigation}/LIEKF.h (100%) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/navigation/LIEKF.h similarity index 100% rename from gtsam/nonlinear/LIEKF.h rename to gtsam/navigation/LIEKF.h From 885ab38a7e7a1ee3d324a4710cfc022950fc2fdf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:30:55 -0400 Subject: [PATCH 16/54] Docs with o4-mini --- examples/LIEKF_NavstateExample.cpp | 113 ++++++++++++++--------------- 1 file changed, 54 insertions(+), 59 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index a99e91398..0747d9190 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,19 +11,14 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief A left invariant Extended Kalman Filter example using the LIEKF - * on NavState using IMU/GPS measurements. - * - * This example uses the templated LIEKF class to estimate the state of - * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses - * a generic dynamics function to predict the state. This simulates a navigation - * state of (pose, velocity, position) - * - * @date Apr 25, 2025 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @brief Example of a Left-Invariant Extended Kalman Filter on NavState + * using IMU (predict) and GPS (update) measurements. + * @date April 25, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ + +#include +#include #include #include @@ -32,85 +27,85 @@ using namespace std; using namespace gtsam; -// Define a dynamics function. -// The dynamics function for NavState returns a result vector of -// size 9 of [angular_velocity, 0, 0, 0, linear_acceleration] as well as -// a Jacobian of the dynamics function with respect to the state X. -// Since this is a left invariant EKF, the error dynamics do not rely on the -// state +/** + * @brief Left-invariant dynamics for NavState. + * @param X Current state (unused for left-invariant error dynamics). + * @param imu 6×1 vector [a; ω]: linear accel (first 3) and angular vel (last + * 3). + * @param H Optional 9×9 Jacobian w.r.t. X (always zero here). + * @return 9×1 tangent: [ω; 0₃; a]. + */ Vector9 dynamics(const NavState& X, const Vector6& imu, OptionalJacobian<9, 9> H = {}) { - const auto a = imu.head<3>(); - const auto w = imu.tail<3>(); - Vector9 result; - result << w, Z_3x1, a; - if (H) { - *H = Matrix::Zero(9, 9); - } - return result; + auto a = imu.head<3>(); + auto w = imu.tail<3>(); + Vector9 xi; + xi << w, Vector3::Zero(), a; + if (H) *H = Matrix9::Zero(); + return xi; } -// define a GPS measurement processor. The GPS measurement processor returns -// the expected measurement h(x) = translation of X with a Jacobian H used in -// the update stage of the LIEKF. +/** + * @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 = {}) { - if (H) *H << Z_3x3, Z_3x3, X.R(); + if (H) { + // H = [ 0₃×3, 0₃×3, R ] + *H << Z_3x3, Z_3x3, X.R(); + } return X.t(); } int main() { - // Initialize the filter's state, covariance, and time interval values. + // Initial state, covariance, and time step NavState X0; Matrix9 P0 = Matrix9::Identity() * 0.1; double dt = 1.0; - // Create the filter with the initial state, covariance, and dynamics and - // measurement functions. + // Create the filter with the initial state and covariance. LIEKF ekf(X0, P0); - // Create the process covariance and measurement covariance matrices Q and R. + // Process & measurement noise Matrix9 Q = Matrix9::Identity() * 0.01; Matrix3 R = Matrix3::Identity() * 0.5; // Create the IMU measurements of the form (linear_acceleration, // angular_velocity) Vector6 imu1, imu2; - imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0; + imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0; // Create the GPS measurements of the form (px, py, pz) Vector3 z1, z2; - z1 << 0.0, 0.0, 0.0; - z2 << 0.0, 0.0, 0.0; + z1 << 0.3, 0.0, 0.0; + z2 << 0.6, 0.0, 0.0; - // Run the predict and update stages, and print their results. - cout << "\nInitialization:\n"; - cout << "X0: " << ekf.state() << endl; - cout << "P0: " << ekf.covariance() << endl; + cout << "=== Initialization ===\n" + << "X0: " << ekf.state() << "\n" + << "P0: " << ekf.covariance() << "\n\n"; - // First prediction stage ekf.predict(dynamics, imu1, dt, Q); - cout << "\nFirst Prediction:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After first predict ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n\n"; - // First update stage ekf.update(h_gps, z1, R); - cout << "\nFirst Update:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After first update ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n\n"; - // Second prediction stage ekf.predict(dynamics, imu2, dt, Q); - cout << "\nSecond Prediction:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After second predict ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n\n"; - // Second update stage ekf.update(h_gps, z2, R); - cout << "\nSecond Update:\n"; - cout << "X: " << ekf.state() << endl; - cout << "P: " << ekf.covariance() << endl; + cout << "--- After second update ---\n" + << "X: " << ekf.state() << "\n" + << "P: " << ekf.covariance() << "\n"; return 0; -} \ No newline at end of file +} From 0ee0a8beecd33f0c5c909ed54a29a9f074514d8e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:31:02 -0400 Subject: [PATCH 17/54] Test WIP --- gtsam/navigation/tests/testLIEKF.cpp | 61 ++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 gtsam/navigation/tests/testLIEKF.cpp diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp new file mode 100644 index 000000000..81e80a69d --- /dev/null +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + * 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 testLIEKFNavState.cpp + * @brief Unit test for the NavState dynamics Jacobian in LIEKF example. + * @date April 26, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include + +using namespace gtsam; + +// Duplicate the dynamics function under test: +namespace example { +Vector9 dynamics(const NavState& X, const Vector6& imu, + OptionalJacobian<9, 9> H = {}) { + auto a = imu.head<3>(); + auto w = imu.tail<3>(); + Vector9 xi; + xi << w, Vector3::Zero(), a; + if (H) *H = Matrix9::Zero(); + return xi; +} +} // namespace example + +TEST(LIEKFNavState, dynamicsJacobian) { + // Construct a nontrivial state and IMU input + NavState X(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, 2.0, 3.0), + Vector3(0.5, -0.5, 0.5)); + Vector6 imu; + imu << 0.1, -0.1, 0.2, // acceleration + 0.01, -0.02, 0.03; // angular velocity + + // Analytic Jacobian (always zero for left-invariant dynamics) + OptionalJacobian<9, 9> H_analytic; + example::dynamics(X, imu, H_analytic); + Matrix actualH = *H_analytic; + + // Numeric Jacobian w.r.t. the state X + auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); }; + Matrix expectedH = numericalDerivative11(f, X, 1e-6); + + // Compare + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From 5af25dc30f6c4fe846af49a71c4df89aa814eb1c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:33:05 -0400 Subject: [PATCH 18/54] Use nav version --- examples/LIEKF_NavstateExample.cpp | 2 +- examples/LIEKF_SE2_SimpleGPSExample.cpp | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 0747d9190..67ae23fd1 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index 1f0a01212..ecef67cf7 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -29,12 +29,10 @@ * Beard, and Joshua G. Mangelson * * @date Apr 25, 2025 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #include -#include +#include #include From 062bdf64ea680e3f13d53427a21df6e174ee4ead Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 10:40:28 -0400 Subject: [PATCH 19/54] Added Rot3 example to show state-dependent control --- examples/LIEKF_NavstateExample.cpp | 81 ++++++++++++---------------- examples/LIEKF_Rot3Example.cpp | 78 +++++++++++++++++++++++++++ gtsam/navigation/LIEKF.h | 22 ++++++++ gtsam/navigation/tests/testLIEKF.cpp | 36 ++++++------- 4 files changed, 150 insertions(+), 67 deletions(-) create mode 100644 examples/LIEKF_Rot3Example.cpp diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 67ae23fd1..250d5cda8 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,16 +11,15 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief Example of a Left-Invariant Extended Kalman Filter on NavState - * using IMU (predict) and GPS (update) measurements. + * @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) * @date April 25, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #include #include -#include #include +#include #include @@ -29,19 +28,14 @@ using namespace gtsam; /** * @brief Left-invariant dynamics for NavState. - * @param X Current state (unused for left-invariant error dynamics). - * @param imu 6×1 vector [a; ω]: linear accel (first 3) and angular vel (last - * 3). - * @param H Optional 9×9 Jacobian w.r.t. X (always zero here). + * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @return 9×1 tangent: [ω; 0₃; a]. */ -Vector9 dynamics(const NavState& X, const Vector6& imu, - OptionalJacobian<9, 9> H = {}) { +Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) { auto a = imu.head<3>(); auto w = imu.tail<3>(); Vector9 xi; xi << w, Vector3::Zero(), a; - if (H) *H = Matrix9::Zero(); return xi; } @@ -52,60 +46,51 @@ Vector9 dynamics(const NavState& X, const Vector6& imu, * @return 3×1 position vector. */ Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { - if (H) { - // H = [ 0₃×3, 0₃×3, R ] - *H << Z_3x3, Z_3x3, X.R(); - } + if (H) *H << Z_3x3, Z_3x3, X.R().matrix(); return X.t(); } int main() { - // Initial state, covariance, and time step - NavState X0; + // Initial state & covariances + NavState X0; // R=I, v=0, t=0 Matrix9 P0 = Matrix9::Identity() * 0.1; - double dt = 1.0; - - // Create the filter with the initial state and covariance. LIEKF ekf(X0, P0); - // Process & measurement noise + // Noise & timestep + double dt = 1.0; Matrix9 Q = Matrix9::Identity() * 0.01; Matrix3 R = Matrix3::Identity() * 0.5; - // Create the IMU measurements of the form (linear_acceleration, - // angular_velocity) - Vector6 imu1, imu2; - imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0; - imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0; + // 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; - // Create the GPS measurements of the form (px, py, pz) - Vector3 z1, z2; - z1 << 0.3, 0.0, 0.0; - z2 << 0.6, 0.0, 0.0; + // Two GPS fixes + Vector3 z1; + z1 << 0.3, 0, 0; + Vector3 z2; + z2 << 0.6, 0, 0; - cout << "=== Initialization ===\n" - << "X0: " << ekf.state() << "\n" - << "P0: " << ekf.covariance() << "\n\n"; - - ekf.predict(dynamics, imu1, dt, Q); - cout << "--- After first predict ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n\n"; + 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 first update ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n\n"; - - ekf.predict(dynamics, imu2, dt, Q); - cout << "--- After second predict ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n\n"; + 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 second update ---\n" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n"; + cout << "--- After update 2 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n"; return 0; } diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/LIEKF_Rot3Example.cpp new file mode 100644 index 000000000..331c01bb1 --- /dev/null +++ b/examples/LIEKF_Rot3Example.cpp @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * 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 LIEKF_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 +#include +#include +#include + +#include + + +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φ; + Vector3 φ = Rot3::Logmap(X, Dφ); + // zero out yaw + φ[2] = 0.0; + Dφ.row(2).setZero(); + + if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR + return -k * φ; // 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; + LIEKF 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; +} diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index 840d004a7..843365c6d 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -98,6 +98,28 @@ class LIEKF { /** * Predict step with state-dependent dynamics: + * xi = f(X, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) + * + * @param f dynamics functor depending on state and control + * @param dt time step + * @param Q process noise covariance + */ + template + void predict(Dynamics&& f, double dt, const Matrix& Q) { + typename G::Jacobian F; + auto xi = f(X_, F); + G U = G::Expmap(xi * dt); + auto A = U.inverse().AdjointMap() * F; + X_ = X_.compose(U); + P_ = A * P_ * A.transpose() + Q; + } + + /** + * Predict step with state and control input dynamics: * xi = f(X, u, F) * U = Expmap(xi * dt) * A = Ad_{U^{-1}} * F diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 81e80a69d..23f812274 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -21,35 +21,33 @@ using namespace gtsam; -// Duplicate the dynamics function under test: +// Duplicate the dynamics function in LIEKF_Rot3Example namespace example { -Vector9 dynamics(const NavState& X, const Vector6& imu, - OptionalJacobian<9, 9> H = {}) { - auto a = imu.head<3>(); - auto w = imu.tail<3>(); - Vector9 xi; - xi << w, Vector3::Zero(), a; - if (H) *H = Matrix9::Zero(); - return xi; +static constexpr double k = 0.5; +Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { + // φ = Logmap(R), Dφ = ∂φ/∂δR + Matrix3 Dφ; + Vector3 φ = Rot3::Logmap(X, Dφ); + // zero out yaw + φ[2] = 0.0; + Dφ.row(2).setZero(); + + if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR + return -k * φ; // xi ∈ 𝔰𝔬(3) } } // namespace example TEST(LIEKFNavState, dynamicsJacobian) { // Construct a nontrivial state and IMU input - NavState X(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, 2.0, 3.0), - Vector3(0.5, -0.5, 0.5)); - Vector6 imu; - imu << 0.1, -0.1, 0.2, // acceleration - 0.01, -0.02, 0.03; // angular velocity + Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); // Analytic Jacobian (always zero for left-invariant dynamics) - OptionalJacobian<9, 9> H_analytic; - example::dynamics(X, imu, H_analytic); - Matrix actualH = *H_analytic; + Matrix3 actualH; + example::dynamics(R, actualH); // Numeric Jacobian w.r.t. the state X - auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); }; - Matrix expectedH = numericalDerivative11(f, X, 1e-6); + auto f = [&](const Rot3& X_) { return example::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R, 1e-6); // Compare EXPECT(assert_equal(expectedH, actualH, 1e-8)); From 1ced4d0470862c5478afc11a0debc065482b793d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 13:35:24 -0400 Subject: [PATCH 20/54] Review comments --- examples/LIEKF_NavstateExample.cpp | 2 +- examples/LIEKF_Rot3Example.cpp | 12 ++++++------ examples/LIEKF_SE2_SimpleGPSExample.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 250d5cda8..9798dca02 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -31,7 +31,7 @@ using namespace gtsam; * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @return 9×1 tangent: [ω; 0₃; a]. */ -Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) { +Vector9 dynamics(const Vector6& imu) { auto a = imu.head<3>(); auto w = imu.tail<3>(); Vector9 xi; diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/LIEKF_Rot3Example.cpp index 331c01bb1..172af894d 100644 --- a/examples/LIEKF_Rot3Example.cpp +++ b/examples/LIEKF_Rot3Example.cpp @@ -32,14 +32,14 @@ using namespace gtsam; static constexpr double k = 0.5; Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR - Matrix3 Dφ; - Vector3 φ = Rot3::Logmap(X, Dφ); + Matrix3 D_phi; + Vector3 phi = Rot3::Logmap(X, D_phi); // zero out yaw - φ[2] = 0.0; - Dφ.row(2).setZero(); + phi[2] = 0.0; + D_phi.row(2).setZero(); - if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR - return -k * φ; // xi ∈ 𝔰𝔬(3) + if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR + return -k * phi; // xi ∈ 𝔰𝔬(3) } // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index ecef67cf7..0985d2372 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -58,7 +58,7 @@ int main() { // Define the process covariance and measurement covariance matrices Q and R. Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); - Matrix2 R = (Vector2(0.01, 0.01)).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. From a0c6902c8efbf5303095b6e7477e73a57a97b21d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 13:35:56 -0400 Subject: [PATCH 21/54] SFINAE to restrict template matching --- gtsam/navigation/LIEKF.h | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index 843365c6d..a8a136094 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -29,7 +29,7 @@ #include #include - +#include namespace gtsam { @@ -92,7 +92,7 @@ class LIEKF { * @note Use this if your dynamics does not depend on the state, e.g. * predict(f(u), dt, q) where u is a control input */ - void predict(const Vector& u, double dt, const Matrix& Q) { + void predict(const typename G::TangentVector& u, double dt, const Matrix& Q) { predict(G::Expmap(u * dt), Q); } @@ -108,12 +108,18 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template + template + // …and is callable as a true functor + && std::is_invocable_r_v&> > > void predict(Dynamics&& f, double dt, const Matrix& Q) { typename G::Jacobian F; - auto xi = f(X_, F); + typename G::TangentVector xi = f(X_, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + typename G::Jacobian A = U.inverse().AdjointMap() * F; X_ = X_.compose(U); P_ = A * P_ * A.transpose() + Q; } @@ -133,12 +139,19 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template + template & // H + > > > void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; - auto xi = f(X_, u, F); + typename G::TangentVector xi = f(X_, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + typename G::Jacobian A = U.inverse().AdjointMap() * F; X_ = X_.compose(U); P_ = A * P_ * A.transpose() + Q; } From 4f2a62aa3a45d113660b451921a8c09a5d8e4808 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 17:59:34 -0400 Subject: [PATCH 22/54] Update testLIEKF.cpp --- gtsam/navigation/tests/testLIEKF.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 23f812274..65325c63e 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -26,14 +26,14 @@ namespace example { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR - Matrix3 Dφ; - Vector3 φ = Rot3::Logmap(X, Dφ); + Matrix3 D_phi; + Vector3 phi = Rot3::Logmap(X, D_phi); // zero out yaw - φ[2] = 0.0; - Dφ.row(2).setZero(); + phi[2] = 0.0; + D_phi.row(2).setZero(); - if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR - return -k * φ; // xi ∈ 𝔰𝔬(3) + if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR + return -k * phi; // xi ∈ 𝔰𝔬(3) } } // namespace example From 104341f4cf84d3a845daf07658a1a01d214519f7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 20:42:17 -0400 Subject: [PATCH 23/54] well-tested predictMean --- gtsam/navigation/LIEKF.h | 51 ++++++++++++++++++++-------- gtsam/navigation/tests/testLIEKF.cpp | 42 ++++++++++++++++++----- 2 files changed, 71 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index a8a136094..fda75ccd5 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -96,6 +96,34 @@ class LIEKF { predict(G::Expmap(u * dt), Q); } + /** + * Predict mean only with state-dependent dynamics: + * xi = f(X, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) + * + * @param f dynamics functor depending on state and control + * @param dt time step + * @param Q process noise covariance + */ + template + // …and is callable as a true functor + && std::is_invocable_r_v>>> + G predictMean(Dynamics&& f, double dt, const Matrix& Q, + OptionalJacobian A = {}) const { + typename G::Jacobian F1, F2; + typename G::TangentVector xi = f(X_, F1); + G U = G::Expmap(xi * dt, F2); + if (A) *A = U.inverse().AdjointMap() + F2 * F1 * dt; + return X_.compose(U); + } + /** * Predict step with state-dependent dynamics: * xi = f(X, F) @@ -114,13 +142,10 @@ class LIEKF { !std::is_convertible_v // …and is callable as a true functor && std::is_invocable_r_v&> > > + const G&, OptionalJacobian>>> void predict(Dynamics&& f, double dt, const Matrix& Q) { - typename G::Jacobian F; - typename G::TangentVector xi = f(X_, F); - G U = G::Expmap(xi * dt); - typename G::Jacobian A = U.inverse().AdjointMap() * F; - X_ = X_.compose(U); + typename G::Jacobian A; + X_ = predictMean(f, dt, Q, &A); P_ = A * P_ * A.transpose() + Q; } @@ -145,15 +170,13 @@ class LIEKF { Dynamics, // functor const G&, // X const Control&, // u - OptionalJacobian& // H - > > > + OptionalJacobian // H + >>> void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - typename G::Jacobian F; - typename G::TangentVector xi = f(X_, u, F); - G U = G::Expmap(xi * dt); - typename G::Jacobian A = U.inverse().AdjointMap() * F; - X_ = X_.compose(U); - P_ = A * P_ * A.transpose() + Q; + auto g = [f, u](const G& X, OptionalJacobian F) { + return f(X, u, F); + }; + predict(g, dt, Q); } /** diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 65325c63e..9790833f8 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -17,12 +17,14 @@ #include #include #include +#include +#include #include using namespace gtsam; // Duplicate the dynamics function in LIEKF_Rot3Example -namespace example { +namespace exampleSO3 { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR @@ -35,22 +37,46 @@ Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR return -k * phi; // xi ∈ 𝔰𝔬(3) } -} // namespace example +} // namespace exampleSO3 -TEST(LIEKFNavState, dynamicsJacobian) { +TEST(IEKF, dynamicsJacobian) { // Construct a nontrivial state and IMU input Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); - // Analytic Jacobian (always zero for left-invariant dynamics) + // Analytic Jacobian Matrix3 actualH; - example::dynamics(R, actualH); + exampleSO3::dynamics(R, actualH); // Numeric Jacobian w.r.t. the state X - auto f = [&](const Rot3& X_) { return example::dynamics(X_); }; - Matrix3 expectedH = numericalDerivative11(f, R, 1e-6); + auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R); // Compare - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH)); +} + +TEST(IEKF, PredictNumericState) { + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + double dt = 0.1; + Matrix3 Q = Matrix3::Zero(); + + // Analytic Jacobian + Matrix3 actualH; + LIEKF iekf0(R0, P0); + iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + LIEKF iekf(R, P0); + return iekf.predictMean(exampleSO3::dynamics, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); } int main() { From 87c4445f53ee53f39b2aba7a226fdae32fd97bfd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:01:12 -0400 Subject: [PATCH 24/54] predictMean for state=control f --- gtsam/navigation/LIEKF.h | 83 ++++++++++++++++------------ gtsam/navigation/tests/testLIEKF.cpp | 30 ++++++++++ 2 files changed, 78 insertions(+), 35 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index fda75ccd5..9b4b6d2b5 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -96,53 +96,51 @@ class LIEKF { predict(G::Expmap(u * dt), Q); } + /** + * SFINAE check for correctly typed state-dependent dynamics function. + * xi = f(X, F) + * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) + */ + template + using enable_if_dynamics = std::enable_if_t< + !std::is_convertible_v && + std::is_invocable_r_v&>>; /** * Predict mean only with state-dependent dynamics: * xi = f(X, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * * @param f dynamics functor depending on state and control * @param dt time step * @param Q process noise covariance */ - template - // …and is callable as a true functor - && std::is_invocable_r_v>>> + template > G predictMean(Dynamics&& f, double dt, const Matrix& Q, OptionalJacobian A = {}) const { - typename G::Jacobian F1, F2; - typename G::TangentVector xi = f(X_, F1); - G U = G::Expmap(xi * dt, F2); - if (A) *A = U.inverse().AdjointMap() + F2 * F1 * dt; + typename G::Jacobian Df, Dexp; + typename G::TangentVector xi = f(X_, Df); + G U = G::Expmap(xi * dt, Dexp); + // derivative of mean = d[X.compose(U)]/dLog(X) + // = Ad_{U⁻¹} (the “left‐invariant” part) + // plus the effect of X→U via Expmap: + // Dexp * Df * dt (how U moves when X moves through f) + if (A) *A = U.inverse().AdjointMap() + Dexp * Df * dt; return X_.compose(U); } /** * Predict step with state-dependent dynamics: * xi = f(X, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * * @param f dynamics functor depending on state and control * @param dt time step * @param Q process noise covariance */ - template - // …and is callable as a true functor - && std::is_invocable_r_v>>> + template > void predict(Dynamics&& f, double dt, const Matrix& Q) { typename G::Jacobian A; X_ = predictMean(f, dt, Q, &A); @@ -150,10 +148,9 @@ class LIEKF { } /** - * Predict step with state and control input dynamics: + * Predict mean only with state and control input dynamics: * xi = f(X, u, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Control control input type * @tparam Dynamics signature: G f(const G&, const Control&, @@ -164,14 +161,30 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template // H - >>> + template + G predictMean(Dynamics&& f, const Control& u, double dt, const Matrix& Q, + OptionalJacobian A = {}) { + auto g = [f, u](const G& X, OptionalJacobian F) { + return f(X, u, F); + }; + return predictMean(g, dt, Q, A); + } + + /** + * Predict step with state and control input dynamics: + * xi = f(X, u, F) + * X_.expmap(xi * dt) + * + * @tparam Control control input type + * @tparam Dynamics signature: G f(const G&, const Control&, + * OptionalJacobian&) + * + * @param f dynamics functor depending on state and control + * @param u control input + * @param dt time step + * @param Q process noise covariance + */ + template void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { auto g = [f, u](const G& X, OptionalJacobian F) { return f(X, u, F); diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 9790833f8..5577a4357 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -79,6 +79,36 @@ TEST(IEKF, PredictNumericState) { EXPECT(assert_equal(expectedH, actualH)); } +TEST(IEKF, StateAndControl) { + auto f = [](const Rot3& X, const Vector2& dummy_u, + OptionalJacobian<3, 3> H = {}) { + return exampleSO3::dynamics(X, H); + }; + + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + Vector2 dummy_u(1, 2); + double dt = 0.1; + Matrix3 Q = Matrix3::Zero(); + + // Analytic Jacobian + Matrix3 actualH; + LIEKF iekf0(R0, P0); + iekf0.predictMean(f, dummy_u, dt, Q, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + LIEKF iekf(R, P0); + return iekf.predictMean(f, dummy_u, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 5bc01081ee1bf4e4b28683c85a7ec7fb7c8bc38a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:03:34 -0400 Subject: [PATCH 25/54] Correct jacobian for h_gps --- examples/LIEKF_NavstateExample.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 9798dca02..a32d367b2 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -46,8 +46,7 @@ Vector9 dynamics(const Vector6& imu) { * @return 3×1 position vector. */ Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { - if (H) *H << Z_3x3, Z_3x3, X.R().matrix(); - return X.t(); + return X.position(H); } int main() { From ab605770fb0c774e6494504ae16133fb0cdb9e57 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:11:11 -0400 Subject: [PATCH 26/54] Rename --- examples/{LIEKF_NavstateExample.cpp => IEKF_NavstateExample.cpp} | 0 examples/{LIEKF_Rot3Example.cpp => IEKF_Rot3Example.cpp} | 0 examples/{LIEKF_SE2_SimpleGPSExample.cpp => IEKF_SE2Example.cpp} | 0 gtsam/navigation/{LIEKF.h => InvariantEKF.h} | 0 gtsam/navigation/tests/{testLIEKF.cpp => testInvariantEKF.cpp} | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename examples/{LIEKF_NavstateExample.cpp => IEKF_NavstateExample.cpp} (100%) rename examples/{LIEKF_Rot3Example.cpp => IEKF_Rot3Example.cpp} (100%) rename examples/{LIEKF_SE2_SimpleGPSExample.cpp => IEKF_SE2Example.cpp} (100%) rename gtsam/navigation/{LIEKF.h => InvariantEKF.h} (100%) rename gtsam/navigation/tests/{testLIEKF.cpp => testInvariantEKF.cpp} (100%) diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/IEKF_NavstateExample.cpp similarity index 100% rename from examples/LIEKF_NavstateExample.cpp rename to examples/IEKF_NavstateExample.cpp diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/IEKF_Rot3Example.cpp similarity index 100% rename from examples/LIEKF_Rot3Example.cpp rename to examples/IEKF_Rot3Example.cpp diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/IEKF_SE2Example.cpp similarity index 100% rename from examples/LIEKF_SE2_SimpleGPSExample.cpp rename to examples/IEKF_SE2Example.cpp diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/InvariantEKF.h similarity index 100% rename from gtsam/navigation/LIEKF.h rename to gtsam/navigation/InvariantEKF.h diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp similarity index 100% rename from gtsam/navigation/tests/testLIEKF.cpp rename to gtsam/navigation/tests/testInvariantEKF.cpp From 51e89d298e3e94cac02fa7804be20b05fc40c02a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:11:30 -0400 Subject: [PATCH 27/54] Fix compilation --- examples/IEKF_NavstateExample.cpp | 8 +++---- examples/IEKF_Rot3Example.cpp | 9 ++++---- examples/IEKF_SE2Example.cpp | 25 +++++++++++---------- gtsam/navigation/InvariantEKF.h | 16 ++++++------- gtsam/navigation/tests/testInvariantEKF.cpp | 16 ++++++------- 5 files changed, 37 insertions(+), 37 deletions(-) diff --git a/examples/IEKF_NavstateExample.cpp b/examples/IEKF_NavstateExample.cpp index a32d367b2..ef3ef204b 100644 --- a/examples/IEKF_NavstateExample.cpp +++ b/examples/IEKF_NavstateExample.cpp @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF_NavstateExample.cpp - * @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) + * @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 #include -#include +#include #include #include @@ -53,7 +53,7 @@ int main() { // Initial state & covariances NavState X0; // R=I, v=0, t=0 Matrix9 P0 = Matrix9::Identity() * 0.1; - LIEKF ekf(X0, P0); + InvariantEKF ekf(X0, P0); // Noise & timestep double dt = 1.0; diff --git a/examples/IEKF_Rot3Example.cpp b/examples/IEKF_Rot3Example.cpp index 172af894d..88989c67e 100644 --- a/examples/IEKF_Rot3Example.cpp +++ b/examples/IEKF_Rot3Example.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF_Rot3Example.cpp + * @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 @@ -20,11 +20,10 @@ #include #include #include -#include +#include #include - using namespace std; using namespace gtsam; @@ -39,7 +38,7 @@ Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) { D_phi.row(2).setZero(); if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR - return -k * phi; // xi ∈ 𝔰𝔬(3) + return -k * phi; // xi ∈ 𝔰𝔬(3) } // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- @@ -54,7 +53,7 @@ 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; - LIEKF ekf(R0, P0); + InvariantEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/examples/IEKF_SE2Example.cpp b/examples/IEKF_SE2Example.cpp index 0985d2372..1a7772c7c 100644 --- a/examples/IEKF_SE2Example.cpp +++ b/examples/IEKF_SE2Example.cpp @@ -10,17 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF_SE2_SimpleGPSExample.cpp + * @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 LIEKF class to estimate the state of - * an object using odometry / GPS measurements The prediction stage of the LIEKF - * uses a Lie Group element to propagate the stage in a discrete LIEKF. 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 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 @@ -32,7 +32,7 @@ * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #include -#include +#include #include @@ -45,8 +45,9 @@ using namespace gtsam; Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) { return X.translation(H); } -// Define a LIEKF class that uses the Pose2 Lie group as the state and the -// Vector2 measurement type. + +// 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); @@ -54,7 +55,7 @@ int main() { // Create the filter with the initial state, covariance, and measurement // function. - LIEKF ekf(X0, P0); + InvariantEKF ekf(X0, P0); // Define the process covariance and measurement covariance matrices Q and R. Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 9b4b6d2b5..443f730c4 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF.h - * @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation + * @file InvariantEKF.h + * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) implementation * - * This file defines the LIEKF class template for performing prediction and + * This file defines the InvariantEKF class template for performing prediction and * update steps of an Extended Kalman Filter on states residing in a Lie group. * The class supports state evolution via group composition and dynamics * functions, along with measurement updates using tangent-space corrections. @@ -34,8 +34,8 @@ namespace gtsam { /** - * @class LIEKF - * @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G + * @class InvariantEKF + * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) on a Lie group G * * @tparam G Lie group type providing: * - static int dimension = tangent dimension @@ -50,7 +50,7 @@ namespace gtsam { * using the left-invariant error in the tangent space. */ template -class LIEKF { +class InvariantEKF { public: /// Tangent-space dimension static constexpr int n = traits::dimension; @@ -59,7 +59,7 @@ class LIEKF { using MatrixN = Eigen::Matrix; /// Constructor: initialize with state and covariance - LIEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} + InvariantEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} /// @return current state estimate const G& state() const { return X_; } @@ -229,6 +229,6 @@ class LIEKF { // Define static identity I_n template -const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); +const typename InvariantEKF::MatrixN InvariantEKF::I_n = InvariantEKF::MatrixN::Identity(); } // namespace gtsam diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 5577a4357..c3f93d429 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -8,8 +8,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testLIEKFNavState.cpp - * @brief Unit test for the NavState dynamics Jacobian in LIEKF example. + * @file testInvariantEKFNavState.cpp + * @brief Unit test for the NavState dynamics Jacobian in InvariantEKF example. * @date April 26, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -18,12 +18,12 @@ #include #include #include -#include +#include #include using namespace gtsam; -// Duplicate the dynamics function in LIEKF_Rot3Example +// Duplicate the dynamics function in InvariantEKF_Rot3Example namespace exampleSO3 { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { @@ -64,12 +64,12 @@ TEST(IEKF, PredictNumericState) { // Analytic Jacobian Matrix3 actualH; - LIEKF iekf0(R0, P0); + InvariantEKF iekf0(R0, P0); iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); // wrap predict into a state->state functor (mapping on SO(3)) auto g = [&](const Rot3& R) -> Rot3 { - LIEKF iekf(R, P0); + InvariantEKF iekf(R, P0); return iekf.predictMean(exampleSO3::dynamics, dt, Q); }; @@ -94,12 +94,12 @@ TEST(IEKF, StateAndControl) { // Analytic Jacobian Matrix3 actualH; - LIEKF iekf0(R0, P0); + InvariantEKF iekf0(R0, P0); iekf0.predictMean(f, dummy_u, dt, Q, actualH); // wrap predict into a state->state functor (mapping on SO(3)) auto g = [&](const Rot3& R) -> Rot3 { - LIEKF iekf(R, P0); + InvariantEKF iekf(R, P0); return iekf.predictMean(f, dummy_u, dt, Q); }; From 4cc2e22b59515767f79d41934c99749116521fa0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Apr 2025 00:20:04 -0400 Subject: [PATCH 28/54] Add value to concepts --- gtsam/base/Lie.h | 6 +++++- gtsam/base/Manifold.h | 5 ++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 5b3173b37..4248f16b2 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -277,6 +277,10 @@ inline Class expmap_default(const Class& t, const Vector& d) { template class IsLieGroup: public IsGroup, public IsManifold { public: + // Concept marker: allows checking IsLieGroup::value in templates + static constexpr bool value = + std::is_base_of::structure_category>::value; + typedef typename traits::structure_category structure_category_tag; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; @@ -284,7 +288,7 @@ public: GTSAM_CONCEPT_USAGE(IsLieGroup) { static_assert( - (std::is_base_of::value), + value, "This type's trait does not assert it is a Lie group (or derived)"); // group operations with Jacobians diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 80a9cd166..18ae9b616 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -138,10 +138,13 @@ public: static const int dim = traits::dimension; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; + // Concept marker: allows checking IsManifold::value in templates + static constexpr bool value = + std::is_base_of::value; GTSAM_CONCEPT_USAGE(IsManifold) { static_assert( - (std::is_base_of::value), + value, "This type's structure_category trait does not assert it as a manifold (or derived)"); static_assert(TangentVector::SizeAtCompileTime == dim); From 3bcc5563eb6b1f639ba3031096023ce07ab90d94 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Apr 2025 00:20:41 -0400 Subject: [PATCH 29/54] EKF hierarchy --- ...F_Rot3Example.cpp => GEKF_Rot3Example.cpp} | 4 +- gtsam/navigation/GroupEKF.h | 196 +++++++++++++ gtsam/navigation/InvariantEKF.h | 267 ++++-------------- gtsam/navigation/ManifoldEKF.h | 155 ++++++++++ gtsam/navigation/tests/testGroupEKF.cpp | 114 ++++++++ gtsam/navigation/tests/testInvariantEKF.cpp | 172 +++++------ gtsam/navigation/tests/testManifoldEKF.cpp | 154 ++++++++++ 7 files changed, 771 insertions(+), 291 deletions(-) rename examples/{IEKF_Rot3Example.cpp => GEKF_Rot3Example.cpp} (93%) create mode 100644 gtsam/navigation/GroupEKF.h create mode 100644 gtsam/navigation/ManifoldEKF.h create mode 100644 gtsam/navigation/tests/testGroupEKF.cpp create mode 100644 gtsam/navigation/tests/testManifoldEKF.cpp diff --git a/examples/IEKF_Rot3Example.cpp b/examples/GEKF_Rot3Example.cpp similarity index 93% rename from examples/IEKF_Rot3Example.cpp rename to examples/GEKF_Rot3Example.cpp index 88989c67e..d931b7909 100644 --- a/examples/IEKF_Rot3Example.cpp +++ b/examples/GEKF_Rot3Example.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include @@ -53,7 +53,7 @@ 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; - InvariantEKF ekf(R0, P0); + GroupEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/gtsam/navigation/GroupEKF.h b/gtsam/navigation/GroupEKF.h new file mode 100644 index 000000000..f2f2990e6 --- /dev/null +++ b/gtsam/navigation/GroupEKF.h @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------------- + + * 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 GroupEKF.h + * @brief Extended Kalman Filter derived class for Lie groups G. + * + * This file defines the GroupEKF class template, inheriting from ManifoldEKF, + * for performing EKF steps specifically on states residing in a Lie group. + * It provides predict methods utilizing group composition, tangent space + * controls (via exponential map), and state-dependent dynamics functions. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#pragma once + +#include // Include the base class +#include // Include for Lie group traits and operations + +#include +#include +#include // For std::function + +namespace gtsam { + + /** + * @class GroupEKF + * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF + * + * @tparam G Lie group type providing group operations and Expmap/AdjointMap. + * Must satisfy LieGroup concept (`gtsam::IsLieGroup::value`). + * + * This filter specializes ManifoldEKF for Lie groups, offering convenient + * prediction methods based on group composition or dynamics functions defining + * motion in the tangent space. + */ + template + class GroupEKF : public ManifoldEKF { + public: + using Base = ManifoldEKF; ///< Base class type + static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) + using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G + using MatrixN = typename Base::MatrixN; ///< Square matrix of size n for covariance and Jacobians + using Jacobian = Eigen::Matrix; ///< Jacobian matrix type specific to the group G + + /// Constructor: initialize with state and covariance + GroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) { + static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); + } + + /** + * Predict step via group composition (Left-Invariant): + * X_{k+1} = X_k * U + * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q + * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to + * F = Ad_{U^{-1}} in the base class predict method. + * + * @param U Lie group element representing the motion increment. + * @param Q Process noise covariance in the tangent space (size nxn). + */ + void predict(const G& U, const Matrix& Q) { + G X_next = this->X_.compose(U); + // TODO(dellaert): traits::AdjointMap should exist + Jacobian A = traits::Inverse(U).AdjointMap(); // A = Adjoint(U.inverse()) + Base::predict(X_next, A, Q); // Call base class predict + } + + /** + * Predict step via tangent control vector: + * U = Expmap(u * dt) + * Then calls predict(U, Q). + * + * @param u Tangent space control vector. + * @param dt Time interval. + * @param Q Process noise covariance matrix (size nxn). + */ + void predict(const TangentVector& u, double dt, const Matrix& Q) { + G U = traits::Expmap(u * dt); + predict(U, Q); // Call the group composition predict + } + + /** + * SFINAE check for correctly typed state-dependent dynamics function. + * Signature: TangentVector f(const G& X, OptionalJacobian Df) + * Df = d(xi)/d(local(X)) + */ + template + using enable_if_dynamics = std::enable_if_t< + !std::is_convertible_v&& + std::is_invocable_r_v&>>; + + /** + * Predict mean and Jacobian A with state-dependent dynamics: + * xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df) + * U = Expmap(xi * dt, Dexp) (Compute motion increment U and Expmap Jacobian Dexp) + * X_{k+1} = X_k * U (Predict next state) + * F = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) + * + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @param f Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X). + * @param dt Time step. + * @param A Optional pointer to store the computed state transition Jacobian A. + * @return Predicted state X_{k+1}. + */ + template > + G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { + Jacobian Df, Dexp; + TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) + G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt + G X_next = this->X_.compose(U); + + if (A) { + // Full state transition Jacobian for left-invariant EKF: + *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + } + return X_next; + } + + + /** + * Predict step with state-dependent dynamics: + * Uses predictMean to compute X_{k+1} and F, then calls base predict. + * X_{k+1}, F = predictMean(f, dt) + * P_{k+1} = F P_k F^T + Q + * + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @param f Dynamics functor. + * @param dt Time step. + * @param Q Process noise covariance (size nxn). + */ + template > + void predict(Dynamics&& f, double dt, const Matrix& Q) { + Jacobian A; + G X_next = predictMean(std::forward(f), dt, A); + Base::predict(X_next, A, Q); // Call base class predict + } + + /** + * SFINAE check for state- and control-dependent dynamics function. + * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) + */ + template + using enable_if_full_dynamics = std::enable_if_t< + std::is_invocable_r_v&> + >; + + /** + * Predict mean and Jacobian A with state and control input dynamics: + * Wraps the dynamics function and calls the state-only predictMean. + * xi = f(X_k, u, Df) + * + * @tparam Control Control input type. + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) + * @param f Dynamics functor. + * @param u Control input. + * @param dt Time step. + * @param A Optional pointer to store the computed state transition Jacobian A. + * @return Predicted state X_{k+1}. + */ + template > + G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { + return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); + } + + + /** + * Predict step with state and control input dynamics: + * Wraps the dynamics function and calls the state-only predict. + * xi = f(X_k, u, Df) + * + * @tparam Control Control input type. + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) + * @param f Dynamics functor. + * @param u Control input. + * @param dt Time step. + * @param Q Process noise covariance (size nxn). + */ + template > + void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { + return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); + } + + }; // GroupEKF + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 443f730c4..1d39981d7 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -9,226 +9,77 @@ * -------------------------------------------------------------------------- */ -/** - * @file InvariantEKF.h - * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) implementation - * - * This file defines the InvariantEKF class template for performing prediction and - * update steps of an Extended Kalman Filter on states residing in a Lie group. - * The class supports state evolution via group composition and dynamics - * functions, along with measurement updates using tangent-space corrections. - * - * @date April 24, 2025 - * @authors Scott Baker, Matt Kielo, Frank Dellaert - */ + /** + * @file InvariantEKF.h + * @brief Left-Invariant Extended Kalman Filter implementation. + * + * This file defines the InvariantEKF class template, inheriting from GroupEKF, + * which specifically implements the Left-Invariant EKF formulation. It restricts + * prediction methods to only those based on group composition (state-independent + * motion models), hiding the state-dependent prediction variants from GroupEKF. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ #pragma once -#include -#include -#include - -#include -#include +#include // Include the base class namespace gtsam { -/** - * @class InvariantEKF - * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) on a Lie group G - * - * @tparam G Lie group type providing: - * - static int dimension = tangent dimension - * - using TangentVector = Eigen::Vector... - * - using Jacobian = Eigen::Matrix... - * - methods: Expmap(), expmap(), compose(), inverse().AdjointMap() - * - * This filter maintains a state X in the group G and covariance P in the - * tangent space. Prediction steps are performed via group composition or a - * user-supplied dynamics function. Updates apply a measurement function h - * returning both predicted measurement and its Jacobian H, and correct state - * using the left-invariant error in the tangent space. - */ -template -class InvariantEKF { - public: - /// Tangent-space dimension - static constexpr int n = traits::dimension; - - /// Square matrix of size n for covariance and Jacobians - using MatrixN = Eigen::Matrix; - - /// Constructor: initialize with state and covariance - InvariantEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} - - /// @return current state estimate - const G& state() const { return X_; } - - /// @return current covariance estimate - const Matrix& covariance() const { return P_; } - /** - * Predict step via group composition: - * X_{k+1} = X_k * U - * P_{k+1} = A P_k A^T + Q - * where A = Ad_{U^{-1}}. i.e., d(X.compose(U))/dX evaluated at X_k. + * @class InvariantEKF + * @brief Left-Invariant Extended Kalman Filter on a Lie group G. * - * @param U Lie group increment (e.g., Expmap of control * dt) - * @param Q process noise covariance in tangent space + * @tparam G Lie group type (must satisfy LieGroup concept). + * + * This filter inherits from GroupEKF but restricts the prediction interface + * to only the left-invariant prediction methods: + * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)` + * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)` + * + * The state-dependent prediction methods from GroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/GroupEKF. */ - void predict(const G& U, const Matrix& Q) { - typename G::Jacobian A; - X_ = X_.compose(U, A); - P_ = A * P_ * A.transpose() + Q; - } + template + class InvariantEKF : public GroupEKF { + public: + using Base = GroupEKF; ///< Base class type + using TangentVector = typename Base::TangentVector; ///< Tangent vector type + using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc. - /** - * Predict step via tangent control vector: - * U = Expmap(u * dt) - * @param u tangent control vector - * @param dt Time interval - * @param Q Process noise covariance matrix. - * - * @note Use this if your dynamics does not depend on the state, e.g. - * predict(f(u), dt, q) where u is a control input - */ - void predict(const typename G::TangentVector& u, double dt, const Matrix& Q) { - predict(G::Expmap(u * dt), Q); - } + /// Constructor: forwards to GroupEKF constructor + InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} - /** - * SFINAE check for correctly typed state-dependent dynamics function. - * xi = f(X, F) - * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - */ - template - using enable_if_dynamics = std::enable_if_t< - !std::is_convertible_v && - std::is_invocable_r_v&>>; - /** - * Predict mean only with state-dependent dynamics: - * xi = f(X, F) - * X_.expmap(xi * dt) - * - * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * @param f dynamics functor depending on state and control - * @param dt time step - * @param Q process noise covariance - */ - template > - G predictMean(Dynamics&& f, double dt, const Matrix& Q, - OptionalJacobian A = {}) const { - typename G::Jacobian Df, Dexp; - typename G::TangentVector xi = f(X_, Df); - G U = G::Expmap(xi * dt, Dexp); - // derivative of mean = d[X.compose(U)]/dLog(X) - // = Ad_{U⁻¹} (the “left‐invariant” part) - // plus the effect of X→U via Expmap: - // Dexp * Df * dt (how U moves when X moves through f) - if (A) *A = U.inverse().AdjointMap() + Dexp * Df * dt; - return X_.compose(U); - } + // --- Expose only the Invariant Prediction Methods --- - /** - * Predict step with state-dependent dynamics: - * xi = f(X, F) - * X_.expmap(xi * dt) - * - * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * @param f dynamics functor depending on state and control - * @param dt time step - * @param Q process noise covariance - */ - template > - void predict(Dynamics&& f, double dt, const Matrix& Q) { - typename G::Jacobian A; - X_ = predictMean(f, dt, Q, &A); - P_ = A * P_ * A.transpose() + Q; - } + /** + * Predict step via group composition (Left-Invariant): + * X_{k+1} = X_k * U + * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q + * Calls the base class implementation. + * + * @param U Lie group element representing the motion increment. + * @param Q Process noise covariance in the tangent space (size nxn). + */ + void predict(const G& U, const Matrix& Q) { + Base::predict(U, Q); + } - /** - * Predict mean only with state and control input dynamics: - * xi = f(X, u, F) - * X_.expmap(xi * dt) - * - * @tparam Control control input type - * @tparam Dynamics signature: G f(const G&, const Control&, - * OptionalJacobian&) - * - * @param f dynamics functor depending on state and control - * @param u control input - * @param dt time step - * @param Q process noise covariance - */ - template - G predictMean(Dynamics&& f, const Control& u, double dt, const Matrix& Q, - OptionalJacobian A = {}) { - auto g = [f, u](const G& X, OptionalJacobian F) { - return f(X, u, F); - }; - return predictMean(g, dt, Q, A); - } + /** + * Predict step via tangent control vector: + * U = Expmap(u * dt) + * Then calls predict(U, Q). Calls the base class implementation. + * + * @param u Tangent space control vector. + * @param dt Time interval. + * @param Q Process noise covariance matrix (size nxn). + */ + void predict(const TangentVector& u, double dt, const Matrix& Q) { + Base::predict(u, dt, Q); + } - /** - * Predict step with state and control input dynamics: - * xi = f(X, u, F) - * X_.expmap(xi * dt) - * - * @tparam Control control input type - * @tparam Dynamics signature: G f(const G&, const Control&, - * OptionalJacobian&) - * - * @param f dynamics functor depending on state and control - * @param u control input - * @param dt time step - * @param Q process noise covariance - */ - template - void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - auto g = [f, u](const G& X, OptionalJacobian F) { - return f(X, u, F); - }; - predict(g, dt, Q); - } + }; // InvariantEKF - /** - * Measurement update: - * z_pred, H = h(X) - * K = P H^T (H P H^T + R)^{-1} - * X <- Expmap(-K (z_pred - z)) * X - * P <- (I - K H) P - * - * @tparam Measurement measurement type (e.g., Vector) - * @tparam Prediction functor signature: Measurement h(const G&, - * OptionalJacobian&) - * - * @param h measurement model returning predicted z and Jacobian H - * @param z observed measurement - * @param R measurement noise covariance - */ - template - void update(Prediction&& h, const Measurement& z, const Matrix& R) { - Eigen::Matrix::dimension, n> H; - auto z_pred = h(X_, H); - auto y = z_pred - z; - Matrix S = H * P_ * H.transpose() + R; - Matrix K = P_ * H.transpose() * S.inverse(); - X_ = X_.expmap(-K * y); - P_ = (I_n - K * H) * P_; - } - - protected: - G X_; ///< group state estimate - Matrix P_; ///< covariance in tangent space - - private: - /// Identity matrix of size n - static const MatrixN I_n; -}; - -// Define static identity I_n -template -const typename InvariantEKF::MatrixN InvariantEKF::I_n = InvariantEKF::MatrixN::Identity(); - -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h new file mode 100644 index 000000000..7951b88cb --- /dev/null +++ b/gtsam/navigation/ManifoldEKF.h @@ -0,0 +1,155 @@ +/* ---------------------------------------------------------------------------- + + * 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 ManifoldEKF.h + * @brief Extended Kalman Filter base class on a generic manifold M + * + * This file defines the ManifoldEKF class template for performing prediction + * and update steps of an Extended Kalman Filter on states residing in a + * differentiable manifold. It relies on the manifold's retract and + * localCoordinates operations. + * + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include // Include for traits + +#include +#include + +namespace gtsam { + + /** + * @class ManifoldEKF + * @brief Extended Kalman Filter on a generic manifold M + * + * @tparam M Manifold type providing: + * - static int dimension = tangent dimension + * - using TangentVector = Eigen::Vector... + * - A `retract(const TangentVector&)` method (member or static) + * - A `localCoordinates(const M&)` method (member or static) + * - `gtsam::traits` specialization must exist. + * + * This filter maintains a state X in the manifold M and covariance P in the + * tangent space at X. Prediction requires providing the predicted next state + * and the state transition Jacobian F. Updates apply a measurement function h + * and correct the state using the tangent space error. + */ + template + class ManifoldEKF { + public: + /// Manifold dimension (tangent space dimension) + static constexpr int n = traits::dimension; + + /// Tangent vector type for the manifold M + using TangentVector = typename traits::TangentVector; + + /// Square matrix of size n for covariance and Jacobians + using MatrixN = Eigen::Matrix; + + /// Constructor: initialize with state and covariance + ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) { + static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold"); + } + + virtual ~ManifoldEKF() = default; // Add virtual destructor for base class + + /// @return current state estimate + const M& state() const { return X_; } + + /// @return current covariance estimate + const MatrixN& covariance() const { return P_; } + + /** + * Basic predict step: Updates state and covariance given the predicted + * next state and the state transition Jacobian F. + * X_{k+1} = X_next + * P_{k+1} = F P_k F^T + Q + * where F = d(local(X_{k+1})) / d(local(X_k)) is the Jacobian of the + * state transition in local coordinates around X_k. + * + * @param X_next The predicted state at time k+1. + * @param F The state transition Jacobian (size nxn). + * @param Q Process noise covariance matrix in the tangent space (size nxn). + */ + void predict(const M& X_next, const MatrixN& F, const Matrix& Q) { + X_ = X_next; + P_ = F * P_ * F.transpose() + Q; + } + + /** + * Measurement update: Corrects the state and covariance using a measurement. + * z_pred, H = h(X) + * y = z - z_pred (innovation, or z_pred - z depending on convention) + * S = H P H^T + R (innovation covariance) + * K = P H^T S^{-1} (Kalman gain) + * delta_xi = -K * y (correction in tangent space) + * X <- X.retract(delta_xi) + * P <- (I - K H) P + * + * @tparam Measurement Type of the measurement vector (e.g., VectorN) + * @tparam Prediction Functor signature: Measurement h(const M&, + * OptionalJacobian&) + * where m is the measurement dimension. + * + * @param h Measurement model functor returning predicted measurement z_pred + * and its Jacobian H = d(h)/d(local(X)). + * @param z Observed measurement. + * @param R Measurement noise covariance (size m x m). + */ + template + void update(Prediction&& h, const Measurement& z, const Matrix& R) { + constexpr int m = traits::dimension; + Eigen::Matrix H; + + // Predict measurement and get Jacobian H = dh/dlocal(X) + Measurement z_pred = h(X_, H); + + // Innovation + // Ensure consistent subtraction for manifold types if Measurement is one + Vector innovation = traits::Local(z, z_pred); // y = z_pred (-) z (in tangent space) + + // Innovation covariance and Kalman Gain + auto S = H * P_ * H.transpose() + R; + Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) + + // Correction vector in tangent space + TangentVector delta_xi = -K * innovation; // delta_xi = - K * y + + // Update state using retract + X_ = traits::Retract(X_, delta_xi); // X <- X.retract(delta_xi) + + // Update covariance using Joseph form: + MatrixN I_KH = I_n - K * H; + P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + } + + protected: + M X_; ///< manifold state estimate + MatrixN P_; ///< covariance in tangent space at X_ + + private: + /// Identity matrix of size n + static const MatrixN I_n; + }; + + // Define static identity I_n + template + const typename ManifoldEKF::MatrixN ManifoldEKF::I_n = ManifoldEKF::MatrixN::Identity(); + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testGroupEKF.cpp b/gtsam/navigation/tests/testGroupEKF.cpp new file mode 100644 index 000000000..5ce7c477b --- /dev/null +++ b/gtsam/navigation/tests/testGroupEKF.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + * 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 testGroupEKF.cpp + * @brief Unit test for GroupEKF, as well as dynamics used in Rot3 example. + * @date April 26, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +// Duplicate the dynamics function in GroupEKF_Rot3Example +namespace exampleSO3 { + static constexpr double k = 0.5; + Vector3 dynamics(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) + } +} // namespace exampleSO3 + +TEST(GroupeEKF, DynamicsJacobian) { + // Construct a nontrivial state and IMU input + Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); + + // Analytic Jacobian + Matrix3 actualH; + exampleSO3::dynamics(R, actualH); + + // Numeric Jacobian w.r.t. the state X + auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R); + + // Compare + EXPECT(assert_equal(expectedH, actualH)); +} + +TEST(GroupeEKF, PredictNumericState) { + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + double dt = 0.1; + + // Analytic Jacobian + Matrix3 actualH; + GroupEKF ekf0(R0, P0); + ekf0.predictMean(exampleSO3::dynamics, dt, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + GroupEKF ekf(R, P0); + return ekf.predictMean(exampleSO3::dynamics, dt); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); +} + +TEST(GroupeEKF, StateAndControl) { + auto f = [](const Rot3& X, const Vector2& dummy_u, + OptionalJacobian<3, 3> H = {}) { + return exampleSO3::dynamics(X, H); + }; + + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + Vector2 dummy_u(1, 2); + double dt = 0.1; + Matrix3 Q = Matrix3::Zero(); + + // Analytic Jacobian + Matrix3 actualH; + GroupEKF ekf0(R0, P0); + ekf0.predictMean(f, dummy_u, dt, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + GroupEKF ekf(R, P0); + return ekf.predictMean(f, dummy_u, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index c3f93d429..74a70fbbe 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -7,109 +7,119 @@ * See LICENSE for the license information * -------------------------------------------------------------------------- */ -/** - * @file testInvariantEKFNavState.cpp - * @brief Unit test for the NavState dynamics Jacobian in InvariantEKF example. - * @date April 26, 2025 - * @authors Scott Baker, Matt Kielo, Frank Dellaert - */ + /** + * @file testInvariantEKF_Pose2.cpp + * @brief Unit test for the InvariantEKF using Pose2 state. + * Based on the logic from IEKF_SE2Example.cpp + * @date April 26, 2025 + * @authors Frank Dellaert (adapted from example by Scott Baker, Matt Kielo) + */ #include #include #include -#include +#include #include -#include +#include + +using namespace std; using namespace gtsam; -// Duplicate the dynamics function in InvariantEKF_Rot3Example -namespace exampleSO3 { -static constexpr double k = 0.5; -Vector3 dynamics(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) -} -} // namespace exampleSO3 - -TEST(IEKF, dynamicsJacobian) { - // Construct a nontrivial state and IMU input - Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); - - // Analytic Jacobian - Matrix3 actualH; - exampleSO3::dynamics(R, actualH); - - // Numeric Jacobian w.r.t. the state X - auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; - Matrix3 expectedH = numericalDerivative11(f, R); - - // Compare - EXPECT(assert_equal(expectedH, actualH)); +// 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. +// (Copied from IEKF_SE2Example.cpp) +Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) { + return X.translation(H); } -TEST(IEKF, PredictNumericState) { - // GIVEN - Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); - Matrix3 P0 = Matrix3::Identity() * 0.2; - double dt = 0.1; - Matrix3 Q = Matrix3::Zero(); - // Analytic Jacobian - Matrix3 actualH; - InvariantEKF iekf0(R0, P0); - iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); +TEST(IEKF_Pose2, PredictUpdateSequence) { + // GIVEN: Initial state, covariance, noises, motions, measurements + // (from IEKF_SE2Example.cpp) + Pose2 X0(0.0, 0.0, 0.0); + Matrix3 P0 = Matrix3::Identity() * 0.1; - // wrap predict into a state->state functor (mapping on SO(3)) - auto g = [&](const Rot3& R) -> Rot3 { - InvariantEKF iekf(R, P0); - return iekf.predictMean(exampleSO3::dynamics, dt, Q); - }; + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = I_2x2 * 0.01; - // numeric Jacobian of g at R0 - Matrix3 expectedH = numericalDerivative11(g, R0); + Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0); - EXPECT(assert_equal(expectedH, actualH)); -} + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; -TEST(IEKF, StateAndControl) { - auto f = [](const Rot3& X, const Vector2& dummy_u, - OptionalJacobian<3, 3> H = {}) { - return exampleSO3::dynamics(X, H); - }; + // Create the filter + InvariantEKF ekf(X0, P0); + EXPECT(assert_equal(X0, ekf.state())); + EXPECT(assert_equal(P0, ekf.covariance())); - // GIVEN - Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); - Matrix3 P0 = Matrix3::Identity() * 0.2; - Vector2 dummy_u(1, 2); - double dt = 0.1; - Matrix3 Q = Matrix3::Zero(); + // --- First Prediction --- + ekf.predict(U1, Q); - // Analytic Jacobian - Matrix3 actualH; - InvariantEKF iekf0(R0, P0); - iekf0.predictMean(f, dummy_u, dt, Q, actualH); + // Calculate expected state and covariance + Pose2 X1_expected = X0.compose(U1); + Matrix3 Ad_U1_inv = U1.inverse().AdjointMap(); + Matrix3 P1_expected = Ad_U1_inv * P0 * Ad_U1_inv.transpose() + Q; - // wrap predict into a state->state functor (mapping on SO(3)) - auto g = [&](const Rot3& R) -> Rot3 { - InvariantEKF iekf(R, P0); - return iekf.predictMean(f, dummy_u, dt, Q); - }; + // Verify + EXPECT(assert_equal(X1_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P1_expected, ekf.covariance(), 1e-9)); - // numeric Jacobian of g at R0 - Matrix3 expectedH = numericalDerivative11(g, R0); - EXPECT(assert_equal(expectedH, actualH)); + // --- First Update --- + ekf.update(h_gps, z1, R); + + // Calculate expected state and covariance (manual Kalman steps) + Matrix H1; // H = dh/dlocal(X) -> 2x3 + Vector2 z_pred1 = h_gps(X1_expected, H1); + Vector2 y1 = z_pred1 - z1; // Innovation + Matrix S1 = H1 * P1_expected * H1.transpose() + R; + Matrix K1 = P1_expected * H1.transpose() * S1.inverse(); // Gain (3x2) + Vector3 delta_xi1 = -K1 * y1; // Correction (tangent space) + Pose2 X1_updated_expected = X1_expected.retract(delta_xi1); + Matrix3 I_KH1 = Matrix3::Identity() - K1 * H1; + Matrix3 P1_updated_expected = I_KH1 * P1_expected; // Standard form P = (I-KH)P + + // Verify + EXPECT(assert_equal(X1_updated_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P1_updated_expected, ekf.covariance(), 1e-9)); + + + // --- Second Prediction --- + ekf.predict(U2, Q); + + // Calculate expected state and covariance + Pose2 X2_expected = X1_updated_expected.compose(U2); + Matrix3 Ad_U2_inv = U2.inverse().AdjointMap(); + Matrix3 P2_expected = Ad_U2_inv * P1_updated_expected * Ad_U2_inv.transpose() + Q; + + // Verify + EXPECT(assert_equal(X2_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P2_expected, ekf.covariance(), 1e-9)); + + + // --- Second Update --- + ekf.update(h_gps, z2, R); + + // Calculate expected state and covariance (manual Kalman steps) + Matrix H2; // 2x3 + Vector2 z_pred2 = h_gps(X2_expected, H2); + Vector2 y2 = z_pred2 - z2; // Innovation + Matrix S2 = H2 * P2_expected * H2.transpose() + R; + Matrix K2 = P2_expected * H2.transpose() * S2.inverse(); // Gain (3x2) + Vector3 delta_xi2 = -K2 * y2; // Correction (tangent space) + Pose2 X2_updated_expected = X2_expected.retract(delta_xi2); + Matrix3 I_KH2 = Matrix3::Identity() - K2 * H2; + Matrix3 P2_updated_expected = I_KH2 * P2_expected; // Standard form + + // Verify + EXPECT(assert_equal(X2_updated_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P2_updated_expected, ekf.covariance(), 1e-9)); + } int main() { TestResult tr; return TestRegistry::runAllTests(tr); -} +} \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp new file mode 100644 index 000000000..1d74bb704 --- /dev/null +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + * 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 testManifoldEKF.cpp + * @brief Unit test for the ManifoldEKF base class using Unit3. + * @date April 26, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert + */ + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace gtsam; + +// Define simple dynamics for Unit3: constant velocity in the tangent space +namespace exampleUnit3 { + + // Predicts the next state given current state, tangent velocity, and dt + Unit3 predictNextState(const Unit3& p, const Vector2& v, double dt) { + return p.retract(v * dt); + } + + // Define a measurement model: measure the z-component of the Unit3 direction + // H is the Jacobian dh/d(local(p)) + Vector1 measureZ(const Unit3& p, OptionalJacobian<1, 2> H) { + if (H) { + // H = d(p.point3().z()) / d(local(p)) + // Calculate numerically for simplicity in test + auto h = [](const Unit3& p_) { return Vector1(p_.point3().z()); }; + *H = numericalDerivative11(h, p); + } + return Vector1(p.point3().z()); + } + +} // namespace exampleUnit3 + +// Test fixture for ManifoldEKF with Unit3 +struct Unit3EKFTest { + Unit3 p0; + Matrix2 P0; + Vector2 velocity; + double dt; + Matrix2 Q; // Process noise + Matrix1 R; // Measurement noise + + Unit3EKFTest() : + p0(Unit3(Point3(1, 0, 0))), // Start pointing along X-axis + P0(I_2x2 * 0.01), + velocity((Vector2() << 0.0, M_PI / 4.0).finished()), // Rotate towards +Z axis + dt(0.1), + Q(I_2x2 * 0.001), + R(Matrix1::Identity() * 0.01) { + } +}; + + +TEST(ManifoldEKF_Unit3, Predict) { + Unit3EKFTest data; + + // Initialize the EKF + ManifoldEKF ekf(data.p0, data.P0); + + // --- Prepare inputs for ManifoldEKF::predict --- + // 1. Compute expected next state + Unit3 p_next_expected = exampleUnit3::predictNextState(data.p0, data.velocity, data.dt); + + // 2. Compute state transition Jacobian F = d(local(p_next)) / d(local(p)) + // We can compute this numerically using the predictNextState function. + // GTSAM's numericalDerivative handles derivatives *between* manifolds. + auto predict_wrapper = [&](const Unit3& p) -> Unit3 { + return exampleUnit3::predictNextState(p, data.velocity, data.dt); + }; + Matrix2 F = numericalDerivative11(predict_wrapper, data.p0); + + // --- Perform EKF prediction --- + ekf.predict(p_next_expected, F, data.Q); + + // --- Verification --- + // Check state + EXPECT(assert_equal(p_next_expected, ekf.state(), 1e-8)); + + // Check covariance + Matrix2 P_expected = F * data.P0 * F.transpose() + data.Q; + EXPECT(assert_equal(P_expected, ekf.covariance(), 1e-8)); + + // Check F manually for a simple case (e.g., zero velocity should give Identity) + Vector2 zero_velocity = Vector2::Zero(); + auto predict_wrapper_zero = [&](const Unit3& p) -> Unit3 { + return exampleUnit3::predictNextState(p, zero_velocity, data.dt); + }; + Matrix2 F_zero = numericalDerivative11(predict_wrapper_zero, data.p0); + EXPECT(assert_equal(I_2x2, F_zero, 1e-8)); + +} + +TEST(ManifoldEKF_Unit3, Update) { + Unit3EKFTest data; + + // Use a slightly different starting point and covariance for variety + Unit3 p_start = Unit3(Point3(0, 1, 0)).retract((Vector2() << 0.1, 0).finished()); // Perturb pointing along Y + Matrix2 P_start = I_2x2 * 0.05; + ManifoldEKF ekf(p_start, P_start); + + // Simulate a measurement (e.g., true value + noise) + Vector1 z_true = exampleUnit3::measureZ(p_start, {}); + Vector1 z_observed = z_true + Vector1(0.02); // Add some noise + + // --- Perform EKF update --- + ekf.update(exampleUnit3::measureZ, z_observed, data.R); + + // --- Verification (Manual Kalman Update Steps) --- + // 1. Predict measurement and get Jacobian H + Matrix12 H; // Note: Jacobian is 1x2 for Unit3 + Vector1 z_pred = exampleUnit3::measureZ(p_start, H); + + // 2. Innovation and Covariance + Vector1 y = z_pred - z_observed; // Innovation (using vector subtraction for z) + Matrix1 S = H * P_start * H.transpose() + data.R; // 1x1 matrix + + // 3. Kalman Gain K + Matrix K = P_start * H.transpose() * S.inverse(); // 2x1 matrix + + // 4. State Correction (in tangent space) + Vector2 delta_xi = -K * y; // 2x1 vector + + // 5. Expected Updated State and Covariance + Unit3 p_updated_expected = p_start.retract(delta_xi); + Matrix2 I_KH = I_2x2 - K * H; + Matrix2 P_updated_expected = I_KH * P_start; + + // --- Compare EKF result with manual calculation --- + EXPECT(assert_equal(p_updated_expected, ekf.state(), 1e-8)); + EXPECT(assert_equal(P_updated_expected, ekf.covariance(), 1e-8)); +} + + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} \ No newline at end of file From af70b1792e0441c82989294f5faf93dee7dbcd8c Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 1 May 2025 02:25:06 -0400 Subject: [PATCH 30/54] Add script to clean up gtsam-develop project releases --- .../python_wheels/cleanup_gtsam_develop.sh | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100755 .github/scripts/python_wheels/cleanup_gtsam_develop.sh diff --git a/.github/scripts/python_wheels/cleanup_gtsam_develop.sh b/.github/scripts/python_wheels/cleanup_gtsam_develop.sh new file mode 100755 index 000000000..375f70672 --- /dev/null +++ b/.github/scripts/python_wheels/cleanup_gtsam_develop.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash + +# This script deletes all but the most recent release from the gtsam-develop project on PyPI +# and can be used if the project size exceeds the PyPI limit of 10 GB. The user must have +# owner or maintainer privileges on the project. + +set -euo pipefail + +usage() { + cat < + +Deletes all but the most recent release from the gtsam-develop project on PyPI. +You must supply the PyPI user name that has owner or maintainer privileges on +the project. THIS OPERATION IS PERMANENT. + +Examples + $ $(basename "$0") yambati3 + $ $(basename "$0") # will prompt for user name +EOF +} + +if [[ $# -ge 1 ]]; then + PYPI_USER="$1" +else + read -rp "Enter your PyPI user name: " PYPI_USER + [[ -z "$PYPI_USER" ]] && { echo "No user name supplied."; usage; exit 1; } +fi + +echo "-----------------------------------------------------------------------" +echo "WARNING: This WILL permanently delete all but the most recent release" +echo " of 'gtsam-develop' on PyPI for user '$PYPI_USER'." +echo " This cannot be undone." +echo "-----------------------------------------------------------------------" +read -rp "Proceed? [y/N]: " REPLY +REPLY=${REPLY,,} # to lowercase +[[ "$REPLY" != "y" && "$REPLY" != "yes" ]] && { echo "Aborted."; exit 0; } + +echo "Running pypi_cleanup for user '$PYPI_USER'..." +python3 -m pypi_cleanup.__init__ \ + -p gtsam-develop \ + --leave-most-recent-only \ + --do-it \ + -u "$PYPI_USER" + +echo "Done." From ec8c762bb982d3f4ddece2151d0736b4b3dfe78d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 May 2025 09:55:45 -0400 Subject: [PATCH 31/54] Renamed GroupEKF -> LieGroupEKF, moved invariant predicts --- examples/GEKF_Rot3Example.cpp | 4 +- gtsam/navigation/InvariantEKF.h | 34 ++++++----- .../navigation/{GroupEKF.h => LieGroupEKF.h} | 57 +++++-------------- .../{testGroupEKF.cpp => testLieGroupEKF.cpp} | 14 ++--- 4 files changed, 41 insertions(+), 68 deletions(-) rename gtsam/navigation/{GroupEKF.h => LieGroupEKF.h} (77%) rename gtsam/navigation/tests/{testGroupEKF.cpp => testLieGroupEKF.cpp} (90%) diff --git a/examples/GEKF_Rot3Example.cpp b/examples/GEKF_Rot3Example.cpp index d931b7909..d81e01d54 100644 --- a/examples/GEKF_Rot3Example.cpp +++ b/examples/GEKF_Rot3Example.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include @@ -53,7 +53,7 @@ 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; - GroupEKF ekf(R0, P0); + LieGroupEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 1d39981d7..82a3f8fec 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -13,10 +13,10 @@ * @file InvariantEKF.h * @brief Left-Invariant Extended Kalman Filter implementation. * - * This file defines the InvariantEKF class template, inheriting from GroupEKF, + * This file defines the InvariantEKF class template, inheriting from LieGroupEKF, * which specifically implements the Left-Invariant EKF formulation. It restricts * prediction methods to only those based on group composition (state-independent - * motion models), hiding the state-dependent prediction variants from GroupEKF. + * motion models), hiding the state-dependent prediction variants from LieGroupEKF. * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert @@ -24,7 +24,7 @@ #pragma once -#include // Include the base class +#include // Include the base class namespace gtsam { @@ -34,50 +34,54 @@ namespace gtsam { * * @tparam G Lie group type (must satisfy LieGroup concept). * - * This filter inherits from GroupEKF but restricts the prediction interface + * This filter inherits from LieGroupEKF but restricts the prediction interface * to only the left-invariant prediction methods: * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)` * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)` * - * The state-dependent prediction methods from GroupEKF are hidden. - * The update step remains the same as in ManifoldEKF/GroupEKF. + * The state-dependent prediction methods from LieGroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/LieGroupEKF. */ template - class InvariantEKF : public GroupEKF { + class InvariantEKF : public LieGroupEKF { public: - using Base = GroupEKF; ///< Base class type + using Base = LieGroupEKF; ///< Base class type using TangentVector = typename Base::TangentVector; ///< Tangent vector type using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc. + using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G - /// Constructor: forwards to GroupEKF constructor + /// Constructor: forwards to LieGroupEKF constructor InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} - // --- Expose only the Invariant Prediction Methods --- - /** * Predict step via group composition (Left-Invariant): * X_{k+1} = X_k * U * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q - * Calls the base class implementation. + * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to + * F = Ad_{U^{-1}} in the base class predict method. * * @param U Lie group element representing the motion increment. * @param Q Process noise covariance in the tangent space (size nxn). */ void predict(const G& U, const Matrix& Q) { - Base::predict(U, Q); + this->X_ = this->X_.compose(U); + // TODO(dellaert): traits::AdjointMap should exist + Jacobian A = traits::Inverse(U).AdjointMap(); + this->P_ = A * this->P_ * A.transpose() + Q; } /** * Predict step via tangent control vector: * U = Expmap(u * dt) - * Then calls predict(U, Q). Calls the base class implementation. + * Then calls predict(U, Q). * * @param u Tangent space control vector. * @param dt Time interval. * @param Q Process noise covariance matrix (size nxn). */ void predict(const TangentVector& u, double dt, const Matrix& Q) { - Base::predict(u, dt, Q); + G U = traits::Expmap(u * dt); + predict(U, Q); // Call the group composition predict } }; // InvariantEKF diff --git a/gtsam/navigation/GroupEKF.h b/gtsam/navigation/LieGroupEKF.h similarity index 77% rename from gtsam/navigation/GroupEKF.h rename to gtsam/navigation/LieGroupEKF.h index f2f2990e6..fe16eaefe 100644 --- a/gtsam/navigation/GroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file GroupEKF.h + * @file LieGroupEKF.h * @brief Extended Kalman Filter derived class for Lie groups G. * - * This file defines the GroupEKF class template, inheriting from ManifoldEKF, + * This file defines the LieGroupEKF class template, inheriting from ManifoldEKF, * for performing EKF steps specifically on states residing in a Lie group. - * It provides predict methods utilizing group composition, tangent space - * controls (via exponential map), and state-dependent dynamics functions. + * It provides predict methods with state-dependent dynamics functions. + * Please use the InvariantEKF class for prediction via group composition. * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert @@ -34,18 +34,18 @@ namespace gtsam { /** - * @class GroupEKF + * @class LieGroupEKF * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF * * @tparam G Lie group type providing group operations and Expmap/AdjointMap. * Must satisfy LieGroup concept (`gtsam::IsLieGroup::value`). * - * This filter specializes ManifoldEKF for Lie groups, offering convenient - * prediction methods based on group composition or dynamics functions defining - * motion in the tangent space. + * This filter specializes ManifoldEKF for Lie groups, offering predict methods + * with state-dependent dynamics functions. + * Use the InvariantEKF class for prediction via group composition. */ template - class GroupEKF : public ManifoldEKF { + class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) @@ -54,41 +54,10 @@ namespace gtsam { using Jacobian = Eigen::Matrix; ///< Jacobian matrix type specific to the group G /// Constructor: initialize with state and covariance - GroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) { + LieGroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } - /** - * Predict step via group composition (Left-Invariant): - * X_{k+1} = X_k * U - * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q - * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to - * F = Ad_{U^{-1}} in the base class predict method. - * - * @param U Lie group element representing the motion increment. - * @param Q Process noise covariance in the tangent space (size nxn). - */ - void predict(const G& U, const Matrix& Q) { - G X_next = this->X_.compose(U); - // TODO(dellaert): traits::AdjointMap should exist - Jacobian A = traits::Inverse(U).AdjointMap(); // A = Adjoint(U.inverse()) - Base::predict(X_next, A, Q); // Call base class predict - } - - /** - * Predict step via tangent control vector: - * U = Expmap(u * dt) - * Then calls predict(U, Q). - * - * @param u Tangent space control vector. - * @param dt Time interval. - * @param Q Process noise covariance matrix (size nxn). - */ - void predict(const TangentVector& u, double dt, const Matrix& Q) { - G U = traits::Expmap(u * dt); - predict(U, Q); // Call the group composition predict - } - /** * SFINAE check for correctly typed state-dependent dynamics function. * Signature: TangentVector f(const G& X, OptionalJacobian Df) @@ -142,8 +111,8 @@ namespace gtsam { template > void predict(Dynamics&& f, double dt, const Matrix& Q) { Jacobian A; - G X_next = predictMean(std::forward(f), dt, A); - Base::predict(X_next, A, Q); // Call base class predict + this->X_ = predictMean(std::forward(f), dt, A); + this->P_ = A * this->P_ * A.transpose() + Q; } /** @@ -191,6 +160,6 @@ namespace gtsam { return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } - }; // GroupEKF + }; // LieGroupEKF } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp similarity index 90% rename from gtsam/navigation/tests/testGroupEKF.cpp rename to gtsam/navigation/tests/testLieGroupEKF.cpp index 5ce7c477b..62a35473d 100644 --- a/gtsam/navigation/tests/testGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -9,7 +9,7 @@ /** * @file testGroupEKF.cpp - * @brief Unit test for GroupEKF, as well as dynamics used in Rot3 example. + * @brief Unit test for LieGroupEKF, as well as dynamics used in Rot3 example. * @date April 26, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -18,12 +18,12 @@ #include #include #include -#include +#include #include using namespace gtsam; -// Duplicate the dynamics function in GroupEKF_Rot3Example +// Duplicate the dynamics function in GEKF_Rot3Example namespace exampleSO3 { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { @@ -63,12 +63,12 @@ TEST(GroupeEKF, PredictNumericState) { // Analytic Jacobian Matrix3 actualH; - GroupEKF ekf0(R0, P0); + LieGroupEKF ekf0(R0, P0); ekf0.predictMean(exampleSO3::dynamics, dt, actualH); // wrap predict into a state->state functor (mapping on SO(3)) auto g = [&](const Rot3& R) -> Rot3 { - GroupEKF ekf(R, P0); + LieGroupEKF ekf(R, P0); return ekf.predictMean(exampleSO3::dynamics, dt); }; @@ -93,12 +93,12 @@ TEST(GroupeEKF, StateAndControl) { // Analytic Jacobian Matrix3 actualH; - GroupEKF ekf0(R0, P0); + LieGroupEKF ekf0(R0, P0); ekf0.predictMean(f, dummy_u, dt, actualH); // wrap predict into a state->state functor (mapping on SO(3)) auto g = [&](const Rot3& R) -> Rot3 { - GroupEKF ekf(R, P0); + LieGroupEKF ekf(R, P0); return ekf.predictMean(f, dummy_u, dt, Q); }; From cb9d39871be91e0d872322da2c0f80a05e4a1c60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 May 2025 16:31:36 -0400 Subject: [PATCH 32/54] Format, document, optimize slightly - Gemini 2.5 --- gtsam/linear/RegularHessianFactor.h | 431 ++++++++++++++++++---------- 1 file changed, 277 insertions(+), 154 deletions(-) diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index 707f519ca..2aef50a33 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -9,209 +9,332 @@ * -------------------------------------------------------------------------- */ -/** - * @file RegularHessianFactor.h - * @brief HessianFactor class with constant sized blocks - * @author Sungtae An - * @date March 2014 - */ + /** + * @file RegularHessianFactor.h + * @brief Specialized HessianFactor class for regular problems (fixed-size blocks). + * @details This factor is specifically designed for quadratic cost functions + * where all variables involved have the same, fixed dimension `D`. + * It stores the Hessian and gradient terms and provides optimized + * methods for operations like Hessian-vector products, crucial for + * iterative solvers like Conjugate Gradient. It ensures that all + * involved blocks have the expected dimension `D`. + * @author Sungtae An + * @author Frank Dellaert + * @date March 2014 + */ #pragma once #include #include +#include // For VectorValues and Scatter #include +#include // For std::invalid_argument namespace gtsam { -template -class RegularHessianFactor: public HessianFactor { - -public: - - typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix MatrixD; - - /** Construct an n-way factor. Gs contains the upper-triangle blocks of the - * quadratic term (the Hessian matrix) provided in row-order, gs the pieces - * of the linear vector term, and f the constant term. + /** + * @brief A HessianFactor where all variables have the same dimension D. + * @details Represents a quadratic factor \f$ E(x) = 0.5 x^T G x - g^T x + 0.5 f \f$, + * corresponding to a Gaussian probability density \f$ P(x) \propto \exp(-E(x)) \f$. + * Here, G is the Hessian (or information matrix), g is the gradient vector, + * and f is a constant term (negative log-likelihood at x=0). + * + * This class is templated on the dimension `D` and enforces that all variables + * associated with this factor have this dimension during construction. + * It inherits from HessianFactor but provides specialized, efficient implementations + * for operations like `multiplyHessianAdd` using raw memory access, assuming + * a regular block structure. This can significantly improve performance in + * iterative optimization algorithms when dealing with many variables of the + * same type and dimension (e.g., Pose3 variables in SLAM). + * + * It can be constructed directly from Hessian blocks, from a + * RegularJacobianFactor, or by linearizing a NonlinearFactorGraph. + * + * @tparam D The dimension of each variable block involved in the factor. */ - RegularHessianFactor(const KeyVector& js, + template + class RegularHessianFactor : public HessianFactor { + + public: + + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix MatrixD; + + /** + * @brief Construct an n-way factor from supplied components. + * @details Represents the energy function \f$ E(x) = 0.5 \sum_{i,j} x_i^T G_{ij} x_j - \sum_i g_i^T x_i + 0.5 f \f$. + * Note that the keys in `js` must correspond order-wise to the blocks in `Gs` and `gs`. + * @param js Vector of keys involved in the factor. + * @param Gs Vector of upper-triangular Hessian blocks (row-major order, e.g., G11, G12, G13, G22, G23, G33 for a ternary factor). + * @param gs Vector of gradient vector blocks (-J^T b). + * @param f Constant term \f$ 0.5*f = 0.5*\|b\|^2 \f$. + */ + RegularHessianFactor(const KeyVector& js, const std::vector& Gs, const std::vector& gs, double f) : HessianFactor(js, Gs, gs, f) { - checkInvariants(); - } + checkInvariants(); + } - /** Construct a binary factor. Gxx are the upper-triangle blocks of the - * quadratic term (the Hessian matrix), gx the pieces of the linear vector - * term, and f the constant term. - */ - RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12, + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12, const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { - } + } - /** Construct a ternary factor. Gxx are the upper-triangle blocks of the - * quadratic term (the Hessian matrix), gx the pieces of the linear vector - * term, and f the constant term. - */ - RegularHessianFactor(Key j1, Key j2, Key j3, + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1, const MatrixD& G22, const MatrixD& G23, const VectorD& g2, const MatrixD& G33, const VectorD& g3, double f) : - HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { - } + HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { + } - /** Constructor with an arbitrary number of keys and with the augmented information matrix - * specified as a block matrix. */ - template - RegularHessianFactor(const KEYS& keys, + /** + * @brief Constructor with an arbitrary number of keys and the augmented information matrix + * specified as a block matrix. + * @details The augmented information matrix contains the Hessian blocks G and the + * gradient blocks g, arranged as \f$ [ G, -g ; -g^T, f ] \f$. + * @param keys Container of keys specifying the variables involved and their order. + * @param augmentedInformation The augmented information matrix [H -g; -g' f], laid out as a SymmetricBlockMatrix. + */ + template + RegularHessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) : HessianFactor(keys, augmentedInformation) { - checkInvariants(); - } + checkInvariants(); + } - /// Construct from RegularJacobianFactor - RegularHessianFactor(const RegularJacobianFactor& jf) - : HessianFactor(jf) {} + /** + * @brief Construct a RegularHessianFactor from a RegularJacobianFactor. + * @details Computes \f$ G = J^T J \f$, \f$ g = J^T b \f$, and \f$ f = b^T b \f$. + * Requires that the JacobianFactor also has regular block sizes matching `D`. + * @param jf The RegularJacobianFactor to convert. + */ + RegularHessianFactor(const RegularJacobianFactor& jf) + : HessianFactor(jf) { + } - /// Construct from a GaussianFactorGraph - RegularHessianFactor(const GaussianFactorGraph& factors, - const Scatter& scatter) + /** + * @brief Construct from a GaussianFactorGraph combined using a Scatter. + * @details This is typically used internally by optimizers. It sums the Hessian + * contributions from multiple factors in the graph. + * @param factors The graph containing factors to combine. + * @param scatter A Scatter structure indicating the layout of variables. + */ + RegularHessianFactor(const GaussianFactorGraph& factors, + const Scatter& scatter) : HessianFactor(factors, scatter) { - checkInvariants(); - } + checkInvariants(); + } - /// Construct from a GaussianFactorGraph - RegularHessianFactor(const GaussianFactorGraph& factors) + /** + * @brief Construct from a GaussianFactorGraph. + * @details This is typically used internally by optimizers. It sums the Hessian + * contributions from multiple factors in the graph. Assumes keys are ordered 0..n-1. + * @param factors The graph containing factors to combine. + */ + RegularHessianFactor(const GaussianFactorGraph& factors) : HessianFactor(factors) { - checkInvariants(); - } + checkInvariants(); + } -private: + private: - /// Check invariants after construction - void checkInvariants() { - if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D) - throw std::invalid_argument( - "RegularHessianFactor constructor was given non-regular factors"); - } + /** + * @brief Check if the factor has regular block structure. + * @details Verifies that the dimensions of the stored augmented information matrix + * `info_` correspond to the expected size based on the number of keys and the + * fixed block dimension `D`. Throws `std::invalid_argument` if the structure + * is not regular. This is called internally by constructors. + */ + void checkInvariants() const { + if (info_.cols() != 0 && // Allow zero-key factors (e.g. priors on anchor nodes) + info_.cols() != 1 + (info_.nBlocks() - 1) * static_cast(D)) + throw std::invalid_argument( + "RegularHessianFactor constructor was given non-regular factors or " + "incorrect template dimension D"); + } - // Use Eigen magic to access raw memory - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; + // Use Eigen magic to access raw memory for efficiency in Hessian products + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; - // Scratch space for multiplyHessianAdd - // According to link below this is thread-safe. - // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe - mutable std::vector y_; + // Scratch space for multiplyHessianAdd to avoid re-allocation + // According to link below this is thread-safe. + // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe + mutable std::vector> y_; -public: + public: - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, + /** + * @brief Multiply the Hessian part of the factor times a VectorValues `x` and add the result to `y`. + * @details Computes `y += alpha * H * x`. + * @param alpha Scalar multiplier. + * @param x VectorValues containing the vector to multiply with. + * @param[in,out] y VectorValues to store the result (accumulates). + */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const override { - HessianFactor::multiplyHessianAdd(alpha, x, y); - } + // Note: This implementation just calls the base class. The raw memory versions + // below are specifically optimized for the regular structure of this class. + // Consider using those directly or ensuring the base class implementation + // is efficient enough for your use case if calling this version. + HessianFactor::multiplyHessianAdd(alpha, x, y); + } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const double* x, + /** + * @brief Multiply the Hessian part of the factor times a raw vector `x` and add the result to `y`. (Raw memory version) + * @details Computes `y += alpha * H * x`, optimized for regular block structure. + * Assumes `x` and `yvalues` are pre-allocated, contiguous memory blocks + * where variable `j` corresponds to memory chunk `x + keys_[j] * D` + * and `yvalues + keys_[j] * D`. + * @param alpha Scalar multiplier. + * @param x Raw pointer to the input vector data. + * @param[in,out] yvalues Raw pointer to the output vector data (accumulates). + */ + void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { - // Create a vector of temporary y_ values, corresponding to rows i - y_.resize(size()); - for(VectorD & yi: y_) - yi.setZero(); + // Ensure scratch space is properly sized + const size_t n = size(); + if (y_.size() != n) { + y_.resize(n); + } + for (VectorD& yi : y_) + yi.setZero(); - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y_ values, to be added into yvalues after - VectorD xj(D); - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - Key key = keys_[j]; - const double* xj = x + key * D; - DenseIndex i = 0; - for (; i < j; ++i) - y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj); - // blocks on the diagonal are only half - y_[i] += info_.diagonalBlock(j) * ConstDMap(xj); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj); + // Loop over columns (j) of the Hessian H=info_ + // Accessing x only once per column j + // Fill temporary y_ vector corresponding to rows i + for (DenseIndex j = 0; j < static_cast(n); ++j) { + Key key_j = keys_[j]; + ConstDMap xj(x + key_j * D); // Map to the j-th block of x + + DenseIndex i = 0; + // Off-diagonal blocks G_ij * x_j for i < j + for (; i < j; ++i) { + y_[i] += info_.aboveDiagonalBlock(i, j) * xj; + } + // Diagonal block G_jj * x_j + y_[i] += info_.diagonalBlock(j) * xj; + // Off-diagonal blocks G_ij * x_j for i > j (using transpose G_ji^T * x_j) + for (i = j + 1; i < static_cast(n); ++i) { + y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj; + } + } + + // Add accumulated results from y_ to the output yvalues + for (DenseIndex i = 0; i < static_cast(n); ++i) { + Key key_i = keys_[i]; + DMap map_yi(yvalues + key_i * D); // Map to the i-th block of yvalues + map_yi += alpha * y_[i]; + } } - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { - Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y_[i]; - } - } + /** + * @brief Multiply the Hessian part of the factor times a raw vector `x` and add the result to `y`. + * @details Computes `y += alpha * H * x`, optimized for regular block structure, + * but handles potentially non-contiguous or scattered memory layouts + * for `x` and `yvalues` as defined by the `offsets` vector. + * `offsets[k]` should give the starting index of variable `k` in the + * raw memory arrays. `offsets[k+1] - offsets[k]` should equal `D`. + * @param alpha Scalar multiplier. + * @param x Raw pointer to the input vector data. + * @param[in,out] yvalues Raw pointer to the output vector data (accumulates). + * @param offsets Vector mapping variable keys to their starting index in `x` and `yvalues`. + */ + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, + const std::vector& offsets) const { + // Ensure scratch space is properly sized + const size_t n = size(); + if (y_.size() != n) { + y_.resize(n); + } + for (VectorD& yi : y_) + yi.setZero(); - /// Raw memory version, with offsets TODO document reasoning - void multiplyHessianAdd(double alpha, const double* x, double* yvalues, - std::vector offsets) const { + // Loop over columns (j) of the Hessian H=info_ + for (DenseIndex j = 0; j < static_cast(n); ++j) { + Key key_j = keys_[j]; + size_t offset_j = offsets[key_j]; + // Ensure block size matches D (redundant if checkInvariants worked, but safe) + size_t dim_j = offsets[key_j + 1] - offset_j; + if (dim_j != D) throw std::runtime_error("RegularHessianFactor::multiplyHessianAdd: Mismatched dimension in offset map."); + ConstDMap xj(x + offset_j); // Map to the j-th block of x using offset - // Create a vector of temporary y_ values, corresponding to rows i - y_.resize(size()); - for(VectorD & yi: y_) - yi.setZero(); + DenseIndex i = 0; + // Off-diagonal blocks G_ij * x_j for i < j + for (; i < j; ++i) { + y_[i] += info_.aboveDiagonalBlock(i, j) * xj; + } + // Diagonal block G_jj * x_j + y_[i] += info_.diagonalBlock(j) * xj; + // Off-diagonal blocks G_ij * x_j for i > j (using transpose G_ji^T * x_j) + for (i = j + 1; i < static_cast(n); ++i) { + y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj; + } + } - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y_ values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DenseIndex i = 0; - for (; i < j; ++i) - y_[i] += info_.aboveDiagonalBlock(i, j) - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // blocks on the diagonal are only half - y_[i] += info_.diagonalBlock(j) - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y_[i] += info_.aboveDiagonalBlock(j, i).transpose() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); + // Add accumulated results from y_ to the output yvalues using offsets + for (DenseIndex i = 0; i < static_cast(n); ++i) { + Key key_i = keys_[i]; + size_t offset_i = offsets[key_i]; + size_t dim_i = offsets[key_i + 1] - offset_i; + if (dim_i != D) throw std::runtime_error("RegularHessianFactor::multiplyHessianAdd: Mismatched dimension in offset map."); + DMap map_yi(yvalues + offset_i); // Map to the i-th block of yvalues using offset + map_yi += alpha * y_[i]; + } } - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], - offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i]; - } - - /** Return the diagonal of the Hessian for this factor (raw memory version) */ - void hessianDiagonal(double* d) const override { - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - DMap(d + D * j) += info_.diagonal(pos); + /** + * @brief Return the diagonal of the Hessian for this factor (Raw memory version). + * @details Adds the diagonal elements of the Hessian matrix \f$ H = G \f$ associated + * with this factor to the pre-allocated raw memory block `d`. + * Assumes `d` has the standard contiguous layout `d + keys_[i] * D`. + * @param[in,out] d Raw pointer to the memory block where Hessian diagonals should be added. + */ + void hessianDiagonal(double* d) const override { + // Loop over all variables (diagonal blocks) in the factor + const size_t n = size(); + for (DenseIndex pos = 0; pos < static_cast(n); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block G_jj and add its diagonal elements to d + DMap(d + D * j) += info_.info_.diagonal(pos); + } } - } - /// Add gradient at zero to d TODO: is it really the goal to add ?? - void gradientAtZero(double* d) const override { - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());; + /** + * @brief Add the gradient vector \f$ -g \f$ (gradient at zero) to a raw memory block `d`. + * @details Adds the gradient vector \f$ -g \f$ (where the factor represents + * \f$ 0.5 x^T G x - g^T x + 0.5 f \f$) to the pre-allocated raw + * memory block `d`. Assumes `d` has the standard contiguous layout + * `d + keys_[i] * D`. + * @param[in,out] d Raw pointer to the memory block where the gradient should be added. + */ + void gradientAtZero(double* d) const override { + // The linear term g is stored as the last block column of info_ (negated). + // We add (-g) to d. + const size_t n = size(); + for (DenseIndex pos = 0; pos < static_cast(n); ++pos) { + Key j = keys_[pos]; + // info_.aboveDiagonalBlock(pos, n) accesses the block corresponding to g_j + DMap(d + D * j) += info_.aboveDiagonalBlock(pos, n); + } } - } - /* ************************************************************************* */ + /* ************************************************************************* */ -}; -// end class RegularHessianFactor + }; // end class RegularHessianFactor -// traits -template struct traits > : public Testable< + // traits + template struct traits > : public Testable< RegularHessianFactor > { -}; - -} + }; +} // namespace gtsam \ No newline at end of file From 9f148d014a520fa3848689b9d6f4bc5b4e0b91fc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 May 2025 17:08:38 -0400 Subject: [PATCH 33/54] Fix link --- python/gtsam/examples/EKF_SLAM.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index fc3642728..c69f408f0 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -30,7 +30,7 @@ "id": "colab-button-cell-fls", "metadata": {}, "source": [ - "\"Open" + "\"Open" ] }, { From db260f66ee5178f2c5637700006317601db06eef Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 May 2025 17:09:11 -0400 Subject: [PATCH 34/54] Smart factors math overview --- gtsam/slam/doc/SmartFactors.md | 160 +++++++++++++++++++++++++++++++++ 1 file changed, 160 insertions(+) create mode 100644 gtsam/slam/doc/SmartFactors.md diff --git a/gtsam/slam/doc/SmartFactors.md b/gtsam/slam/doc/SmartFactors.md new file mode 100644 index 000000000..4dbd15375 --- /dev/null +++ b/gtsam/slam/doc/SmartFactors.md @@ -0,0 +1,160 @@ +# Smart Factors + +Smart factors in GTSAM provide an efficient way to handle constraints involving landmarks (like 3D points) in Structure from Motion (SfM) or SLAM problems without explicitly including the landmark variables in the optimized state. Instead, the landmark is implicitly represented and marginalized out, leading to factors that directly constrain only the camera-related variables (poses and/or calibrations). This often results in a smaller state space and can significantly speed up optimization, especially when using iterative linear solvers. + +The core idea is based on the **Schur complement**. If we consider a factor graph involving cameras $C_i$ and a single landmark $p$, the Hessian matrix of the linearized system has a block structure: + +```math +H = \begin{bmatrix} H_{pp} & H_{pc} \\ H_{cp} & H_{cc} \end{bmatrix} +``` + +where $H_{pp}$ relates to the landmark, $H_{cc}$ relates to the cameras, and $H_{pc}$ ($H_{cp}$) are the off-diagonal blocks. Marginalizing out the landmark variable $\delta_p$ corresponds to solving the system using the Schur complement $S = H_{cc} - H_{cp} H_{pp}^{-1} H_{pc}$. Smart factors effectively represent or compute this Schur complement system directly. + +Key Reference: +{cite}`10.1109/ICRA.2014.6907483`. + +## Mathematical Details + +### 1. Triangulation + +The core internal operation of a smart factor is triangulation. Given a set of cameras $C_i$ (which include pose and calibration) and corresponding 2D measurements $z_i$, the factor finds the most likely 3D landmark position $p^*$ by solving: + +```math +p^* = \underset{p}{\arg \min} \sum_i \| \Pi(C_i, p) - z_i \|_{\Sigma_i}^2 +``` + +where $\Pi(C_i, p)$ is the projection function of the $i$-th camera, and $\Sigma_i$ is the noise covariance for the $i$-th measurement (though typically a shared noise model $\Sigma$ is used). GTSAM uses the robust `gtsam.triangulateSafe` function internally, which returns a `gtsam.TriangulationResult` containing the point estimate $p^*$ and status information (valid, degenerate, behind camera, etc.). + +### 2. Error Function + +The nonlinear error minimized by the smart factor is the sum of squared reprojection errors using the *implicitly triangulated* point $p^*$: + +```math +\text{error}( \{C_i\} ) = \frac{1}{2} \sum_i \| \Pi(C_i, p^*) - z_i \|_{\Sigma}^2 +``` + +Note that $p^*$ itself is a function of the cameras $\{C_i\}$. + +### 3. Linearization + +When a smart factor is linearized (e.g., during optimization), it computes a Gaussian factor that only involves the camera variables $\{\delta_{C_i}\}$. This is achieved by implicitly performing the Schur complement. + +Let the standard projection factor error for camera $i$ be $e_i(\delta_{C_i}, \delta_p) = F_i \delta_{C_i} + E_i \delta_p - b_i$, where $F_i$ and $E_i$ are the Jacobians with respect to the camera and point, evaluated at the current estimates $C_i^0$ and the triangulated point $p^*$, and $b_i = z_i - \Pi(C_i^0, p^*)$ is the residual. The combined linearized system is: + +```math +\sum_i \| F_i \delta_{C_i} + E_i \delta_p - b_i \|_{\Sigma}^2 = +\left\| \begin{bmatrix} F & E \end{bmatrix} \begin{bmatrix} \delta_C \\ \delta_p \end{bmatrix} - b \right\|_{\Sigma}^2 +``` + +where $F = \text{blkdiag}(F_i)$, $E = [E_1^T, ..., E_m^T]^T$, $b = [b_1^T, ..., b_m^T]^T$, and $\delta_C = [\delta_{C_1}^T, ..., \delta_{C_m}^T]^T$. The noise model $\Sigma$ is typically block-diagonal with identical blocks. + +Marginalizing $\delta_p$ yields a Gaussian factor on $\delta_C$ with Hessian $H_S$ and information vector $\eta_S$: + +```math +H_S = F^T \Sigma^{-1} F - F^T \Sigma^{-1} E (E^T \Sigma^{-1} E)^{-1} E^T \Sigma^{-1} F +``` + +```math +\eta_S = F^T \Sigma^{-1} b - F^T \Sigma^{-1} E (E^T \Sigma^{-1} E)^{-1} E^T \Sigma^{-1} b +``` + +Let $P = (E^T \Sigma^{-1} E)^{-1}$ be the point covariance and define the projection matrix $Q = \Sigma^{-1} (I - E P E^T \Sigma^{-1})$. Then the system simplifies to: + +```math +H_S = F^T Q F +``` + +```math +\eta_S = F^T Q b +``` + +The smart factor's linearization computes or represents this Schur complement system $(H_S, \eta_S)$. + +## The Smart Factor Family + +GTSAM provides several types of smart factors, primarily for projection measurements. They inherit common functionality from `gtsam.SmartFactorBase`. + +### `gtsam.SmartProjectionFactor` + +This factor is used when **both camera poses and camera calibration** are unknown and being optimized. It connects multiple `CAMERA` variables, where the `CAMERA` type (e.g., `gtsam.PinholeCameraCal3_S2`) encapsulates both the pose and the intrinsic calibration. + +See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorBase.h], [SmartProjectionFactor Notebook](SmartProjectionFactor.ipynb). + +### `gtsam.SmartProjectionPoseFactor` + +This factor is optimized for the common case where **camera calibration is known and fixed**, and only the camera **poses** (`gtsam.Pose3`) are being optimized. It assumes a single, shared calibration object (`gtsam.Cal3_S2`, `gtsam.Cal3Bundler`, etc.) for all measurements within the factor. + +See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartProjectionPoseFactor.h], [SmartProjectionPoseFactor Notebook](SmartProjectionPoseFactor.ipynb). + +### `gtsam.SmartProjectionRigFactor` + +This factor extends the concept to **multi-camera rigs** where the *relative* poses and calibrations of the cameras within the rig are fixed, but the **rig's body pose** (`gtsam.Pose3`) is being optimized. It allows multiple measurements from different cameras within the rig, potentially associated with the same body pose key. + +**Note:** Due to implementation details, the `CAMERA` template parameter used with this factor should typically be `gtsam.PinholePose` rather than `gtsam.PinholeCamera`. + +See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartProjectionRigFactor.h], [SmartProjectionRigFactor Notebook](SmartProjectionRigFactor.ipynb). + +## Configuration + +The behavior of smart factors is controlled by `gtsam.SmartProjectionParams`. Key parameters include: + +* `linearizationMode`: Selects which type of `GaussianFactor` is returned by `linearize`, default is `HESSIAN`, see below. +* `degeneracyMode`: Defines how to handle cases where triangulation fails or is ill-conditioned (e.g., collinear cameras). Options include ignoring it, returning a zero-information factor, or treating the point as being at infinity. +* `triangulation`: Parameters passed to `triangulateSafe`, such as rank tolerance for SVD and outlier rejection thresholds. +* `retriangulationThreshold`: If the camera poses change significantly between linearizations (measured by Frobenius norm), the point is re-triangulated. +* `throwCheirality`/`verboseCheirality`: Control behavior when the triangulated point ends up behind one or more cameras. + +See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorParams.h], [SmartFactorParams Notebook](SmartProjectionParams.ipynb). + +## Linear Factor Representations + +Depending on the `linearizationMode` specified in `gtsam.SmartProjectionParams`, the `linearize` method of a smart factor returns different types of `gtsam.GaussianFactor` objects, all representing the same underlying Schur complement system but optimized for different solvers: + +### 1. `gtsam.RegularHessianFactor` (`HESSIAN` mode, default) + +When a smart factor is linearized using the `HESSIAN` mode, it computes and returns a `gtsam.RegularHessianFactor`. In this specific context, this factor explicitly represents the dense **Schur complement** system obtained after marginalizing out the 3D landmark. + +* **Defining Feature:** This factor requires that all involved camera variables have the *same, fixed dimension D* (e.g., $D=6$ for `gtsam.Pose3`), specified via a template parameter. This contrasts with the base `gtsam.HessianFactor` which handles variable dimensions. +* **Stored Information:** It stores the blocks of the effective Hessian matrix $H_S$ (denoted internally as $G_{ij}$) and the effective information vector $\eta_S$ (denoted internally as $g_i$) that act *only* on the camera variables. +* **Optimized Operators (Benefit for CG):** Crucially, this regularity allows `RegularHessianFactor` to provide highly optimized implementations for key linear algebra operations, especially the raw-memory (`double*`) version of `multiplyHessianAdd`. This operation computes the Hessian-vector product ($y \leftarrow y + \alpha H_S x$) extremely efficiently by leveraging `Eigen::Map` and fixed-size matrix operations, making it ideal for **iterative solvers like Conjugate Gradient (CG)** which rely heavily on this product. The base `HessianFactor`'s implementations are more general and typically involve more overhead. +* **Formation Cost:** Creating this factor from a smart factor can still be computationally expensive (forming the Schur complement $H_S = F^T Q F$), especially for landmarks observed by many cameras. +* **Solver Suitability:** While efficient for CG (using the `double*` methods), it is also suitable for **direct linear solvers** (like Cholesky or QR factorization) as it provides the explicit system matrix $H_S$. + +Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/linear/RegularHessianFactor.h]. + +### 2. `gtsam.RegularImplicitSchurFactor` (`IMPLICIT_SCHUR` mode) + +This factor *stores* the ingredients $F_i$, $E_i$, $P$, and $b_i$ (or rather, their whitened versions) instead of the final Hessian $H_S$. Its key feature is an efficient `multiplyHessianAdd` method that computes the Hessian-vector product $y \leftarrow y + \alpha H_S x$ *without ever forming* the potentially large $H_S$ matrix. It computes this via: + +```math +v_i = F_i x_i \\ +w = E^T \Sigma^{-1} v \\ +d = P w \\ +u_i = E_i d \\ +y_i \leftarrow y_i + \alpha F_i^T \Sigma^{-1} (v_i - u_i) +``` +This is highly efficient for iterative linear solvers like Conjugate Gradient (CG), which primarily rely on Hessian-vector products. + +Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/RegularImplicitSchurFactor.h]. + +### 3. `gtsam.JacobianFactorQ` (`JACOBIAN_Q` mode) + +This factor represents the Schur complement system as a Jacobian factor. It computes a projection matrix $Q_{proj} = \Sigma^{-1/2} (I - E P E^T \Sigma^{-1}) \Sigma^{-1/2}$ (where $\Sigma = \text{blkdiag}(\Sigma_i)$) and represents the error term $\| Q_{proj}^{1/2} (F \delta_C - b) \|^2$. It stores the projected Jacobians $A_i = Q_{proj, i}^{1/2} F_i$ and the projected right-hand side $b' = Q_{proj}^{1/2} b$. + +Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/JacobianFactorQ.h]. + +### 4. `gtsam.JacobianFactorSVD` (`JACOBIAN_SVD` mode) + +This factor uses the "Nullspace Trick". It computes $E_{null}$, a matrix whose columns form an orthonormal basis for the left nullspace of $E$ (i.e., $E_{null}^T E = 0$). Multiplying the original linearized system by $E_{null}^T \Sigma^{-1}$ yields a smaller system involving only $\delta_C$: + +```math +\| E_{null}^T \Sigma^{-1} F \delta_C - E_{null}^T \Sigma^{-1} b \|^2_{(E_{null}^T \Sigma^{-1} E_{null})^{-1}} +``` + +The factor stores the projected Jacobian $A = E_{null}^T \Sigma^{-1} F$, the projected right-hand side $b' = E_{null}^T \Sigma^{-1} b$, and an appropriate noise model derived from $(E_{null}^T \Sigma^{-1} E_{null})^{-1}$. This method is mathematically equivalent to the Schur complement but can offer computational advantages in certain scenarios. + +Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/JacobianFactorSVD.h]. + +## Conclusion + +Smart factors provide a powerful mechanism for efficiently handling landmark-based constraints in SLAM and SfM. By implicitly marginalizing landmarks, they reduce the size of the state space and enable the use of specialized linear factor representations like `gtsam.RegularImplicitSchurFactor`, which are highly effective when combined with iterative solvers like Conjugate Gradient. Understanding the underlying mathematical connection to the Schur complement and the different linearization options allows users to choose the most appropriate configuration for their specific problem and solver. From 2ba3bc360dde8651b23ac7b2e83285d95df57530 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 May 2025 17:15:17 -0400 Subject: [PATCH 35/54] Fix type --- gtsam/linear/RegularHessianFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index 2aef50a33..997c0c188 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -305,7 +305,7 @@ namespace gtsam { for (DenseIndex pos = 0; pos < static_cast(n); ++pos) { Key j = keys_[pos]; // Get the diagonal block G_jj and add its diagonal elements to d - DMap(d + D * j) += info_.info_.diagonal(pos); + DMap(d + D * j) += info_.diagonal(pos); } } From ac295ebcaca91ae673bdc0a1d46847a330d8df9e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 8 May 2025 23:33:14 -0400 Subject: [PATCH 36/54] Add explicit update --- gtsam/navigation/ManifoldEKF.h | 71 ++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 33 deletions(-) diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 7951b88cb..d5acbe0c3 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -92,22 +92,45 @@ namespace gtsam { P_ = F * P_ * F.transpose() + Q; } + /** + * Measurement update: Corrects the state and covariance using a pre-calculated + * predicted measurement and its Jacobian. + * + * @tparam Measurement Type of the measurement vector (e.g., VectorN). + * @param prediction Predicted measurement. + * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X). + * Its dimensions must be m x n. + * @param z Observed measurement. + * @param R Measurement noise covariance (size m x m). + */ + template + void update(const Measurement& prediction, + const Eigen::Matrix::dimension, n>& H, + const Measurement& z, const Matrix& R) { + // Innovation z \ominus prediction + Vector innovation = traits::Local(z, prediction); + + // Innovation covariance and Kalman Gain + auto S = H * P_ * H.transpose() + R; // S is m x m + Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) + + // Correction vector in tangent space + TangentVector delta_xi = -K * innovation; // delta_xi is n x 1 + + // Update state using retract + X_ = traits::Retract(X_, delta_xi); + + // Update covariance using Joseph form for numerical stability + MatrixN I_KH = I_n - K * H; // I_KH is n x n + P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + } + /** * Measurement update: Corrects the state and covariance using a measurement. - * z_pred, H = h(X) - * y = z - z_pred (innovation, or z_pred - z depending on convention) - * S = H P H^T + R (innovation covariance) - * K = P H^T S^{-1} (Kalman gain) - * delta_xi = -K * y (correction in tangent space) - * X <- X.retract(delta_xi) - * P <- (I - K H) P * * @tparam Measurement Type of the measurement vector (e.g., VectorN) - * @tparam Prediction Functor signature: Measurement h(const M&, - * OptionalJacobian&) - * where m is the measurement dimension. - * - * @param h Measurement model functor returning predicted measurement z_pred + * @tparam Prediction Functor signature: Measurement h(const M&, OptionalJacobian&) + * @param h Measurement model functor returning predicted measurement prediction * and its Jacobian H = d(h)/d(local(X)). * @param z Observed measurement. * @param R Measurement noise covariance (size m x m). @@ -115,28 +138,10 @@ namespace gtsam { template void update(Prediction&& h, const Measurement& z, const Matrix& R) { constexpr int m = traits::dimension; - Eigen::Matrix H; - // Predict measurement and get Jacobian H = dh/dlocal(X) - Measurement z_pred = h(X_, H); - - // Innovation - // Ensure consistent subtraction for manifold types if Measurement is one - Vector innovation = traits::Local(z, z_pred); // y = z_pred (-) z (in tangent space) - - // Innovation covariance and Kalman Gain - auto S = H * P_ * H.transpose() + R; - Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) - - // Correction vector in tangent space - TangentVector delta_xi = -K * innovation; // delta_xi = - K * y - - // Update state using retract - X_ = traits::Retract(X_, delta_xi); // X <- X.retract(delta_xi) - - // Update covariance using Joseph form: - MatrixN I_KH = I_n - K * H; - P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); + Eigen::Matrix H; + Measurement prediction = h(X_, H); + update(prediction, H, z, R); // Call the other update function } protected: From eacdf1c7fa6326ac7b98f5b96b02d13e7742dff2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 00:25:35 -0400 Subject: [PATCH 37/54] Now handle dynamic types --- gtsam/navigation/InvariantEKF.h | 31 ++-- gtsam/navigation/LieGroupEKF.h | 27 +-- gtsam/navigation/ManifoldEKF.h | 192 +++++++++++++++------ gtsam/navigation/tests/testManifoldEKF.cpp | 109 ++++++++++++ 4 files changed, 282 insertions(+), 77 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 82a3f8fec..d2c6c2cb8 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -25,6 +25,8 @@ #pragma once #include // Include the base class +#include // For traits (needed for AdjointMap, Expmap) + namespace gtsam { @@ -41,32 +43,41 @@ namespace gtsam { * * The state-dependent prediction methods from LieGroupEKF are hidden. * The update step remains the same as in ManifoldEKF/LieGroupEKF. + * Covariances (P, Q) are `Matrix`. */ template class InvariantEKF : public LieGroupEKF { public: using Base = LieGroupEKF; ///< Base class type using TangentVector = typename Base::TangentVector; ///< Tangent vector type - using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc. - using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G + // Jacobian for group-specific operations like AdjointMap. + // Becomes Matrix if G has dynamic dimension. + using Jacobian = typename Base::Jacobian; - /// Constructor: forwards to LieGroupEKF constructor - InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {} + /** + * Constructor: forwards to LieGroupEKF constructor. + * @param X0 Initial state on Lie group G. + * @param P0 Initial covariance in the tangent space at X0 (must be Matrix). + */ + InvariantEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {} + + // We hide state-dependent predict methods from LieGroupEKF by only providing the + // invariant predict methods below. /** * Predict step via group composition (Left-Invariant): * X_{k+1} = X_k * U * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q - * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to - * F = Ad_{U^{-1}} in the base class predict method. + * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. * * @param U Lie group element representing the motion increment. - * @param Q Process noise covariance in the tangent space (size nxn). + * @param Q Process noise covariance in the tangent space (must be Matrix, size n_ x n_). */ void predict(const G& U, const Matrix& Q) { this->X_ = this->X_.compose(U); // TODO(dellaert): traits::AdjointMap should exist - Jacobian A = traits::Inverse(U).AdjointMap(); + const Jacobian A = traits::Inverse(U).AdjointMap(); + // P_ is Matrix. A is Eigen::Matrix. Q is Matrix. this->P_ = A * this->P_ * A.transpose() + Q; } @@ -77,10 +88,10 @@ namespace gtsam { * * @param u Tangent space control vector. * @param dt Time interval. - * @param Q Process noise covariance matrix (size nxn). + * @param Q Process noise covariance matrix (Matrix, size n_ x n_). */ void predict(const TangentVector& u, double dt, const Matrix& Q) { - G U = traits::Expmap(u * dt); + const G U = traits::Expmap(u * dt); predict(U, Q); // Call the group composition predict } diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index fe16eaefe..47c715531 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -38,7 +38,7 @@ namespace gtsam { * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF * * @tparam G Lie group type providing group operations and Expmap/AdjointMap. - * Must satisfy LieGroup concept (`gtsam::IsLieGroup::value`). + * Must satisfy LieGroup concept (`IsLieGroup::value`). * * This filter specializes ManifoldEKF for Lie groups, offering predict methods * with state-dependent dynamics functions. @@ -48,13 +48,19 @@ namespace gtsam { class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type - static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) + // n is the tangent space dimension of G. If G::dimension is Eigen::Dynamic, n is Eigen::Dynamic. + static constexpr int n = traits::dimension; using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G - using MatrixN = typename Base::MatrixN; ///< Square matrix of size n for covariance and Jacobians - using Jacobian = Eigen::Matrix; ///< Jacobian matrix type specific to the group G + // Jacobian for group-specific operations like Adjoint, Expmap derivatives. + // Becomes Matrix if n is Eigen::Dynamic. + using Jacobian = Eigen::Matrix; - /// Constructor: initialize with state and covariance - LieGroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) { + /** + * Constructor: initialize with state and covariance. + * @param X0 Initial state on Lie group G. + * @param P0 Initial covariance in the tangent space at X0 (must be Matrix). + */ + LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } @@ -62,6 +68,7 @@ namespace gtsam { * SFINAE check for correctly typed state-dependent dynamics function. * Signature: TangentVector f(const G& X, OptionalJacobian Df) * Df = d(xi)/d(local(X)) + * If n is Eigen::Dynamic, OptionalJacobian is OptionalJacobian. */ template using enable_if_dynamics = std::enable_if_t< @@ -79,7 +86,7 @@ namespace gtsam { * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X). * @param dt Time step. - * @param A Optional pointer to store the computed state transition Jacobian A. + * @param A OptionalJacobian to store the computed state transition Jacobian A. * @return Predicted state X_{k+1}. */ template > @@ -99,14 +106,14 @@ namespace gtsam { /** * Predict step with state-dependent dynamics: - * Uses predictMean to compute X_{k+1} and F, then calls base predict. + * Uses predictMean to compute X_{k+1} and F, then updates covariance. * X_{k+1}, F = predictMean(f, dt) * P_{k+1} = F P_k F^T + Q * * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor. * @param dt Time step. - * @param Q Process noise covariance (size nxn). + * @param Q Process noise covariance (Matrix, size n_ x n_). */ template > void predict(Dynamics&& f, double dt, const Matrix& Q) { @@ -153,7 +160,7 @@ namespace gtsam { * @param f Dynamics functor. * @param u Control input. * @param dt Time step. - * @param Q Process noise covariance (size nxn). + * @param Q Process noise covariance (must be Matrix, size n_ x n_). */ template > void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index d5acbe0c3..68eaaa793 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -18,6 +18,9 @@ * differentiable manifold. It relies on the manifold's retract and * localCoordinates operations. * + * Works with manifolds M that may have fixed or dynamic tangent space dimensions. + * Covariances and Jacobians are handled as `Matrix` (dynamic-size Eigen matrices). + * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -27,9 +30,11 @@ #include #include #include -#include // Include for traits +#include // Include for traits and IsManifold #include +#include +#include #include namespace gtsam { @@ -39,41 +44,59 @@ namespace gtsam { * @brief Extended Kalman Filter on a generic manifold M * * @tparam M Manifold type providing: - * - static int dimension = tangent dimension - * - using TangentVector = Eigen::Vector... - * - A `retract(const TangentVector&)` method (member or static) - * - A `localCoordinates(const M&)` method (member or static) - * - `gtsam::traits` specialization must exist. + * - `traits` specialization must exist, defining + * `dimension` (compile-time or Eigen::Dynamic), + * `TangentVector`, `Retract`, and `LocalCoordinates`. + * If `dimension` is Eigen::Dynamic, `GetDimension(const M&)` + * must be provided by traits. * * This filter maintains a state X in the manifold M and covariance P in the - * tangent space at X. Prediction requires providing the predicted next state - * and the state transition Jacobian F. Updates apply a measurement function h - * and correct the state using the tangent space error. + * tangent space at X. The covariance P is always stored as a gtsam::Matrix. + * Prediction requires providing the predicted next state and the state transition Jacobian F. + * Updates apply a measurement function h and correct the state using the tangent space error. */ template class ManifoldEKF { public: - /// Manifold dimension (tangent space dimension) - static constexpr int n = traits::dimension; - - /// Tangent vector type for the manifold M + /// Tangent vector type for the manifold M, as defined by its traits. using TangentVector = typename traits::TangentVector; - /// Square matrix of size n for covariance and Jacobians - using MatrixN = Eigen::Matrix; + /** + * Constructor: initialize with state and covariance. + * @param X0 Initial state on manifold M. + * @param P0 Initial covariance in the tangent space at X0. Must be a square + * Matrix whose dimensions match the tangent space dimension of X0. + */ + ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) { + static_assert(IsManifold::value, + "Template parameter M must be a GTSAM Manifold."); - /// Constructor: initialize with state and covariance - ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) { - static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold"); + // Determine tangent space dimension n_ at runtime. + if constexpr (traits::dimension == Eigen::Dynamic) { + // If M::dimension is dynamic, traits::GetDimension(M) must exist. + n_ = traits::GetDimension(X0); + } + else { + n_ = traits::dimension; + } + + // Validate dimensions of initial covariance P0. + if (P0.rows() != n_ || P0.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF: Initial covariance P0 dimensions (" + + std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) + + ") do not match state's tangent space dimension (" + + std::to_string(n_) + ")."); + } } - virtual ~ManifoldEKF() = default; // Add virtual destructor for base class + virtual ~ManifoldEKF() = default; - /// @return current state estimate + /// @return current state estimate on manifold M. const M& state() const { return X_; } - /// @return current covariance estimate - const MatrixN& covariance() const { return P_; } + /// @return current covariance estimate in the tangent space (always a Matrix). + const Matrix& covariance() const { return P_; } /** * Basic predict step: Updates state and covariance given the predicted @@ -83,11 +106,23 @@ namespace gtsam { * where F = d(local(X_{k+1})) / d(local(X_k)) is the Jacobian of the * state transition in local coordinates around X_k. * - * @param X_next The predicted state at time k+1. + * @param X_next The predicted state at time k+1 on manifold M. * @param F The state transition Jacobian (size nxn). * @param Q Process noise covariance matrix in the tangent space (size nxn). */ - void predict(const M& X_next, const MatrixN& F, const Matrix& Q) { + void predict(const M& X_next, const Matrix& F, const Matrix& Q) { + if (F.rows() != n_ || F.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::predict: Jacobian F dimensions (" + + std::to_string(F.rows()) + "x" + std::to_string(F.cols()) + + ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + "."); + } + if (Q.rows() != n_ || Q.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::predict: Noise Q dimensions (" + + std::to_string(Q.rows()) + "x" + std::to_string(Q.cols()) + + ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + "."); + } X_ = X_next; P_ = F * P_ * F.transpose() + Q; } @@ -96,7 +131,7 @@ namespace gtsam { * Measurement update: Corrects the state and covariance using a pre-calculated * predicted measurement and its Jacobian. * - * @tparam Measurement Type of the measurement vector (e.g., VectorN). + * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). * @param prediction Predicted measurement. * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X). * Its dimensions must be m x n. @@ -105,56 +140,99 @@ namespace gtsam { */ template void update(const Measurement& prediction, - const Eigen::Matrix::dimension, n>& H, - const Measurement& z, const Matrix& R) { - // Innovation z \ominus prediction - Vector innovation = traits::Local(z, prediction); + const Matrix& H, + const Measurement& z, + const Matrix& R) { - // Innovation covariance and Kalman Gain - auto S = H * P_ * H.transpose() + R; // S is m x m - Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m) + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - // Correction vector in tangent space - TangentVector delta_xi = -K * innovation; // delta_xi is n x 1 + int m; // Measurement dimension + if constexpr (traits::dimension == Eigen::Dynamic) { + m = traits::GetDimension(z); + if (traits::GetDimension(prediction) != m) { + throw std::invalid_argument( + "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); + } + } + else { + m = traits::dimension; + } - // Update state using retract + if (H.rows() != m || H.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H dimensions (" + + std::to_string(H.rows()) + "x" + std::to_string(H.cols()) + + ") must be " + std::to_string(m) + "x" + std::to_string(n_) + "."); + } + if (R.rows() != m || R.cols() != m) { + throw std::invalid_argument( + "ManifoldEKF::update: Noise R dimensions (" + + std::to_string(R.rows()) + "x" + std::to_string(R.cols()) + + ") must be " + std::to_string(m) + "x" + std::to_string(m) + "."); + } + + // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z) + // This is `log(prediction.inverse() * z)` if Measurement is a Lie group. + typename traits::TangentVector innovation = + traits::Local(prediction, z); + + // Innovation covariance: S = H P H^T + R + const Matrix S = H * P_ * H.transpose() + R; // S is m x m + + // Kalman Gain: K = P H^T S^-1 + const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m + + // Correction vector in tangent space of M: delta_xi = K * innovation + const TangentVector delta_xi = K * innovation; // delta_xi is n_ x 1 + + // Update state using retract: X_new = retract(X_old, delta_xi) X_ = traits::Retract(X_, delta_xi); // Update covariance using Joseph form for numerical stability - MatrixN I_KH = I_n - K * H; // I_KH is n x n + const auto I_n = Matrix::Identity(n_, n_); + const Matrix I_KH = I_n - K * H; // I_KH is n x n P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); } /** - * Measurement update: Corrects the state and covariance using a measurement. + * Measurement update: Corrects the state and covariance using a measurement + * model function. * - * @tparam Measurement Type of the measurement vector (e.g., VectorN) - * @tparam Prediction Functor signature: Measurement h(const M&, OptionalJacobian&) - * @param h Measurement model functor returning predicted measurement prediction - * and its Jacobian H = d(h)/d(local(X)). + * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). + * @tparam MeasurementFunction Functor/lambda with signature compatible with: + * `Measurement h(const M& x, Jac& H_jacobian)` + * where `Jac` can be `Matrix&` or `OptionalJacobian&`. + * The Jacobian H should be d(h)/d(local(X)). + * @param h Measurement model function. * @param z Observed measurement. - * @param R Measurement noise covariance (size m x m). + * @param R Measurement noise covariance (must be an m x m Matrix). */ - template - void update(Prediction&& h, const Measurement& z, const Matrix& R) { - constexpr int m = traits::dimension; + template + void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) { + static_assert(IsManifold::value, + "Template parameter Measurement must be a GTSAM Manifold."); + + int m; // Measurement dimension + if constexpr (traits::dimension == Eigen::Dynamic) { + m = traits::GetDimension(z); + } + else { + m = traits::dimension; + } + // Predict measurement and get Jacobian H = dh/dlocal(X) - Eigen::Matrix H; + Matrix H(m, n_); Measurement prediction = h(X_, H); - update(prediction, H, z, R); // Call the other update function + + // Call the other update function + update(prediction, H, z, R); } protected: - M X_; ///< manifold state estimate - MatrixN P_; ///< covariance in tangent space at X_ - - private: - /// Identity matrix of size n - static const MatrixN I_n; + M X_; ///< Manifold state estimate. + Matrix P_; ///< Covariance in tangent space at X_ (always a dynamic Matrix). + int n_; ///< Tangent space dimension of M, determined at construction. }; - // Define static identity I_n - template - const typename ManifoldEKF::MatrixN ManifoldEKF::I_n = ManifoldEKF::MatrixN::Identity(); - } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 1d74bb704..50730a8d4 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -147,6 +147,115 @@ TEST(ManifoldEKF_Unit3, Update) { EXPECT(assert_equal(P_updated_expected, ekf.covariance(), 1e-8)); } +// Define simple dynamics and measurement for a 2x2 Matrix state +namespace exampleDynamicMatrix { + + // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + Matrix predictNextState(const Matrix& p, const Vector& vTangent, double dt) { + return traits::Retract(p, vTangent * dt); + } + + // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) + Vector1 measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { + // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] + // trace = p(0,0) + p(1,1) + // H = d(trace)/d(p_flat) = [1, 0, 0, 1] + // The Jacobian H will be 1x4 for a 2x2 matrix. + H->resize(1, 4); + *H << 1.0, 0.0, 0.0, 1.0; + } + return Vector1(p(0, 0) + p(1, 1)); + } + +} // namespace exampleDynamicMatrix + +// Test fixture for ManifoldEKF with a 2x2 Matrix state +struct DynamicMatrixEKFTest { + Matrix p0Matrix; // Initial state (as 2x2 Matrix) + Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4) + Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix) + double dt; + Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4) + Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1) + + DynamicMatrixEKFTest() : + p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()), + p0Covariance(I_4x4 * 0.01), + velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec + dt(0.1), + processNoiseCovariance(I_4x4 * 0.001), + measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005) + { + } +}; + + +TEST(ManifoldEKF_DynamicMatrix, Predict) { + DynamicMatrixEKFTest data; + + ManifoldEKF ekf(data.p0Matrix, data.p0Covariance); + // For a 2x2 Matrix, tangent space dimension is 2*2=4. + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size()); + + // --- Prepare inputs for ManifoldEKF::predict --- + Matrix pNextExpected = exampleDynamicMatrix::predictNextState(data.p0Matrix, data.velocityTangent, data.dt); + + // For this linear prediction model (p_next = p_current + V*dt in tangent space), F is Identity. + Matrix jacobianF = I_4x4; // Jacobian of the state transition function + + // --- Perform EKF prediction --- + ekf.predict(pNextExpected, jacobianF, data.processNoiseCovariance); + + // --- Verification --- + EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); + Matrix pCovarianceExpected = jacobianF * data.p0Covariance * jacobianF.transpose() + data.processNoiseCovariance; + EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); +} + +TEST(ManifoldEKF_DynamicMatrix, Update) { + DynamicMatrixEKFTest data; + + Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); + Matrix pStartCovariance = I_4x4 * 0.02; + ManifoldEKF ekf(pStartMatrix, pStartCovariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + + // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) + Vector1 zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + Vector1 zObserved = zTrue - Vector1(0.03); // Add some "error" + + // --- Perform EKF update --- + ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); + + // --- Verification (Manual Kalman Update Steps) --- + // 1. Predict measurement and get Jacobian H + Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement + Vector1 zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); + + // 2. Innovation and Innovation Covariance + // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) + // For Vector1 (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + Vector1 innovationY = zObserved - zPredictionManual; + Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance; + + // 3. Kalman Gain K + Matrix kalmanGainK = pStartCovariance * H.transpose() * innovationCovarianceS.inverse(); // K is 4x1 + + // 4. State Correction (in tangent space of Matrix) + Vector deltaXiTangent = kalmanGainK * innovationY; // deltaXi is 4x1 Vector + + // 5. Expected Updated State and Covariance + Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); + + // Covariance update: P_new = (I - K H) P_old + Matrix pUpdatedCovarianceExpected = (I_4x4 - kalmanGainK * H) * pStartCovariance; + + // --- Compare EKF result with manual calculation --- + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); +} int main() { TestResult tr; From 16f650c16c91d2923cab26ead7a9dc95de8b55ce Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 00:48:07 -0400 Subject: [PATCH 38/54] Retain efficiency in static case --- gtsam/navigation/InvariantEKF.h | 29 ++-- gtsam/navigation/LieGroupEKF.h | 70 +++++---- gtsam/navigation/ManifoldEKF.h | 165 ++++++++++++--------- gtsam/navigation/tests/testManifoldEKF.cpp | 31 ++-- 4 files changed, 167 insertions(+), 128 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index d2c6c2cb8..c0d95db84 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -38,30 +38,32 @@ namespace gtsam { * * This filter inherits from LieGroupEKF but restricts the prediction interface * to only the left-invariant prediction methods: - * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)` - * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)` + * 1. Prediction via group composition: `predict(const G& U, const Covariance& Q)` + * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Covariance& Q)` * * The state-dependent prediction methods from LieGroupEKF are hidden. * The update step remains the same as in ManifoldEKF/LieGroupEKF. - * Covariances (P, Q) are `Matrix`. + * Covariances (P, Q) are `Eigen::Matrix`. */ template class InvariantEKF : public LieGroupEKF { public: using Base = LieGroupEKF; ///< Base class type using TangentVector = typename Base::TangentVector; ///< Tangent vector type - // Jacobian for group-specific operations like AdjointMap. - // Becomes Matrix if G has dynamic dimension. + /// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix. using Jacobian = typename Base::Jacobian; + /// Covariance matrix type. Eigen::Matrix. + using Covariance = typename Base::Covariance; + /** * Constructor: forwards to LieGroupEKF constructor. * @param X0 Initial state on Lie group G. - * @param P0 Initial covariance in the tangent space at X0 (must be Matrix). + * @param P0_input Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). */ - InvariantEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {} + InvariantEKF(const G& X0, const Matrix& P0_input) : Base(X0, P0_input) {} - // We hide state-dependent predict methods from LieGroupEKF by only providing the + // We hide state-dependent predict methods from LieGroupEKF by only providing the // invariant predict methods below. /** @@ -71,13 +73,14 @@ namespace gtsam { * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. * * @param U Lie group element representing the motion increment. - * @param Q Process noise covariance in the tangent space (must be Matrix, size n_ x n_). + * @param Q Process noise covariance (Eigen::Matrix). */ - void predict(const G& U, const Matrix& Q) { + void predict(const G& U, const Covariance& Q) { this->X_ = this->X_.compose(U); // TODO(dellaert): traits::AdjointMap should exist const Jacobian A = traits::Inverse(U).AdjointMap(); - // P_ is Matrix. A is Eigen::Matrix. Q is Matrix. + // P_ is Covariance. A is Jacobian. Q is Covariance. + // All are Eigen::Matrix. this->P_ = A * this->P_ * A.transpose() + Q; } @@ -88,9 +91,9 @@ namespace gtsam { * * @param u Tangent space control vector. * @param dt Time interval. - * @param Q Process noise covariance matrix (Matrix, size n_ x n_). + * @param Q Process noise covariance matrix (Eigen::Matrix). */ - void predict(const TangentVector& u, double dt, const Matrix& Q) { + void predict(const TangentVector& u, double dt, const Covariance& Q) { const G U = traits::Expmap(u * dt); predict(U, Q); // Call the group composition predict } diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 47c715531..da41a1e3d 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -48,17 +48,16 @@ namespace gtsam { class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type - // n is the tangent space dimension of G. If G::dimension is Eigen::Dynamic, n is Eigen::Dynamic. - static constexpr int n = traits::dimension; - using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G - // Jacobian for group-specific operations like Adjoint, Expmap derivatives. - // Becomes Matrix if n is Eigen::Dynamic. - using Jacobian = Eigen::Matrix; + static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G. + using TangentVector = typename Base::TangentVector; ///< Tangent vector type for G. + /// Jacobian for group operations (Adjoint, Expmap derivatives, F). + using Jacobian = typename Base::Jacobian; // Eigen::Matrix + using Covariance = typename Base::Covariance; // Eigen::Matrix /** * Constructor: initialize with state and covariance. * @param X0 Initial state on Lie group G. - * @param P0 Initial covariance in the tangent space at X0 (must be Matrix). + * @param P0 Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). */ LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); @@ -66,38 +65,39 @@ namespace gtsam { /** * SFINAE check for correctly typed state-dependent dynamics function. - * Signature: TangentVector f(const G& X, OptionalJacobian Df) + * Signature: TangentVector f(const G& X, OptionalJacobian Df) * Df = d(xi)/d(local(X)) - * If n is Eigen::Dynamic, OptionalJacobian is OptionalJacobian. */ template using enable_if_dynamics = std::enable_if_t< !std::is_convertible_v&& std::is_invocable_r_v&>>; + OptionalJacobian&>>; /** * Predict mean and Jacobian A with state-dependent dynamics: - * xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df) + * xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df) * U = Expmap(xi * dt, Dexp) (Compute motion increment U and Expmap Jacobian Dexp) - * X_{k+1} = X_k * U (Predict next state) - * F = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) + * X_{k+1} = X_k * U (Predict next state) + * A = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian) * - * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X). * @param dt Time step. * @param A OptionalJacobian to store the computed state transition Jacobian A. * @return Predicted state X_{k+1}. */ template > - G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { - Jacobian Df, Dexp; + G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { + Jacobian Df, Dexp; // Eigen::Matrix + TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt G X_next = this->X_.compose(U); if (A) { // Full state transition Jacobian for left-invariant EKF: + // AdjointMap returns Jacobian (Eigen::Matrix) *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; } return X_next; @@ -106,29 +106,32 @@ namespace gtsam { /** * Predict step with state-dependent dynamics: - * Uses predictMean to compute X_{k+1} and F, then updates covariance. - * X_{k+1}, F = predictMean(f, dt) - * P_{k+1} = F P_k F^T + Q + * Uses predictMean to compute X_{k+1} and A, then updates covariance. + * X_{k+1}, A = predictMean(f, dt) + * P_{k+1} = A P_k A^T + Q * - * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor. * @param dt Time step. - * @param Q Process noise covariance (Matrix, size n_ x n_). + * @param Q Process noise covariance (Eigen::Matrix). */ template > - void predict(Dynamics&& f, double dt, const Matrix& Q) { - Jacobian A; + void predict(Dynamics&& f, double dt, const Covariance& Q) { + Jacobian A; // Eigen::Matrix + if constexpr (Dim == Eigen::Dynamic) { + A.resize(this->n_, this->n_); + } this->X_ = predictMean(std::forward(f), dt, A); this->P_ = A * this->P_ * A.transpose() + Q; } /** * SFINAE check for state- and control-dependent dynamics function. - * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) + * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) */ template using enable_if_full_dynamics = std::enable_if_t< - std::is_invocable_r_v&> + std::is_invocable_r_v&> >; /** @@ -137,7 +140,7 @@ namespace gtsam { * xi = f(X_k, u, Df) * * @tparam Control Control input type. - * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) * @param f Dynamics functor. * @param u Control input. * @param dt Time step. @@ -145,8 +148,8 @@ namespace gtsam { * @return Predicted state X_{k+1}. */ template > - G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { - return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); + G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { + return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); } @@ -156,15 +159,18 @@ namespace gtsam { * xi = f(X_k, u, Df) * * @tparam Control Control input type. - * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) * @param f Dynamics functor. * @param u Control input. * @param dt Time step. - * @param Q Process noise covariance (must be Matrix, size n_ x n_). + * @param Q Process noise covariance (Eigen::Matrix). */ template > - void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); + void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) { + // Note: The lambda below captures f by reference. Ensure f's lifetime. + // If f is an rvalue, std::forward or std::move might be needed if the lambda is stored. + // Here, the lambda is temporary, so [&] is fine. + return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } }; // LieGroupEKF diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 68eaaa793..0f405c3ae 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -19,7 +19,7 @@ * localCoordinates operations. * * Works with manifolds M that may have fixed or dynamic tangent space dimensions. - * Covariances and Jacobians are handled as `Matrix` (dynamic-size Eigen matrices). + * Covariances and Jacobians leverage Eigen's static or dynamic matrices for efficiency. * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert @@ -51,33 +51,41 @@ namespace gtsam { * must be provided by traits. * * This filter maintains a state X in the manifold M and covariance P in the - * tangent space at X. The covariance P is always stored as a gtsam::Matrix. + * tangent space at X. The covariance P is `Eigen::Matrix`, + * where Dim is the compile-time dimension of M (or Eigen::Dynamic). * Prediction requires providing the predicted next state and the state transition Jacobian F. * Updates apply a measurement function h and correct the state using the tangent space error. */ template class ManifoldEKF { public: - /// Tangent vector type for the manifold M, as defined by its traits. + /// Compile-time dimension of the manifold M. + static constexpr int Dim = traits::dimension; + + /// Tangent vector type for the manifold M. using TangentVector = typename traits::TangentVector; + /// Covariance matrix type (P, Q). + using Covariance = Eigen::Matrix; + /// State transition Jacobian type (F). + using Jacobian = Eigen::Matrix; + /** * Constructor: initialize with state and covariance. * @param X0 Initial state on manifold M. - * @param P0 Initial covariance in the tangent space at X0. Must be a square - * Matrix whose dimensions match the tangent space dimension of X0. + * @param P0 Initial covariance in the tangent space at X0. + * Provided as a dynamic gtsam::Matrix for flexibility, + * but will be stored internally as Covariance. */ - ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) { + ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0) { static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold."); - // Determine tangent space dimension n_ at runtime. - if constexpr (traits::dimension == Eigen::Dynamic) { - // If M::dimension is dynamic, traits::GetDimension(M) must exist. - n_ = traits::GetDimension(X0); + if constexpr (Dim == Eigen::Dynamic) { + n_ = traits::GetDimension(X0); // Runtime dimension for dynamic case } else { - n_ = traits::dimension; + n_ = Dim; // Runtime dimension is fixed compile-time dimension } // Validate dimensions of initial covariance P0. @@ -88,6 +96,7 @@ namespace gtsam { ") do not match state's tangent space dimension (" + std::to_string(n_) + ")."); } + P_ = P0; // Assigns MatrixXd to Eigen::Matrix } virtual ~ManifoldEKF() = default; @@ -95,8 +104,11 @@ namespace gtsam { /// @return current state estimate on manifold M. const M& state() const { return X_; } - /// @return current covariance estimate in the tangent space (always a Matrix). - const Matrix& covariance() const { return P_; } + /// @return current covariance estimate (Eigen::Matrix). + const Covariance& covariance() const { return P_; } + + /// @return runtime dimension of the tangent space. + int dimension() const { return n_; } /** * Basic predict step: Updates state and covariance given the predicted @@ -107,22 +119,19 @@ namespace gtsam { * state transition in local coordinates around X_k. * * @param X_next The predicted state at time k+1 on manifold M. - * @param F The state transition Jacobian (size nxn). - * @param Q Process noise covariance matrix in the tangent space (size nxn). + * @param F The state transition Jacobian (Eigen::Matrix). + * @param Q Process noise covariance matrix (Eigen::Matrix). */ - void predict(const M& X_next, const Matrix& F, const Matrix& Q) { - if (F.rows() != n_ || F.cols() != n_) { - throw std::invalid_argument( - "ManifoldEKF::predict: Jacobian F dimensions (" + - std::to_string(F.rows()) + "x" + std::to_string(F.cols()) + - ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + "."); - } - if (Q.rows() != n_ || Q.cols() != n_) { - throw std::invalid_argument( - "ManifoldEKF::predict: Noise Q dimensions (" + - std::to_string(Q.rows()) + "x" + std::to_string(Q.cols()) + - ") must be " + std::to_string(n_) + "x" + std::to_string(n_) + "."); + void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { + if constexpr (Dim == Eigen::Dynamic) { + if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ || Q.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::predict: Dynamic F/Q dimensions must match state dimension " + + std::to_string(n_) + "."); + } } + // For fixed Dim, types enforce dimensions. + X_ = X_next; P_ = F * P_ * F.transpose() + Q; } @@ -134,64 +143,75 @@ namespace gtsam { * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). * @param prediction Predicted measurement. * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X). - * Its dimensions must be m x n. + * Type: Eigen::Matrix. * @param z Observed measurement. - * @param R Measurement noise covariance (size m x m). + * @param R Measurement noise covariance (Eigen::Matrix). */ template void update(const Measurement& prediction, - const Matrix& H, + const Eigen::Matrix::dimension, Dim>& H, const Measurement& z, - const Matrix& R) { + const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - int m; // Measurement dimension - if constexpr (traits::dimension == Eigen::Dynamic) { - m = traits::GetDimension(z); - if (traits::GetDimension(prediction) != m) { + static constexpr int MeasDim = traits::dimension; + + int m_runtime; // Measurement dimension at runtime + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); + if (traits::GetDimension(prediction) != m_runtime) { throw std::invalid_argument( "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); } + // Runtime check for H and R if they involve dynamic dimensions + if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement."); + } } else { - m = traits::dimension; - } - - if (H.rows() != m || H.cols() != n_) { - throw std::invalid_argument( - "ManifoldEKF::update: Jacobian H dimensions (" + - std::to_string(H.rows()) + "x" + std::to_string(H.cols()) + - ") must be " + std::to_string(m) + "x" + std::to_string(n_) + "."); - } - if (R.rows() != m || R.cols() != m) { - throw std::invalid_argument( - "ManifoldEKF::update: Noise R dimensions (" + - std::to_string(R.rows()) + "x" + std::to_string(R.cols()) + - ") must be " + std::to_string(m) + "x" + std::to_string(m) + "."); + m_runtime = MeasDim; + // Types enforce dimensions for H and R if MeasDim and Dim are fixed. + // If Dim is dynamic but MeasDim is fixed, H.cols() needs check. + if constexpr (Dim == Eigen::Dynamic) { + if (H.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF::update: Jacobian H columns must match state dimension " + std::to_string(n_) + "."); + } + } } // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z) - // This is `log(prediction.inverse() * z)` if Measurement is a Lie group. typename traits::TangentVector innovation = traits::Local(prediction, z); // Innovation covariance: S = H P H^T + R - const Matrix S = H * P_ * H.transpose() + R; // S is m x m + // S will be Eigen::Matrix + Eigen::Matrix S = H * P_ * H.transpose() + R; // Kalman Gain: K = P H^T S^-1 - const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m + // K will be Eigen::Matrix + Eigen::Matrix K = P_ * H.transpose() * S.inverse(); // Correction vector in tangent space of M: delta_xi = K * innovation - const TangentVector delta_xi = K * innovation; // delta_xi is n_ x 1 + const TangentVector delta_xi = K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic) // Update state using retract: X_new = retract(X_old, delta_xi) X_ = traits::Retract(X_, delta_xi); // Update covariance using Joseph form for numerical stability - const auto I_n = Matrix::Identity(n_, n_); - const Matrix I_KH = I_n - K * H; // I_KH is n x n + Jacobian I_n; // Eigen::Matrix + if constexpr (Dim == Eigen::Dynamic) { + I_n = Jacobian::Identity(n_, n_); + } + else { + I_n = Jacobian::Identity(); + } + + // I_KH will be Eigen::Matrix + Jacobian I_KH = I_n - K * H; P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose(); } @@ -202,37 +222,48 @@ namespace gtsam { * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). * @tparam MeasurementFunction Functor/lambda with signature compatible with: * `Measurement h(const M& x, Jac& H_jacobian)` - * where `Jac` can be `Matrix&` or `OptionalJacobian&`. + * where `Jac` can be `Eigen::Matrix&` or + * `OptionalJacobian&`. * The Jacobian H should be d(h)/d(local(X)). * @param h Measurement model function. * @param z Observed measurement. - * @param R Measurement noise covariance (must be an m x m Matrix). + * @param R Measurement noise covariance (Eigen::Matrix). */ template - void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) { + void update(MeasurementFunction&& h_func, const Measurement& z, + const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold."); - int m; // Measurement dimension - if constexpr (traits::dimension == Eigen::Dynamic) { - m = traits::GetDimension(z); + static constexpr int MeasDim = traits::dimension; + + int m_runtime; // Measurement dimension at runtime + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); } else { - m = traits::dimension; + m_runtime = MeasDim; } + // Prepare Jacobian H for the measurement function + Matrix H(m_runtime, n_); + if constexpr (MeasDim == Eigen::Dynamic || Dim == Eigen::Dynamic) { + // If H involves any dynamic dimension, it needs explicit resizing. + H.resize(m_runtime, n_); + } + // If H is fully fixed-size, its dimensions are set at compile time. + // Predict measurement and get Jacobian H = dh/dlocal(X) - Matrix H(m, n_); - Measurement prediction = h(X_, H); + Measurement prediction = h_func(X_, H); // Call the other update function update(prediction, H, z, R); } protected: - M X_; ///< Manifold state estimate. - Matrix P_; ///< Covariance in tangent space at X_ (always a dynamic Matrix). - int n_; ///< Tangent space dimension of M, determined at construction. + M X_; ///< Manifold state estimate. + Covariance P_; ///< Covariance (Eigen::Matrix). + int n_; ///< Runtime tangent space dimension of M. }; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 50730a8d4..7516cd1cc 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -36,14 +36,14 @@ namespace exampleUnit3 { // Define a measurement model: measure the z-component of the Unit3 direction // H is the Jacobian dh/d(local(p)) - Vector1 measureZ(const Unit3& p, OptionalJacobian<1, 2> H) { + double measureZ(const Unit3& p, OptionalJacobian<1, 2> H) { if (H) { // H = d(p.point3().z()) / d(local(p)) // Calculate numerically for simplicity in test - auto h = [](const Unit3& p_) { return Vector1(p_.point3().z()); }; - *H = numericalDerivative11(h, p); + auto h = [](const Unit3& p_) { return p_.point3().z(); }; + *H = numericalDerivative11(h, p); } - return Vector1(p.point3().z()); + return p.point3().z(); } } // namespace exampleUnit3 @@ -116,8 +116,8 @@ TEST(ManifoldEKF_Unit3, Update) { ManifoldEKF ekf(p_start, P_start); // Simulate a measurement (e.g., true value + noise) - Vector1 z_true = exampleUnit3::measureZ(p_start, {}); - Vector1 z_observed = z_true + Vector1(0.02); // Add some noise + double z_true = exampleUnit3::measureZ(p_start, {}); + double z_observed = z_true + 0.02; // Add some noise // --- Perform EKF update --- ekf.update(exampleUnit3::measureZ, z_observed, data.R); @@ -125,10 +125,10 @@ TEST(ManifoldEKF_Unit3, Update) { // --- Verification (Manual Kalman Update Steps) --- // 1. Predict measurement and get Jacobian H Matrix12 H; // Note: Jacobian is 1x2 for Unit3 - Vector1 z_pred = exampleUnit3::measureZ(p_start, H); + double z_pred = exampleUnit3::measureZ(p_start, H); // 2. Innovation and Covariance - Vector1 y = z_pred - z_observed; // Innovation (using vector subtraction for z) + double y = z_pred - z_observed; // Innovation (using vector subtraction for z) Matrix1 S = H * P_start * H.transpose() + data.R; // 1x1 matrix // 3. Kalman Gain K @@ -156,16 +156,15 @@ namespace exampleDynamicMatrix { } // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) - Vector1 measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { if (H) { // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] // trace = p(0,0) + p(1,1) // H = d(trace)/d(p_flat) = [1, 0, 0, 1] // The Jacobian H will be 1x4 for a 2x2 matrix. - H->resize(1, 4); *H << 1.0, 0.0, 0.0, 1.0; } - return Vector1(p(0, 0) + p(1, 1)); + return p(0, 0) + p(1, 1); } } // namespace exampleDynamicMatrix @@ -223,8 +222,8 @@ TEST(ManifoldEKF_DynamicMatrix, Update) { EXPECT_LONGS_EQUAL(4, ekf.state().size()); // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) - Vector1 zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here - Vector1 zObserved = zTrue - Vector1(0.03); // Add some "error" + double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + double zObserved = zTrue - 0.03; // Add some "error" // --- Perform EKF update --- ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); @@ -232,12 +231,12 @@ TEST(ManifoldEKF_DynamicMatrix, Update) { // --- Verification (Manual Kalman Update Steps) --- // 1. Predict measurement and get Jacobian H Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement - Vector1 zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); + double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); // 2. Innovation and Innovation Covariance // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) - // For Vector1 (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. - Vector1 innovationY = zObserved - zPredictionManual; + // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + double innovationY = zObserved - zPredictionManual; Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance; // 3. Kalman Gain K From ea783b0edb9a1698962ed1a542bcb7c2d536918d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 10:40:20 -0400 Subject: [PATCH 39/54] Reduce clutter --- gtsam/navigation/InvariantEKF.h | 11 +-- gtsam/navigation/LieGroupEKF.h | 21 +++--- gtsam/navigation/ManifoldEKF.h | 115 +++++++++++++++----------------- 3 files changed, 67 insertions(+), 80 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index c0d95db84..08ba24def 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -43,7 +43,8 @@ namespace gtsam { * * The state-dependent prediction methods from LieGroupEKF are hidden. * The update step remains the same as in ManifoldEKF/LieGroupEKF. - * Covariances (P, Q) are `Eigen::Matrix`. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. */ template class InvariantEKF : public LieGroupEKF { @@ -59,9 +60,9 @@ namespace gtsam { /** * Constructor: forwards to LieGroupEKF constructor. * @param X0 Initial state on Lie group G. - * @param P0_input Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). + * @param P0 Initial covariance in the tangent space at X0. */ - InvariantEKF(const G& X0, const Matrix& P0_input) : Base(X0, P0_input) {} + InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {} // We hide state-dependent predict methods from LieGroupEKF by only providing the // invariant predict methods below. @@ -73,7 +74,7 @@ namespace gtsam { * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. * * @param U Lie group element representing the motion increment. - * @param Q Process noise covariance (Eigen::Matrix). + * @param Q Process noise covariance. */ void predict(const G& U, const Covariance& Q) { this->X_ = this->X_.compose(U); @@ -91,7 +92,7 @@ namespace gtsam { * * @param u Tangent space control vector. * @param dt Time interval. - * @param Q Process noise covariance matrix (Eigen::Matrix). + * @param Q Process noise covariance matrix. */ void predict(const TangentVector& u, double dt, const Covariance& Q) { const G U = traits::Expmap(u * dt); diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index da41a1e3d..07efa0b0b 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -37,12 +37,13 @@ namespace gtsam { * @class LieGroupEKF * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF * - * @tparam G Lie group type providing group operations and Expmap/AdjointMap. - * Must satisfy LieGroup concept (`IsLieGroup::value`). + * @tparam G Lie group type (must satisfy LieGroup concept). * * This filter specializes ManifoldEKF for Lie groups, offering predict methods * with state-dependent dynamics functions. * Use the InvariantEKF class for prediction via group composition. + * For details on how static and dynamic dimensions are handled, please refer to + * the `ManifoldEKF` class documentation. */ template class LieGroupEKF : public ManifoldEKF { @@ -57,9 +58,9 @@ namespace gtsam { /** * Constructor: initialize with state and covariance. * @param X0 Initial state on Lie group G. - * @param P0 Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix). + * @param P0 Initial covariance in the tangent space at X0. */ - LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) { + LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } @@ -96,8 +97,7 @@ namespace gtsam { G X_next = this->X_.compose(U); if (A) { - // Full state transition Jacobian for left-invariant EKF: - // AdjointMap returns Jacobian (Eigen::Matrix) + // State transition Jacobian for left-invariant EKF: *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; } return X_next; @@ -113,11 +113,11 @@ namespace gtsam { * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @param f Dynamics functor. * @param dt Time step. - * @param Q Process noise covariance (Eigen::Matrix). + * @param Q Process noise covariance. */ template > void predict(Dynamics&& f, double dt, const Covariance& Q) { - Jacobian A; // Eigen::Matrix + Jacobian A; if constexpr (Dim == Eigen::Dynamic) { A.resize(this->n_, this->n_); } @@ -163,13 +163,10 @@ namespace gtsam { * @param f Dynamics functor. * @param u Control input. * @param dt Time step. - * @param Q Process noise covariance (Eigen::Matrix). + * @param Q Process noise covariance. */ template > void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) { - // Note: The lambda below captures f by reference. Ensure f's lifetime. - // If f is an rvalue, std::forward or std::move might be needed if the lambda is stored. - // Here, the lambda is temporary, so [&] is fine. return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 0f405c3ae..d45f1fdf9 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -18,9 +18,6 @@ * differentiable manifold. It relies on the manifold's retract and * localCoordinates operations. * - * Works with manifolds M that may have fixed or dynamic tangent space dimensions. - * Covariances and Jacobians leverage Eigen's static or dynamic matrices for efficiency. - * * @date April 24, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ @@ -40,22 +37,30 @@ namespace gtsam { /** - * @class ManifoldEKF - * @brief Extended Kalman Filter on a generic manifold M - * - * @tparam M Manifold type providing: - * - `traits` specialization must exist, defining - * `dimension` (compile-time or Eigen::Dynamic), - * `TangentVector`, `Retract`, and `LocalCoordinates`. - * If `dimension` is Eigen::Dynamic, `GetDimension(const M&)` - * must be provided by traits. - * - * This filter maintains a state X in the manifold M and covariance P in the - * tangent space at X. The covariance P is `Eigen::Matrix`, - * where Dim is the compile-time dimension of M (or Eigen::Dynamic). - * Prediction requires providing the predicted next state and the state transition Jacobian F. - * Updates apply a measurement function h and correct the state using the tangent space error. - */ + * @class ManifoldEKF + * @brief Extended Kalman Filter on a generic manifold M + * + * @tparam M Manifold type (must satisfy Manifold concept). + * + * This filter maintains a state X in the manifold M and covariance P in the + * tangent space at X. + * Prediction requires providing the predicted next state and the state transition Jacobian F. + * Updates apply a measurement function h and correct the state using the tangent space error. + * + * **Handling Static and Dynamic Dimensions:** + * The filter supports manifolds M with either a compile-time fixed dimension or a + * runtime dynamic dimension. This is determined by `gtsam::traits::dimension`. + * - If `dimension` is an integer (e.g., 3, 6), it's a fixed-size manifold. + * - If `dimension` is `Eigen::Dynamic`, it's a dynamically-sized manifold. In this case, + * `gtsam::traits::GetDimension(const M&)` must be available to retrieve the + * actual dimension at runtime. + * The internal protected member `n_` stores this runtime dimension. + * Covariance matrices (e.g., `P_`, method argument `Q`) and Jacobians (e.g., method argument `F`) + * are typed using `Covariance` and `Jacobian` typedefs, which are specializations of + * `Eigen::Matrix`, where `Dim` is `traits::dimension`. + * For dynamically-sized manifolds (`Dim == Eigen::Dynamic`), these Eigen types + * represent dynamically-sized matrices. + */ template class ManifoldEKF { public: @@ -73,30 +78,28 @@ namespace gtsam { /** * Constructor: initialize with state and covariance. * @param X0 Initial state on manifold M. - * @param P0 Initial covariance in the tangent space at X0. - * Provided as a dynamic gtsam::Matrix for flexibility, - * but will be stored internally as Covariance. + * @param P0 Initial covariance in the tangent space at X0 */ - ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0) { + ManifoldEKF(const M& X0, const Covariance& P0) : X_(X0) { static_assert(IsManifold::value, "Template parameter M must be a GTSAM Manifold."); if constexpr (Dim == Eigen::Dynamic) { - n_ = traits::GetDimension(X0); // Runtime dimension for dynamic case + n_ = traits::GetDimension(X0); + // Validate dimensions of initial covariance P0. + if (P0.rows() != n_ || P0.cols() != n_) { + throw std::invalid_argument( + "ManifoldEKF: Initial covariance P0 dimensions (" + + std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) + + ") do not match state's tangent space dimension (" + + std::to_string(n_) + ")."); + } } else { - n_ = Dim; // Runtime dimension is fixed compile-time dimension + n_ = Dim; } - // Validate dimensions of initial covariance P0. - if (P0.rows() != n_ || P0.cols() != n_) { - throw std::invalid_argument( - "ManifoldEKF: Initial covariance P0 dimensions (" + - std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) + - ") do not match state's tangent space dimension (" + - std::to_string(n_) + ")."); - } - P_ = P0; // Assigns MatrixXd to Eigen::Matrix + P_ = P0; } virtual ~ManifoldEKF() = default; @@ -104,10 +107,10 @@ namespace gtsam { /// @return current state estimate on manifold M. const M& state() const { return X_; } - /// @return current covariance estimate (Eigen::Matrix). + /// @return current covariance estimate. const Covariance& covariance() const { return P_; } - /// @return runtime dimension of the tangent space. + /// @return runtime dimension of the manifold. int dimension() const { return n_; } /** @@ -119,8 +122,8 @@ namespace gtsam { * state transition in local coordinates around X_k. * * @param X_next The predicted state at time k+1 on manifold M. - * @param F The state transition Jacobian (Eigen::Matrix). - * @param Q Process noise covariance matrix (Eigen::Matrix). + * @param F The state transition Jacobian. + * @param Q Process noise covariance matrix. */ void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { if constexpr (Dim == Eigen::Dynamic) { @@ -130,7 +133,6 @@ namespace gtsam { std::to_string(n_) + "."); } } - // For fixed Dim, types enforce dimensions. X_ = X_next; P_ = F * P_ * F.transpose() + Q; @@ -143,9 +145,8 @@ namespace gtsam { * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). * @param prediction Predicted measurement. * @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X). - * Type: Eigen::Matrix. * @param z Observed measurement. - * @param R Measurement noise covariance (Eigen::Matrix). + * @param R Measurement noise covariance. */ template void update(const Measurement& prediction, @@ -158,14 +159,13 @@ namespace gtsam { static constexpr int MeasDim = traits::dimension; - int m_runtime; // Measurement dimension at runtime + int m_runtime; if constexpr (MeasDim == Eigen::Dynamic) { m_runtime = traits::GetDimension(z); if (traits::GetDimension(prediction) != m_runtime) { throw std::invalid_argument( "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions."); } - // Runtime check for H and R if they involve dynamic dimensions if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) { throw std::invalid_argument( "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement."); @@ -173,8 +173,6 @@ namespace gtsam { } else { m_runtime = MeasDim; - // Types enforce dimensions for H and R if MeasDim and Dim are fixed. - // If Dim is dynamic but MeasDim is fixed, H.cols() needs check. if constexpr (Dim == Eigen::Dynamic) { if (H.cols() != n_) { throw std::invalid_argument( @@ -219,25 +217,23 @@ namespace gtsam { * Measurement update: Corrects the state and covariance using a measurement * model function. * - * @tparam Measurement Type of the measurement vector (e.g., VectorN, Vector). - * @tparam MeasurementFunction Functor/lambda with signature compatible with: - * `Measurement h(const M& x, Jac& H_jacobian)` - * where `Jac` can be `Eigen::Matrix&` or - * `OptionalJacobian&`. - * The Jacobian H should be d(h)/d(local(X)). + * @tparam Measurement Type of the measurement vector. + * @tparam MeasurementFunction Functor/lambda providing measurement and its Jacobian. + * Signature: `Measurement h(const M& x, Jac& H_jacobian)` + * where H = d(h)/d(local(X)). * @param h Measurement model function. * @param z Observed measurement. - * @param R Measurement noise covariance (Eigen::Matrix). + * @param R Measurement noise covariance. */ template - void update(MeasurementFunction&& h_func, const Measurement& z, + void update(MeasurementFunction&& h, const Measurement& z, const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold."); static constexpr int MeasDim = traits::dimension; - int m_runtime; // Measurement dimension at runtime + int m_runtime; if constexpr (MeasDim == Eigen::Dynamic) { m_runtime = traits::GetDimension(z); } @@ -245,16 +241,9 @@ namespace gtsam { m_runtime = MeasDim; } - // Prepare Jacobian H for the measurement function - Matrix H(m_runtime, n_); - if constexpr (MeasDim == Eigen::Dynamic || Dim == Eigen::Dynamic) { - // If H involves any dynamic dimension, it needs explicit resizing. - H.resize(m_runtime, n_); - } - // If H is fully fixed-size, its dimensions are set at compile time. - // Predict measurement and get Jacobian H = dh/dlocal(X) - Measurement prediction = h_func(X_, H); + Matrix H(m_runtime, n_); + Measurement prediction = h(X_, H); // Call the other update function update(prediction, H, z, R); From 2a22123d5d92572631b6e7fa1c9fff2f835f6c8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:46:25 -0400 Subject: [PATCH 40/54] Add AdjointMap into traits --- gtsam/base/Lie.h | 29 +++++++++++++++++++---------- gtsam/base/VectorSpace.h | 33 ++++++++++++++++++--------------- gtsam/navigation/InvariantEKF.h | 6 +++--- 3 files changed, 40 insertions(+), 28 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 4248f16b2..17c35446e 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -169,32 +169,34 @@ namespace internal { /// To use this for your gtsam type, define: /// template<> struct traits : public internal::LieGroupTraits {}; /// Assumes existence of: identity, dimension, localCoordinates, retract, -/// and additionally Logmap, Expmap, compose, between, and inverse +/// and additionally Logmap, Expmap, AdjointMap, compose, between, and inverse template -struct LieGroupTraits: public GetDimensionImpl { +struct LieGroupTraits : public GetDimensionImpl { using structure_category = lie_group_tag; /// @name Group /// @{ using group_flavor = multiplicative_group_tag; - static Class Identity() { return Class::Identity();} + static Class Identity() { return Class::Identity(); } /// @} /// @name Manifold /// @{ using ManifoldType = Class; + // Note: Class::dimension can be an int or Eigen::Dynamic. + // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj). inline constexpr static auto dimension = Class::dimension; using TangentVector = Eigen::Matrix; using ChartJacobian = OptionalJacobian; static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { - return origin.localCoordinates(other, Horigin, Hother); + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + return origin.localCoordinates(other, H1, H2); } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian Horigin = {}, ChartJacobian Hv = {}) { - return origin.retract(v, Horigin, Hv); + ChartJacobian H = {}, ChartJacobian Hv = {}) { + return origin.retract(v, H, Hv); } /// @} @@ -209,19 +211,26 @@ struct LieGroupTraits: public GetDimensionImpl { } static Class Compose(const Class& m1, const Class& m2, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.compose(m2, H1, H2); } static Class Between(const Class& m1, const Class& m2, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.between(m2, H1, H2); } static Class Inverse(const Class& m, // - ChartJacobian H = {}) { + ChartJacobian H = {}) { return m.inverse(H); } + + static Eigen::Matrix AdjointMap(const Class& m) { + // This assumes that the Class itself provides a member function `AdjointMap()` + // For dynamically-sized types (dimension == Eigen::Dynamic), + // m.AdjointMap() must return a gtsam::Matrix of the correct runtime dimensions. + return m.AdjointMap(); + } /// @} }; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 1a486886d..b81b19ac3 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -35,7 +35,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); - Class v = other-origin; + Class v = other - origin; return v.vector(); } @@ -82,12 +82,12 @@ struct VectorSpaceImpl { return -v; } - static LieAlgebra Hat(const TangentVector& v) { - return v; - } + static LieAlgebra Hat(const TangentVector& v) { return v; } - static TangentVector Vee(const LieAlgebra& X) { - return X; + static TangentVector Vee(const LieAlgebra& X) { return X; } + + static Jacobian AdjointMap(const Class& /*m*/) { + return Jacobian::Identity(); } /// @} }; @@ -118,7 +118,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(origin); if (H2) *H2 = Eye(other); - Class v = other-origin; + Class v = other - origin; return v.vector(); } @@ -141,8 +141,7 @@ struct VectorSpaceImpl { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Class result(v); - if (Hv) - *Hv = Eye(v); + if (Hv) *Hv = Eye(v); return result; } @@ -165,6 +164,7 @@ struct VectorSpaceImpl { return -v; } + static Eigen::MatrixXd AdjointMap(const Class& m) { return Eye(m); } /// @} }; @@ -273,8 +273,8 @@ struct ScalarTraits : VectorSpaceImpl { if (H) (*H)[0] = 1.0; return v[0]; } + // AdjointMap for ScalarTraits is inherited from VectorSpaceImpl /// @} - }; } // namespace internal @@ -352,6 +352,9 @@ struct traits > : if (H) *H = Jacobian::Identity(); return m + Eigen::Map(v.data()); } + + // AdjointMap for fixed-size Eigen matrices is inherited from + // internal::VectorSpaceImpl< Eigen::Matrix , M*N > /// @} }; @@ -404,7 +407,7 @@ struct DynamicTraits { static TangentVector Local(const Dynamic& m, const Dynamic& other, // ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(m); - if (H2) *H2 = Eye(m); + if (H2) *H2 = Eye(m); TangentVector v(GetDimension(m)); Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; return v; @@ -425,7 +428,7 @@ struct DynamicTraits { static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = Eye(m); TangentVector result(GetDimension(m)); - Eigen::Map(result.data(), m.cols(), m.rows()) = m; + Eigen::Map(result.data(), m.rows(), m.cols()) = m; return result; } @@ -460,7 +463,8 @@ struct DynamicTraits { static TangentVector Vee(const LieAlgebra& X) { return X; } - + + static Jacobian AdjointMap(const Dynamic& m) { return Eye(m); } /// @} }; @@ -505,5 +509,4 @@ private: T p, q, r; }; -} // namespace gtsam - +} // namespace gtsam diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 08ba24def..20d74285f 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -77,9 +77,9 @@ namespace gtsam { * @param Q Process noise covariance. */ void predict(const G& U, const Covariance& Q) { - this->X_ = this->X_.compose(U); - // TODO(dellaert): traits::AdjointMap should exist - const Jacobian A = traits::Inverse(U).AdjointMap(); + this->X_ = traits::Compose(this->X_, U); + const G U_inv = traits::Inverse(U); + const Jacobian A = traits::AdjointMap(U_inv); // P_ is Covariance. A is Jacobian. Q is Covariance. // All are Eigen::Matrix. this->P_ = A * this->P_ * A.transpose() + Q; From 192b6a26ffd178c8c778a95d8920741a89714b36 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:48:56 -0400 Subject: [PATCH 41/54] Deal with Matrix --- gtsam/navigation/InvariantEKF.h | 10 ++- gtsam/navigation/LieGroupEKF.h | 31 +++++-- gtsam/navigation/tests/testInvariantEKF.cpp | 93 +++++++++++++++++++++ 3 files changed, 125 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 20d74285f..a2eea3719 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -95,7 +95,15 @@ namespace gtsam { * @param Q Process noise covariance matrix. */ void predict(const TangentVector& u, double dt, const Covariance& Q) { - const G U = traits::Expmap(u * dt); + G U; + if constexpr (std::is_same_v) { + const Matrix& X = static_cast(this->X_); + U.resize(X.rows(), X.cols()); + Eigen::Map(static_cast(U).data(), U.size()) = u * dt; + } + else { + U = traits::Expmap(u * dt); + } predict(U, Q); // Call the group composition predict } diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 07efa0b0b..4421ab660 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -92,15 +92,30 @@ namespace gtsam { G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { Jacobian Df, Dexp; // Eigen::Matrix - TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) - G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt - G X_next = this->X_.compose(U); - - if (A) { - // State transition Jacobian for left-invariant EKF: - *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + if constexpr (std::is_same_v) { + std::cout << "We are here" << std::endl; + Jacobian Df; // Eigen::Matrix + TangentVector xi = f(this->X_, Df); + const Matrix& X = static_cast(this->X_); + G U = Matrix(X.rows(), X.cols()); + Eigen::Map(static_cast(U).data(), U.size()) = xi * dt; + if (A) { + const Matrix I_n = Matrix::Identity(this->n_, this->n_); + const Matrix& Dexp = I_n; + *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix + } + return this->X_ + U; // Matrix addition + } + else { + Jacobian Df, Dexp; // Eigen::Matrix + TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX) + G U = traits::Expmap(xi * dt, A ? &Dexp : nullptr); + if (A) { + // State transition Jacobian for left-invariant EKF: + *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + } + return this->X_.compose(U); } - return X_next; } diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 74a70fbbe..2916d6da0 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -119,6 +119,99 @@ TEST(IEKF_Pose2, PredictUpdateSequence) { } +// Define simple dynamics and measurement for a 2x2 Matrix state +namespace exampleDynamicMatrix { + // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + // This is mainly for verification; IEKF predict will use tangent vector directly. + Matrix predictNextStateManually(const Matrix& p, const Vector& vTangent, double dt) { + return traits::Retract(p, vTangent * dt); + } + // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) + double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { + // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] + // trace = p(0,0) + p(1,1) + // H = d(trace)/d(p_flat) = [1, 0, 0, 1] + // The Jacobian H will be 1x4 for a 2x2 matrix. + H->resize(1, p.size()); // p.size() is rows*cols + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2, so 1x4 + } + return p(0, 0) + p(1, 1); + } +} // namespace exampleDynamicMatrix + +// Test fixture for InvariantEKF with a 2x2 Matrix state +struct DynamicMatrixEKFTest { + Matrix p0Matrix; // Initial state (as 2x2 Matrix) + Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4) + Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix) + double dt; + Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4) + Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1) + DynamicMatrixEKFTest() : + p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()), + p0Covariance(I_4x4 * 0.01), + velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec + dt(0.1), + processNoiseCovariance(I_4x4 * 0.001), + measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005) { + } +}; + +TEST(InvariantEKF_DynamicMatrix, Predict) { + DynamicMatrixEKFTest data; + InvariantEKF ekf(data.p0Matrix, data.p0Covariance); + // For a 2x2 Matrix, tangent space dimension is 2*2=4. + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + // --- Perform EKF prediction using InvariantEKF::predict(tangentVector, dt, Q) --- + ekf.predict(data.velocityTangent, data.dt, data.processNoiseCovariance); + // --- Verification --- + // 1. Calculate expected next state + Matrix pNextExpected = exampleDynamicMatrix::predictNextStateManually(data.p0Matrix, data.velocityTangent, data.dt); + EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); + // 2. Calculate expected covariance + // For VectorSpace, AdjointMap is Identity. So P_next = P_prev + Q. + Matrix pCovarianceExpected = data.p0Covariance + data.processNoiseCovariance; + EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); +} + +TEST(InvariantEKF_DynamicMatrix, Update) { + DynamicMatrixEKFTest data; + Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); + Matrix pStartCovariance = I_4x4 * 0.02; + InvariantEKF ekf(pStartMatrix, pStartCovariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) + double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + double zObserved = zTrue - 0.03; // Add some "error" + // --- Perform EKF update --- + // The update method is inherited from ManifoldEKF. + ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); + // --- Verification (Manual Kalman Update Steps) --- + // 1. Predict measurement and get Jacobian H + Matrix H_manual(1, 4); // This will be 1x4 for a 2x2 matrix measurement + double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H_manual); + // 2. Innovation and Innovation Covariance + // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) + // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + double innovationY_tangent = zObserved - zPredictionManual; + Matrix innovationCovarianceS = H_manual * pStartCovariance * H_manual.transpose() + data.measurementNoiseCovariance; + // 3. Kalman Gain K + Matrix kalmanGainK = pStartCovariance * H_manual.transpose() * innovationCovarianceS.inverse(); // K is 4x1 + // 4. State Correction (in tangent space of Matrix) + Vector deltaXiTangent = kalmanGainK * innovationY_tangent; // deltaXi is 4x1 Vector + // 5. Expected Updated State and Covariance (using Joseph form) + Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); + Matrix I_KH = I_4x4 - kalmanGainK * H_manual; + Matrix pUpdatedCovarianceExpected = I_KH * pStartCovariance * I_KH.transpose() + kalmanGainK * data.measurementNoiseCovariance * kalmanGainK.transpose(); + // --- Compare EKF result with manual calculation --- + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 5758c7ef0cc83ab3d738d699947062d65af6b712 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:58:15 -0400 Subject: [PATCH 42/54] Simplify tests --- gtsam/navigation/tests/testInvariantEKF.cpp | 121 ++++++--------- gtsam/navigation/tests/testManifoldEKF.cpp | 157 +++++++++----------- 2 files changed, 118 insertions(+), 160 deletions(-) diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 2916d6da0..4797f79a1 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -121,97 +121,66 @@ TEST(IEKF_Pose2, PredictUpdateSequence) { // Define simple dynamics and measurement for a 2x2 Matrix state namespace exampleDynamicMatrix { - // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. - // This is mainly for verification; IEKF predict will use tangent vector directly. - Matrix predictNextStateManually(const Matrix& p, const Vector& vTangent, double dt) { + Matrix f(const Matrix& p, const Vector& vTangent, double dt) { return traits::Retract(p, vTangent * dt); } - // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) - double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { if (H) { - // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] - // trace = p(0,0) + p(1,1) - // H = d(trace)/d(p_flat) = [1, 0, 0, 1] - // The Jacobian H will be 1x4 for a 2x2 matrix. - H->resize(1, p.size()); // p.size() is rows*cols - (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2, so 1x4 + H->resize(1, p.size()); + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2 } - return p(0, 0) + p(1, 1); + return p(0, 0) + p(1, 1); // trace ! } } // namespace exampleDynamicMatrix -// Test fixture for InvariantEKF with a 2x2 Matrix state -struct DynamicMatrixEKFTest { - Matrix p0Matrix; // Initial state (as 2x2 Matrix) - Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4) - Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix) - double dt; - Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4) - Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1) - DynamicMatrixEKFTest() : - p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()), - p0Covariance(I_4x4 * 0.01), - velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec - dt(0.1), - processNoiseCovariance(I_4x4 * 0.001), - measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005) { - } -}; +TEST(InvariantEKF_DynamicMatrix, PredictAndUpdate) { + // --- Setup --- + Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix p0Covariance = I_4x4 * 0.01; + Vector velocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); + double dt = 0.1; + Matrix Q = I_4x4 * 0.001; + Matrix R = Matrix::Identity(1, 1) * 0.005; -TEST(InvariantEKF_DynamicMatrix, Predict) { - DynamicMatrixEKFTest data; - InvariantEKF ekf(data.p0Matrix, data.p0Covariance); - // For a 2x2 Matrix, tangent space dimension is 2*2=4. - EXPECT_LONGS_EQUAL(4, ekf.state().size()); - EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size()); - EXPECT_LONGS_EQUAL(4, ekf.dimension()); - // --- Perform EKF prediction using InvariantEKF::predict(tangentVector, dt, Q) --- - ekf.predict(data.velocityTangent, data.dt, data.processNoiseCovariance); - // --- Verification --- - // 1. Calculate expected next state - Matrix pNextExpected = exampleDynamicMatrix::predictNextStateManually(data.p0Matrix, data.velocityTangent, data.dt); - EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); - // 2. Calculate expected covariance - // For VectorSpace, AdjointMap is Identity. So P_next = P_prev + Q. - Matrix pCovarianceExpected = data.p0Covariance + data.processNoiseCovariance; - EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); -} - -TEST(InvariantEKF_DynamicMatrix, Update) { - DynamicMatrixEKFTest data; - Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); - Matrix pStartCovariance = I_4x4 * 0.02; - InvariantEKF ekf(pStartMatrix, pStartCovariance); + InvariantEKF ekf(p0Matrix, p0Covariance); EXPECT_LONGS_EQUAL(4, ekf.state().size()); EXPECT_LONGS_EQUAL(4, ekf.dimension()); - // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) - double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here - double zObserved = zTrue - 0.03; // Add some "error" - // --- Perform EKF update --- - // The update method is inherited from ManifoldEKF. - ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); - // --- Verification (Manual Kalman Update Steps) --- - // 1. Predict measurement and get Jacobian H - Matrix H_manual(1, 4); // This will be 1x4 for a 2x2 matrix measurement - double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H_manual); - // 2. Innovation and Innovation Covariance - // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) - // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + + // --- Predict --- + ekf.predict(velocityTangent, dt, Q); + + // Verification for Predict + Matrix pPredictedExpected = exampleDynamicMatrix::f(p0Matrix, velocityTangent, dt); + Matrix pCovariancePredictedExpected = p0Covariance + Q; + EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9)); + + // --- Update --- + // Use the state after prediction for the update step + Matrix pStateBeforeUpdate = ekf.state(); + Matrix pCovarianceBeforeUpdate = ekf.covariance(); + + double zTrue = exampleDynamicMatrix::h(pStateBeforeUpdate); + double zObserved = zTrue - 0.03; // Simulated measurement with some error + + ekf.update(exampleDynamicMatrix::h, zObserved, R); + + // Verification for Update (Manual Kalman Steps) + Matrix H(1, 4); + double zPredictionManual = exampleDynamicMatrix::h(pStateBeforeUpdate, H); double innovationY_tangent = zObserved - zPredictionManual; - Matrix innovationCovarianceS = H_manual * pStartCovariance * H_manual.transpose() + data.measurementNoiseCovariance; - // 3. Kalman Gain K - Matrix kalmanGainK = pStartCovariance * H_manual.transpose() * innovationCovarianceS.inverse(); // K is 4x1 - // 4. State Correction (in tangent space of Matrix) - Vector deltaXiTangent = kalmanGainK * innovationY_tangent; // deltaXi is 4x1 Vector - // 5. Expected Updated State and Covariance (using Joseph form) - Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); - Matrix I_KH = I_4x4 - kalmanGainK * H_manual; - Matrix pUpdatedCovarianceExpected = I_KH * pStartCovariance * I_KH.transpose() + kalmanGainK * data.measurementNoiseCovariance * kalmanGainK.transpose(); - // --- Compare EKF result with manual calculation --- + Matrix S = H * pCovarianceBeforeUpdate * H.transpose() + R; + Matrix kalmanGainK = pCovarianceBeforeUpdate * H.transpose() * S.inverse(); + Vector deltaXiTangent = kalmanGainK * innovationY_tangent; + Matrix pUpdatedExpected = traits::Retract(pStateBeforeUpdate, deltaXiTangent); + Matrix I_KH = I_4x4 - kalmanGainK * H; + Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + kalmanGainK * R * kalmanGainK.transpose(); + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); } + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 7516cd1cc..8fe96557d 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -151,110 +151,99 @@ TEST(ManifoldEKF_Unit3, Update) { namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. Matrix predictNextState(const Matrix& p, const Vector& vTangent, double dt) { return traits::Retract(p, vTangent * dt); } // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) - double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { - if (H) { - // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] - // trace = p(0,0) + p(1,1) - // H = d(trace)/d(p_flat) = [1, 0, 0, 1] - // The Jacobian H will be 1x4 for a 2x2 matrix. - *H << 1.0, 0.0, 0.0, 1.0; + // H is the Jacobian dh/d(flatten(p)) + double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H_opt = {}) { + if (H_opt) { + // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). + // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. + Matrix H = Matrix::Zero(1, p.size()); + if (p.rows() >= 2 && p.cols() >= 2) { // Ensure it's at least 2x2 for trace definition + H(0, 0) = 1.0; // d(trace)/dp00 + H(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) + // For 2x2: H(0, 2*1 + 1) = H(0,3) + } + *H_opt = H; } - return p(0, 0) + p(1, 1); + if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error + double trace = 0.0; + for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) { + trace += p(i, i); + } + return trace; } } // namespace exampleDynamicMatrix -// Test fixture for ManifoldEKF with a 2x2 Matrix state -struct DynamicMatrixEKFTest { - Matrix p0Matrix; // Initial state (as 2x2 Matrix) - Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4) - Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix) - double dt; - Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4) - Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1) +TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) { + Matrix p_initial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix P_initial = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) + Vector v_tangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec + double dt = 0.1; + Matrix Q = I_4x4 * 0.001; // Process noise covariance (4x4) + Matrix R = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1) - DynamicMatrixEKFTest() : - p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()), - p0Covariance(I_4x4 * 0.01), - velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec - dt(0.1), - processNoiseCovariance(I_4x4 * 0.001), - measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005) - { - } -}; - - -TEST(ManifoldEKF_DynamicMatrix, Predict) { - DynamicMatrixEKFTest data; - - ManifoldEKF ekf(data.p0Matrix, data.p0Covariance); + ManifoldEKF ekf(p_initial, P_initial); // For a 2x2 Matrix, tangent space dimension is 2*2=4. EXPECT_LONGS_EQUAL(4, ekf.state().size()); - EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size()); + EXPECT_LONGS_EQUAL(p_initial.rows() * p_initial.cols(), ekf.state().size()); - // --- Prepare inputs for ManifoldEKF::predict --- - Matrix pNextExpected = exampleDynamicMatrix::predictNextState(data.p0Matrix, data.velocityTangent, data.dt); + // Predict Step + Matrix p_predicted_mean = exampleDynamicMatrix::predictNextState(p_initial, v_tangent, dt); - // For this linear prediction model (p_next = p_current + V*dt in tangent space), F is Identity. - Matrix jacobianF = I_4x4; // Jacobian of the state transition function + // For this linear prediction model (p_next = p_current + V*dt in tangent space), + // Derivative w.r.t delta_xi is Identity. + Matrix F = I_4x4; - // --- Perform EKF prediction --- - ekf.predict(pNextExpected, jacobianF, data.processNoiseCovariance); + ekf.predict(p_predicted_mean, F, Q); - // --- Verification --- - EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); - Matrix pCovarianceExpected = jacobianF * data.p0Covariance * jacobianF.transpose() + data.processNoiseCovariance; - EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(p_predicted_mean, ekf.state(), 1e-9)); + Matrix P_predicted_expected = F * P_initial * F.transpose() + Q; + EXPECT(assert_equal(P_predicted_expected, ekf.covariance(), 1e-9)); + + // Update Step + Matrix p_current_for_update = ekf.state(); + Matrix P_current_for_update = ekf.covariance(); + + // True trace of p_current_for_update (which is p_predicted_mean) + // p_predicted_mean = p_initial + v_tangent*dt + // = [1.0, 2.0; 3.0, 4.0] + [0.05, -0.01; 0.01, -0.05] (v_tangent reshaped to 2x2 col-major) + // Trace = 1.05 + 3.95 = 5.0 + double z_true = exampleDynamicMatrix::measureTrace(p_current_for_update); + EXPECT_DOUBLES_EQUAL(5.0, z_true, 1e-9); + double z_observed = z_true - 0.03; + + ekf.update(exampleDynamicMatrix::measureTrace, z_observed, R); + + // Manual Kalman Update Steps for Verification + Matrix H(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) + double z_prediction_manual = exampleDynamicMatrix::measureTrace(p_current_for_update, H); + Matrix H_expected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); + EXPECT(assert_equal(H_expected, H, 1e-9)); + + + // Innovation: y = z_observed - z_prediction_manual (since measurement is double) + double y_innovation = z_observed - z_prediction_manual; + Matrix S = H * P_current_for_update * H.transpose() + R; // S is 1x1 + + Matrix K_gain = P_current_for_update * H.transpose() * S.inverse(); // K is 4x1 + + // State Correction (in tangent space of Matrix) + Vector delta_xi_tangent = K_gain * y_innovation; // delta_xi is 4x1 Vector + + Matrix p_updated_manual_expected = traits::Retract(p_current_for_update, delta_xi_tangent); + Matrix P_updated_manual_expected = (I_4x4 - K_gain * H) * P_current_for_update; + + EXPECT(assert_equal(p_updated_manual_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P_updated_manual_expected, ekf.covariance(), 1e-9)); } -TEST(ManifoldEKF_DynamicMatrix, Update) { - DynamicMatrixEKFTest data; - Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); - Matrix pStartCovariance = I_4x4 * 0.02; - ManifoldEKF ekf(pStartMatrix, pStartCovariance); - EXPECT_LONGS_EQUAL(4, ekf.state().size()); - - // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) - double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here - double zObserved = zTrue - 0.03; // Add some "error" - - // --- Perform EKF update --- - ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); - - // --- Verification (Manual Kalman Update Steps) --- - // 1. Predict measurement and get Jacobian H - Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement - double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); - - // 2. Innovation and Innovation Covariance - // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) - // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. - double innovationY = zObserved - zPredictionManual; - Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance; - - // 3. Kalman Gain K - Matrix kalmanGainK = pStartCovariance * H.transpose() * innovationCovarianceS.inverse(); // K is 4x1 - - // 4. State Correction (in tangent space of Matrix) - Vector deltaXiTangent = kalmanGainK * innovationY; // deltaXi is 4x1 Vector - - // 5. Expected Updated State and Covariance - Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); - - // Covariance update: P_new = (I - K H) P_old - Matrix pUpdatedCovarianceExpected = (I_4x4 - kalmanGainK * H) * pStartCovariance; - - // --- Compare EKF result with manual calculation --- - EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); - EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); -} int main() { TestResult tr; From 8efce78419a3c220fb7fd048f89e9a499b2d8798 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 17:46:52 -0400 Subject: [PATCH 43/54] Added dynamic test for LieGroupEKF as well --- gtsam/navigation/InvariantEKF.h | 1 + gtsam/navigation/LieGroupEKF.h | 13 +--- gtsam/navigation/tests/testLieGroupEKF.cpp | 88 ++++++++++++++++++++++ 3 files changed, 93 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index a2eea3719..d33e0829b 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -97,6 +97,7 @@ namespace gtsam { void predict(const TangentVector& u, double dt, const Covariance& Q) { G U; if constexpr (std::is_same_v) { + // Specialize to Matrix case as its Expmap is not defined. const Matrix& X = static_cast(this->X_); U.resize(X.rows(), X.cols()); Eigen::Map(static_cast(U).data(), U.size()) = u * dt; diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 4421ab660..007ed3ad0 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -93,21 +93,16 @@ namespace gtsam { Jacobian Df, Dexp; // Eigen::Matrix if constexpr (std::is_same_v) { - std::cout << "We are here" << std::endl; - Jacobian Df; // Eigen::Matrix - TangentVector xi = f(this->X_, Df); - const Matrix& X = static_cast(this->X_); - G U = Matrix(X.rows(), X.cols()); - Eigen::Map(static_cast(U).data(), U.size()) = xi * dt; + // Specialize to Matrix case as its Expmap is not defined. + TangentVector xi = f(this->X_, A ? &Df : nullptr); + const Matrix nextX = traits::Retract(this->X_, xi * dt, A ? &Dexp : nullptr); // just addition if (A) { const Matrix I_n = Matrix::Identity(this->n_, this->n_); - const Matrix& Dexp = I_n; *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix } - return this->X_ + U; // Matrix addition + return nextX; } else { - Jacobian Df, Dexp; // Eigen::Matrix TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX) G U = traits::Expmap(xi * dt, A ? &Dexp : nullptr); if (A) { diff --git a/gtsam/navigation/tests/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp index 62a35473d..f70511293 100644 --- a/gtsam/navigation/tests/testLieGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -108,6 +108,94 @@ TEST(GroupeEKF, StateAndControl) { EXPECT(assert_equal(expectedH, actualH)); } +// Namespace for dynamic Matrix LieGroupEKF test +namespace exampleLieGroupDynamicMatrix { + // Constant tangent vector for dynamics (same as "velocityTangent" in IEKF test) + const Vector kFixedVelocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); + + // Dynamics function: xi = f(X, H_X) + // Returns a constant tangent vector, so Df_DX = 0. + // H_X is D(xi)/D(X_local), where X_local is the tangent space perturbation of X. + Vector f(const Matrix& X, OptionalJacobian H_X = {}) { + if (H_X) { + size_t state_dim = X.size(); + size_t tangent_dim = kFixedVelocityTangent.size(); + // Ensure Jacobian dimensions are consistent even if state or tangent is 0-dim + H_X->setZero(tangent_dim, state_dim); + } + return kFixedVelocityTangent; + } + + // Measurement function h(X, H_p) + // H_p is D(h)/D(X_local) + double h(const Matrix& p, OptionalJacobian<-1, -1> H_p = {}) { + if (p.size() == 0) { + if (H_p) H_p->resize(1, 0); // Jacobian dh/dX_local is 1x0 + return 0.0; // Define trace of empty matrix as 0 for consistency + } + // This test specifically uses 2x2 matrices (size 4) + if (H_p) { + H_p->resize(1, p.size()); // p.size() is 4 + (*H_p) << 1.0, 0.0, 0.0, 1.0; // d(trace)/d(p_flat) for p_flat = [p00,p10,p01,p11] + } + return p(0, 0) + p(1, 1); // trace + } +} // namespace exampleLieGroupDynamicMatrix + +TEST(LieGroupEKF_DynamicMatrix, PredictAndUpdate) { + // --- Setup --- + Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix p0Covariance = I_4x4 * 0.01; + double dt = 0.1; + Matrix process_noise_Q = I_4x4 * 0.001; + Matrix measurement_noise_R = Matrix::Identity(1, 1) * 0.005; + + LieGroupEKF ekf(p0Matrix, p0Covariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + + // --- Predict --- + // ekf.predict takes f(X, H_X), dt, process_noise_Q + ekf.predict(exampleLieGroupDynamicMatrix::f, dt, process_noise_Q); + + // Verification for Predict + // For f, Df_DXk = 0 (Jacobian of xi w.r.t X_local is Zero). + // State transition Jacobian A = Ad_Uinv + Dexp * Df_DXk * dt. + // For Matrix (VectorSpace): Ad_Uinv = I, Dexp = I. + // So, A = I + I * 0 * dt = I. + // Covariance update: P_next = A * P_current * A.transpose() + Q = I * P_current * I + Q = P_current + Q. + Matrix pPredictedExpected = traits::Retract(p0Matrix, exampleLieGroupDynamicMatrix::kFixedVelocityTangent * dt); + Matrix pCovariancePredictedExpected = p0Covariance + process_noise_Q; + + EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9)); + + // --- Update --- + Matrix pStateBeforeUpdate = ekf.state(); + Matrix pCovarianceBeforeUpdate = ekf.covariance(); + + double zTrue = exampleLieGroupDynamicMatrix::h(pStateBeforeUpdate); + double zObserved = zTrue - 0.03; // Simulated measurement with some error + + ekf.update(exampleLieGroupDynamicMatrix::h, zObserved, measurement_noise_R); + + // Verification for Update (Manual Kalman Steps) + Matrix H_update(1, 4); // Measurement Jacobian: 1x4 for 2x2 matrix, trace measurement + double zPredictionManual = exampleLieGroupDynamicMatrix::h(pStateBeforeUpdate, H_update); + // Innovation: y_tangent = traits::Local(prediction, observation) + // For double (scalar), Local(A,B) is B-A. + double innovationY_tangent = zObserved - zPredictionManual; + Matrix S_innovation_cov = H_update * pCovarianceBeforeUpdate * H_update.transpose() + measurement_noise_R; + Matrix K_gain = pCovarianceBeforeUpdate * H_update.transpose() * S_innovation_cov.inverse(); + Vector deltaXiTangent = K_gain * innovationY_tangent; // Tangent space correction for Matrix state + Matrix pUpdatedExpected = traits::Retract(pStateBeforeUpdate, deltaXiTangent); + Matrix I_KH = I_4x4 - K_gain * H_update; // I_4x4 because state dimension is 4 + Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + K_gain * measurement_noise_R * K_gain.transpose(); + + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From da09c4a2c30226c10e05d88ae8d2d118a5c1c689 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 17:57:21 -0400 Subject: [PATCH 44/54] naming --- gtsam/navigation/tests/testManifoldEKF.cpp | 104 ++++++++++----------- 1 file changed, 47 insertions(+), 57 deletions(-) diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 8fe96557d..ff7a9a68d 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -30,7 +30,7 @@ using namespace gtsam; namespace exampleUnit3 { // Predicts the next state given current state, tangent velocity, and dt - Unit3 predictNextState(const Unit3& p, const Vector2& v, double dt) { + Unit3 f(const Unit3& p, const Vector2& v, double dt) { return p.retract(v * dt); } @@ -76,13 +76,13 @@ TEST(ManifoldEKF_Unit3, Predict) { // --- Prepare inputs for ManifoldEKF::predict --- // 1. Compute expected next state - Unit3 p_next_expected = exampleUnit3::predictNextState(data.p0, data.velocity, data.dt); + Unit3 p_next_expected = exampleUnit3::f(data.p0, data.velocity, data.dt); // 2. Compute state transition Jacobian F = d(local(p_next)) / d(local(p)) - // We can compute this numerically using the predictNextState function. + // We can compute this numerically using the f function. // GTSAM's numericalDerivative handles derivatives *between* manifolds. auto predict_wrapper = [&](const Unit3& p) -> Unit3 { - return exampleUnit3::predictNextState(p, data.velocity, data.dt); + return exampleUnit3::f(p, data.velocity, data.dt); }; Matrix2 F = numericalDerivative11(predict_wrapper, data.p0); @@ -100,7 +100,7 @@ TEST(ManifoldEKF_Unit3, Predict) { // Check F manually for a simple case (e.g., zero velocity should give Identity) Vector2 zero_velocity = Vector2::Zero(); auto predict_wrapper_zero = [&](const Unit3& p) -> Unit3 { - return exampleUnit3::predictNextState(p, zero_velocity, data.dt); + return exampleUnit3::f(p, zero_velocity, data.dt); }; Matrix2 F_zero = numericalDerivative11(predict_wrapper_zero, data.p0); EXPECT(assert_equal(I_2x2, F_zero, 1e-8)); @@ -152,23 +152,19 @@ namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. - Matrix predictNextState(const Matrix& p, const Vector& vTangent, double dt) { + Matrix f(const Matrix& p, const Vector& vTangent, double dt) { return traits::Retract(p, vTangent * dt); } // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) // H is the Jacobian dh/d(flatten(p)) - double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H_opt = {}) { - if (H_opt) { + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. - Matrix H = Matrix::Zero(1, p.size()); - if (p.rows() >= 2 && p.cols() >= 2) { // Ensure it's at least 2x2 for trace definition - H(0, 0) = 1.0; // d(trace)/dp00 - H(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) - // For 2x2: H(0, 2*1 + 1) = H(0,3) - } - *H_opt = H; + H->resize(1, p.size()); + (*H)(0, 0) = 1.0; // d(trace)/dp00 + (*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) } if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error double trace = 0.0; @@ -181,70 +177,64 @@ namespace exampleDynamicMatrix { } // namespace exampleDynamicMatrix TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) { - Matrix p_initial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); - Matrix P_initial = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) - Vector v_tangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec - double dt = 0.1; - Matrix Q = I_4x4 * 0.001; // Process noise covariance (4x4) - Matrix R = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1) + Matrix pInitial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix pInitialCovariance = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) + Vector vTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec + double deltaTime = 0.1; + Matrix processNoiseCovariance = I_4x4 * 0.001; // Process noise covariance (4x4) + Matrix measurementNoiseCovariance = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1) - ManifoldEKF ekf(p_initial, P_initial); + ManifoldEKF ekf(pInitial, pInitialCovariance); // For a 2x2 Matrix, tangent space dimension is 2*2=4. EXPECT_LONGS_EQUAL(4, ekf.state().size()); - EXPECT_LONGS_EQUAL(p_initial.rows() * p_initial.cols(), ekf.state().size()); + EXPECT_LONGS_EQUAL(pInitial.rows() * pInitial.cols(), ekf.state().size()); // Predict Step - Matrix p_predicted_mean = exampleDynamicMatrix::predictNextState(p_initial, v_tangent, dt); + Matrix pPredictedMean = exampleDynamicMatrix::f(pInitial, vTangent, deltaTime); - // For this linear prediction model (p_next = p_current + V*dt in tangent space), - // Derivative w.r.t delta_xi is Identity. - Matrix F = I_4x4; + // For this linear prediction model (pNext = pCurrent + V*dt in tangent space), + // Derivative w.r.t deltaXi is Identity. + Matrix fJacobian = I_4x4; - ekf.predict(p_predicted_mean, F, Q); + ekf.predict(pPredictedMean, fJacobian, processNoiseCovariance); - EXPECT(assert_equal(p_predicted_mean, ekf.state(), 1e-9)); - Matrix P_predicted_expected = F * P_initial * F.transpose() + Q; - EXPECT(assert_equal(P_predicted_expected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(pPredictedMean, ekf.state(), 1e-9)); + Matrix pPredictedCovarianceExpected = fJacobian * pInitialCovariance * fJacobian.transpose() + processNoiseCovariance; + EXPECT(assert_equal(pPredictedCovarianceExpected, ekf.covariance(), 1e-9)); // Update Step - Matrix p_current_for_update = ekf.state(); - Matrix P_current_for_update = ekf.covariance(); + Matrix pCurrentForUpdate = ekf.state(); + Matrix pCurrentCovarianceForUpdate = ekf.covariance(); - // True trace of p_current_for_update (which is p_predicted_mean) - // p_predicted_mean = p_initial + v_tangent*dt - // = [1.0, 2.0; 3.0, 4.0] + [0.05, -0.01; 0.01, -0.05] (v_tangent reshaped to 2x2 col-major) - // Trace = 1.05 + 3.95 = 5.0 - double z_true = exampleDynamicMatrix::measureTrace(p_current_for_update); - EXPECT_DOUBLES_EQUAL(5.0, z_true, 1e-9); - double z_observed = z_true - 0.03; + // True trace of pCurrentForUpdate (which is pPredictedMean) + double zTrue = exampleDynamicMatrix::h(pCurrentForUpdate); + EXPECT_DOUBLES_EQUAL(5.0, zTrue, 1e-9); + double zObserved = zTrue - 0.03; - ekf.update(exampleDynamicMatrix::measureTrace, z_observed, R); + ekf.update(exampleDynamicMatrix::h, zObserved, measurementNoiseCovariance); // Manual Kalman Update Steps for Verification - Matrix H(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) - double z_prediction_manual = exampleDynamicMatrix::measureTrace(p_current_for_update, H); - Matrix H_expected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); - EXPECT(assert_equal(H_expected, H, 1e-9)); + Matrix hJacobian(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) + double zPredictionManual = exampleDynamicMatrix::h(pCurrentForUpdate, hJacobian); + Matrix hJacobianExpected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); + EXPECT(assert_equal(hJacobianExpected, hJacobian, 1e-9)); + // Innovation: y = zObserved - zPredictionManual (since measurement is double) + double yInnovation = zObserved - zPredictionManual; + Matrix innovationCovariance = hJacobian * pCurrentCovarianceForUpdate * hJacobian.transpose() + measurementNoiseCovariance; - // Innovation: y = z_observed - z_prediction_manual (since measurement is double) - double y_innovation = z_observed - z_prediction_manual; - Matrix S = H * P_current_for_update * H.transpose() + R; // S is 1x1 - - Matrix K_gain = P_current_for_update * H.transpose() * S.inverse(); // K is 4x1 + Matrix kalmanGain = pCurrentCovarianceForUpdate * hJacobian.transpose() * innovationCovariance.inverse(); // K is 4x1 // State Correction (in tangent space of Matrix) - Vector delta_xi_tangent = K_gain * y_innovation; // delta_xi is 4x1 Vector + Vector deltaXiTangent = kalmanGain * yInnovation; // deltaXi is 4x1 Vector - Matrix p_updated_manual_expected = traits::Retract(p_current_for_update, delta_xi_tangent); - Matrix P_updated_manual_expected = (I_4x4 - K_gain * H) * P_current_for_update; + Matrix pUpdatedManualExpected = traits::Retract(pCurrentForUpdate, deltaXiTangent); + Matrix pUpdatedCovarianceManualExpected = (I_4x4 - kalmanGain * hJacobian) * pCurrentCovarianceForUpdate; - EXPECT(assert_equal(p_updated_manual_expected, ekf.state(), 1e-9)); - EXPECT(assert_equal(P_updated_manual_expected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(pUpdatedManualExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceManualExpected, ekf.covariance(), 1e-9)); } - - int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 87797c687e0219b7e7ab66092b49821da9ee5109 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 10 May 2025 10:29:47 -0400 Subject: [PATCH 45/54] Fix bug in Debug --- gtsam/navigation/tests/testLieGroupEKF.cpp | 20 +++++++++----------- gtsam/navigation/tests/testManifoldEKF.cpp | 20 +++++++------------- 2 files changed, 16 insertions(+), 24 deletions(-) diff --git a/gtsam/navigation/tests/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp index f70511293..031ab743c 100644 --- a/gtsam/navigation/tests/testLieGroupEKF.cpp +++ b/gtsam/navigation/tests/testLieGroupEKF.cpp @@ -126,19 +126,17 @@ namespace exampleLieGroupDynamicMatrix { return kFixedVelocityTangent; } - // Measurement function h(X, H_p) - // H_p is D(h)/D(X_local) - double h(const Matrix& p, OptionalJacobian<-1, -1> H_p = {}) { - if (p.size() == 0) { - if (H_p) H_p->resize(1, 0); // Jacobian dh/dX_local is 1x0 - return 0.0; // Define trace of empty matrix as 0 for consistency + // Measurement function h(X, H) + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + // Specialized for a 2x2 matrix! + if (p.rows() != 2 || p.cols() != 2) { + throw std::invalid_argument("Matrix must be 2x2."); } - // This test specifically uses 2x2 matrices (size 4) - if (H_p) { - H_p->resize(1, p.size()); // p.size() is 4 - (*H_p) << 1.0, 0.0, 0.0, 1.0; // d(trace)/d(p_flat) for p_flat = [p00,p10,p01,p11] + if (H) { + H->resize(1, p.size()); + *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11 } - return p(0, 0) + p(1, 1); // trace + return p(0, 0) + p(1, 1); // Trace of the matrix } } // namespace exampleLieGroupDynamicMatrix diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index ff7a9a68d..50edbe049 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -151,27 +151,21 @@ TEST(ManifoldEKF_Unit3, Update) { namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. - // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. Matrix f(const Matrix& p, const Vector& vTangent, double dt) { - return traits::Retract(p, vTangent * dt); + return traits::Retract(p, vTangent * dt); // + } // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) - // H is the Jacobian dh/d(flatten(p)) double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + // Specialized for a 2x2 matrix! + if (p.rows() != 2 || p.cols() != 2) { + throw std::invalid_argument("Matrix must be 2x2."); + } if (H) { - // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). - // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. H->resize(1, p.size()); - (*H)(0, 0) = 1.0; // d(trace)/dp00 - (*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) + *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11 } - if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error - double trace = 0.0; - for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) { - trace += p(i, i); - } - return trace; + return p(0, 0) + p(1, 1); // Trace of the matrix } } // namespace exampleDynamicMatrix From 1c5544e53b1ac630d1ad63b5e0630fd70c8b6553 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 10 May 2025 11:54:20 -0400 Subject: [PATCH 46/54] Fix figure to autosize, also remove menu --- python/gtsam/examples/EKF_SLAM.ipynb | 29 +++++++++++++++++---------- python/gtsam/examples/gtsam_plotly.py | 7 ++++--- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index c69f408f0..7309c5adb 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -102,7 +102,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 3, "id": "params-code-fls", "metadata": {}, "outputs": [], @@ -145,7 +145,7 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 4, "id": "ground-truth-call-fls", "metadata": {}, "outputs": [ @@ -196,7 +196,7 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 5, "id": "smoother-init-code", "metadata": {}, "outputs": [ @@ -281,7 +281,7 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 6, "id": "smoother-loop-code", "metadata": {}, "outputs": [ @@ -295,7 +295,7 @@ { "data": { "application/vnd.jupyter.widget-view+json": { - "model_id": "65c6048ecb324a96ac367007900bb015", + "model_id": "21976a68ac2846559c4b7b45e35d06e2", "version_major": 2, "version_minor": 0 }, @@ -402,7 +402,7 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 7, "id": "plotly-animation-call-code-fls", "metadata": { "tags": [] @@ -418,7 +418,7 @@ { "data": { "application/vnd.jupyter.widget-view+json": { - "model_id": "7a03e84472a1445ebfd1e59f4a5b76ff", + "model_id": "a0452bf011b144be88c1c3c1a17465b5", "version_major": 2, "version_minor": 0 }, @@ -441,6 +441,7 @@ "data": { "application/vnd.plotly.v1+json": { "config": { + "displayModeBar": false, "plotlyServerURL": "https://plot.ly" }, "data": [ @@ -14455,6 +14456,7 @@ } ], "layout": { + "autosize": true, "height": 600, "hovermode": "closest", "images": [ @@ -14478,6 +14480,12 @@ "x": 0.52, "y": 1 }, + "margin": { + "b": 50, + "l": 0, + "r": 0, + "t": 50 + }, "shapes": [ { "fillcolor": "rgba(255,0,255,0.2)", @@ -14491,7 +14499,7 @@ "yref": "y" } ], - "showlegend": true, + "showlegend": false, "sliders": [ { "active": 0, @@ -16337,7 +16345,7 @@ "direction": "left", "pad": { "r": 10, - "t": 87 + "t": 20 }, "showactive": false, "type": "buttons", @@ -16347,7 +16355,6 @@ "yanchor": "top" } ], - "width": 1000, "xaxis": { "constrain": "domain", "domain": [ @@ -16390,7 +16397,7 @@ ")\n", "\n", "print(\"Displaying animation...\")\n", - "fig.show()" + "fig.show(config={'displayModeBar': False})" ] }, { diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index 06bf73a67..689ee10db 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -353,7 +353,7 @@ def configure_figure_layout( type="buttons", showactive=False, direction="left", - pad={"r": 10, "t": 87}, + pad={"r": 10, "t": 20}, x=plot_domain[0], xanchor="left", y=0, @@ -401,20 +401,21 @@ def configure_figure_layout( scaleratio=1, domain=[0, 1], ), - width=1000, + autosize=True, height=600, hovermode="closest", updatemenus=updatemenus, sliders=sliders, shapes=initial_shapes, # Initial shapes (frame 0) images=([initial_image] if initial_image else []), # Initial image (frame 0) - showlegend=True, # Keep legend for clarity legend=dict( x=plot_domain[0], y=1, traceorder="normal", # Position legend bgcolor="rgba(255,255,255,0.5)", ), + showlegend=False, + margin=dict(l=0, r=0, t=50, b=50), ) From 222234df4fcc93bcc47aa939a822f7a9ba81ff22 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 May 2025 16:05:23 -0400 Subject: [PATCH 47/54] include IncrementalFixedLagSmoother from gtsam and not gtsam_unstable --- .../nonlinear/tests/testIncrementalFixedLagSmoother.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 269177b5d..a31f3646f 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -30,7 +30,7 @@ #include #include #include // For writeG2o -#include +#include #include From 5c5260bbc3fd901c045be6be693d0283f412d2c2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 May 2025 16:10:18 -0400 Subject: [PATCH 48/54] move IncrementalFixedLagSmoother tests to gtsam directory --- .../nonlinear/tests/testIncrementalFixedLagSmoother.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {gtsam_unstable => gtsam}/nonlinear/tests/testIncrementalFixedLagSmoother.cpp (100%) diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam/nonlinear/tests/testIncrementalFixedLagSmoother.cpp similarity index 100% rename from gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp rename to gtsam/nonlinear/tests/testIncrementalFixedLagSmoother.cpp From 9f4ab83cee14db7f5784a0f18930310e491ee715 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 13 May 2025 22:45:17 -0400 Subject: [PATCH 49/54] separate DiscreteMarginals into .h and .cpp --- gtsam/discrete/DiscreteMarginals.cpp | 58 ++++++++++++++++++++++++++++ gtsam/discrete/DiscreteMarginals.h | 28 +++----------- 2 files changed, 63 insertions(+), 23 deletions(-) create mode 100644 gtsam/discrete/DiscreteMarginals.cpp diff --git a/gtsam/discrete/DiscreteMarginals.cpp b/gtsam/discrete/DiscreteMarginals.cpp new file mode 100644 index 000000000..d7cc79a5c --- /dev/null +++ b/gtsam/discrete/DiscreteMarginals.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteMarginals.cpp + * @brief A class for computing marginals in a DiscreteFactorGraph + * @author Abhijit Kundu + * @author Richard Roberts + * @author Varun Agrawal + * @author Frank Dellaert + * @date June 4, 2012 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +DiscreteMarginals::DiscreteMarginals(const DiscreteFactorGraph& graph) { + bayesTree_ = graph.eliminateMultifrontal(); +} + +/* ************************************************************************* */ +DiscreteFactor::shared_ptr DiscreteMarginals::operator()(Key variable) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor = + bayesTree_->marginalFactor(variable, &EliminateDiscrete); + return marginalFactor; +} + +/* ************************************************************************* */ +Vector DiscreteMarginals::marginalProbabilities(const DiscreteKey& key) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor = this->operator()(key.first); + + // Create result + Vector vResult(key.second); + for (size_t state = 0; state < key.second; ++state) { + DiscreteValues values; + values[key.first] = state; + vResult(state) = (*marginalFactor)(values); + } + return vResult; +} + +} /* namespace gtsam */ diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index 62c80e657..fd6c93fe4 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -14,6 +14,7 @@ * @brief A class for computing marginals in a DiscreteFactorGraph * @author Abhijit Kundu * @author Richard Roberts + * @author Varun Agrawal * @author Frank Dellaert * @date June 4, 2012 */ @@ -38,38 +39,19 @@ class DiscreteMarginals { DiscreteMarginals() {} /** Construct a marginals class. - * @param graph The factor graph defining the full joint + * @param graph The factor graph defining the full joint * distribution on all variables. */ - DiscreteMarginals(const DiscreteFactorGraph& graph) { - bayesTree_ = graph.eliminateMultifrontal(); - } + DiscreteMarginals(const DiscreteFactorGraph& graph); /** Compute the marginal of a single variable */ - DiscreteFactor::shared_ptr operator()(Key variable) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor = - bayesTree_->marginalFactor(variable, &EliminateDiscrete); - return marginalFactor; - } + DiscreteFactor::shared_ptr operator()(Key variable) const; /** Compute the marginal of a single variable * @param key DiscreteKey of the Variable * @return Vector of marginal probabilities */ - Vector marginalProbabilities(const DiscreteKey& key) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor = this->operator()(key.first); - - // Create result - Vector vResult(key.second); - for (size_t state = 0; state < key.second; ++state) { - DiscreteValues values; - values[key.first] = state; - vResult(state) = (*marginalFactor)(values); - } - return vResult; - } + Vector marginalProbabilities(const DiscreteKey& key) const; }; } /* namespace gtsam */ From 27fc6a7a4e3b01cecb65511db36b47fa649a9f79 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 13 May 2025 22:49:51 -0400 Subject: [PATCH 50/54] add print method to DiscreteMarginals --- gtsam/discrete/DiscreteMarginals.cpp | 8 ++++++++ gtsam/discrete/DiscreteMarginals.h | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/gtsam/discrete/DiscreteMarginals.cpp b/gtsam/discrete/DiscreteMarginals.cpp index d7cc79a5c..74da37e73 100644 --- a/gtsam/discrete/DiscreteMarginals.cpp +++ b/gtsam/discrete/DiscreteMarginals.cpp @@ -55,4 +55,12 @@ Vector DiscreteMarginals::marginalProbabilities(const DiscreteKey& key) const { return vResult; } +/* ************************************************************************* */ +void DiscreteMarginals::print( + const std::string& s = "", + const KeyFormatter formatter = DefaultKeyFormatter) const { + std::cout << (s.empty() ? "Discrete Marginals of:" : s + " ") << std::endl; + bayesTree_.print("", formatter); +} + } /* namespace gtsam */ diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index fd6c93fe4..be2e17bcb 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -52,6 +52,10 @@ class DiscreteMarginals { * @return Vector of marginal probabilities */ Vector marginalProbabilities(const DiscreteKey& key) const; + + /// Print details + void print(const std::string& s = "", + const KeyFormatter formatter = DefaultKeyFormatter) const; }; } /* namespace gtsam */ From 53333170791b618f146ec87112d25a1b3ed1acea Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 13 May 2025 22:50:30 -0400 Subject: [PATCH 51/54] wrap DiscreteMarginals class --- gtsam/discrete/discrete.i | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 2f15de4c6..27f1fdfa1 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -494,4 +494,18 @@ class DiscreteSearch { std::vector run(size_t K = 1) const; }; +#include + +class DiscreteMarginals { + DiscreteMarginals(); + DiscreteMarginals(const gtsam::DiscreteFactorGraph& graph); + + gtsam::DiscreteFactor* operator()(gtsam::Key variable) const; + gtsam::Vector marginalProbabilities(const gtsam::DiscreteKey& key) const; + + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + } // namespace gtsam From a01ece4515b7e956415bfdc08f270a440eefa1a7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 13 May 2025 22:51:30 -0400 Subject: [PATCH 52/54] fix includes --- gtsam/discrete/DiscreteMarginals.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/discrete/DiscreteMarginals.cpp b/gtsam/discrete/DiscreteMarginals.cpp index 74da37e73..1a9a986fd 100644 --- a/gtsam/discrete/DiscreteMarginals.cpp +++ b/gtsam/discrete/DiscreteMarginals.cpp @@ -21,9 +21,7 @@ #pragma once -#include -#include -#include +#include namespace gtsam { From e5c0557dba782d1b56cef29f837de40eacf85a0f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 13 May 2025 22:52:38 -0400 Subject: [PATCH 53/54] small fixes --- gtsam/discrete/DiscreteMarginals.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DiscreteMarginals.cpp b/gtsam/discrete/DiscreteMarginals.cpp index 1a9a986fd..60f9ba896 100644 --- a/gtsam/discrete/DiscreteMarginals.cpp +++ b/gtsam/discrete/DiscreteMarginals.cpp @@ -19,8 +19,6 @@ * @date June 4, 2012 */ -#pragma once - #include namespace gtsam { @@ -54,11 +52,10 @@ Vector DiscreteMarginals::marginalProbabilities(const DiscreteKey& key) const { } /* ************************************************************************* */ -void DiscreteMarginals::print( - const std::string& s = "", - const KeyFormatter formatter = DefaultKeyFormatter) const { +void DiscreteMarginals::print(const std::string& s, + const KeyFormatter formatter) const { std::cout << (s.empty() ? "Discrete Marginals of:" : s + " ") << std::endl; - bayesTree_.print("", formatter); + bayesTree_->print("", formatter); } } /* namespace gtsam */ From 1514a0d62e5b1bdb0c6739beea9c4341c153908f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 May 2025 10:29:03 -0400 Subject: [PATCH 54/54] mark DiscreteMarginals with GTSAM_EXPORT --- gtsam/discrete/DiscreteMarginals.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index be2e17bcb..91082f491 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -31,7 +31,7 @@ namespace gtsam { * A class for computing marginals of variables in a DiscreteFactorGraph * @ingroup discrete */ -class DiscreteMarginals { +class GTSAM_EXPORT DiscreteMarginals { protected: DiscreteBayesTree::shared_ptr bayesTree_;