From ea783b0edb9a1698962ed1a542bcb7c2d536918d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 10:40:20 -0400 Subject: [PATCH] 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);