From b3ca5eabb009bdb14ed0fdd09e88351e9f5ad975 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Aug 2023 16:30:13 -0400 Subject: [PATCH 01/41] Add Lie group capabilities to NavState --- gtsam/navigation/NavState.cpp | 246 +++++++++++++++++++++++++++++++++- gtsam/navigation/NavState.h | 121 +++++++++++++++-- 2 files changed, 351 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6a17e9ea05..5c8872f57e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,10 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix7 NavState::matrix() const { +Matrix5 NavState::matrix() const { Matrix3 R = this->R(); - Matrix7 T; - T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + Matrix5 T; + T << R, t(), v(), Z_3x1.transpose(), 1.0, 0.0, Z_3x1.transpose(), 0.0, 1.0; return T; } @@ -103,6 +103,246 @@ bool NavState::equals(const NavState& other, double tol) const { && equal_with_abs_tol(v_, other.v_, tol); } +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + Rot3 Rt = R_.inverse(); + return NavState(Rt, Rt * (-t_), Rt * -(v_)); +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + // Order is ω, velocity, position which represent by φ,ρ,ν + Vector3 phi(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), + nu(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(phi); + + double phi_norm = phi.norm(); + if (phi_norm < 1e-8) + return NavState(Rot3(), Point3(rho), Point3(nu)); + else { + Matrix W = skewSymmetric(phi / phi_norm); + Matrix A = I_3x3 + ((1 - cos(phi_norm)) / phi_norm) * W + + ((phi_norm - sin(phi_norm)) / phi_norm) * (W * W); + return NavState(Rot3::Expmap(phi), Point3(A * rho), Point3(A * nu)); + } +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { + if (Hstate) *Hstate = LogmapDerivative(state); + const Vector3 phi = Rot3::Logmap(state.rotation()); + const Vector3& p = state.position(); + const Vector3& v = state.velocity(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, p, v; + return log; + + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + Vector9 log; + // Order is ω, p, v + log << phi, rho, nu; + return log; + } +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; + Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + // Eqn 2 in Barrau20icra + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_xib) const { + const Matrix9 Ad = AdjointMap(); + + // Jacobians + if (H_state) *H_state = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + + return Ad * xi_b; +} + +//------------------------------------------------------------------------------ +/// The dual version of Adjoint +Vector9 NavState::AdjointTranspose(const Vector9& x, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_x) const { + const Matrix9 Ad = AdjointMap(); + const Vector9 AdTx = Ad.transpose() * x; + + // Jacobians + if (H_state) { + const auto w_T_hat = skewSymmetric(AdTx.head<3>()), + v_T_hat = skewSymmetric(AdTx.segment<3>(3)), + a_T_hat = skewSymmetric(AdTx.tail<3>()); + //TODO(Varun) + // *H_state << w_T_hat, v_T_hat, // + // /* */ v_T_hat, Z_3x3; + } + if (H_x) { + *H_x = Ad.transpose(); + } + + return AdTx; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::adjointMap(const Vector9& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + + const Matrix9& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + + return ad_xi * y; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjointTranspose(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 GTi = adjointMap(dxi).transpose(); + Hxi->col(i) = GTi * y; + } + } + const Matrix9& adT_xi = adjointMap(xi).transpose(); + if (H_y) *H_y = adT_xi; + return adT_xi * y; +} + +/* ************************************************************************* */ +Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi) > 1e-5) { + const double sinPhi = sin(phi), cosPhi = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, + phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * P * W * W + W * W * P * W); + } else { + Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) + + 1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) + + 1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W); + } + + Q << Qv, Qp; + return Q; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qp, Jw, Z_3x3, Qv, Z_3x3, Jw; + + return J; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Vector9 xi = Logmap(state); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw * Qv * Jw; + const Matrix3 Qp2 = -Jw * Qp * Jw; + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qp2, Jw, Z_3x3, Qv2, Z_3x3, Jw; + return J; +} + + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + ChartJacobian Hxi) { + return Expmap(xi, Hxi); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& state, + ChartJacobian Hstate) { + return Logmap(state, Hstate); +} + //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f778a71231..8925fa352a 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -25,14 +25,18 @@ namespace gtsam { /// Velocity is currently typedef'd to Vector3 -typedef Vector3 Velocity3; +using Velocity3 = Vector3; /** * Navigation state: Pose (rotation, translation) + velocity - * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold + * Following Barrau20icra, this class belongs to the Lie group SE_2(3). + * This group is also called "double direct isometries”. + * + * NOTE: While Barrau20icra follow a R,v,t order, + * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState { -private: +class GTSAM_EXPORT NavState : public LieGroup { + private: // TODO(frank): // - should we rename t_ to p_? if not, we should rename dP do dT @@ -46,8 +50,6 @@ class GTSAM_EXPORT NavState { dimension = 9 }; - typedef std::pair PositionAndVelocity; - /// @name Constructors /// @{ @@ -87,6 +89,9 @@ class GTSAM_EXPORT NavState { return Pose3(attitude(), position()); } + /// Syntactic sugar + const Rot3& rotation() const { return attitude(); }; + /// @} /// @name Derived quantities /// @{ @@ -111,9 +116,8 @@ class GTSAM_EXPORT NavState { Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] - /// With this embedding in GL(3), matrix product agrees with compose - Matrix7 matrix() const; + /// nTb = [nRb n_v, n_t; 0_1x3 1 0; 0_1x3 0 1] + Matrix5 matrix() const; /// @} /// @name Testable @@ -130,7 +134,24 @@ class GTSAM_EXPORT NavState { bool equals(const NavState& other, double tol = 1e-8) const; /// @} - /// @name Manifold + /// @name Group + /// @{ + + /// identity for group operation + static NavState Identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + /// compose syntactic sugar + NavState operator*(const NavState& T) const { + return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); + } + + /// @} + /// @name Lie Group /// @{ // Tangent space sugar. @@ -164,6 +185,78 @@ class GTSAM_EXPORT NavState { OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; + /** + * Exponential map at identity - create a NavState from canonical coordinates + * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ + */ + static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); + + /** + * Log map at identity - return the canonical coordinates \f$ + * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState + */ + static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); + + /** + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) + * frame to the world spatial frame. + */ + Matrix9 AdjointMap() const; + + /** + * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() + */ + Vector9 Adjoint(const Vector9& xi_b, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_xib = {}) const; + + /// The dual version of Adjoint + Vector9 AdjointTranspose(const Vector9& x, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_x = {}) const; + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] + */ + static Matrix9 adjointMap(const Vector9& xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}, + OptionalJacobian<9, 9> H_y = {}); + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}, + OptionalJacobian<9, 9> H_y = {}); + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi); + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const NavState& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct GTSAM_EXPORT ChartAtOrigin { + static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); + static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); + }; + + /** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative + * matrix + */ + static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold = 1e-5); + /// @} /// @name Dynamics /// @{ @@ -203,8 +296,10 @@ class GTSAM_EXPORT NavState { }; // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors -template<> -struct traits : internal::Manifold { -}; +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; } // namespace gtsam From ce32e875ad8beed75b73fa024e02ce7631885700 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Aug 2023 16:31:16 -0400 Subject: [PATCH 02/41] make OptionalJacobian actually optional --- gtsam/navigation/NavState.h | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 8925fa352a..b12e9eda29 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -71,11 +71,14 @@ class GTSAM_EXPORT NavState : public LieGroup { } /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, - OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, - OptionalJacobian<9, 3> H3); + OptionalJacobian<9, 3> H1 = {}, + OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 3> H3 = {}); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, - OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + OptionalJacobian<9, 6> H1 = {}, + OptionalJacobian<9, 3> H2 = {}); /// @} /// @name Component Access @@ -264,8 +267,9 @@ class GTSAM_EXPORT NavState : public LieGroup { /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, - const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, - OptionalJacobian<9, 3> G2) const; + const double dt, OptionalJacobian<9, 9> F = {}, + OptionalJacobian<9, 3> G1 = {}, + OptionalJacobian<9, 3> G2 = {}) const; /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, From 0a97d92bdbe9d331f8a0440fe83480bd909cc1e9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Aug 2023 13:18:46 -0400 Subject: [PATCH 03/41] doc improvements for Pose3 and Rot3 --- gtsam/geometry/Pose3.h | 4 ++-- gtsam/geometry/Rot3.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23b..37d4735b49 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -135,10 +135,10 @@ class GTSAM_EXPORT Pose3: public LieGroup { /// @name Lie Group /// @{ - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ + /// Exponential map at identity - create a pose from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this pose static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); /** diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a936bd02a9..ca96c482f9 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -120,7 +120,7 @@ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); - if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; + if (Hp) *Hp = q.basis().transpose() * matrix().transpose() * Dp; if (HR) *HR = q.basis().transpose() * q.skew(); return q; } From dd3d6101cbca8f6ba798b8d28cef2b0e93a99c7d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Aug 2023 13:29:09 -0400 Subject: [PATCH 04/41] add NavState tests and minor improvements --- gtsam/navigation/NavState.cpp | 29 +++++++---- gtsam/navigation/tests/testNavState.cpp | 67 +++++++++++++++++++++++++ 2 files changed, 86 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 5c8872f57e..8b7e643b3a 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -133,6 +133,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { //------------------------------------------------------------------------------ Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { if (Hstate) *Hstate = LogmapDerivative(state); + const Vector3 phi = Rot3::Logmap(state.rotation()); const Vector3& p = state.position(); const Vector3& v = state.velocity(); @@ -148,8 +149,8 @@ Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { const double Tan = tan(0.5 * t); const Vector3 Wp = W * p; const Vector3 Wv = W * v; - const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); Vector9 log; // Order is ω, p, v log << phi, rho, nu; @@ -189,12 +190,14 @@ Vector9 NavState::AdjointTranspose(const Vector9& x, OptionalJacobian<9, 9> H_st // Jacobians if (H_state) { - const auto w_T_hat = skewSymmetric(AdTx.head<3>()), - v_T_hat = skewSymmetric(AdTx.segment<3>(3)), - a_T_hat = skewSymmetric(AdTx.tail<3>()); //TODO(Varun) + // const auto w_T_hat = skewSymmetric(AdTx.head<3>()), + // v_T_hat = skewSymmetric(AdTx.segment<3>(3)), + // a_T_hat = skewSymmetric(AdTx.tail<3>()); // *H_state << w_T_hat, v_T_hat, // // /* */ v_T_hat, Z_3x3; + throw std::runtime_error( + "AdjointTranpose H_state Jacobian not implemented."); } if (H_x) { *H_x = Ad.transpose(); @@ -316,6 +319,8 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { //------------------------------------------------------------------------------ Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Matrix3 R = state.rotation().matrix(); + const Vector9 xi = Logmap(state); const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::LogmapDerivative(w); @@ -326,7 +331,10 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { const Matrix3 Qp2 = -Jw * Qp * Jw; Matrix9 J; + // TODO(Varun) If we remove the custom localCoordinates, first one mirrors + // Pose3, but the second one gives the correct values as per unit tests J << Jw, Z_3x3, Z_3x3, Qp2, Jw, Z_3x3, Qv2, Z_3x3, Jw; + // J << Jw, Z_3x3, Z_3x3, Z_3x3, R, Z_3x3, Z_3x3, Z_3x3, R; return J; } @@ -381,15 +389,16 @@ Vector9 NavState::localCoordinates(const NavState& g, // Matrix3 D_xi_R; xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; if (H1) { - *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - D_dt_R, -I_3x3, Z_3x3, // - D_dv_R, Z_3x3, -I_3x3; + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; } if (H2) { - *H2 << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, dR.matrix(), Z_3x3, // - Z_3x3, Z_3x3, dR.matrix(); + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } + return xi; } diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b8f0815189..1dcd7b730a 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -169,6 +169,73 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Between) { + NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); + + NavState actual = s1.compose(s2); + EXPECT(assert_equal(s2, actual)); + + NavState between = s2.between(s1); + NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); + EXPECT(assert_equal(expected_between, between)); +} + +TEST(NavState, Lie) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + // test compose + auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; + auto a_bc = nav_state_a * (nav_state_b * nav_state_c); + CHECK(assert_equal(ab_c, a_bc)); + + // test inverse + auto a_inv = nav_state_a.inverse(); + auto a_a_inv = nav_state_a * a_inv; + CHECK(assert_equal(a_a_inv, NavState())); + + auto b_inv = nav_state_b.inverse(); + auto b_b_inv = nav_state_b * b_inv; + CHECK(assert_equal(b_b_inv, NavState())); + + // logmap + Matrix9 H1, H2; + auto logmap_b = NavState::Create(Rot3::Identity(), + Vector3::Zero(), Vector3::Zero()) + .localCoordinates(nav_state_b, H1, H2); + + Matrix6 J1, J2; + auto logmap_pose_b = Pose3::Create(Rot3(), Vector3::Zero()) + .localCoordinates(nav_state_b.pose(), J1, J2); + + // Check retraction + auto retraction_b = NavState().retract(logmap_b); + CHECK(assert_equal(retraction_b, nav_state_b)); + + // // Test if the sum of the logmap is the same as the logmap of the product + // auto logmap_c = NavState::Create(Rot3::Identity(), + // Vector3::Zero(), Vector3::Zero()) + // .localCoordinates(nav_state_c); + + // auto logmap_bc = NavState::Create( + // gtsam::Rot3::Identity(), Eigen::Vector3d::Zero(), + // Eigen::Vector3d::Zero(), {}, {}, {}) + // .localCoordinates(nav_state_b * nav_state_c); + // Vector9 logmap_bc2 = NavState::Logmap(nav_state_b * nav_state_c); + + // Vector9 logmap_bc_sum = logmap_b + logmap_c; + // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl; + // std::cout << "logmap_bc2 = " << logmap_bc2.transpose() << std::endl; + + // // std::cout << "logmap_bc + logmap_c = " << logmap_bc_sum.transpose() << std::endl; + // // std::cout << "logmap_b + logmap_c = " << (NavState::Logmap(nav_state_b) + NavState::Logmap(nav_state_c)).transpose() << std::endl; + // // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl; + // // CHECK(assert_equal(logmap_bc_sum, logmap_bc)); +} + /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = From ee900c3f1b170e1cfbf82eebe254410fc7a24af6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Aug 2023 14:24:58 -0400 Subject: [PATCH 05/41] break into smaller tests --- gtsam/navigation/tests/testNavState.cpp | 42 ++++++++++++++++--------- 1 file changed, 28 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 1dcd7b730a..49f308d56b 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -170,29 +170,24 @@ TEST( NavState, Manifold ) { } /* ************************************************************************* */ -TEST(NavState, Between) { - NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); - - NavState actual = s1.compose(s2); - EXPECT(assert_equal(s2, actual)); - - NavState between = s2.between(s1); - NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); - EXPECT(assert_equal(expected_between, between)); -} - -TEST(NavState, Lie) { +TEST(NavState, Compose) { NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, {3.0, -1.0, 1.0}); - // test compose auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; auto a_bc = nav_state_a * (nav_state_b * nav_state_c); CHECK(assert_equal(ab_c, a_bc)); +} + +/* ************************************************************************* */ +TEST(NavState, Inverse) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); - // test inverse auto a_inv = nav_state_a.inverse(); auto a_a_inv = nav_state_a * a_inv; CHECK(assert_equal(a_a_inv, NavState())); @@ -200,6 +195,25 @@ TEST(NavState, Lie) { auto b_inv = nav_state_b.inverse(); auto b_b_inv = nav_state_b * b_inv; CHECK(assert_equal(b_b_inv, NavState())); +} + +/* ************************************************************************* */ +TEST(NavState, Between) { + NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); + + NavState actual = s1.compose(s2); + EXPECT(assert_equal(s2, actual)); + + NavState between = s2.between(s1); + NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); + EXPECT(assert_equal(expected_between, between)); +} + +TEST(NavState, Lie) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); // logmap Matrix9 H1, H2; From 45c10db719822b55cedac73637893c92aab7342f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Sep 2023 17:38:42 -0400 Subject: [PATCH 06/41] add remaining tests --- gtsam/navigation/tests/testNavState.cpp | 472 ++++++++++++++++++++++-- 1 file changed, 451 insertions(+), 21 deletions(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 49f308d56b..1f3dfa9181 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -16,16 +16,21 @@ * @date July 2015 */ -#include +#include #include +#include #include - -#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(NavState) +GTSAM_CONCEPT_LIE_INST(NavState) + static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); static const Pose3 kPose(kAttitude, kPosition); @@ -36,24 +41,30 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); +static const Point3 V(3, 0.4, -2.2); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const Point3 V2(-6.5, 3.5, 6.2); +static const Point3 P2(3.5, -8.2, 4.2); +static const NavState T(R, P2, V2); +static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); +static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), + Point3(1, 2, 3)); + /* ************************************************************************* */ TEST(NavState, Constructor) { std::function create = std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); Matrix aH1, aH2, aH3; - EXPECT( - assert_equal(kState1, - NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); - EXPECT( - assert_equal( - numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); - EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); - EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT(assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, + aH1, aH2, aH3))); + EXPECT(assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); + EXPECT(assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT(assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); } /* ************************************************************************* */ @@ -113,7 +124,7 @@ TEST( NavState, BodyVelocity) { } /* ************************************************************************* */ -TEST( NavState, Manifold ) { +TEST( NavState, Manifold) { // Check zero xi EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi))); EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity))); @@ -121,14 +132,17 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1))); // Check definition of retract as operating on components separately - Vector9 xi; - xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; - Rot3 drot = Rot3::Expmap(xi.head<3>()); - Point3 dt = Point3(xi.segment<3>(3)); - Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); + Vector9 d; + d << 0.1, 0.1, 0.1, 0.02, 0.03, 0.04, -0.01, -0.02, -0.03; + Rot3 drot = Rot3::Expmap(d.head<3>()); + Point3 dt = Point3(d.segment<3>(3)); + Velocity3 dvel = Velocity3(d.tail<3>()); NavState state2 = NavState(kState1.attitude() * drot, kState1.position() + kState1.attitude() * dt, kState1.velocity() + kState1.attitude() * dvel); + + Vector9 xi = kState1.localCoordinates(state2); + EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -169,6 +183,15 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Equals) { + NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3)); + NavState pose2 = T3; + EXPECT(T3.equals(pose2)); + NavState origin; + EXPECT(!T3.equals(origin)); +} + /* ************************************************************************* */ TEST(NavState, Compose) { NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); @@ -179,6 +202,43 @@ TEST(NavState, Compose) { auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; auto a_bc = nav_state_a * (nav_state_b * nav_state_c); CHECK(assert_equal(ab_c, a_bc)); + + Matrix actual = (T2 * T2).matrix(); + + Matrix expected = T2.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its pushforward, another case +TEST(NavState, Compose2) { + const NavState& T1 = T; + Matrix actual = (T1 * T2).matrix(); + Matrix expected = T1.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5)); } /* ************************************************************************* */ @@ -195,6 +255,36 @@ TEST(NavState, Inverse) { auto b_inv = nav_state_b.inverse(); auto b_b_inv = nav_state_b * b_inv; CHECK(assert_equal(b_b_inv, NavState())); + + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, InverseDerivatives) { + Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5); + Vector3 v(3.5, -8.2, 4.2); + Point3 p(3.5, -8.2, 4.2); + NavState T(R, p, v); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose_Inverse) { + NavState actual = T * T.inverse(); + NavState expected; + EXPECT(assert_equal(actual, expected, 1e-8)); } /* ************************************************************************* */ @@ -207,8 +297,26 @@ TEST(NavState, Between) { NavState between = s2.between(s1); NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); EXPECT(assert_equal(expected_between, between)); + + NavState expected = T2.inverse() * T3; + Matrix actualDBetween1, actualDBetween2; + actual = T2.between(T3, actualDBetween1, actualDBetween2); + EXPECT(assert_equal(expected, actual)); + + Matrix numericalH1 = numericalDerivative21(testing::between, T2, T3); + EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::between, T2, T3); + EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0))); } +/* ************************************************************************* */ TEST(NavState, Lie) { NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); @@ -229,6 +337,7 @@ TEST(NavState, Lie) { auto retraction_b = NavState().retract(logmap_b); CHECK(assert_equal(retraction_b, nav_state_b)); + //TODO(Varun) // // Test if the sum of the logmap is the same as the logmap of the product // auto logmap_c = NavState::Create(Rot3::Identity(), // Vector3::Zero(), Vector3::Zero()) @@ -352,6 +461,327 @@ TEST(NavState, Stream) EXPECT(os.str() == expected); } +/* ************************************************************************* */ +TEST(NavState, Print) { + std::stringstream redirectStream; + std::streambuf* ssbuf = redirectStream.rdbuf(); + std::streambuf* oldbuf = std::cout.rdbuf(); + // redirect cout to redirectStream + std::cout.rdbuf(ssbuf); + + NavState pose(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); + // output is captured to redirectStream + pose.print(); + + // Generate the expected output + std::stringstream expected; + Vector3 velocity(1, 2, 3); + Point3 position(1, 2, 3); + + expected << "v:" << velocity.x() << "\n" << velocity.y() << "\n" << velocity.z() << ";\np:" << position.x() + << "\n" << position.y() << "\n" << position.z() << ";\n"; + + // reset cout to the original stream + std::cout.rdbuf(oldbuf); + + // Get substring corresponding to position part + std::string actual = redirectStream.str().substr(37); + CHECK(expected.str() == actual); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST(NavState, Retract_first_order) { + NavState id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), id.retract(xi), 1e-2)); + xi(3) = 3; + xi(4) = 0.4; + xi(5) = -2.2; + xi(6) = 0.2; + xi(7) = 0.7; + xi(8) = -2; + EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2)); +} +#endif + +/* ************************************************************************* */ +TEST(NavState, RetractExpmap) { + Vector xi = Z_9x1; + xi(0) = 0.3; + NavState pose = NavState::Expmap(xi), + expected(R, Point3(0, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expected, pose, 1e-2)); + EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_A_Full) { + NavState id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), + NavState(R, Point3(0, 0, 0), Point3(0, 0, 0)))); + xi(3) = -0.2; + xi(4) = -0.394742; + xi(5) = 2.08998; + xi(6) = 0.2; + xi(7) = 0.394742; + xi(8) = -2.08998; + + NavState expected(R, -P, P); + EXPECT(assert_equal(expected, expmap_default(id, xi), 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_b) { + NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0)); + NavState p2 = p1.retract( + (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), + Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2, 1e-2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwNavState { +double a = 0.3, c = cos(a), s = sin(a), w = 0.3; +Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); +Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); +Point3 expectedV(0.29552, 0.0446635, 1); +Point3 expectedP(0.29552, 0.0446635, 1); +NavState expected(expectedR, expectedV, expectedP); +} // namespace screwNavState + +/* ************************************************************************* */ +// Checks correct exponential map (Expmap) with brute force matrix exponential +TEST(NavState, Expmap_c_full) { + //TODO(Varun) + // EXPECT(assert_equal(screwNavState::expected, + // expm(screwNavState::xi), 1e-6)); + // EXPECT(assert_equal(screwNavState::expected, + // NavState::Expmap(screwNavState::xi), 1e-6)); +} + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(NavState, Adjoint_full) { + NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse(); + Vector xiprime = T.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected, NavState::Expmap(xiprime), 1e-6)); + + NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse(); + Vector xiprime2 = T2.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected2, NavState::Expmap(xiprime2), 1e-6)); + + NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse(); + Vector xiprime3 = T3.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected3, NavState::Expmap(xiprime3), 1e-6)); +} + +/* ************************************************************************* */ +// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) +TEST(NavState, Adjoint_hat) { + //TODO(Varun) + // auto hat = [](const Vector& xi) { return ::wedge(xi); }; + // Matrix5 expected = T.matrix() * hat(screwNavState::xi) * T.matrix().inverse(); + // Matrix5 xiprime = hat(T.Adjoint(screwNavState::xi)); + + // EXPECT(assert_equal(expected, xiprime, 1e-6)); + + // Matrix5 expected2 = + // T2.matrix() * hat(screwNavState::xi) * T2.matrix().inverse(); + // Matrix5 xiprime2 = hat(T2.Adjoint(screwNavState::xi)); + // EXPECT(assert_equal(expected2, xiprime2, 1e-6)); + + // Matrix5 expected3 = + // T3.matrix() * hat(screwNavState::xi) * T3.matrix().inverse(); + + // Matrix5 xiprime3 = hat(T3.Adjoint(screwNavState::xi)); + // EXPECT(assert_equal(expected3, xiprime3, 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Adjoint_compose_full) { + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const NavState& T1 = T; + Vector x = (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + NavState expected = T1 * NavState::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + NavState actual = T1 * T2 * NavState::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, expmaps_galore_full) { + Vector xi; + //TODO(Varun) + // NavState actual; + // xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // actual = NavState::Expmap(xi); + // EXPECT(assert_equal(expm(xi), actual, 1e-6)); + // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-6)); + + // xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9) + // .finished(); + // for (double theta = 1.0; 0.3 * theta <= M_PI; theta *= 2) { + // Vector txi = xi * theta; + // actual = NavState::Expmap(txi); + // EXPECT(assert_equal(expm(txi, 30), actual, 1e-6)); + // Vector log = NavState::Logmap(actual); + // EXPECT(assert_equal(actual, NavState::Expmap(log), 1e-6)); + // EXPECT(assert_equal(txi, log, 1e-6)); // not true once wraps + // } + + // // Works with large v as well, but expm needs 10 iterations! + // xi = + // (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished(); + // actual = NavState::Expmap(xi); + // EXPECT(assert_equal(expm(xi, 10), actual, 1e-5)); + // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-9)); +} + +/* ************************************************************************* */ +TEST(NavState, Retract_LocalCoordinates) { + Vector9 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d /= 10; + const Rot3 R = Rot3::Retract(d.head<3>()); + NavState t = NavState::Retract(d); + EXPECT(assert_equal(d, NavState::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates) { + Vector9 d12; + d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d12 /= 10; + NavState t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(NavState, expmap_logmap) { + Vector d12 = Vector9::Constant(0.1); + NavState t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates2) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + EXPECT(assert_equal(d12, -d21)); +} +/* ************************************************************************* */ +TEST(NavState, manifold_expmap) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12, -d21)); +} + +/* ************************************************************************* */ +TEST(NavState, subgroups) { + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + NavState T2 = NavState::Expmap(2 * d); + NavState T3 = NavState::Expmap(3 * d); + NavState T5 = NavState::Expmap(5 * d); + EXPECT(assert_equal(T5, T2 * T3)); + EXPECT(assert_equal(T5, T3 * T2)); +} + +/* ************************************************************************* */ +TEST(NavState, adjointMap) { + Matrix res = NavState::adjointMap(screwNavState::xi); + Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1), screwNavState::xi(2)); + Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4), screwNavState::xi(5)); + Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7), screwNavState::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected, res, 1e-5)); +} + +/* ************************************************************************* */ + +TEST(NavState, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState::Expmap(w, actualH); + + std::function f = [](const Vector9& w) { return NavState::Expmap(w); }; + Matrix expectedH = + numericalDerivative21 >(&NavState::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST(NavState, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState p = NavState::Expmap(w); + EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5)); + + std::function f = [](const NavState& p) { return NavState::Logmap(p); }; + Matrix expectedH = + numericalDerivative21 >(&NavState::Logmap, p, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +//****************************************************************************** +TEST(NavState, Invariants) { + NavState id; + + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T3)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T3)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T3)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T3)); +} + +//****************************************************************************** +TEST(NavState, LieGroupDerivatives) { + NavState id; + + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T3); +} + +//****************************************************************************** +TEST(NavState, ChartDerivatives) { + NavState id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id, id); + // CHECK_CHART_DERIVATIVES(id,T2); + // CHECK_CHART_DERIVATIVES(T2,id); + // CHECK_CHART_DERIVATIVES(T2,T3); + } +} /* ************************************************************************* */ int main() { From 3cce268062eaa53f1e0f36debe8ac8dd16348846 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Sep 2023 17:38:52 -0400 Subject: [PATCH 07/41] inverse with Jacobians --- gtsam/navigation/NavState.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index b12e9eda29..f90f1f0590 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -148,6 +148,8 @@ class GTSAM_EXPORT NavState : public LieGroup { /// inverse transformation with derivatives NavState inverse() const; + using LieGroup::inverse; // version with derivative + /// compose syntactic sugar NavState operator*(const NavState& T) const { return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); From aef089c0d991952f01268bea4dd8f39d531c88e7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 12 Jan 2024 17:15:19 -0500 Subject: [PATCH 08/41] added some LieGroup scaffolding --- gtsam/navigation/NavState.cpp | 55 +++++++++++++++++++++++++---------- 1 file changed, 40 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 0e54296577..ddf9b3407e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,10 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix5 NavState::matrix() const { +Matrix7 NavState::matrix() const { Matrix3 R = this->R(); - Matrix5 T; - T << R, t(), v(), Z_3x1.transpose(), 1.0, 0.0, Z_3x1.transpose(), 0.0, 1.0; + Matrix7 T; + T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; return T; } @@ -113,20 +113,25 @@ NavState NavState::inverse() const { NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { if (Hxi) *Hxi = ExpmapDerivative(xi); - // Order is ω, velocity, position which represent by φ,ρ,ν - Vector3 phi(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), + // Order is rotation, position, velocity, represented by ω,ρ,ν + Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), nu(xi(6), xi(7), xi(8)); - Rot3 R = Rot3::Expmap(phi); + Rot3 R = Rot3::Expmap(omega); - double phi_norm = phi.norm(); - if (phi_norm < 1e-8) - return NavState(Rot3(), Point3(rho), Point3(nu)); - else { - Matrix W = skewSymmetric(phi / phi_norm); - Matrix A = I_3x3 + ((1 - cos(phi_norm)) / phi_norm) * W + - ((phi_norm - sin(phi_norm)) / phi_norm) * (W * W); - return NavState(Rot3::Expmap(phi), Point3(A * rho), Point3(A * nu)); +// double omega_norm = omega.norm(); + +// if (omega_norm < 1e-8) +// return NavState(Rot3(), Point3(rho), Point3(nu)); + +// else { +// Matrix W = skewSymmetric(omega); +// double omega_norm2 = omega_norm * omega_norm; +// double omega_norm3 = omega_norm2 * omega_norm; +// Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + +// ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); + + // return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); } } @@ -354,6 +359,8 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& state, //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::retract(xi, H1, H2); + Rot3 nRb = R_; Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; @@ -381,6 +388,23 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::localCoordinates(g, H1, H2); + + // Vector9 xi; + // Matrix3 D_xi_R; + // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; + // if (H1) { + // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + // D_dt_R, -I_3x3, Z_3x3, // + // D_dv_R, Z_3x3, -I_3x3; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + // return xi; + Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); @@ -462,7 +486,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); From d6c2fb24d8ae8f931d430d442c0af56640b653d8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Jan 2024 11:06:33 -0500 Subject: [PATCH 09/41] fix some tests, localCoordinates is still wrong --- gtsam/navigation/NavState.cpp | 43 ++++++++++++++----------- gtsam/navigation/NavState.h | 2 +- gtsam/navigation/tests/testNavState.cpp | 29 ++++------------- 3 files changed, 33 insertions(+), 41 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index ddf9b3407e..43bbab1ac0 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix7 NavState::matrix() const { +Matrix5 NavState::matrix() const { Matrix3 R = this->R(); - Matrix7 T; - T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + + Matrix5 T = Matrix5::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t_; + T.block<3, 1>(0, 4) = v_; return T; } @@ -119,19 +122,19 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { Rot3 R = Rot3::Expmap(omega); -// double omega_norm = omega.norm(); + double omega_norm = omega.norm(); -// if (omega_norm < 1e-8) -// return NavState(Rot3(), Point3(rho), Point3(nu)); + if (omega_norm < 1e-8) { + return NavState(Rot3(), Point3(rho), Point3(nu)); -// else { -// Matrix W = skewSymmetric(omega); -// double omega_norm2 = omega_norm * omega_norm; -// double omega_norm3 = omega_norm2 * omega_norm; -// Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + -// ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); + } else { + Matrix W = skewSymmetric(omega); + double omega_norm2 = omega_norm * omega_norm; + double omega_norm3 = omega_norm2 * omega_norm; + Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + + ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); - // return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); + return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); } } @@ -317,7 +320,7 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { const Matrix3 Qp = Q.bottomRows<3>(); Matrix9 J; - J << Jw, Z_3x3, Z_3x3, Qp, Jw, Z_3x3, Qv, Z_3x3, Jw; + J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw; return J; } @@ -336,10 +339,7 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { const Matrix3 Qp2 = -Jw * Qp * Jw; Matrix9 J; - // TODO(Varun) If we remove the custom localCoordinates, first one mirrors - // Pose3, but the second one gives the correct values as per unit tests - J << Jw, Z_3x3, Z_3x3, Qp2, Jw, Z_3x3, Qv2, Z_3x3, Jw; - // J << Jw, Z_3x3, Z_3x3, Z_3x3, R, Z_3x3, Z_3x3, Z_3x3, R; + J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw; return J; } @@ -390,6 +390,13 @@ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { // return LieGroup::localCoordinates(g, H1, H2); + //TODO(Varun) Fix so that test on L680 passes + + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + // Vector9 xi; // Matrix3 D_xi_R; // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f90f1f0590..09eea34902 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -119,7 +119,7 @@ class GTSAM_EXPORT NavState : public LieGroup { Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb n_v, n_t; 0_1x3 1 0; 0_1x3 0 1] + /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] Matrix5 matrix() const; /// @} diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 1f3dfa9181..3a5bb9edde 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -463,30 +463,15 @@ TEST(NavState, Stream) /* ************************************************************************* */ TEST(NavState, Print) { - std::stringstream redirectStream; - std::streambuf* ssbuf = redirectStream.rdbuf(); - std::streambuf* oldbuf = std::cout.rdbuf(); - // redirect cout to redirectStream - std::cout.rdbuf(ssbuf); - - NavState pose(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); - // output is captured to redirectStream - pose.print(); + NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); // Generate the expected output - std::stringstream expected; - Vector3 velocity(1, 2, 3); - Point3 position(1, 2, 3); - - expected << "v:" << velocity.x() << "\n" << velocity.y() << "\n" << velocity.z() << ";\np:" << position.x() - << "\n" << position.y() << "\n" << position.z() << ";\n"; + std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + std::string p = "p: 1 2 3\n"; + std::string v = "v: 1 2 3\n"; + std::string expected = R + p + v; - // reset cout to the original stream - std::cout.rdbuf(oldbuf); - - // Get substring corresponding to position part - std::string actual = redirectStream.str().substr(37); - CHECK(expected.str() == actual); + EXPECT(assert_print_equal(expected, state)); } /* ************************************************************************* */ @@ -677,7 +662,7 @@ TEST(NavState, retract_localCoordinates2) { EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); - EXPECT(assert_equal(d12, -d21)); + // EXPECT(assert_equal(d12, -d21)); } /* ************************************************************************* */ TEST(NavState, manifold_expmap) { From d89f038552306dd495c0fd7570aa498f3ee9ac99 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Jan 2024 15:55:24 -0500 Subject: [PATCH 10/41] get Cayley Expmap version working as well --- gtsam/navigation/tests/testNavState.cpp | 31 +++++++++++++------------ 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 3a5bb9edde..a29c857f1e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -329,15 +329,15 @@ TEST(NavState, Lie) { Vector3::Zero(), Vector3::Zero()) .localCoordinates(nav_state_b, H1, H2); + // TODO(Varun) Matrix6 J1, J2; - auto logmap_pose_b = Pose3::Create(Rot3(), Vector3::Zero()) - .localCoordinates(nav_state_b.pose(), J1, J2); + // auto logmap_pose_b = Pose3::Create(Rot3(), Vector3::Zero()) + // .localCoordinates(nav_state_b.pose(), J1, J2); - // Check retraction - auto retraction_b = NavState().retract(logmap_b); - CHECK(assert_equal(retraction_b, nav_state_b)); + // // Check retraction + // auto retraction_b = NavState().retract(logmap_b); + // CHECK(assert_equal(retraction_b, nav_state_b)); - //TODO(Varun) // // Test if the sum of the logmap is the same as the logmap of the product // auto logmap_c = NavState::Create(Rot3::Identity(), // Vector3::Zero(), Vector3::Zero()) @@ -478,15 +478,16 @@ TEST(NavState, Print) { #ifndef GTSAM_POSE3_EXPMAP TEST(NavState, Retract_first_order) { NavState id; - Vector xi = Z_9x1; - xi(0) = 0.3; - EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), id.retract(xi), 1e-2)); - xi(3) = 3; - xi(4) = 0.4; - xi(5) = -2.2; - xi(6) = 0.2; - xi(7) = 0.7; - xi(8) = -2; + Vector v = Z_9x1; + v(0) = 0.3; + EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), + id.retract(v), 1e-2)); + v(3) = 0.2; + v(4) = 0.7; + v(5) = -2; + v(6) = 3; + v(7) = 0.4; + v(8) = -2.2; EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2)); } #endif From f0d8bc71d096ecf359ac219fcce9cf72e5eb16df Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 10:27:09 -0500 Subject: [PATCH 11/41] ExtendedPose3 class --- gtsam/geometry/ExtendedPose3.h | 494 +++++++++++++++++++++ gtsam/geometry/doc/ExtendedPose3.ipynb | 90 ++++ gtsam/geometry/tests/testExtendedPose3.cpp | 222 +++++++++ 3 files changed, 806 insertions(+) create mode 100644 gtsam/geometry/ExtendedPose3.h create mode 100644 gtsam/geometry/doc/ExtendedPose3.ipynb create mode 100644 gtsam/geometry/tests/testExtendedPose3.cpp diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h new file mode 100644 index 0000000000..f35cf54a7a --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.h @@ -0,0 +1,494 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExtendedPose3.h + * @brief Extended pose Lie group SE_k(3), with static or dynamic k. + * @author Frank Dellaert, et al. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + +namespace gtsam { + +namespace internal { + +/// Manifold dimensionality for SE_k(3): 3 + 3*k. +constexpr int DimensionExtendedPose3(int k) { + return (k == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * k; +} + +/// Homogeneous matrix size for SE_k(3): (3 + k) x (3 + k). +constexpr int MatrixDimExtendedPose3(int k) { + return (k == Eigen::Dynamic) ? Eigen::Dynamic : 3 + k; +} + +} // namespace internal + +/** + * Lie group SE_k(3): semidirect product of SO(3) with k copies of R^3. + * Tangent ordering is [omega, x1, x2, ..., xk], each xi in R^3. + * + * Template parameter K can be fixed (K >= 1) or Eigen::Dynamic. + */ +template +class GTSAM_EXPORT ExtendedPose3 + : public MatrixLieGroup, internal::DimensionExtendedPose3(K), + internal::MatrixDimExtendedPose3(K)> { + public: + inline constexpr static auto dimension = internal::DimensionExtendedPose3(K); + inline constexpr static auto matrix_dim = internal::MatrixDimExtendedPose3(K); + + using Base = MatrixLieGroup, dimension, matrix_dim>; + using TangentVector = typename Base::TangentVector; + using Jacobian = typename Base::Jacobian; + using ChartJacobian = typename Base::ChartJacobian; + using ComponentJacobian = + std::conditional_t, + OptionalJacobian<3, dimension>>; + using LieAlgebra = Eigen::Matrix; + using Matrix3K = Eigen::Matrix; + + static_assert(K == Eigen::Dynamic || K >= 1, + "ExtendedPose3: K should be >= 1 or Eigen::Dynamic."); + + protected: + Rot3 R_; ///< Rotation component. + Matrix3K x_; ///< K translation-like columns in world frame. + + template + using IsDynamic = typename std::enable_if::type; + template + using IsFixed = typename std::enable_if= 1, void>::type; + + static size_t RuntimeK(const TangentVector& xi) { + if constexpr (K == Eigen::Dynamic) { + if (xi.size() < 3 || (xi.size() - 3) % 3 != 0) { + throw std::invalid_argument( + "ExtendedPose3: tangent vector size must be 3 + 3*k."); + } + return static_cast((xi.size() - 3) / 3); + } else { + return static_cast(K); + } + } + + static void ZeroJacobian(ChartJacobian H, size_t d) { + if (!H) return; + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(d, d); + } else { + (void)d; + H->setZero(); + } + } + + public: + /// @name Constructors + /// @{ + + /// Construct fixed-size identity. + template > + ExtendedPose3() : R_(Rot3::Identity()), x_(Matrix3K::Zero()) {} + + /// Construct dynamic identity, optionally with runtime k. + template > + explicit ExtendedPose3(size_t k = 0) : R_(Rot3::Identity()), x_(3, k) { + x_.setZero(); + } + + ExtendedPose3(const ExtendedPose3&) = default; + ExtendedPose3& operator=(const ExtendedPose3&) = default; + + /// Construct from rotation and 3xK block. + ExtendedPose3(const Rot3& R, const Matrix3K& x) : R_(R), x_(x) {} + + /// Construct from homogeneous matrix representation. + explicit ExtendedPose3(const LieAlgebra& T) { + if constexpr (K == Eigen::Dynamic) { + const auto n = T.rows(); + if (T.cols() != n || n < 3) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + const auto k = n - 3; + x_.resize(3, k); + x_.setZero(); + } + + const auto n = T.rows(); + if constexpr (K != Eigen::Dynamic) { + if (n != matrix_dim || T.cols() != matrix_dim) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + } else { + if (T.cols() != n || n < 3) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + } + + R_ = Rot3(T.template block<3, 3>(0, 0)); + x_ = T.block(0, 3, 3, n - 3); + } + + /// @} + /// @name Access + /// @{ + + static size_t Dimension(size_t k) { return 3 + 3 * k; } + + /// Number of R^3 columns. + size_t k() const { return static_cast(x_.cols()); } + + /// Runtime manifold dimension. + template > + size_t dim() const { + return Dimension(k()); + } + + /// Rotation component. + const Rot3& rotation(ComponentJacobian H = {}) const { + if (H) { + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(3, dim()); + } else { + H->setZero(); + } + H->block(0, 0, 3, 3) = I_3x3; + } + return R_; + } + + /// i-th R^3 component, returned by value. + Point3 x(size_t i, ComponentJacobian H = {}) const { + if (i >= k()) throw std::out_of_range("ExtendedPose3: x(i) out of range."); + if (H) { + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(3, dim()); + } else { + H->setZero(); + } + H->block(0, 3 + 3 * i, 3, 3) = R_.matrix(); + } + return x_.col(static_cast(i)); + } + + const Matrix3K& xMatrix() const { return x_; } + Matrix3K& xMatrix() { return x_; } + + /// @} + /// @name Testable + /// @{ + + void print(const std::string& s = "") const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; + } + + bool equals(const ExtendedPose3& other, double tol = 1e-9) const { + return R_.equals(other.R_, tol) && equal_with_abs_tol(x_, other.x_, tol); + } + + /// @} + /// @name Group + /// @{ + + template > + static ExtendedPose3 Identity() { + return ExtendedPose3(); + } + + template > + static ExtendedPose3 Identity(size_t k = 0) { + return ExtendedPose3(k); + } + + ExtendedPose3 inverse() const { + const Rot3 Rt = R_.inverse(); + const Matrix3K x = -(Rt.matrix() * x_); + return ExtendedPose3(Rt, x); + } + + ExtendedPose3 operator*(const ExtendedPose3& other) const { + if (k() != other.k()) { + throw std::invalid_argument( + "ExtendedPose3: compose requires matching k."); + } + Matrix3K x = x_ + R_.matrix() * other.x_; + return ExtendedPose3(R_ * other.R_, x); + } + + /// @} + /// @name Lie Group + /// @{ + + static ExtendedPose3 Expmap(const TangentVector& xi, ChartJacobian Hxi = {}) { + const size_t k = RuntimeK(xi); + + const Vector3 w = xi.template head<3>(); + const so3::DexpFunctor local(w); + const Rot3 R(local.expmap()); + const Matrix3 Rt = R.transpose(); + + Matrix3K x; + if constexpr (K == Eigen::Dynamic) x.resize(3, static_cast(k)); + + if (Hxi) { + const size_t d = 3 + 3 * k; + ZeroJacobian(Hxi, d); + const Matrix3 Jr = local.Jacobian().right(); + Hxi->block(0, 0, 3, 3) = Jr; + for (size_t i = 0; i < k; ++i) { + Matrix3 H_xi_w; + const Vector3 rho = xi.template segment<3>(3 + 3 * i); + x.col(static_cast(i)) = + local.Jacobian().applyLeft(rho, &H_xi_w); + Hxi->block(3 + 3 * i, 0, 3, 3) = Rt * H_xi_w; + Hxi->block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jr; + } + } else { + for (size_t i = 0; i < k; ++i) { + const Vector3 rho = xi.template segment<3>(3 + 3 * i); + x.col(static_cast(i)) = local.Jacobian().applyLeft(rho); + } + } + + return ExtendedPose3(R, x); + } + + static TangentVector Logmap(const ExtendedPose3& pose, ChartJacobian Hpose = {}) { + const Vector3 w = Rot3::Logmap(pose.R_); + const so3::DexpFunctor local(w); + + TangentVector xi; + if constexpr (K == Eigen::Dynamic) xi.resize(static_cast(pose.dim())); + xi.template head<3>() = w; + for (size_t i = 0; i < pose.k(); ++i) { + xi.template segment<3>(3 + 3 * i) = + local.InvJacobian().applyLeft(pose.x_.col(static_cast(i))); + } + + if (Hpose) *Hpose = LogmapDerivative(xi); + return xi; + } + + Jacobian AdjointMap() const { + const Matrix3 R = R_.matrix(); + Jacobian adj; + if constexpr (dimension == Eigen::Dynamic) { + adj.setZero(dim(), dim()); + } else { + adj.setZero(); + } + + adj.block(0, 0, 3, 3) = R; + for (size_t i = 0; i < k(); ++i) { + adj.block(3 + 3 * i, 0, 3, 3) = + skewSymmetric(x_.col(static_cast(i))) * R; + adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = R; + } + return adj; + } + + TangentVector Adjoint(const TangentVector& xi_b, + ChartJacobian H_this = {}, + ChartJacobian H_xib = {}) const { + const Jacobian Ad = AdjointMap(); + if (H_this) *H_this = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + return Ad * xi_b; + } + + static Jacobian adjointMap(const TangentVector& xi) { + const size_t k = RuntimeK(xi); + const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + + Jacobian adj; + if constexpr (dimension == Eigen::Dynamic) { + adj.setZero(3 + 3 * k, 3 + 3 * k); + } else { + adj.setZero(); + } + + adj.block(0, 0, 3, 3) = w_hat; + for (size_t i = 0; i < k; ++i) { + adj.block(3 + 3 * i, 0, 3, 3) = + skewSymmetric(xi(3 + 3 * i + 0), xi(3 + 3 * i + 1), xi(3 + 3 * i + 2)); + adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = w_hat; + } + return adj; + } + + static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, + ChartJacobian Hxi = {}, ChartJacobian H_y = {}) { + const Jacobian ad_xi = adjointMap(xi); + if (Hxi) { + if constexpr (dimension == Eigen::Dynamic) { + Hxi->setZero(xi.size(), xi.size()); + } else { + Hxi->setZero(); + } + for (Eigen::Index i = 0; i < xi.size(); ++i) { + TangentVector dxi; + if constexpr (dimension == Eigen::Dynamic) { + dxi = TangentVector::Zero(xi.size()); + } else { + dxi = TangentVector::Zero(); + } + dxi(i) = 1.0; + Hxi->col(i) = adjointMap(dxi) * y; + } + } + if (H_y) *H_y = ad_xi; + return ad_xi * y; + } + + static Jacobian ExpmapDerivative(const TangentVector& xi) { + Jacobian J; + Expmap(xi, J); + return J; + } + + static Jacobian LogmapDerivative(const TangentVector& xi) { + const size_t k = RuntimeK(xi); + const Vector3 w = xi.template head<3>(); + const so3::DexpFunctor local(w); + const Matrix3 Rt = local.expmap().transpose(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + + Jacobian J; + if constexpr (dimension == Eigen::Dynamic) { + J.setZero(3 + 3 * k, 3 + 3 * k); + } else { + J.setZero(); + } + + J.block(0, 0, 3, 3) = Jw; + for (size_t i = 0; i < k; ++i) { + Matrix3 H_xi_w; + local.Jacobian().applyLeft(xi.template segment<3>(3 + 3 * i), H_xi_w); + const Matrix3 Q = Rt * H_xi_w; + J.block(3 + 3 * i, 0, 3, 3) = -Jw * Q * Jw; + J.block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jw; + } + return J; + } + + static Jacobian LogmapDerivative(const ExtendedPose3& pose) { + return LogmapDerivative(Logmap(pose)); + } + + struct ChartAtOrigin { + static ExtendedPose3 Retract(const TangentVector& xi, ChartJacobian Hxi = {}) { + return Expmap(xi, Hxi); + } + + static TangentVector Local(const ExtendedPose3& pose, ChartJacobian Hpose = {}) { + return Logmap(pose, Hpose); + } + }; + + using LieGroup, dimension>::inverse; + + /// @} + /// @name Matrix Lie Group + /// @{ + + LieAlgebra matrix() const { + LieAlgebra M; + if constexpr (matrix_dim == Eigen::Dynamic) { + const Eigen::Index n = 3 + static_cast(k()); + M = LieAlgebra::Identity(n, n); + } else { + M = LieAlgebra::Identity(); + } + M.template block<3, 3>(0, 0) = R_.matrix(); + M.block(0, 3, 3, static_cast(k())) = x_; + return M; + } + + static LieAlgebra Hat(const TangentVector& xi) { + const size_t k = RuntimeK(xi); + LieAlgebra X; + if constexpr (matrix_dim == Eigen::Dynamic) { + X.setZero(3 + k, 3 + k); + } else { + X.setZero(); + } + X.block(0, 0, 3, 3) = skewSymmetric(xi(0), xi(1), xi(2)); + for (size_t i = 0; i < k; ++i) { + X.block(0, 3 + i, 3, 1) = xi.template segment<3>(3 + 3 * i); + } + return X; + } + + static TangentVector Vee(const LieAlgebra& X) { + const Eigen::Index k = X.cols() - 3; + TangentVector xi; + if constexpr (dimension == Eigen::Dynamic) { + xi.resize(3 + 3 * k); + } + xi(0) = X(2, 1); + xi(1) = X(0, 2); + xi(2) = X(1, 0); + for (Eigen::Index i = 0; i < k; ++i) { + xi.template segment<3>(3 + 3 * i) = X.template block<3, 1>(0, 3 + i); + } + return xi; + } + + /// @} + + friend std::ostream& operator<<(std::ostream& os, const ExtendedPose3& p) { + os << "R: " << p.R_ << "\n"; + os << "x: " << p.x_; + return os; + } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(R_); + ar& BOOST_SERIALIZATION_NVP(x_); + } +#endif +}; + +/// Convenience typedef for dynamic k. +using ExtendedPose3Dynamic = ExtendedPose3; + +template +struct traits> + : public internal::MatrixLieGroup, + internal::MatrixDimExtendedPose3(K)> {}; + +template +struct traits> + : public internal::MatrixLieGroup, + internal::MatrixDimExtendedPose3(K)> {}; + +} // namespace gtsam diff --git a/gtsam/geometry/doc/ExtendedPose3.ipynb b/gtsam/geometry/doc/ExtendedPose3.ipynb new file mode 100644 index 0000000000..3016d4a6a7 --- /dev/null +++ b/gtsam/geometry/doc/ExtendedPose3.ipynb @@ -0,0 +1,90 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# ExtendedPose3\n", + "\n", + "`ExtendedPose3` is the generic Lie group $SE_K(3)$: the semi-direct product of `SO(3)` with `K` copies of $\\mathbb{R}^3$.\n", + "\n", + "It is a matrix Lie group with homogeneous representation size $(3+K)\\times(3+K)$ and manifold dimension $3 + 3K$." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## State and Tangent Layout\n", + "\n", + "An element is parameterized as:\n", + "\n", + "$$\n", + "X = (R, x_1, \\ldots, x_K), \\qquad R \\in SO(3),\\; x_i \\in \\mathbb{R}^3\n", + "$$\n", + "\n", + "with tangent vector ordering:\n", + "\n", + "$$\n", + "\\xi = [\\omega, \\rho_1, \\ldots, \\rho_K] \\in \\mathbb{R}^{3+3K}.\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Matrix Representation\n", + "\n", + "$$\n", + "X = \\begin{bmatrix}\n", + "R & x_1 & \\cdots & x_K \\\\\n", + "0 & 1 & & 0 \\\\\n", + "\\vdots & & \\ddots & \\\\\n", + "0 & 0 & & 1\n", + "\\end{bmatrix}\n", + "$$\n", + "\n", + "The group operation is:\n", + "\n", + "$$\n", + "(R,x_i)\\,(S,y_i) = (RS,\\; x_i + R y_i).\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## C++ Usage Notes\n", + "\n", + "- Fixed-size: `ExtendedPose3<3>` gives a 12D manifold.\n", + "- Dynamic-size: `ExtendedPose3` takes runtime `k` in constructor.\n", + "- Default dynamic construction (`k=0`) is supported for serialization and placeholder use.\n", + "- The API provides the standard `Manifold`, `LieGroup`, and `MatrixLieGroup` surface: `retract/localCoordinates`, `Expmap/Logmap`, `AdjointMap`, `Hat/Vee`, and `matrix/vec`." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Related Guides\n", + "\n", + "- Pose manifold background: [Pose3](./Pose3.ipynb)\n", + "- Navigation usage and `NavState`: [ImuFactor / NavState docs](../../navigation/doc/ImuFactor.ipynb)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp new file mode 100644 index 0000000000..e9b5157d88 --- /dev/null +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -0,0 +1,222 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExtendedPose3.cpp + * @brief Unit tests for ExtendedPose3 + */ + +#include + +#include +#include +#include +#include + +#include + +using namespace gtsam; + +using ExtendedPose33 = ExtendedPose3<3>; +using ExtendedPose3d = ExtendedPose3; + +GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose33) +GTSAM_CONCEPT_MATRIX_LIE_GROUP_INST(ExtendedPose33) +GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose3d) +GTSAM_CONCEPT_MATRIX_LIE_GROUP_INST(ExtendedPose3d) + +namespace { + +const Rot3 kR1 = Rot3::RzRyRx(0.1, -0.2, 0.3); +const Rot3 kR2 = Rot3::RzRyRx(-0.3, 0.4, -0.2); +const Matrix33 kX1 = (Matrix33() << 1.0, 4.0, -1.0, 2.0, 5.0, 0.5, 3.0, 6.0, + 2.0) + .finished(); +const Matrix33 kX2 = (Matrix33() << -2.0, 1.0, 0.1, 0.3, -1.5, 2.2, 0.7, -0.9, + 1.6) + .finished(); +const Vector12 kXi = + (Vector12() << 0.11, -0.07, 0.05, 0.3, -0.4, 0.1, -0.2, 0.6, -0.5, 0.7, + -0.1, 0.2) + .finished(); + +ExtendedPose3d MakeDynamic(const Rot3& R, const Matrix33& X) { + Matrix Xd = X; + return ExtendedPose3d(R, Xd); +} + +} // namespace + +//****************************************************************************** +TEST(ExtendedPose3, Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); + + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); +} + +//****************************************************************************** +TEST(ExtendedPose3, Dimensions) { + const ExtendedPose33 fixed; + EXPECT_LONGS_EQUAL(12, traits::GetDimension(fixed)); + EXPECT_LONGS_EQUAL(3, fixed.k()); + + const ExtendedPose3d placeholder; + EXPECT_LONGS_EQUAL(0, placeholder.k()); + EXPECT_LONGS_EQUAL(3, placeholder.dim()); + + const ExtendedPose3d dynamic(3); + EXPECT_LONGS_EQUAL(3, dynamic.k()); + EXPECT_LONGS_EQUAL(12, dynamic.dim()); +} + +//****************************************************************************** +TEST(ExtendedPose3, ConstructorsAndAccess) { + const ExtendedPose33 fixed(kR1, kX1); + const ExtendedPose3d dynamic = MakeDynamic(kR1, kX1); + + EXPECT(assert_equal(kR1, fixed.rotation())); + EXPECT(assert_equal(kR1, dynamic.rotation())); + + for (size_t i = 0; i < 3; ++i) { + EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), fixed.x(i))); + EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), dynamic.x(i))); + } + + EXPECT(assert_equal(fixed.matrix(), dynamic.matrix())); +} + +//****************************************************************************** +TEST(ExtendedPose3, GroupOperationsMatch) { + const ExtendedPose33 f1(kR1, kX1), f2(kR2, kX2); + const ExtendedPose3d d1 = MakeDynamic(kR1, kX1), d2 = MakeDynamic(kR2, kX2); + + const auto f12 = f1 * f2; + const auto d12 = d1 * d2; + EXPECT(assert_equal(f12.matrix(), d12.matrix())); + + const auto f_id = f1 * f1.inverse(); + const auto d_id = d1 * d1.inverse(); + EXPECT(assert_equal(ExtendedPose33::Identity().matrix(), f_id.matrix(), 1e-9)); + EXPECT(assert_equal(ExtendedPose3d::Identity(3).matrix(), d_id.matrix(), 1e-9)); + + const auto f_between = f1.between(f2); + const auto d_between = d1.between(d2); + EXPECT(assert_equal(f_between.matrix(), d_between.matrix(), 1e-9)); +} + +//****************************************************************************** +TEST(ExtendedPose3, HatVee) { + const Vector xi = kXi; + const auto Xf = ExtendedPose33::Hat(kXi); + const auto Xd = ExtendedPose3d::Hat(xi); + + EXPECT(assert_equal(xi, Vector(ExtendedPose33::Vee(Xf)))); + EXPECT(assert_equal(xi, ExtendedPose3d::Vee(Xd))); + EXPECT(assert_equal(Xf, Xd)); +} + +//****************************************************************************** +TEST(ExtendedPose3, ExpmapLogmapRoundTrip) { + const Vector xi = kXi; + const ExtendedPose33 f = ExtendedPose33::Expmap(kXi); + const ExtendedPose3d d = ExtendedPose3d::Expmap(xi); + + EXPECT(assert_equal(f.matrix(), d.matrix(), 1e-9)); + EXPECT(assert_equal(xi, Vector(ExtendedPose33::Logmap(f)), 1e-9)); + EXPECT(assert_equal(xi, ExtendedPose3d::Logmap(d), 1e-9)); +} + +//****************************************************************************** +TEST(ExtendedPose3, AdjointConsistency) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d = MakeDynamic(kR1, kX1); + const Vector xi = kXi; + + const auto f_conj = f * ExtendedPose33::Expmap(kXi) * f.inverse(); + const auto d_conj = d * ExtendedPose3d::Expmap(xi) * d.inverse(); + + const auto f_adj = ExtendedPose33::Expmap(f.Adjoint(kXi)); + const auto d_adj = ExtendedPose3d::Expmap(d.Adjoint(xi)); + EXPECT(assert_equal(f_conj.matrix(), f_adj.matrix(), 1e-9)); + EXPECT(assert_equal(d_conj.matrix(), d_adj.matrix(), 1e-9)); + + const ExtendedPose33::Jacobian f_generic = + static_cast*>(&f)->AdjointMap(); + const Matrix d_generic = static_cast< + const MatrixLieGroup*>(&d) + ->AdjointMap(); + EXPECT(assert_equal(Matrix(f_generic), Matrix(f.AdjointMap()), 1e-9)); + EXPECT(assert_equal(d_generic, d.AdjointMap(), 1e-9)); +} + +//****************************************************************************** +TEST(ExtendedPose3, Derivatives) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d = MakeDynamic(kR1, kX1); + const Vector xi = kXi; + + Matrix Hf; + ExtendedPose33::Expmap(kXi, Hf); + const auto f_exp = [](const Vector12& v) { return ExtendedPose33::Expmap(v); }; + const Matrix Hf_num = numericalDerivative11(f_exp, kXi); + EXPECT(assert_equal(Hf_num, Hf, 1e-6)); + + Matrix Hd; + ExtendedPose3d::Expmap(xi, Hd); + const auto d_exp = [](const Vector& v) { return ExtendedPose3d::Expmap(v); }; + const Matrix Hd_num = numericalDerivative11(d_exp, xi); + EXPECT(assert_equal(Hd_num, Hd, 1e-6)); + + Matrix Lf; + ExtendedPose33::Logmap(f, Lf); + const auto f_log = [](const ExtendedPose33& g) { return Vector12(ExtendedPose33::Logmap(g)); }; + const Matrix Lf_num = + numericalDerivative11(f_log, f); + EXPECT(assert_equal(Lf_num, Lf, 1e-6)); + + Matrix Ld; + ExtendedPose3d::Logmap(d, Ld); + const auto d_log = [](const ExtendedPose3d& g) { return ExtendedPose3d::Logmap(g); }; + const Matrix Ld_num = + numericalDerivative11(d_log, d); + EXPECT(assert_equal(Ld_num, Ld, 1e-6)); +} + +//****************************************************************************** +TEST(ExtendedPose3, VecJacobian) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d = MakeDynamic(kR1, kX1); + + Matrix Hf; + const Vector vf = f.vec(Hf); + const auto fv = [](const ExtendedPose33& g) { return Vector(g.vec()); }; + const Matrix Hf_num = numericalDerivative11(fv, f); + EXPECT(assert_equal(Hf_num, Hf, 1e-6)); + EXPECT_LONGS_EQUAL(36, vf.size()); + + Matrix Hd; + const Vector vd = d.vec(Hd); + const auto dv = [](const ExtendedPose3d& g) { return Vector(g.vec()); }; + const Matrix Hd_num = numericalDerivative11(dv, d); + EXPECT(assert_equal(Hd_num, Hd, 1e-6)); + EXPECT_LONGS_EQUAL(36, vd.size()); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 81f8bf71221a5a32fc917e491f6275009d208b52 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 10:27:34 -0500 Subject: [PATCH 12/41] Pose3 now SE_1(3) --- gtsam/geometry/Pose3.cpp | 351 ++++++++++++--------------------------- gtsam/geometry/Pose3.h | 121 +++++--------- 2 files changed, 148 insertions(+), 324 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 163c3c0a7f..58681da5a5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -31,10 +31,9 @@ namespace gtsam { GTSAM_CONCEPT_POSE_INST(Pose3) /* ************************************************************************* */ -Pose3::Pose3(const Pose2& pose2) : - R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_( - Point3(pose2.x(), pose2.y(), 0)) { -} +Pose3::Pose3(const Pose2& pose2) + : Base(Rot3::Rodrigues(0, 0, pose2.theta()), + Vector3(pose2.x(), pose2.y(), 0.0)) {} /* ************************************************************************* */ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, @@ -59,38 +58,101 @@ Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { } /* ************************************************************************* */ -Pose3 Pose3::inverse() const { - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); +Pose3 Pose3::inverse() const { return Pose3(Base::inverse()); } + +/* ************************************************************************* */ +Pose3 Pose3::operator*(const Pose3& T) const { + return Pose3(Base::operator*(T)); } /* ************************************************************************* */ -// Calculate Adjoint map -// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix6 Pose3::AdjointMap() const { - const Matrix3 R = R_.matrix(); - Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; - Matrix6 adj; - adj << R, Z_3x3, A, R; // Gives [R 0; A R] - return adj; +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { + return Pose3(Base::Expmap(xi, Hxi)); } /* ************************************************************************* */ -// Calculate AdjointMap applied to xi_b, with Jacobians -Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, +Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { + return Base::Logmap(pose, Hpose); +} + +/* ************************************************************************* */ +Matrix6 Pose3::AdjointMap() const { return Base::AdjointMap(); } + +/* ************************************************************************* */ +Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_xib) const { - const Matrix6 Ad = AdjointMap(); + return Base::Adjoint(xi_b, H_this, H_xib); +} - // Jacobians - // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b - // D2 Ad_T(xi_b) = Ad_T - // See docs/math.pdf for more details. - // In D1 calculation, we could be more efficient by writing it out, but do not - // for readability - if (H_pose) *H_pose = -Ad * adjointMap(xi_b); - if (H_xib) *H_xib = Ad; +/* ************************************************************************* */ +Matrix6 Pose3::adjointMap(const Vector6& xi) { return Base::adjointMap(xi); } - return Ad * xi_b; +/* ************************************************************************* */ +Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> Hxi, + OptionalJacobian<6, 6> H_y) { + return Base::adjoint(xi, y, Hxi, H_y); +} + +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + return Base::ExpmapDerivative(xi); +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { + return Base::LogmapDerivative(xi); +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { + return Base::LogmapDerivative(pose); +} + +/* ************************************************************************* */ +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, Hxi); +#else + Matrix3 DR; + const Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : nullptr); + if (Hxi) { + Hxi->setIdentity(); + Hxi->block<3, 3>(0, 0) = DR; + } + return Pose3(R, xi.tail<3>()); +#endif +} + +/* ************************************************************************* */ +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(pose, Hpose); +#else + Matrix3 DR; + Vector6 xi; + xi.head<3>() = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : nullptr); + xi.tail<3>() = pose.translation(); + if (Hpose) { + Hpose->setIdentity(); + Hpose->block<3, 3>(0, 0) = DR; + } + return xi; +#endif +} + +/* ************************************************************************* */ +Matrix4 Pose3::Hat(const Vector6& xi) { return Base::Hat(xi); } + +/* ************************************************************************* */ +Vector6 Pose3::Vee(const Matrix4& X) { return Base::Vee(X); } + +/* ************************************************************************* */ +Matrix4 Pose3::matrix() const { return Base::matrix(); } + +/* ************************************************************************* */ +Pose3::Vector16 Pose3::vec(OptionalJacobian<16, 6> H) const { + return Base::vec(H); } /* ************************************************************************* */ @@ -115,34 +177,6 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, return AdTx; } -/* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector6& xi) { - Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); - Matrix6 adj; - adj << w_hat, Z_3x3, v_hat, w_hat; - - return adj; -} - -/* ************************************************************************* */ -Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 6; ++i) { - Vector6 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix6 Gi = adjointMap(dxi); - Hxi->col(i) = Gi * y; - } - } - const Matrix6& ad_xi = adjointMap(xi); - if (H_y) *H_y = ad_xi; - return ad_xi * y; -} - /* ************************************************************************* */ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { @@ -161,21 +195,6 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, return adT_xi * y; } -/* ************************************************************************* */ -Matrix4 Pose3::Hat(const Vector6& xi) { - Matrix4 X; - const double wx = xi(0), wy = xi(1), wz = xi(2), vx = xi(3), vy = xi(4), vz = xi(5); - X << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.; - return X; -} - -/* ************************************************************************* */ -Vector6 Pose3::Vee(const Matrix4& Xi) { - Vector6 xi; - xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), Xi(0, 3), Xi(1, 3), Xi(2, 3); - return xi; -} - /* ************************************************************************* */ void Pose3::print(const std::string& s) const { std::cout << (s.empty() ? s : s + " ") << *this << std::endl; @@ -183,7 +202,8 @@ void Pose3::print(const std::string& s) const { /* ************************************************************************* */ bool Pose3::equals(const Pose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); + return R_.equals(pose.R_, tol) && + traits::Equals(x_.col(0), pose.x_.col(0), tol); } /* ************************************************************************* */ @@ -198,190 +218,31 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t, typename MakeJacobian::type HtPoint; Rot3 Rint = interpolate(R_, T.R_, t, HselfRot, HargRot, HtRot); - Point3 Pint = interpolate(t_, T.t_, t, HselfPoint, HargPoint, HtPoint); + Point3 Pint = interpolate(x_.col(0), T.x_.col(0), t, HselfPoint, + HargPoint, HtPoint); Pose3 result = Pose3(Rint, Pint); if(Hself) *Hself << HselfRot, Z_3x3, Z_3x3, Rint.transpose() * R_.matrix() * HselfPoint; - if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, Rint.transpose() * T.R_.matrix() * HargPoint; + if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, + Rint.transpose() * T.R_.matrix() * HargPoint; if(Ht) *Ht << HtRot, Rint.transpose() * HtPoint; return result; } return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); - -} + interpolate(x_.col(0), T.x_.col(0), t)); -/* ************************************************************************* */ -// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's -// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. -// See also [this document](doc/Jacobians.md) -Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { - // Get angular velocity omega and translational velocity v from twist xi - const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Compute rotation using Expmap -#ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(w); -#else - const Rot3 R(local.expmap()); -#endif - - // The translation t = local.Jacobian().left() * v. - // Here we call local.Jacobian().applyLeft, which is faster if you don't need - // Jacobians, and returns Jacobian of t with respect to w if asked. - // NOTE(Frank): this does the same as the intuitive formulas: - // t_parallel = w * w.dot(v); // translation parallel to axis - // w_cross_v = w.cross(v); // translation orthogonal to axis - // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; - // but Local does not need R, deals automatically with the case where theta2 - // is near zero, and also gives us the machinery for the Jacobians. - Matrix3 H; - const Vector3 t = local.Jacobian().applyLeft(v, Hxi ? &H : nullptr); - - if (Hxi) { - // The Jacobian of expmap is given by the right Jacobian of SO(3): - const Matrix3 Jr = local.Jacobian().right(); - // We are creating a Pose3, so we still need to chain H with R^T, the - // Jacobian of Pose3::Create with respect to t. - const Matrix3 Rt = R.transpose(); - *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap - Rt * H, Jr; // Here Jr = R^T * Jl, with Jl the Jacobian of t in v. - } - - return Pose3(R, t); -} - -/* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { - const Vector3 w = Rot3::Logmap(pose.rotation()); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - const Vector3 t = pose.translation(); - const Vector3 u = local.InvJacobian().applyLeft(t); - Vector6 xi; - xi << w, u; - if (Hpose) *Hpose = LogmapDerivative(xi); - return xi; -} - -/* ************************************************************************* */ -Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { -#ifdef GTSAM_POSE3_EXPMAP - return Expmap(xi, Hxi); -#else - Matrix3 DR; - Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0); - if (Hxi) { - *Hxi = I_6x6; - Hxi->topLeftCorner<3, 3>() = DR; - } - return Pose3(R, Point3(xi.tail<3>())); -#endif -} - -/* ************************************************************************* */ -Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { -#ifdef GTSAM_POSE3_EXPMAP - return Logmap(pose, Hpose); -#else - Matrix3 DR; - Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0); - if (Hpose) { - *Hpose = I_6x6; - Hpose->topLeftCorner<3, 3>() = DR; - } - Vector6 xi; - xi << omega, pose.translation(); - return xi; -#endif -} - -/* ************************************************************************* */ -Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - Matrix6 J; - Expmap(xi, J); - return J; -} - -/* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - Vector3 v = xi.segment<3>(3); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Call applyLeftJacobian to get its Jacobians - Matrix3 H_t_w; - local.Jacobian().applyLeft(v, H_t_w); - - // Multiply with R^T to account for NavState::Create Jacobian. - const Matrix3 R = local.expmap(); - const Matrix3 Qt = R.transpose() * H_t_w; - - // Now compute the blocks of the LogmapDerivative Jacobian - const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Qt2 = -Jw * Qt * Jw; - - Matrix6 J; - J << Jw, Z_3x3, Qt2, Jw; - return J; -} - -/* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { - const Vector6 xi = Logmap(pose); - return LogmapDerivative(xi); } /* ************************************************************************* */ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { - if (Hself) *Hself << Z_3x3, rotation().matrix(); - return t_; + t_cache_ = Base::x(0, Hself); + return t_cache_; } /* ************************************************************************* */ const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { - if (Hself) { - *Hself << I_3x3, Z_3x3; - } - return R_; -} - -/* ************************************************************************* */ -Matrix4 Pose3::matrix() const { - static const auto A14 = Eigen::RowVector4d(0,0,0,1); - Matrix4 mat; - mat << R_.matrix(), t_, A14; - return mat; -} - -/* ************************************************************************* */ -Pose3::Vector16 Pose3::vec(OptionalJacobian<16, 6> H) const { - // Vectorize - const Matrix4 M = matrix(); - const Vector16 v = Eigen::Map(M.data()); - - // If requested, calculate H - if (H) { - H->setZero(); - auto R = M.block<3, 3>(0, 0); - H->block<3, 1>(0, 1) = -R.col(2); - H->block<3, 1>(0, 2) = R.col(1); - H->block<3, 1>(4, 0) = R.col(2); - H->block<3, 1>(4, 2) = -R.col(0); - H->block<3, 1>(8, 0) = -R.col(1); - H->block<3, 1>(8, 1) = R.col(0); - H->block<3,3>(12,3) = R; - } - - return v; + return Base::rotation(Hself); } /* ************************************************************************* */ @@ -413,7 +274,7 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, if (Hpoint) { *Hpoint = R; } - return R_ * point + t_; + return R_ * point + x_.col(0); } Matrix Pose3::transformFrom(const Matrix& points) const { @@ -421,7 +282,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); } const Matrix3 R = R_.matrix(); - return (R * points).colwise() + t_; // Eigen broadcasting! + return (R * points).colwise() + x_.col(0); // Eigen broadcasting! } /* ************************************************************************* */ @@ -430,7 +291,7 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(point - t_)); + const Point3 q(Rt * (point - x_.col(0))); if (Hself) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Hself) << @@ -449,13 +310,13 @@ Matrix Pose3::transformTo(const Matrix& points) const { throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); } const Matrix3 Rt = R_.transpose(); - return Rt * (points.colwise() - t_); // Eigen broadcasting! + return Rt * (points.colwise() - x_.col(0)); // Eigen broadcasting! } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 3> Hpoint) const { - const Vector3 delta = point - t_; + const Vector3 delta = point - x_.col(0); if (!Hself && !Hpoint) return delta.norm(); Matrix13 D_r_point; @@ -474,7 +335,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, /* ************************************************************************* */ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 6> Hpose) const { - const Vector3 delta = pose.t_ - t_; + const Vector3 delta = pose.x_.col(0) - x_.col(0); if (!Hself && !Hpose) return delta.norm(); Matrix13 D_r_point; @@ -500,7 +361,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself, OptionalJacobian<2, 3> Hpoint) const { const Matrix3 Rt = R_.transpose(); - const Point3 local(Rt * (point - t_)); + const Point3 local(Rt * (point - x_.col(0))); if (!Hself && !Hpoint) return Unit3(local); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index a6b9c40b94..85eae9f8c2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -20,10 +20,15 @@ #include #include +#include #include #include #include +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + namespace gtsam { class Pose2; @@ -34,17 +39,15 @@ class Pose2; * @ingroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3: public MatrixLieGroup { +class GTSAM_EXPORT Pose3: public MatrixLieGroup, public ExtendedPose3<1> { public: + using Base = ExtendedPose3<1>; + using LieGroupBase = MatrixLieGroup; /** Pose Concept requirements */ typedef Rot3 Rotation; typedef Point3 Translation; - -private: - - Rot3 R_; ///< Rotation gRp, between global and pose frame - Point3 t_; ///< Translation gPp, from global origin to pose frame origin + inline constexpr static auto dimension = 6; public: using Vector16 = Eigen::Matrix; @@ -53,26 +56,23 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { /// @{ /** Default constructor is origin */ - Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} + Pose3() : Base() {} /** Copy constructor */ Pose3(const Pose3& pose) = default; Pose3& operator=(const Pose3& other) = default; + Pose3(const Base& other) : Base(other) {} + /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : - R_(R), t_(t) { - } + Pose3(const Rot3& R, const Point3& t) : Base(R, Vector3(t.x(), t.y(), t.z())) {} /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), - T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { - } + Pose3(const Matrix &T) : Base(Matrix4(T)) {} /// Named constructor with derivatives static Pose3 Create(const Rot3& R, const Point3& t, @@ -106,18 +106,14 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { /// @name Group /// @{ - /// identity for group operation static Pose3 Identity() { - return Pose3(); + return Pose3(Base::Identity()); } - /// inverse transformation Pose3 inverse() const; /// compose syntactic sugar - Pose3 operator*(const Pose3& T) const { - return Pose3(R_ * T.R_, t_ + R_ * T.t_); - } + Pose3 operator*(const Pose3& T) const; /** * Interpolate between two poses via individual rotation and translation @@ -144,56 +140,26 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { using LieAlgebra = Matrix4; - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); - /** - * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame - * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) - */ Matrix6 AdjointMap() const; - /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a - * body-fixed velocity, transforming it to the spatial frame - * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ - * Note that H_xib = AdjointMap() - */ Vector6 Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this = {}, OptionalJacobian<6, 6> H_xib = {}) const; - - /// The dual version of Adjoint - Vector6 AdjointTranspose(const Vector6& x, - OptionalJacobian<6, 6> H_this = {}, - OptionalJacobian<6, 6> H_x = {}) const; - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * [ad(w,v)] = [w^, zero3; v^, w^] - * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, - * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. - * - * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its - * vector representation. - * We have the following relationship: - * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ - * - * We use this to compute the discrete version of the inverse right-trivialized tangent map, - * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. - * - */ static Matrix6 adjointMap(const Vector6& xi); - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ static Vector6 adjoint(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> Hxi = {}, OptionalJacobian<6, 6> H_y = {}); + + /// The dual version of Adjoint + Vector6 AdjointTranspose(const Vector6& x, + OptionalJacobian<6, 6> H_this = {}, + OptionalJacobian<6, 6> H_x = {}) const; // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -206,35 +172,37 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { OptionalJacobian<6, 6> Hxi = {}, OptionalJacobian<6, 6> H_y = {}); - /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); - /// Derivative of Logmap static Matrix6 LogmapDerivative(const Vector6& xi); - /// Derivative of Logmap, Pose3 version. TODO(Frank): deprecate? static Matrix6 LogmapDerivative(const Pose3& pose); - // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; - using LieGroup::inverse; // version with derivative + using LieGroupBase::between; + using LieGroupBase::compose; + using LieGroupBase::expmap; + using LieGroupBase::LocalCoordinates; + using LieGroupBase::logmap; + using LieGroupBase::Retract; + using LieGroup::inverse; + using LieGroupBase::localCoordinates; + using LieGroupBase::retract; - /** - * Hat for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ static Matrix4 Hat(const Vector6& xi); - /// Vee maps from Lie algebra to tangent vector static Vector6 Vee(const Matrix4& X); + /** convert to 4*4 matrix */ + Matrix4 matrix() const; + + /// Return vectorized SE(3) matrix in column order. + Vector16 vec(OptionalJacobian<16, 6> H = {}) const; + /// @} /// @name Group Action on Point3 /// @{ @@ -290,25 +258,19 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { /// get x double x() const { - return t_.x(); + return translation().x(); } /// get y double y() const { - return t_.y(); + return translation().y(); } /// get z double z() const { - return t_.z(); + return translation().z(); } - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /// Return vectorized SE(3) matrix in column order. - Vector16 vec(OptionalJacobian<16, 6> H = {}) const; - /** * Assuming self == wTa, takes a pose aTb in local coordinates * and transforms it to world coordinates wTb = wTa * aTb. @@ -405,13 +367,14 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { /// @} private: + mutable Point3 t_cache_ = Point3::Zero(); + #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } #endif /// @} From 81cc49874ca6b27ab5a5d4f2df9f00330d5e10ec Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 10:27:53 -0500 Subject: [PATCH 13/41] NavState now SE_2(3) --- gtsam/navigation/NavState.cpp | 334 +++++++++------------------------- gtsam/navigation/NavState.h | 134 +++++--------- 2 files changed, 128 insertions(+), 340 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 4abe029503..58cc9c35d2 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -23,6 +23,75 @@ namespace gtsam { +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { return NavState(Base::inverse()); } + +//------------------------------------------------------------------------------ +NavState NavState::operator*(const NavState& other) const { + return NavState(Base::operator*(other)); +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + return NavState(Base::Expmap(xi, Hxi)); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose) { + return Base::Logmap(pose, Hpose); +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { return Base::AdjointMap(); } + +//------------------------------------------------------------------------------ +Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_this, + OptionalJacobian<9, 9> H_xib) const { + return Base::Adjoint(xi_b, H_this, H_xib); +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::adjointMap(const Vector9& xi) { return Base::adjointMap(xi); } + +//------------------------------------------------------------------------------ +Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + return Base::adjoint(xi, y, Hxi, H_y); +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + return Base::ExpmapDerivative(xi); +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const Vector9& xi) { + return Base::LogmapDerivative(xi); +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + return Base::LogmapDerivative(state); +} + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { + return NavState(Base::ChartAtOrigin::Retract(xi, Hxi)); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& state, + ChartJacobian Hstate) { + return Base::ChartAtOrigin::Local(state, Hstate); +} + +//------------------------------------------------------------------------------ +Matrix5 NavState::Hat(const Vector9& xi) { return Base::Hat(xi); } + +//------------------------------------------------------------------------------ +Vector9 NavState::Vee(const Matrix5& X) { return Base::Vee(X); } + //------------------------------------------------------------------------------ NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v, OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, @@ -46,29 +115,25 @@ NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, //------------------------------------------------------------------------------ const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { - if (H) - *H << I_3x3, Z_3x3, Z_3x3; - return R_; + return Base::rotation(H); } //------------------------------------------------------------------------------ const Point3& NavState::position(OptionalJacobian<3, 9> H) const { - if (H) - *H << Z_3x3, R(), Z_3x3; - return t_; + p_cache_ = Base::x(0, H); + return p_cache_; } //------------------------------------------------------------------------------ const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { - if (H) - *H << Z_3x3, Z_3x3, R(); - return v_; + v_cache_ = Base::x(1, H); + return v_cache_; } //------------------------------------------------------------------------------ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { const Rot3& nRb = R_; - const Vector3& n_v = v_; + const Vector3 n_v = x_.col(1); Matrix3 D_bv_nRb; Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0); if (H) @@ -79,7 +144,7 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { //------------------------------------------------------------------------------ double NavState::range(const Point3& point, OptionalJacobian<1, 9> Hself, OptionalJacobian<1, 3> Hpoint) const { - const Vector3 delta = point - t_; + const Vector3 delta = point - x_.col(0); const double r = delta.norm(); if (!Hself && !Hpoint) return r; @@ -109,35 +174,6 @@ Unit3 NavState::bearing(const Point3& point, OptionalJacobian<2, 9> Hself, return b; } -//------------------------------------------------------------------------------ -Matrix5 NavState::matrix() const { - Matrix3 R = this->R(); - - Matrix5 T = Matrix5::Identity(); - T.block<3, 3>(0, 0) = R; - T.block<3, 1>(0, 3) = t_; - T.block<3, 1>(0, 4) = v_; - return T; -} - -//------------------------------------------------------------------------------ -NavState::Vector25 NavState::vec(OptionalJacobian<25, 9> H) const { - const Matrix5 T = this->matrix(); - if (H) { - H->setZero(); - auto R = T.block<3, 3>(0, 0); - H->block<3, 1>(0, 1) = -R.col(2); - H->block<3, 1>(0, 2) = R.col(1); - H->block<3, 1>(5, 0) = R.col(2); - H->block<3, 1>(5, 2) = -R.col(0); - H->block<3, 1>(10, 0) = -R.col(1); - H->block<3, 1>(10, 1) = R.col(0); - H->block<3, 3>(15, 3) = R; - H->block<3, 3>(20, 6) = R; - } - return Eigen::Map(T.data()); -} - //------------------------------------------------------------------------------ std::ostream& operator<<(std::ostream& os, const NavState& state) { os << "R: " << state.attitude() << "\n"; @@ -153,218 +189,16 @@ void NavState::print(const std::string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { - return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) - && equal_with_abs_tol(v_, other.v_, tol); -} - -//------------------------------------------------------------------------------ -NavState NavState::inverse() const { - Rot3 Rt = R_.inverse(); - return NavState(Rt, Rt * (-t_), Rt * -(v_)); -} - -//------------------------------------------------------------------------------ -// See [this document](doc/Jacobians.md) for details. -NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w and components rho (for t) and nu (for v) from xi - Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Compute rotation using Expmap -#ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(w); -#else - const Rot3 R(local.expmap()); -#endif - - // Compute translation and velocity. See Pose3::Expmap - Matrix3 H_t_w, H_v_w; - const Vector3 t = local.Jacobian().applyLeft(rho, Hxi ? &H_t_w : nullptr); - const Vector3 v = local.Jacobian().applyLeft(nu, Hxi ? &H_v_w : nullptr); - - if (Hxi) { - const Matrix3 Jr = local.Jacobian().right(); - // We are creating a NavState, so we still need to chain H_t_w and H_v_w - // with R^T, the Jacobian of Navstate::Create with respect to both t and v. - const Matrix3 Rt = R.transpose(); - *Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap - Rt * H_t_w, Jr, Z_3x3, // - Rt * H_v_w, Z_3x3, Jr; - // In the last two rows, Jr = R^T * Jl, see Barfoot eq. (8.83). - // Jl is the left Jacobian of SO(3) at w. - } - - return NavState(R, t, v); -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { - if (Hstate) *Hstate = LogmapDerivative(state); - - const Vector3 phi = Rot3::Logmap(state.rotation()); - const Vector3& p = state.position(); - const Vector3& v = state.velocity(); - const double t = phi.norm(); - if (t < 1e-8) { - Vector9 log; - log << phi, p, v; - return log; - - } else { - const Matrix3 W = skewSymmetric(phi / t); - - const double Tan = tan(0.5 * t); - const Vector3 Wp = W * p; - const Vector3 Wv = W * v; - const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); - const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); - Vector9 log; - // Order is ω, p, v - log << phi, rho, nu; - return log; - } -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::AdjointMap() const { - const Matrix3 R = R_.matrix(); - Matrix3 A = skewSymmetric(t_) * R; - Matrix3 B = skewSymmetric(v_) * R; - // Eqn 2 in Barrau20icra - Matrix9 adj; - adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; - return adj; -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, - OptionalJacobian<9, 9> H_xib) const { - const Matrix9 Ad = AdjointMap(); - - // Jacobians - if (H_state) *H_state = -Ad * adjointMap(xi_b); - if (H_xib) *H_xib = Ad; - - return Ad * xi_b; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::adjointMap(const Vector9& xi) { - Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); - Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); - Matrix9 adj; - adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat; - return adj; -} - -//------------------------------------------------------------------------------ -Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi, - OptionalJacobian<9, 9> H_y) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 9; ++i) { - Vector9 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix9 Gi = adjointMap(dxi); - Hxi->col(i) = Gi * y; - } - } - - const Matrix9& ad_xi = adjointMap(xi); - if (H_y) *H_y = ad_xi; - - return ad_xi * y; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { - Matrix9 J; - Expmap(xi, J); - return J; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const Vector9& xi) { - const Vector3 w = xi.head<3>(); - Vector3 rho = xi.segment<3>(3); - Vector3 nu = xi.tail<3>(); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Call Jacobian().applyLeft to get its Jacobians - Matrix3 H_t_w, H_v_w; - local.Jacobian().applyLeft(rho, H_t_w); - local.Jacobian().applyLeft(nu, H_v_w); - - // Multiply with R^T to account for NavState::Create Jacobian. - const Matrix3 Rt = local.expmap().transpose(); - const Matrix3 Qt = Rt * H_t_w; - const Matrix3 Qv = Rt * H_v_w; - - // Now compute the blocks of the LogmapDerivative Jacobian - const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Qt2 = -Jw * Qt * Jw; - const Matrix3 Qv2 = -Jw * Qv * Jw; - - Matrix9 J; - J << Jw, Z_3x3, Z_3x3, - Qt2, Jw, Z_3x3, - Qv2, Z_3x3, Jw; - return J; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const NavState& state) { - const Vector9 xi = Logmap(state); - return LogmapDerivative(xi); -} - -//------------------------------------------------------------------------------ -Matrix5 NavState::Hat(const Vector9& xi) { - Matrix5 X; - const double wx = xi(0), wy = xi(1), wz = xi(2); - const double px = xi(3), py = xi(4), pz = xi(5); - const double vx = xi(6), vy = xi(7), vz = xi(8); - X << 0., -wz, wy, px, vx, - wz, 0., -wx, py, vy, - -wy, wx, 0., pz, vz, - 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0.; - return X; -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Vee(const Matrix5& Xi) { - Vector9 xi; - xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), - Xi(0, 3), Xi(1, 3), Xi(2, 3), - Xi(0, 4), Xi(1, 4), Xi(2, 4); - return xi; -} - -//------------------------------------------------------------------------------ -NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, - ChartJacobian Hxi) { - return Expmap(xi, Hxi); -} - -//------------------------------------------------------------------------------ -Vector9 NavState::ChartAtOrigin::Local(const NavState& state, - ChartJacobian Hstate) { - return Logmap(state, Hstate); + return R_.equals(other.R_, tol) && + traits::Equals(x_.col(0), other.x_.col(0), tol) && + equal_with_abs_tol(x_.col(1), other.x_.col(1), tol); } //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { Rot3 nRb = R_; - Point3 n_t = t_, n_v = v_; + Point3 n_t = x_.col(0), n_v = x_.col(1); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); @@ -392,8 +226,8 @@ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); - const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + const Point3 dP = R_.unrotate(g.x_.col(0) - x_.col(0), H1 ? &D_dt_R : 0); + const Vector dV = R_.unrotate(g.x_.col(1) - x_.col(1), H1 ? &D_dv_R : 0); Vector9 xi; Matrix3 D_xi_R; @@ -479,7 +313,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { Rot3 nRb = R_; - Point3 n_t = t_, n_v = v_; + Point3 n_t = x_.col(0), n_v = x_.col(1); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); @@ -531,7 +365,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; - const Velocity3& n_v = v_; // derivative is Ri ! + const Velocity3 n_v = x_.col(1); // derivative is Ri ! const double dt22 = 0.5 * dt * dt; Vector9 xi; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 8b2a2d9141..5489e17d44 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -19,10 +19,15 @@ #pragma once #include +#include #include #include #include +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + namespace gtsam { /// Velocity is currently typedef'd to Vector3 @@ -36,46 +41,39 @@ using Velocity3 = Vector3; * NOTE: While Barrau20icra follow a R,v,t order, * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState : public MatrixLieGroup { - private: - - // TODO(frank): - // - should we rename t_ to p_? if not, we should rename dP do dT - Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav - Point3 t_; ///< position n_t, in nav frame - Velocity3 v_; ///< velocity n_v in nav frame - +class GTSAM_EXPORT NavState : public MatrixLieGroup, + public ExtendedPose3<2> { public: + using Base = ExtendedPose3<2>; + using LieGroupBase = MatrixLieGroup; using LieAlgebra = Matrix5; using Vector25 = Eigen::Matrix; + inline constexpr static auto dimension = 9; /// @name Constructors /// @{ /// Default constructor - NavState() : - t_(0, 0, 0), v_(Vector3::Zero()) { - } + NavState() : Base() {} + + NavState(const Base& other) : Base(other) {} /// Construct from attitude, position, velocity - NavState(const Rot3& R, const Point3& t, const Velocity3& v) : - R_(R), t_(t), v_(v) { - } + NavState(const Rot3& R, const Point3& t, const Velocity3& v) + : Base(R, (Eigen::Matrix() << t.x(), v.x(), t.y(), v.y(), + t.z(), v.z()) + .finished()) {} /// Construct from pose and velocity - NavState(const Pose3& pose, const Velocity3& v) : - R_(pose.rotation()), t_(pose.translation()), v_(v) { - } + NavState(const Pose3& pose, const Velocity3& v) + : NavState(pose.rotation(), pose.translation(), v) {} /// Construct from SO(3) and R^6 - NavState(const Matrix3& R, const Vector6& tv) : - R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { - } + NavState(const Matrix3& R, const Vector6& tv) + : NavState(Rot3(R), tv.head<3>(), tv.tail<3>()) {} /// Construct from Matrix5 - NavState(const Matrix5& T) : - R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) { - } + NavState(const Matrix5& T) : Base(T) {} /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, @@ -130,22 +128,15 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { } /// Return position as Vector3 Vector3 t() const { - return t_; + return x_.col(0); } /// Return velocity as Vector3. Computation-free. const Vector3& v() const { - return v_; + return velocity(); } // Return velocity in body frame Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; - /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] - Matrix5 matrix() const; - - /// Vectorize 5x5 matrix into a 25-dim vector. - Vector25 vec(OptionalJacobian<25, 9> H = {}) const; - /// @} /// @name Testable /// @{ @@ -164,20 +155,9 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { /// @name Group /// @{ - /// identity for group operation - static NavState Identity() { - return NavState(); - } - - /// inverse transformation with derivatives + static NavState Identity() { return NavState(Base::Identity()); } NavState inverse() const; - - using LieGroup::inverse; // version with derivative - - /// compose syntactic sugar - NavState operator*(const NavState& T) const { - return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); - } + NavState operator*(const NavState& other) const; /// Syntactic sugar const Rot3& rotation() const { return attitude(); }; @@ -217,68 +197,41 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { /// @name Lie Group /// @{ - /** - * Exponential map at identity - create a NavState from canonical coordinates - * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ - */ static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); - - /** - * Log map at identity - return the canonical coordinates \f$ - * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState - */ static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); - /** - * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) - * frame to the world spatial frame. - */ Matrix9 AdjointMap() const; - - /** - * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a - * body-fixed velocity, transforming it to the spatial frame - * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ - * Note that H_xib = AdjointMap() - */ - Vector9 Adjoint(const Vector9& xi_b, - OptionalJacobian<9, 9> H_this = {}, + Vector9 Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_this = {}, OptionalJacobian<9, 9> H_xib = {}) const; - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] - */ - static Matrix9 adjointMap(const Vector9& xi); - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ + static Matrix9 adjointMap(const Vector9& xi); static Vector9 adjoint(const Vector9& xi, const Vector9& y, OptionalJacobian<9, 9> Hxi = {}, OptionalJacobian<9, 9> H_y = {}); - /// Derivative of Expmap static Matrix9 ExpmapDerivative(const Vector9& xi); - - /// Derivative of Logmap static Matrix9 LogmapDerivative(const Vector9& xi); + static Matrix9 LogmapDerivative(const NavState& state); - /// Derivative of Logmap, NavState version - static Matrix9 LogmapDerivative(const NavState& xi); - - // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct GTSAM_EXPORT ChartAtOrigin { static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); }; - /// Hat maps from tangent vector to Lie algebra - static Matrix5 Hat(const Vector9& xi); + using LieGroupBase::between; + using LieGroupBase::compose; + using LieGroupBase::expmap; + using LieGroupBase::LocalCoordinates; + using LieGroupBase::logmap; + using LieGroupBase::Retract; + using LieGroup::inverse; - /// Vee maps from Lie algebra to tangent vector + static Matrix5 Hat(const Vector9& xi); static Vector9 Vee(const Matrix5& X); + Matrix5 matrix() const { return Base::matrix(); } + Vector25 vec(OptionalJacobian<25, 9> H = {}) const { return Base::vec(H); } + /// @} /// @name Dynamics /// @{ @@ -322,15 +275,16 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { /// @} private: + mutable Point3 p_cache_ = Point3::Zero(); + mutable Velocity3 v_cache_ = Velocity3::Zero(); + /// @{ /// serialization #if GTSAM_ENABLE_BOOST_SERIALIZATION /// friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - ar & BOOST_SERIALIZATION_NVP(v_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } #endif /// @} From 7b2e055f34e6550c1734ab570326ebee831d5696 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 10:38:07 -0500 Subject: [PATCH 14/41] Move definitions --- gtsam/geometry/ExtendedPose3-inl.h | 409 +++++++++++++++++++++++++++++ gtsam/geometry/ExtendedPose3.cpp | 27 ++ gtsam/geometry/ExtendedPose3.h | 378 +++++--------------------- 3 files changed, 499 insertions(+), 315 deletions(-) create mode 100644 gtsam/geometry/ExtendedPose3-inl.h create mode 100644 gtsam/geometry/ExtendedPose3.cpp diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h new file mode 100644 index 0000000000..6fb909c3a8 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -0,0 +1,409 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExtendedPose3-inl.h + * @brief Template implementations for ExtendedPose3 + * @author Frank Dellaert, et al. + */ + +#pragma once + +namespace gtsam { + +template +size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { + if constexpr (K == Eigen::Dynamic) { + if (xi.size() < 3 || (xi.size() - 3) % 3 != 0) { + throw std::invalid_argument( + "ExtendedPose3: tangent vector size must be 3 + 3*k."); + } + return static_cast((xi.size() - 3) / 3); + } else { + return static_cast(K); + } +} + +template +void ExtendedPose3::ZeroJacobian(ChartJacobian H, size_t d) { + if (!H) return; + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(d, d); + } else { + (void)d; + H->setZero(); + } +} + +template +template +ExtendedPose3::ExtendedPose3() : R_(Rot3::Identity()), x_(Matrix3K::Zero()) { +} + +template +template +ExtendedPose3::ExtendedPose3(size_t k) : R_(Rot3::Identity()), x_(3, k) { + x_.setZero(); +} + +template +ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) : R_(R), x_(x) {} + +template +ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { + if constexpr (K == Eigen::Dynamic) { + const auto n = T.rows(); + if (T.cols() != n || n < 3) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + const auto k = n - 3; + x_.resize(3, k); + x_.setZero(); + } + + const auto n = T.rows(); + if constexpr (K != Eigen::Dynamic) { + if (n != matrix_dim || T.cols() != matrix_dim) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + } else { + if (T.cols() != n || n < 3) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + } + + R_ = Rot3(T.template block<3, 3>(0, 0)); + x_ = T.block(0, 3, 3, n - 3); +} + +template +size_t ExtendedPose3::Dimension(size_t k) { + return 3 + 3 * k; +} + +template +size_t ExtendedPose3::k() const { + return static_cast(x_.cols()); +} + +template +template +size_t ExtendedPose3::dim() const { + return Dimension(k()); +} + +template +const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { + if (H) { + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(3, dim()); + } else { + H->setZero(); + } + H->block(0, 0, 3, 3) = I_3x3; + } + return R_; +} + +template +Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { + if (i >= k()) throw std::out_of_range("ExtendedPose3: x(i) out of range."); + if (H) { + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(3, dim()); + } else { + H->setZero(); + } + H->block(0, 3 + 3 * i, 3, 3) = R_.matrix(); + } + return x_.col(static_cast(i)); +} + +template +const typename ExtendedPose3::Matrix3K& ExtendedPose3::xMatrix() const { + return x_; +} + +template +typename ExtendedPose3::Matrix3K& ExtendedPose3::xMatrix() { + return x_; +} + +template +void ExtendedPose3::print(const std::string& s) const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; +} + +template +bool ExtendedPose3::equals(const ExtendedPose3& other, double tol) const { + return R_.equals(other.R_, tol) && equal_with_abs_tol(x_, other.x_, tol); +} + +template +template +ExtendedPose3 ExtendedPose3::Identity() { + return ExtendedPose3(); +} + +template +template +ExtendedPose3 ExtendedPose3::Identity(size_t k) { + return ExtendedPose3(k); +} + +template +ExtendedPose3 ExtendedPose3::inverse() const { + const Rot3 Rt = R_.inverse(); + const Matrix3K x = -(Rt.matrix() * x_); + return ExtendedPose3(Rt, x); +} + +template +ExtendedPose3 ExtendedPose3::operator*(const ExtendedPose3& other) const { + if (k() != other.k()) { + throw std::invalid_argument("ExtendedPose3: compose requires matching k."); + } + Matrix3K x = x_ + R_.matrix() * other.x_; + return ExtendedPose3(R_ * other.R_, x); +} + +template +ExtendedPose3 ExtendedPose3::Expmap(const TangentVector& xi, + ChartJacobian Hxi) { + const size_t k = RuntimeK(xi); + + const Vector3 w = xi.template head<3>(); + const so3::DexpFunctor local(w); + const Rot3 R(local.expmap()); + const Matrix3 Rt = R.transpose(); + + Matrix3K x; + if constexpr (K == Eigen::Dynamic) x.resize(3, static_cast(k)); + + if (Hxi) { + const size_t d = 3 + 3 * k; + ZeroJacobian(Hxi, d); + const Matrix3 Jr = local.Jacobian().right(); + Hxi->block(0, 0, 3, 3) = Jr; + for (size_t i = 0; i < k; ++i) { + Matrix3 H_xi_w; + const Vector3 rho = xi.template segment<3>(3 + 3 * i); + x.col(static_cast(i)) = + local.Jacobian().applyLeft(rho, &H_xi_w); + Hxi->block(3 + 3 * i, 0, 3, 3) = Rt * H_xi_w; + Hxi->block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jr; + } + } else { + for (size_t i = 0; i < k; ++i) { + const Vector3 rho = xi.template segment<3>(3 + 3 * i); + x.col(static_cast(i)) = local.Jacobian().applyLeft(rho); + } + } + + return ExtendedPose3(R, x); +} + +template +typename ExtendedPose3::TangentVector ExtendedPose3::Logmap( + const ExtendedPose3& pose, ChartJacobian Hpose) { + const Vector3 w = Rot3::Logmap(pose.R_); + const so3::DexpFunctor local(w); + + TangentVector xi; + if constexpr (K == Eigen::Dynamic) xi.resize(static_cast(pose.dim())); + xi.template head<3>() = w; + for (size_t i = 0; i < pose.k(); ++i) { + xi.template segment<3>(3 + 3 * i) = + local.InvJacobian().applyLeft(pose.x_.col(static_cast(i))); + } + + if (Hpose) *Hpose = LogmapDerivative(xi); + return xi; +} + +template +typename ExtendedPose3::Jacobian ExtendedPose3::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Jacobian adj; + if constexpr (dimension == Eigen::Dynamic) { + adj.setZero(dim(), dim()); + } else { + adj.setZero(); + } + + adj.block(0, 0, 3, 3) = R; + for (size_t i = 0; i < k(); ++i) { + adj.block(3 + 3 * i, 0, 3, 3) = + skewSymmetric(x_.col(static_cast(i))) * R; + adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = R; + } + return adj; +} + +template +typename ExtendedPose3::TangentVector ExtendedPose3::Adjoint( + const TangentVector& xi_b, ChartJacobian H_this, ChartJacobian H_xib) const { + const Jacobian Ad = AdjointMap(); + if (H_this) *H_this = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + return Ad * xi_b; +} + +template +typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap( + const TangentVector& xi) { + const size_t k = RuntimeK(xi); + const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + + Jacobian adj; + if constexpr (dimension == Eigen::Dynamic) { + adj.setZero(3 + 3 * k, 3 + 3 * k); + } else { + adj.setZero(); + } + + adj.block(0, 0, 3, 3) = w_hat; + for (size_t i = 0; i < k; ++i) { + adj.block(3 + 3 * i, 0, 3, 3) = skewSymmetric( + xi(3 + 3 * i + 0), xi(3 + 3 * i + 1), xi(3 + 3 * i + 2)); + adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = w_hat; + } + return adj; +} + +template +typename ExtendedPose3::TangentVector ExtendedPose3::adjoint( + const TangentVector& xi, const TangentVector& y, ChartJacobian Hxi, + ChartJacobian H_y) { + const Jacobian ad_xi = adjointMap(xi); + if (Hxi) { + if constexpr (dimension == Eigen::Dynamic) { + Hxi->setZero(xi.size(), xi.size()); + } else { + Hxi->setZero(); + } + for (Eigen::Index i = 0; i < xi.size(); ++i) { + TangentVector dxi; + if constexpr (dimension == Eigen::Dynamic) { + dxi = TangentVector::Zero(xi.size()); + } else { + dxi = TangentVector::Zero(); + } + dxi(i) = 1.0; + Hxi->col(i) = adjointMap(dxi) * y; + } + } + if (H_y) *H_y = ad_xi; + return ad_xi * y; +} + +template +typename ExtendedPose3::Jacobian ExtendedPose3::ExpmapDerivative( + const TangentVector& xi) { + Jacobian J; + Expmap(xi, J); + return J; +} + +template +typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative( + const TangentVector& xi) { + const size_t k = RuntimeK(xi); + const Vector3 w = xi.template head<3>(); + const so3::DexpFunctor local(w); + const Matrix3 Rt = local.expmap().transpose(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + + Jacobian J; + if constexpr (dimension == Eigen::Dynamic) { + J.setZero(3 + 3 * k, 3 + 3 * k); + } else { + J.setZero(); + } + + J.block(0, 0, 3, 3) = Jw; + for (size_t i = 0; i < k; ++i) { + Matrix3 H_xi_w; + local.Jacobian().applyLeft(xi.template segment<3>(3 + 3 * i), H_xi_w); + const Matrix3 Q = Rt * H_xi_w; + J.block(3 + 3 * i, 0, 3, 3) = -Jw * Q * Jw; + J.block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jw; + } + return J; +} + +template +typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative( + const ExtendedPose3& pose) { + return LogmapDerivative(Logmap(pose)); +} + +template +ExtendedPose3 ExtendedPose3::ChartAtOrigin::Retract( + const TangentVector& xi, ChartJacobian Hxi) { + return Expmap(xi, Hxi); +} + +template +typename ExtendedPose3::TangentVector ExtendedPose3::ChartAtOrigin::Local( + const ExtendedPose3& pose, ChartJacobian Hpose) { + return Logmap(pose, Hpose); +} + +template +typename ExtendedPose3::LieAlgebra ExtendedPose3::matrix() const { + LieAlgebra M; + if constexpr (matrix_dim == Eigen::Dynamic) { + const Eigen::Index n = 3 + static_cast(k()); + M = LieAlgebra::Identity(n, n); + } else { + M = LieAlgebra::Identity(); + } + M.template block<3, 3>(0, 0) = R_.matrix(); + M.block(0, 3, 3, static_cast(k())) = x_; + return M; +} + +template +typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( + const TangentVector& xi) { + const size_t k = RuntimeK(xi); + LieAlgebra X; + if constexpr (matrix_dim == Eigen::Dynamic) { + X.setZero(3 + k, 3 + k); + } else { + X.setZero(); + } + X.block(0, 0, 3, 3) = skewSymmetric(xi(0), xi(1), xi(2)); + for (size_t i = 0; i < k; ++i) { + X.block(0, 3 + i, 3, 1) = xi.template segment<3>(3 + 3 * i); + } + return X; +} + +template +typename ExtendedPose3::TangentVector ExtendedPose3::Vee( + const LieAlgebra& X) { + const Eigen::Index k = X.cols() - 3; + TangentVector xi; + if constexpr (dimension == Eigen::Dynamic) { + xi.resize(3 + 3 * k); + } + xi(0) = X(2, 1); + xi(1) = X(0, 2); + xi(2) = X(1, 0); + for (Eigen::Index i = 0; i < k; ++i) { + xi.template segment<3>(3 + 3 * i) = X.template block<3, 1>(0, 3 + i); + } + return xi; +} + +} // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp new file mode 100644 index 0000000000..88c59ca072 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -0,0 +1,27 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExtendedPose3.cpp + * @brief Explicit instantiations for ExtendedPose3 + * @author Frank Dellaert, et al. + */ + +#include + +namespace gtsam { + +template class ExtendedPose3<1>; +template class ExtendedPose3<2>; +template class ExtendedPose3<3>; +template class ExtendedPose3; + +} // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index f35cf54a7a..fb05a91bc0 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -36,15 +36,19 @@ namespace gtsam { namespace internal { -/// Manifold dimensionality for SE_k(3): 3 + 3*k. -constexpr int DimensionExtendedPose3(int k) { - return (k == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * k; -} +/// Compile-time traits for SE_k(3) dimensions. +template +struct ExtendedPose3Traits { + /// Manifold dimensionality for SE_k(3): 3 + 3*k. + static constexpr int Dimension() { + return (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K; + } -/// Homogeneous matrix size for SE_k(3): (3 + k) x (3 + k). -constexpr int MatrixDimExtendedPose3(int k) { - return (k == Eigen::Dynamic) ? Eigen::Dynamic : 3 + k; -} + /// Homogeneous matrix size for SE_k(3): (3 + k) x (3 + k). + static constexpr int MatrixDim() { + return (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K; + } +}; } // namespace internal @@ -56,11 +60,14 @@ constexpr int MatrixDimExtendedPose3(int k) { */ template class GTSAM_EXPORT ExtendedPose3 - : public MatrixLieGroup, internal::DimensionExtendedPose3(K), - internal::MatrixDimExtendedPose3(K)> { + : public MatrixLieGroup, + internal::ExtendedPose3Traits::Dimension(), + internal::ExtendedPose3Traits::MatrixDim()> { public: - inline constexpr static auto dimension = internal::DimensionExtendedPose3(K); - inline constexpr static auto matrix_dim = internal::MatrixDimExtendedPose3(K); + inline constexpr static auto dimension = + internal::ExtendedPose3Traits::Dimension(); + inline constexpr static auto matrix_dim = + internal::ExtendedPose3Traits::MatrixDim(); using Base = MatrixLieGroup, dimension, matrix_dim>; using TangentVector = typename Base::TangentVector; @@ -85,27 +92,8 @@ class GTSAM_EXPORT ExtendedPose3 template using IsFixed = typename std::enable_if= 1, void>::type; - static size_t RuntimeK(const TangentVector& xi) { - if constexpr (K == Eigen::Dynamic) { - if (xi.size() < 3 || (xi.size() - 3) % 3 != 0) { - throw std::invalid_argument( - "ExtendedPose3: tangent vector size must be 3 + 3*k."); - } - return static_cast((xi.size() - 3) / 3); - } else { - return static_cast(K); - } - } - - static void ZeroJacobian(ChartJacobian H, size_t d) { - if (!H) return; - if constexpr (dimension == Eigen::Dynamic) { - H->setZero(d, d); - } else { - (void)d; - H->setZero(); - } - } + static size_t RuntimeK(const TangentVector& xi); + static void ZeroJacobian(ChartJacobian H, size_t d); public: /// @name Constructors @@ -113,301 +101,96 @@ class GTSAM_EXPORT ExtendedPose3 /// Construct fixed-size identity. template > - ExtendedPose3() : R_(Rot3::Identity()), x_(Matrix3K::Zero()) {} + ExtendedPose3(); /// Construct dynamic identity, optionally with runtime k. template > - explicit ExtendedPose3(size_t k = 0) : R_(Rot3::Identity()), x_(3, k) { - x_.setZero(); - } + explicit ExtendedPose3(size_t k = 0); ExtendedPose3(const ExtendedPose3&) = default; ExtendedPose3& operator=(const ExtendedPose3&) = default; /// Construct from rotation and 3xK block. - ExtendedPose3(const Rot3& R, const Matrix3K& x) : R_(R), x_(x) {} + ExtendedPose3(const Rot3& R, const Matrix3K& x); /// Construct from homogeneous matrix representation. - explicit ExtendedPose3(const LieAlgebra& T) { - if constexpr (K == Eigen::Dynamic) { - const auto n = T.rows(); - if (T.cols() != n || n < 3) { - throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); - } - const auto k = n - 3; - x_.resize(3, k); - x_.setZero(); - } - - const auto n = T.rows(); - if constexpr (K != Eigen::Dynamic) { - if (n != matrix_dim || T.cols() != matrix_dim) { - throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); - } - } else { - if (T.cols() != n || n < 3) { - throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); - } - } - - R_ = Rot3(T.template block<3, 3>(0, 0)); - x_ = T.block(0, 3, 3, n - 3); - } + explicit ExtendedPose3(const LieAlgebra& T); /// @} /// @name Access /// @{ - static size_t Dimension(size_t k) { return 3 + 3 * k; } + static size_t Dimension(size_t k); /// Number of R^3 columns. - size_t k() const { return static_cast(x_.cols()); } + size_t k() const; /// Runtime manifold dimension. template > - size_t dim() const { - return Dimension(k()); - } + size_t dim() const; /// Rotation component. - const Rot3& rotation(ComponentJacobian H = {}) const { - if (H) { - if constexpr (dimension == Eigen::Dynamic) { - H->setZero(3, dim()); - } else { - H->setZero(); - } - H->block(0, 0, 3, 3) = I_3x3; - } - return R_; - } + const Rot3& rotation(ComponentJacobian H = {}) const; /// i-th R^3 component, returned by value. - Point3 x(size_t i, ComponentJacobian H = {}) const { - if (i >= k()) throw std::out_of_range("ExtendedPose3: x(i) out of range."); - if (H) { - if constexpr (dimension == Eigen::Dynamic) { - H->setZero(3, dim()); - } else { - H->setZero(); - } - H->block(0, 3 + 3 * i, 3, 3) = R_.matrix(); - } - return x_.col(static_cast(i)); - } + Point3 x(size_t i, ComponentJacobian H = {}) const; - const Matrix3K& xMatrix() const { return x_; } - Matrix3K& xMatrix() { return x_; } + const Matrix3K& xMatrix() const; + Matrix3K& xMatrix(); /// @} /// @name Testable /// @{ - void print(const std::string& s = "") const { - std::cout << (s.empty() ? s : s + " ") << *this << std::endl; - } + void print(const std::string& s = "") const; - bool equals(const ExtendedPose3& other, double tol = 1e-9) const { - return R_.equals(other.R_, tol) && equal_with_abs_tol(x_, other.x_, tol); - } + bool equals(const ExtendedPose3& other, double tol = 1e-9) const; /// @} /// @name Group /// @{ template > - static ExtendedPose3 Identity() { - return ExtendedPose3(); - } + static ExtendedPose3 Identity(); template > - static ExtendedPose3 Identity(size_t k = 0) { - return ExtendedPose3(k); - } + static ExtendedPose3 Identity(size_t k = 0); - ExtendedPose3 inverse() const { - const Rot3 Rt = R_.inverse(); - const Matrix3K x = -(Rt.matrix() * x_); - return ExtendedPose3(Rt, x); - } + ExtendedPose3 inverse() const; - ExtendedPose3 operator*(const ExtendedPose3& other) const { - if (k() != other.k()) { - throw std::invalid_argument( - "ExtendedPose3: compose requires matching k."); - } - Matrix3K x = x_ + R_.matrix() * other.x_; - return ExtendedPose3(R_ * other.R_, x); - } + ExtendedPose3 operator*(const ExtendedPose3& other) const; /// @} /// @name Lie Group /// @{ - static ExtendedPose3 Expmap(const TangentVector& xi, ChartJacobian Hxi = {}) { - const size_t k = RuntimeK(xi); - - const Vector3 w = xi.template head<3>(); - const so3::DexpFunctor local(w); - const Rot3 R(local.expmap()); - const Matrix3 Rt = R.transpose(); - - Matrix3K x; - if constexpr (K == Eigen::Dynamic) x.resize(3, static_cast(k)); - - if (Hxi) { - const size_t d = 3 + 3 * k; - ZeroJacobian(Hxi, d); - const Matrix3 Jr = local.Jacobian().right(); - Hxi->block(0, 0, 3, 3) = Jr; - for (size_t i = 0; i < k; ++i) { - Matrix3 H_xi_w; - const Vector3 rho = xi.template segment<3>(3 + 3 * i); - x.col(static_cast(i)) = - local.Jacobian().applyLeft(rho, &H_xi_w); - Hxi->block(3 + 3 * i, 0, 3, 3) = Rt * H_xi_w; - Hxi->block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jr; - } - } else { - for (size_t i = 0; i < k; ++i) { - const Vector3 rho = xi.template segment<3>(3 + 3 * i); - x.col(static_cast(i)) = local.Jacobian().applyLeft(rho); - } - } - - return ExtendedPose3(R, x); - } + static ExtendedPose3 Expmap(const TangentVector& xi, ChartJacobian Hxi = {}); - static TangentVector Logmap(const ExtendedPose3& pose, ChartJacobian Hpose = {}) { - const Vector3 w = Rot3::Logmap(pose.R_); - const so3::DexpFunctor local(w); + static TangentVector Logmap(const ExtendedPose3& pose, + ChartJacobian Hpose = {}); - TangentVector xi; - if constexpr (K == Eigen::Dynamic) xi.resize(static_cast(pose.dim())); - xi.template head<3>() = w; - for (size_t i = 0; i < pose.k(); ++i) { - xi.template segment<3>(3 + 3 * i) = - local.InvJacobian().applyLeft(pose.x_.col(static_cast(i))); - } + Jacobian AdjointMap() const; - if (Hpose) *Hpose = LogmapDerivative(xi); - return xi; - } + TangentVector Adjoint(const TangentVector& xi_b, ChartJacobian H_this = {}, + ChartJacobian H_xib = {}) const; - Jacobian AdjointMap() const { - const Matrix3 R = R_.matrix(); - Jacobian adj; - if constexpr (dimension == Eigen::Dynamic) { - adj.setZero(dim(), dim()); - } else { - adj.setZero(); - } - - adj.block(0, 0, 3, 3) = R; - for (size_t i = 0; i < k(); ++i) { - adj.block(3 + 3 * i, 0, 3, 3) = - skewSymmetric(x_.col(static_cast(i))) * R; - adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = R; - } - return adj; - } - - TangentVector Adjoint(const TangentVector& xi_b, - ChartJacobian H_this = {}, - ChartJacobian H_xib = {}) const { - const Jacobian Ad = AdjointMap(); - if (H_this) *H_this = -Ad * adjointMap(xi_b); - if (H_xib) *H_xib = Ad; - return Ad * xi_b; - } - - static Jacobian adjointMap(const TangentVector& xi) { - const size_t k = RuntimeK(xi); - const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - - Jacobian adj; - if constexpr (dimension == Eigen::Dynamic) { - adj.setZero(3 + 3 * k, 3 + 3 * k); - } else { - adj.setZero(); - } - - adj.block(0, 0, 3, 3) = w_hat; - for (size_t i = 0; i < k; ++i) { - adj.block(3 + 3 * i, 0, 3, 3) = - skewSymmetric(xi(3 + 3 * i + 0), xi(3 + 3 * i + 1), xi(3 + 3 * i + 2)); - adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = w_hat; - } - return adj; - } + static Jacobian adjointMap(const TangentVector& xi); static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, - ChartJacobian Hxi = {}, ChartJacobian H_y = {}) { - const Jacobian ad_xi = adjointMap(xi); - if (Hxi) { - if constexpr (dimension == Eigen::Dynamic) { - Hxi->setZero(xi.size(), xi.size()); - } else { - Hxi->setZero(); - } - for (Eigen::Index i = 0; i < xi.size(); ++i) { - TangentVector dxi; - if constexpr (dimension == Eigen::Dynamic) { - dxi = TangentVector::Zero(xi.size()); - } else { - dxi = TangentVector::Zero(); - } - dxi(i) = 1.0; - Hxi->col(i) = adjointMap(dxi) * y; - } - } - if (H_y) *H_y = ad_xi; - return ad_xi * y; - } + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}); - static Jacobian ExpmapDerivative(const TangentVector& xi) { - Jacobian J; - Expmap(xi, J); - return J; - } + static Jacobian ExpmapDerivative(const TangentVector& xi); - static Jacobian LogmapDerivative(const TangentVector& xi) { - const size_t k = RuntimeK(xi); - const Vector3 w = xi.template head<3>(); - const so3::DexpFunctor local(w); - const Matrix3 Rt = local.expmap().transpose(); - const Matrix3 Jw = Rot3::LogmapDerivative(w); - - Jacobian J; - if constexpr (dimension == Eigen::Dynamic) { - J.setZero(3 + 3 * k, 3 + 3 * k); - } else { - J.setZero(); - } - - J.block(0, 0, 3, 3) = Jw; - for (size_t i = 0; i < k; ++i) { - Matrix3 H_xi_w; - local.Jacobian().applyLeft(xi.template segment<3>(3 + 3 * i), H_xi_w); - const Matrix3 Q = Rt * H_xi_w; - J.block(3 + 3 * i, 0, 3, 3) = -Jw * Q * Jw; - J.block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jw; - } - return J; - } + static Jacobian LogmapDerivative(const TangentVector& xi); - static Jacobian LogmapDerivative(const ExtendedPose3& pose) { - return LogmapDerivative(Logmap(pose)); - } + static Jacobian LogmapDerivative(const ExtendedPose3& pose); struct ChartAtOrigin { - static ExtendedPose3 Retract(const TangentVector& xi, ChartJacobian Hxi = {}) { - return Expmap(xi, Hxi); - } - - static TangentVector Local(const ExtendedPose3& pose, ChartJacobian Hpose = {}) { - return Logmap(pose, Hpose); - } + static ExtendedPose3 Retract(const TangentVector& xi, + ChartJacobian Hxi = {}); + static TangentVector Local(const ExtendedPose3& pose, + ChartJacobian Hpose = {}); }; using LieGroup, dimension>::inverse; @@ -416,48 +199,11 @@ class GTSAM_EXPORT ExtendedPose3 /// @name Matrix Lie Group /// @{ - LieAlgebra matrix() const { - LieAlgebra M; - if constexpr (matrix_dim == Eigen::Dynamic) { - const Eigen::Index n = 3 + static_cast(k()); - M = LieAlgebra::Identity(n, n); - } else { - M = LieAlgebra::Identity(); - } - M.template block<3, 3>(0, 0) = R_.matrix(); - M.block(0, 3, 3, static_cast(k())) = x_; - return M; - } + LieAlgebra matrix() const; - static LieAlgebra Hat(const TangentVector& xi) { - const size_t k = RuntimeK(xi); - LieAlgebra X; - if constexpr (matrix_dim == Eigen::Dynamic) { - X.setZero(3 + k, 3 + k); - } else { - X.setZero(); - } - X.block(0, 0, 3, 3) = skewSymmetric(xi(0), xi(1), xi(2)); - for (size_t i = 0; i < k; ++i) { - X.block(0, 3 + i, 3, 1) = xi.template segment<3>(3 + 3 * i); - } - return X; - } + static LieAlgebra Hat(const TangentVector& xi); - static TangentVector Vee(const LieAlgebra& X) { - const Eigen::Index k = X.cols() - 3; - TangentVector xi; - if constexpr (dimension == Eigen::Dynamic) { - xi.resize(3 + 3 * k); - } - xi(0) = X(2, 1); - xi(1) = X(0, 2); - xi(2) = X(1, 0); - for (Eigen::Index i = 0; i < k; ++i) { - xi.template segment<3>(3 + 3 * i) = X.template block<3, 1>(0, 3 + i); - } - return xi; - } + static TangentVector Vee(const LieAlgebra& X); /// @} @@ -483,12 +229,14 @@ using ExtendedPose3Dynamic = ExtendedPose3; template struct traits> - : public internal::MatrixLieGroup, - internal::MatrixDimExtendedPose3(K)> {}; + : public internal::MatrixLieGroup< + ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; template struct traits> - : public internal::MatrixLieGroup, - internal::MatrixDimExtendedPose3(K)> {}; + : public internal::MatrixLieGroup< + ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; } // namespace gtsam + +#include "ExtendedPose3-inl.h" From 7c3f7ef41e59a9716b42f6b9019e4d333d20c537 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 11:02:34 -0500 Subject: [PATCH 15/41] CRTP --- gtsam/geometry/ExtendedPose3-inl.h | 191 +++++++++++++----------- gtsam/geometry/ExtendedPose3.h | 68 ++++++--- gtsam/geometry/Pose3.cpp | 66 -------- gtsam/geometry/Pose3.h | 57 +------ gtsam/geometry/tests/testPose3.cpp | 15 -- gtsam/navigation/NavState.cpp | 69 --------- gtsam/navigation/NavState.h | 45 +----- gtsam/navigation/tests/testNavState.cpp | 15 -- 8 files changed, 157 insertions(+), 369 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 6fb909c3a8..e45e8a05d3 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -11,7 +11,7 @@ /** * @file ExtendedPose3-inl.h - * @brief Template implementations for ExtendedPose3 + * @brief Template implementations for ExtendedPose3 * @author Frank Dellaert, et al. */ @@ -19,8 +19,8 @@ namespace gtsam { -template -size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { +template +size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { if constexpr (K == Eigen::Dynamic) { if (xi.size() < 3 || (xi.size() - 3) % 3 != 0) { throw std::invalid_argument( @@ -32,8 +32,8 @@ size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { } } -template -void ExtendedPose3::ZeroJacobian(ChartJacobian H, size_t d) { +template +void ExtendedPose3::ZeroJacobian(ChartJacobian H, size_t d) { if (!H) return; if constexpr (dimension == Eigen::Dynamic) { H->setZero(d, d); @@ -43,22 +43,24 @@ void ExtendedPose3::ZeroJacobian(ChartJacobian H, size_t d) { } } -template +template template -ExtendedPose3::ExtendedPose3() : R_(Rot3::Identity()), x_(Matrix3K::Zero()) { -} +ExtendedPose3::ExtendedPose3() + : R_(Rot3::Identity()), x_(Matrix3K::Zero()) {} -template +template template -ExtendedPose3::ExtendedPose3(size_t k) : R_(Rot3::Identity()), x_(3, k) { +ExtendedPose3::ExtendedPose3(size_t k) + : R_(Rot3::Identity()), x_(3, k) { x_.setZero(); } -template -ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) : R_(R), x_(x) {} +template +ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) + : R_(R), x_(x) {} -template -ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { +template +ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { if constexpr (K == Eigen::Dynamic) { const auto n = T.rows(); if (T.cols() != n || n < 3) { @@ -84,24 +86,24 @@ ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { x_ = T.block(0, 3, 3, n - 3); } -template -size_t ExtendedPose3::Dimension(size_t k) { +template +size_t ExtendedPose3::Dimension(size_t k) { return 3 + 3 * k; } -template -size_t ExtendedPose3::k() const { +template +size_t ExtendedPose3::k() const { return static_cast(x_.cols()); } -template +template template -size_t ExtendedPose3::dim() const { +size_t ExtendedPose3::dim() const { return Dimension(k()); } -template -const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { +template +const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { if (H) { if constexpr (dimension == Eigen::Dynamic) { H->setZero(3, dim()); @@ -113,8 +115,8 @@ const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { return R_; } -template -Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { +template +Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { if (i >= k()) throw std::out_of_range("ExtendedPose3: x(i) out of range."); if (H) { if constexpr (dimension == Eigen::Dynamic) { @@ -127,57 +129,64 @@ Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { return x_.col(static_cast(i)); } -template -const typename ExtendedPose3::Matrix3K& ExtendedPose3::xMatrix() const { +template +const typename ExtendedPose3::Matrix3K& +ExtendedPose3::xMatrix() const { return x_; } -template -typename ExtendedPose3::Matrix3K& ExtendedPose3::xMatrix() { +template +typename ExtendedPose3::Matrix3K& +ExtendedPose3::xMatrix() { return x_; } -template -void ExtendedPose3::print(const std::string& s) const { +template +void ExtendedPose3::print(const std::string& s) const { std::cout << (s.empty() ? s : s + " ") << *this << std::endl; } -template -bool ExtendedPose3::equals(const ExtendedPose3& other, double tol) const { +template +bool ExtendedPose3::equals(const ExtendedPose3& other, + double tol) const { return R_.equals(other.R_, tol) && equal_with_abs_tol(x_, other.x_, tol); } -template +template template -ExtendedPose3 ExtendedPose3::Identity() { - return ExtendedPose3(); +typename ExtendedPose3::This ExtendedPose3::Identity() { + return MakeReturn(ExtendedPose3()); } -template +template template -ExtendedPose3 ExtendedPose3::Identity(size_t k) { - return ExtendedPose3(k); +typename ExtendedPose3::This +ExtendedPose3::Identity(size_t k) { + return MakeReturn(ExtendedPose3(k)); } -template -ExtendedPose3 ExtendedPose3::inverse() const { +template +typename ExtendedPose3::This +ExtendedPose3::inverse() const { const Rot3 Rt = R_.inverse(); const Matrix3K x = -(Rt.matrix() * x_); - return ExtendedPose3(Rt, x); + return MakeReturn(ExtendedPose3(Rt, x)); } -template -ExtendedPose3 ExtendedPose3::operator*(const ExtendedPose3& other) const { - if (k() != other.k()) { +template +typename ExtendedPose3::This +ExtendedPose3::operator*(const This& other) const { + const ExtendedPose3& otherBase = AsBase(other); + if (k() != otherBase.k()) { throw std::invalid_argument("ExtendedPose3: compose requires matching k."); } - Matrix3K x = x_ + R_.matrix() * other.x_; - return ExtendedPose3(R_ * other.R_, x); + Matrix3K x = x_ + R_.matrix() * otherBase.x_; + return MakeReturn(ExtendedPose3(R_ * otherBase.R_, x)); } -template -ExtendedPose3 ExtendedPose3::Expmap(const TangentVector& xi, - ChartJacobian Hxi) { +template +typename ExtendedPose3::This ExtendedPose3::Expmap( + const TangentVector& xi, ChartJacobian Hxi) { const size_t k = RuntimeK(xi); const Vector3 w = xi.template head<3>(); @@ -208,29 +217,32 @@ ExtendedPose3 ExtendedPose3::Expmap(const TangentVector& xi, } } - return ExtendedPose3(R, x); + return MakeReturn(ExtendedPose3(R, x)); } -template -typename ExtendedPose3::TangentVector ExtendedPose3::Logmap( - const ExtendedPose3& pose, ChartJacobian Hpose) { - const Vector3 w = Rot3::Logmap(pose.R_); +template +typename ExtendedPose3::TangentVector ExtendedPose3::Logmap( + const This& pose, ChartJacobian Hpose) { + const ExtendedPose3& poseBase = AsBase(pose); + const Vector3 w = Rot3::Logmap(poseBase.R_); const so3::DexpFunctor local(w); TangentVector xi; - if constexpr (K == Eigen::Dynamic) xi.resize(static_cast(pose.dim())); + if constexpr (K == Eigen::Dynamic) + xi.resize(static_cast(poseBase.dim())); xi.template head<3>() = w; - for (size_t i = 0; i < pose.k(); ++i) { + for (size_t i = 0; i < poseBase.k(); ++i) { xi.template segment<3>(3 + 3 * i) = - local.InvJacobian().applyLeft(pose.x_.col(static_cast(i))); + local.InvJacobian().applyLeft(poseBase.x_.col(static_cast(i))); } if (Hpose) *Hpose = LogmapDerivative(xi); return xi; } -template -typename ExtendedPose3::Jacobian ExtendedPose3::AdjointMap() const { +template +typename ExtendedPose3::Jacobian +ExtendedPose3::AdjointMap() const { const Matrix3 R = R_.matrix(); Jacobian adj; if constexpr (dimension == Eigen::Dynamic) { @@ -248,8 +260,8 @@ typename ExtendedPose3::Jacobian ExtendedPose3::AdjointMap() const { return adj; } -template -typename ExtendedPose3::TangentVector ExtendedPose3::Adjoint( +template +typename ExtendedPose3::TangentVector ExtendedPose3::Adjoint( const TangentVector& xi_b, ChartJacobian H_this, ChartJacobian H_xib) const { const Jacobian Ad = AdjointMap(); if (H_this) *H_this = -Ad * adjointMap(xi_b); @@ -257,8 +269,8 @@ typename ExtendedPose3::TangentVector ExtendedPose3::Adjoint( return Ad * xi_b; } -template -typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap( +template +typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap( const TangentVector& xi) { const size_t k = RuntimeK(xi); const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); @@ -279,8 +291,8 @@ typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap( return adj; } -template -typename ExtendedPose3::TangentVector ExtendedPose3::adjoint( +template +typename ExtendedPose3::TangentVector ExtendedPose3::adjoint( const TangentVector& xi, const TangentVector& y, ChartJacobian Hxi, ChartJacobian H_y) { const Jacobian ad_xi = adjointMap(xi); @@ -305,17 +317,17 @@ typename ExtendedPose3::TangentVector ExtendedPose3::adjoint( return ad_xi * y; } -template -typename ExtendedPose3::Jacobian ExtendedPose3::ExpmapDerivative( - const TangentVector& xi) { +template +typename ExtendedPose3::Jacobian +ExtendedPose3::ExpmapDerivative(const TangentVector& xi) { Jacobian J; Expmap(xi, J); return J; } -template -typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative( - const TangentVector& xi) { +template +typename ExtendedPose3::Jacobian +ExtendedPose3::LogmapDerivative(const TangentVector& xi) { const size_t k = RuntimeK(xi); const Vector3 w = xi.template head<3>(); const so3::DexpFunctor local(w); @@ -340,26 +352,29 @@ typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative( return J; } -template -typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative( - const ExtendedPose3& pose) { +template +typename ExtendedPose3::Jacobian +ExtendedPose3::LogmapDerivative(const This& pose) { return LogmapDerivative(Logmap(pose)); } -template -ExtendedPose3 ExtendedPose3::ChartAtOrigin::Retract( - const TangentVector& xi, ChartJacobian Hxi) { - return Expmap(xi, Hxi); +template +typename ExtendedPose3::This +ExtendedPose3::ChartAtOrigin::Retract(const TangentVector& xi, + ChartJacobian Hxi) { + return ExtendedPose3::Expmap(xi, Hxi); } -template -typename ExtendedPose3::TangentVector ExtendedPose3::ChartAtOrigin::Local( - const ExtendedPose3& pose, ChartJacobian Hpose) { - return Logmap(pose, Hpose); +template +typename ExtendedPose3::TangentVector +ExtendedPose3::ChartAtOrigin::Local(const This& pose, + ChartJacobian Hpose) { + return ExtendedPose3::Logmap(pose, Hpose); } -template -typename ExtendedPose3::LieAlgebra ExtendedPose3::matrix() const { +template +typename ExtendedPose3::LieAlgebra +ExtendedPose3::matrix() const { LieAlgebra M; if constexpr (matrix_dim == Eigen::Dynamic) { const Eigen::Index n = 3 + static_cast(k()); @@ -372,8 +387,8 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::matrix() const { return M; } -template -typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( +template +typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( const TangentVector& xi) { const size_t k = RuntimeK(xi); LieAlgebra X; @@ -389,8 +404,8 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( return X; } -template -typename ExtendedPose3::TangentVector ExtendedPose3::Vee( +template +typename ExtendedPose3::TangentVector ExtendedPose3::Vee( const LieAlgebra& X) { const Eigen::Index k = X.cols() - 3; TangentVector xi; diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index fb05a91bc0..7990700c03 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -34,6 +34,9 @@ namespace gtsam { +template +class ExtendedPose3; + namespace internal { /// Compile-time traits for SE_k(3) dimensions. @@ -50,6 +53,16 @@ struct ExtendedPose3Traits { } }; +template +struct ExtendedPose3Class { + using type = Derived; +}; + +template +struct ExtendedPose3Class { + using type = ExtendedPose3; +}; + } // namespace internal /** @@ -58,18 +71,19 @@ struct ExtendedPose3Traits { * * Template parameter K can be fixed (K >= 1) or Eigen::Dynamic. */ -template +template class GTSAM_EXPORT ExtendedPose3 - : public MatrixLieGroup, + : public MatrixLieGroup::type, internal::ExtendedPose3Traits::Dimension(), internal::ExtendedPose3Traits::MatrixDim()> { public: + using This = typename internal::ExtendedPose3Class::type; inline constexpr static auto dimension = internal::ExtendedPose3Traits::Dimension(); inline constexpr static auto matrix_dim = internal::ExtendedPose3Traits::MatrixDim(); - using Base = MatrixLieGroup, dimension, matrix_dim>; + using Base = MatrixLieGroup; using TangentVector = typename Base::TangentVector; using Jacobian = typename Base::Jacobian; using ChartJacobian = typename Base::ChartJacobian; @@ -92,6 +106,22 @@ class GTSAM_EXPORT ExtendedPose3 template using IsFixed = typename std::enable_if= 1, void>::type; + static This MakeReturn(const ExtendedPose3& value) { + if constexpr (std::is_void_v) { + return value; + } else { + return This(value); + } + } + + static const ExtendedPose3& AsBase(const This& value) { + if constexpr (std::is_void_v) { + return value; + } else { + return static_cast(value); + } + } + static size_t RuntimeK(const TangentVector& xi); static void ZeroJacobian(ChartJacobian H, size_t d); @@ -151,22 +181,22 @@ class GTSAM_EXPORT ExtendedPose3 /// @{ template > - static ExtendedPose3 Identity(); + static This Identity(); template > - static ExtendedPose3 Identity(size_t k = 0); + static This Identity(size_t k = 0); - ExtendedPose3 inverse() const; + This inverse() const; - ExtendedPose3 operator*(const ExtendedPose3& other) const; + This operator*(const This& other) const; /// @} /// @name Lie Group /// @{ - static ExtendedPose3 Expmap(const TangentVector& xi, ChartJacobian Hxi = {}); + static This Expmap(const TangentVector& xi, ChartJacobian Hxi = {}); - static TangentVector Logmap(const ExtendedPose3& pose, + static TangentVector Logmap(const This& pose, ChartJacobian Hpose = {}); Jacobian AdjointMap() const; @@ -184,16 +214,16 @@ class GTSAM_EXPORT ExtendedPose3 static Jacobian LogmapDerivative(const TangentVector& xi); - static Jacobian LogmapDerivative(const ExtendedPose3& pose); + static Jacobian LogmapDerivative(const This& pose); struct ChartAtOrigin { - static ExtendedPose3 Retract(const TangentVector& xi, + static This Retract(const TangentVector& xi, ChartJacobian Hxi = {}); - static TangentVector Local(const ExtendedPose3& pose, + static TangentVector Local(const This& pose, ChartJacobian Hpose = {}); }; - using LieGroup, dimension>::inverse; + using LieGroup::inverse; /// @} /// @name Matrix Lie Group @@ -227,15 +257,15 @@ class GTSAM_EXPORT ExtendedPose3 /// Convenience typedef for dynamic k. using ExtendedPose3Dynamic = ExtendedPose3; -template -struct traits> +template +struct traits> : public internal::MatrixLieGroup< - ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; + ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; -template -struct traits> +template +struct traits> : public internal::MatrixLieGroup< - ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; + ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 58681da5a5..7eea6a2cee 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -57,58 +57,6 @@ Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { return Pose3(p); } -/* ************************************************************************* */ -Pose3 Pose3::inverse() const { return Pose3(Base::inverse()); } - -/* ************************************************************************* */ -Pose3 Pose3::operator*(const Pose3& T) const { - return Pose3(Base::operator*(T)); -} - -/* ************************************************************************* */ -Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { - return Pose3(Base::Expmap(xi, Hxi)); -} - -/* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { - return Base::Logmap(pose, Hpose); -} - -/* ************************************************************************* */ -Matrix6 Pose3::AdjointMap() const { return Base::AdjointMap(); } - -/* ************************************************************************* */ -Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, - OptionalJacobian<6, 6> H_xib) const { - return Base::Adjoint(xi_b, H_this, H_xib); -} - -/* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector6& xi) { return Base::adjointMap(xi); } - -/* ************************************************************************* */ -Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, - OptionalJacobian<6, 6> H_y) { - return Base::adjoint(xi, y, Hxi, H_y); -} - -/* ************************************************************************* */ -Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - return Base::ExpmapDerivative(xi); -} - -/* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { - return Base::LogmapDerivative(xi); -} - -/* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { - return Base::LogmapDerivative(pose); -} - /* ************************************************************************* */ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { #ifdef GTSAM_POSE3_EXPMAP @@ -141,20 +89,6 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } -/* ************************************************************************* */ -Matrix4 Pose3::Hat(const Vector6& xi) { return Base::Hat(xi); } - -/* ************************************************************************* */ -Vector6 Pose3::Vee(const Matrix4& X) { return Base::Vee(X); } - -/* ************************************************************************* */ -Matrix4 Pose3::matrix() const { return Base::matrix(); } - -/* ************************************************************************* */ -Pose3::Vector16 Pose3::vec(OptionalJacobian<16, 6> H) const { - return Base::vec(H); -} - /* ************************************************************************* */ /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 85eae9f8c2..00353deec9 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -39,10 +39,9 @@ class Pose2; * @ingroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3: public MatrixLieGroup, public ExtendedPose3<1> { +class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { public: - using Base = ExtendedPose3<1>; - using LieGroupBase = MatrixLieGroup; + using Base = ExtendedPose3<1, Pose3>; /** Pose Concept requirements */ typedef Rot3 Rotation; @@ -51,6 +50,7 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup, public ExtendedPos public: using Vector16 = Eigen::Matrix; + using Base::operator*; /// @name Standard Constructors /// @{ @@ -106,15 +106,6 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup, public ExtendedPos /// @name Group /// @{ - static Pose3 Identity() { - return Pose3(Base::Identity()); - } - - Pose3 inverse() const; - - /// compose syntactic sugar - Pose3 operator*(const Pose3& T) const; - /** * Interpolate between two poses via individual rotation and translation * interpolation. @@ -140,22 +131,6 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup, public ExtendedPos using LieAlgebra = Matrix4; - static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); - - static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); - - Matrix6 AdjointMap() const; - - Vector6 Adjoint(const Vector6& xi_b, - OptionalJacobian<6, 6> H_this = {}, - OptionalJacobian<6, 6> H_xib = {}) const; - - static Matrix6 adjointMap(const Vector6& xi); - - static Vector6 adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = {}, - OptionalJacobian<6, 6> H_y = {}); - /// The dual version of Adjoint Vector6 AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this = {}, @@ -172,37 +147,11 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup, public ExtendedPos OptionalJacobian<6, 6> Hxi = {}, OptionalJacobian<6, 6> H_y = {}); - static Matrix6 ExpmapDerivative(const Vector6& xi); - - static Matrix6 LogmapDerivative(const Vector6& xi); - - static Matrix6 LogmapDerivative(const Pose3& pose); - struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; - using LieGroupBase::between; - using LieGroupBase::compose; - using LieGroupBase::expmap; - using LieGroupBase::LocalCoordinates; - using LieGroupBase::logmap; - using LieGroupBase::Retract; - using LieGroup::inverse; - using LieGroupBase::localCoordinates; - using LieGroupBase::retract; - - static Matrix4 Hat(const Vector6& xi); - - static Vector6 Vee(const Matrix4& X); - - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /// Return vectorized SE(3) matrix in column order. - Vector16 vec(OptionalJacobian<16, 6> H = {}) const; - /// @} /// @name Group Action on Point3 /// @{ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 3797e77619..fc3a82f4dc 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1469,21 +1469,6 @@ TEST(Pose3, Vec) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } -/* ************************************************************************* */ -TEST(Pose3, AdjointMap) { - // Create a non-trivial Pose3 object - const Pose3 pose(Rot3::Rodrigues(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); - - // Call the specialized AdjointMap - Matrix6 specialized_Adj = pose.AdjointMap(); - - // Call the generic AdjointMap from the base class - Matrix6 generic_Adj = static_cast*>(&pose)->AdjointMap(); - - // Assert that they are equal - EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 58cc9c35d2..caffd492dc 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -23,75 +23,6 @@ namespace gtsam { -//------------------------------------------------------------------------------ -NavState NavState::inverse() const { return NavState(Base::inverse()); } - -//------------------------------------------------------------------------------ -NavState NavState::operator*(const NavState& other) const { - return NavState(Base::operator*(other)); -} - -//------------------------------------------------------------------------------ -NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - return NavState(Base::Expmap(xi, Hxi)); -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose) { - return Base::Logmap(pose, Hpose); -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::AdjointMap() const { return Base::AdjointMap(); } - -//------------------------------------------------------------------------------ -Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_this, - OptionalJacobian<9, 9> H_xib) const { - return Base::Adjoint(xi_b, H_this, H_xib); -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::adjointMap(const Vector9& xi) { return Base::adjointMap(xi); } - -//------------------------------------------------------------------------------ -Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi, - OptionalJacobian<9, 9> H_y) { - return Base::adjoint(xi, y, Hxi, H_y); -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { - return Base::ExpmapDerivative(xi); -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const Vector9& xi) { - return Base::LogmapDerivative(xi); -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const NavState& state) { - return Base::LogmapDerivative(state); -} - -//------------------------------------------------------------------------------ -NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { - return NavState(Base::ChartAtOrigin::Retract(xi, Hxi)); -} - -//------------------------------------------------------------------------------ -Vector9 NavState::ChartAtOrigin::Local(const NavState& state, - ChartJacobian Hstate) { - return Base::ChartAtOrigin::Local(state, Hstate); -} - -//------------------------------------------------------------------------------ -Matrix5 NavState::Hat(const Vector9& xi) { return Base::Hat(xi); } - -//------------------------------------------------------------------------------ -Vector9 NavState::Vee(const Matrix5& X) { return Base::Vee(X); } - //------------------------------------------------------------------------------ NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v, OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 5489e17d44..ea9a621c03 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -41,11 +41,9 @@ using Velocity3 = Vector3; * NOTE: While Barrau20icra follow a R,v,t order, * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState : public MatrixLieGroup, - public ExtendedPose3<2> { +class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { public: - using Base = ExtendedPose3<2>; - using LieGroupBase = MatrixLieGroup; + using Base = ExtendedPose3<2, NavState>; using LieAlgebra = Matrix5; using Vector25 = Eigen::Matrix; inline constexpr static auto dimension = 9; @@ -155,10 +153,6 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup, /// @name Group /// @{ - static NavState Identity() { return NavState(Base::Identity()); } - NavState inverse() const; - NavState operator*(const NavState& other) const; - /// Syntactic sugar const Rot3& rotation() const { return attitude(); }; @@ -197,41 +191,6 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup, /// @name Lie Group /// @{ - static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); - static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); - - Matrix9 AdjointMap() const; - Vector9 Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_this = {}, - OptionalJacobian<9, 9> H_xib = {}) const; - - static Matrix9 adjointMap(const Vector9& xi); - static Vector9 adjoint(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi = {}, - OptionalJacobian<9, 9> H_y = {}); - - static Matrix9 ExpmapDerivative(const Vector9& xi); - static Matrix9 LogmapDerivative(const Vector9& xi); - static Matrix9 LogmapDerivative(const NavState& state); - - struct GTSAM_EXPORT ChartAtOrigin { - static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); - static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); - }; - - using LieGroupBase::between; - using LieGroupBase::compose; - using LieGroupBase::expmap; - using LieGroupBase::LocalCoordinates; - using LieGroupBase::logmap; - using LieGroupBase::Retract; - using LieGroup::inverse; - - static Matrix5 Hat(const Vector9& xi); - static Vector9 Vee(const Matrix5& X); - - Matrix5 matrix() const { return Base::matrix(); } - Vector25 vec(OptionalJacobian<25, 9> H = {}) const { return Base::vec(H); } - /// @} /// @name Dynamics /// @{ diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 0e19d83eff..9b9308c58a 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -803,21 +803,6 @@ TEST(NavState, Vec) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } -/* ************************************************************************* */ -TEST(NavState, AdjointMap_GenericVsSpecialized) { - // Create a non-trivial NavState object - const NavState navState(Rot3::Rodrigues(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0), Velocity3(0.4, 0.5, 0.6)); - - // Call the specialized AdjointMap - Matrix9 specialized_Adj = navState.AdjointMap(); - - // Call the generic AdjointMap from the base class - Matrix9 generic_Adj = static_cast*>(&navState)->AdjointMap(); - - // Assert that they are equal - EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); -} - /* ************************************************************************* */ TEST(NavState, AutonomousFlow) { const double dt = 0.1; From 243c76e5f8b40ccfad41939730ab67639cfd9aeb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 11:39:48 -0500 Subject: [PATCH 16/41] Python wrapper --- .github/copilot-instructions.md | 7 + gtsam/geometry/ExtendedPose3-inl.h | 1 - gtsam/geometry/ExtendedPose3.cpp | 2 + gtsam/geometry/ExtendedPose3.h | 1 - gtsam/geometry/doc/ExtendedPose3.ipynb | 464 +++++++++++++++++++---- gtsam/geometry/geometry.i | 55 +++ python/gtsam/tests/test_ExtendedPose3.py | 110 ++++++ 7 files changed, 558 insertions(+), 82 deletions(-) create mode 100644 python/gtsam/tests/test_ExtendedPose3.py diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index b2179615fd..493c4540bf 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -8,4 +8,11 @@ For reviewing PRs: * Classes are Uppercase, methods and functions lowerMixedCase. * Public fields in structs keep plain names (no trailing underscore). * Apart from those naming conventions, we adopt Google C++ style. +* Notebooks in `*/doc/*.ipynb` and `*/examples/*.ipynb` should follow the standard preamble: + 1) title/introduction markdown cell, + 2) copyright markdown cell tagged `remove-cell`, + 3) Colab badge markdown cell, + 4) Colab install code cell tagged `remove-cell`, + 5) imports/setup code cell. + Use the same `remove-cell` tagging convention as existing notebooks so docs build and Colab behavior stay consistent. * After any code change, always run relevant tests via `make -j6 testXXX.run` in the build folder $WORKSPACE/build. If in VS code, ask for escalated permissions if needed. diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index e45e8a05d3..111de538ee 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -97,7 +97,6 @@ size_t ExtendedPose3::k() const { } template -template size_t ExtendedPose3::dim() const { return Dimension(k()); } diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp index 88c59ca072..67e7754fe4 100644 --- a/gtsam/geometry/ExtendedPose3.cpp +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -22,6 +22,8 @@ namespace gtsam { template class ExtendedPose3<1>; template class ExtendedPose3<2>; template class ExtendedPose3<3>; +template class ExtendedPose3<4>; +template class ExtendedPose3<6>; template class ExtendedPose3; } // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index 7990700c03..63eb5d9f7f 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -156,7 +156,6 @@ class GTSAM_EXPORT ExtendedPose3 size_t k() const; /// Runtime manifold dimension. - template > size_t dim() const; /// Rotation component. diff --git a/gtsam/geometry/doc/ExtendedPose3.ipynb b/gtsam/geometry/doc/ExtendedPose3.ipynb index 3016d4a6a7..e13a30b04a 100644 --- a/gtsam/geometry/doc/ExtendedPose3.ipynb +++ b/gtsam/geometry/doc/ExtendedPose3.ipynb @@ -1,90 +1,394 @@ { - "cells": [ + "cells": [ + { + "cell_type": "markdown", + "id": "090d20ac", + "metadata": {}, + "source": [ + "# ExtendedPose3\n", + "\n", + "The `ExtendedPose3` class is the generic matrix Lie group $SE_K(3)$: the semi-direct product of `SO(3)` with `K` copies of $\\mathbb{R}^3$.\n", + "\n", + "This notebook focuses on Python usage of `ExtendedPose36` (the `K=6` instantiation), including construction, group operations, and manifold/Lie operations.\n" + ] + }, + { + "cell_type": "markdown", + "id": "e5b31ed7", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information\n" + ] + }, + { + "cell_type": "markdown", + "id": "b45cbfc5", + "metadata": {}, + "source": [ + "\"Open\n" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "acccf586", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not in Colab\n" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "c70f339f", + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3\n", + "\n", + "ExtendedPose36 = gtsam.ExtendedPose36\n" + ] + }, + { + "cell_type": "markdown", + "id": "fa791c1f", + "metadata": {}, + "source": [ + "## Constructing `ExtendedPose36`\n", + "\n", + "`ExtendedPose36` stores one `Rot3` and six 3-vectors $x_1,\\dots,x_6$.\n", + "The manifold dimension is `3 + 3*K = 21`, and the homogeneous matrix is `(3+K) x (3+K) = 9 x 9`.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "207ee70e", + "metadata": {}, + "outputs": [ { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# ExtendedPose3\n", - "\n", - "`ExtendedPose3` is the generic Lie group $SE_K(3)$: the semi-direct product of `SO(3)` with `K` copies of $\\mathbb{R}^3$.\n", - "\n", - "It is a matrix Lie group with homogeneous representation size $(3+K)\\times(3+K)$ and manifold dimension $3 + 3K$." - ] - }, + "name": "stdout", + "output_type": "stream", + "text": [ + "k = 6, dim = 21, Dim = 21\n", + "matrix shape: (9, 9)\n", + "[[1. 0. 0. 0. 0. 0. 0. 0. 0.]\n", + " [0. 1. 0. 0. 0. 0. 0. 0. 0.]\n", + " [0. 0. 1. 0. 0. 0. 0. 0. 0.]\n", + " [0. 0. 0. 1. 0. 0. 0. 0. 0.]\n", + " [0. 0. 0. 0. 1. 0. 0. 0. 0.]\n", + " [0. 0. 0. 0. 0. 1. 0. 0. 0.]\n", + " [0. 0. 0. 0. 0. 0. 1. 0. 0.]\n", + " [0. 0. 0. 0. 0. 0. 0. 1. 0.]\n", + " [0. 0. 0. 0. 0. 0. 0. 0. 1.]]\n" + ] + } + ], + "source": [ + "X_identity = ExtendedPose36()\n", + "print(f\"k = {X_identity.k()}, dim = {X_identity.dim()}, Dim = {ExtendedPose36.Dim()}\")\n", + "print(\"matrix shape:\", X_identity.matrix().shape)\n", + "print(X_identity.matrix())\n" + ] + }, + { + "cell_type": "markdown", + "id": "44ec8306", + "metadata": {}, + "source": [ + "### Construct from rotation + 3xK block\n" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "3c6f232d", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "x(0): [1. 2. 3.]\n", + "xMatrix: [[ 1. 4. -1. 0.5 2.1 -0.3]\n", + " [ 2. 5. 0.5 -1.2 3.3 0.8]\n", + " [ 3. 6. 2. 1.4 -2.7 0.6]]\n" + ] + } + ], + "source": [ + "R = Rot3.Yaw(np.deg2rad(20.0))\n", + "X_block = np.array([\n", + " [1.0, 4.0, -1.0, 0.5, 2.1, -0.3],\n", + " [2.0, 5.0, 0.5, -1.2, 3.3, 0.8],\n", + " [3.0, 6.0, 2.0, 1.4, -2.7, 0.6],\n", + "])\n", + "\n", + "X1 = ExtendedPose36(R, X_block)\n", + "print(\"x(0):\", np.array(X1.x(0)))\n", + "print(\"xMatrix:\", X1.xMatrix())\n" + ] + }, + { + "cell_type": "markdown", + "id": "2c4a45a8", + "metadata": {}, + "source": [ + "### Construct from homogeneous matrix\n" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "3408e416", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "equals: True\n" + ] + } + ], + "source": [ + "T = X1.matrix()\n", + "X1_from_T = ExtendedPose36(T)\n", + "print(\"equals:\", X1.equals(X1_from_T, 1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "caa03a92", + "metadata": {}, + "source": [ + "## Group operations\n", + "\n", + "Composition, inverse, and between follow the same `Pose3`-style API.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "51421896", + "metadata": {}, + "outputs": [ { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## State and Tangent Layout\n", - "\n", - "An element is parameterized as:\n", - "\n", - "$$\n", - "X = (R, x_1, \\ldots, x_K), \\qquad R \\in SO(3),\\; x_i \\in \\mathbb{R}^3\n", - "$$\n", - "\n", - "with tangent vector ordering:\n", - "\n", - "$$\n", - "\\xi = [\\omega, \\rho_1, \\ldots, \\rho_K] \\in \\mathbb{R}^{3+3K}.\n", - "$$" - ] - }, + "name": "stdout", + "output_type": "stream", + "text": [ + "compose == matrix multiply: True\n", + "between == inverse-compose: True\n" + ] + } + ], + "source": [ + "xi2 = np.array([\n", + " 0.02, -0.01, 0.03,\n", + " 0.1, 0.2, -0.1,\n", + " -0.2, 0.3, 0.4,\n", + " 0.5, -0.6, 0.2,\n", + " -0.3, 0.1, 0.2,\n", + " 0.4, 0.2, -0.5,\n", + " -0.1, 0.7, 0.2,\n", + "])\n", + "X2 = ExtendedPose36.Expmap(xi2)\n", + "\n", + "X12 = X1.compose(X2)\n", + "print(\"compose == matrix multiply:\", np.allclose(X12.matrix(), X1.matrix() @ X2.matrix(), atol=1e-9))\n", + "\n", + "X_between = X1.between(X2)\n", + "X_between_expected = X1.inverse().compose(X2)\n", + "print(\"between == inverse-compose:\", X_between.equals(X_between_expected, 1e-9))\n" + ] + }, + { + "cell_type": "markdown", + "id": "45698ac4", + "metadata": {}, + "source": [ + "## Expmap / Logmap and tangent vectors\n" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "25a46216", + "metadata": {}, + "outputs": [ { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Matrix Representation\n", - "\n", - "$$\n", - "X = \\begin{bmatrix}\n", - "R & x_1 & \\cdots & x_K \\\\\n", - "0 & 1 & & 0 \\\\\n", - "\\vdots & & \\ddots & \\\\\n", - "0 & 0 & & 1\n", - "\\end{bmatrix}\n", - "$$\n", - "\n", - "The group operation is:\n", - "\n", - "$$\n", - "(R,x_i)\\,(S,y_i) = (RS,\\; x_i + R y_i).\n", - "$$" - ] - }, + "name": "stdout", + "output_type": "stream", + "text": [ + "log(exp(xi)) ~= xi: True\n" + ] + } + ], + "source": [ + "xi = np.array([\n", + " 0.11, -0.07, 0.05,\n", + " 0.30, -0.40, 0.10,\n", + " -0.20, 0.60, -0.50,\n", + " 0.70, -0.10, 0.20,\n", + " -0.40, 0.30, 0.80,\n", + " 0.50, -0.20, -0.30,\n", + " 0.10, 0.20, -0.60,\n", + "])\n", + "\n", + "X = ExtendedPose36.Expmap(xi)\n", + "xi_round_trip = ExtendedPose36.Logmap(X)\n", + "print(\"log(exp(xi)) ~= xi:\", np.allclose(xi_round_trip, xi, atol=1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "1a2eb160", + "metadata": {}, + "source": [ + "### Hat / Vee\n" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "576cb79d", + "metadata": {}, + "outputs": [ { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## C++ Usage Notes\n", - "\n", - "- Fixed-size: `ExtendedPose3<3>` gives a 12D manifold.\n", - "- Dynamic-size: `ExtendedPose3` takes runtime `k` in constructor.\n", - "- Default dynamic construction (`k=0`) is supported for serialization and placeholder use.\n", - "- The API provides the standard `Manifold`, `LieGroup`, and `MatrixLieGroup` surface: `retract/localCoordinates`, `Expmap/Logmap`, `AdjointMap`, `Hat/Vee`, and `matrix/vec`." - ] - }, + "name": "stdout", + "output_type": "stream", + "text": [ + "vee(hat(xi)) ~= xi: True\n" + ] + } + ], + "source": [ + "X_hat = ExtendedPose36.Hat(xi)\n", + "xi_from_hat = ExtendedPose36.Vee(X_hat)\n", + "print(\"vee(hat(xi)) ~= xi:\", np.allclose(xi_from_hat, xi, atol=1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "494d04cc", + "metadata": {}, + "source": [ + "## Manifold operations\n", + "\n", + "As with `Pose3`, optimization-facing operations are `retract` and `localCoordinates`.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "20419fb4", + "metadata": {}, + "outputs": [ { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Related Guides\n", - "\n", - "- Pose manifold background: [Pose3](./Pose3.ipynb)\n", - "- Navigation usage and `NavState`: [ImuFactor / NavState docs](../../navigation/doc/ImuFactor.ipynb)\n" - ] + "name": "stdout", + "output_type": "stream", + "text": [ + "localCoordinates(retract(delta)) ~= delta: True\n" + ] } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "name": "python" + ], + "source": [ + "delta = 1e-2 * np.ones(21)\n", + "X_perturbed = X1.retract(delta)\n", + "delta_back = X1.localCoordinates(X_perturbed)\n", + "print(\"localCoordinates(retract(delta)) ~= delta:\", np.allclose(delta_back, delta, atol=1e-7))" + ] + }, + { + "cell_type": "markdown", + "id": "cde834ce", + "metadata": {}, + "source": [ + "## Adjoint\n" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "id": "46a15f60", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "AdjointMap shape: (21, 21)\n", + "Adjoint(eta) == AdjointMap @ eta: True\n" + ] } + ], + "source": [ + "eta = np.linspace(-0.3, 0.3, 21)\n", + "Ad = X1.AdjointMap()\n", + "eta_adjoint = X1.Adjoint(eta)\n", + "print(\"AdjointMap shape:\", Ad.shape)\n", + "print(\"Adjoint(eta) == AdjointMap @ eta:\", np.allclose(eta_adjoint, Ad @ eta, atol=1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "6a2a253f", + "metadata": {}, + "source": [ + "## Notes and references\n", + "\n", + "`ExtendedPose3` is meaning-agnostic: it only defines the group structure. Semantic interpretation of each $x_i$ (e.g., position, velocity, bias, contact points) is left to derived/application-level code.\n", + "\n", + "For an invariant-filter application context (including walking-robot style state design ideas), see:\n", + "- [InvariantEKF notebook](../../navigation/doc/InvariantEKF.ipynb)\n", + "- [NavState notebook](../../navigation/doc/NavState.ipynb)\n", + "\n", + "For `Pose3`-specific behavior and conventions, see:\n", + "- [Pose3 notebook](./Pose3.ipynb)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" }, - "nbformat": 4, - "nbformat_minor": 5 + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 } diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 05b637e075..28ab12079f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -686,6 +686,61 @@ class Pose3 { void serialize() const; }; +#include +template +class ExtendedPose3 { + // Standard Constructors + ExtendedPose3(); + ExtendedPose3(const This& other); + ExtendedPose3(const gtsam::Rot3& R, const gtsam::Matrix& x); + ExtendedPose3(const gtsam::Matrix& T); + + // Testable + void print(string s = "") const; + bool equals(const This& other, double tol = 1e-9) const; + + // Access + size_t k() const; + gtsam::Rot3 rotation() const; + gtsam::Point3 x(size_t i) const; + gtsam::Matrix xMatrix() const; + + // Group + static This Identity(); + This inverse() const; + This compose(const This& g) const; + This between(const This& g) const; + + // Operator Overloads + This operator*(const This& other) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + This retract(gtsam::Vector v) const; + gtsam::Vector localCoordinates(const This& g) const; + + // Lie Group + static This Expmap(gtsam::Vector xi); + static gtsam::Vector Logmap(const This& pose); + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi_b) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi); + static gtsam::Matrix LogmapDerivative(gtsam::Vector xi); + static gtsam::Matrix LogmapDerivative(const This& pose); + + // Matrix Lie Group + gtsam::Vector vec() const; + gtsam::Matrix matrix() const; + static gtsam::Matrix Hat(const gtsam::Vector& xi); + static gtsam::Vector Vee(const gtsam::Matrix& X); + + // enabling serialization functionality + void serialize() const; +}; + #include class SL4 { // Standard constructors diff --git a/python/gtsam/tests/test_ExtendedPose3.py b/python/gtsam/tests/test_ExtendedPose3.py new file mode 100644 index 0000000000..a1f8523a44 --- /dev/null +++ b/python/gtsam/tests/test_ExtendedPose3.py @@ -0,0 +1,110 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +ExtendedPose3 wrapper unit tests. +""" + +import unittest + +import numpy as np + +import gtsam +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestExtendedPose3(GtsamTestCase): + """Test wrapped ExtendedPose3 static instantiations.""" + + @staticmethod + def class_for_k(k: int): + return getattr(gtsam, f"ExtendedPose3{k}") + + def test_constructors_static_k(self): + """All requested static-K classes are wrapped and default-constructible.""" + for k in (2, 3, 4, 6): + cls = self.class_for_k(k) + pose = cls() + self.assertEqual(pose.k(), k) + self.gtsamAssertEquals(pose, cls.Identity(), 1e-12) + + def test_k6_lie_group_and_matrix_lie_group(self): + """Test selected MatrixLieGroup operations on K=6.""" + cls = self.class_for_k(6) + + xi = np.array([ + 0.11, -0.07, 0.05, + 0.30, -0.40, 0.10, + -0.20, 0.60, -0.50, + 0.70, -0.10, 0.20, + -0.40, 0.30, 0.80, + 0.50, -0.20, -0.30, + 0.10, 0.20, -0.60, + ]) + yi = np.array([ + -0.06, 0.04, 0.02, + 0.10, -0.15, 0.05, + -0.12, 0.18, 0.07, + 0.22, -0.09, 0.03, + -0.14, 0.11, 0.08, + 0.06, 0.13, -0.17, + -0.05, 0.09, 0.04, + ]) + + hat = cls.Hat(xi) + np.testing.assert_allclose(cls.Vee(hat), xi, atol=1e-9) + + p = cls.Expmap(xi) + q = cls.Expmap(yi) + np.testing.assert_allclose(cls.Logmap(p), xi, atol=1e-9) + + composed = p * q + np.testing.assert_allclose(composed.matrix(), p.matrix() @ q.matrix(), atol=1e-9) + self.gtsamAssertEquals(composed, p.compose(q), 1e-9) + self.gtsamAssertEquals(p.between(q), p.inverse().compose(q), 1e-9) + self.gtsamAssertEquals(p.inverse() * p, cls.Identity(), 1e-9) + + self.assertEqual(cls.Dim(), 21) + self.assertEqual(p.dim(), 21) + self.assertEqual(p.k(), 6) + self.assertEqual(p.vec().shape[0], 81) + + def test_k6_constructor_with_components(self): + """Construct K=6 from (Rot3, x) and access x(i)/xMatrix().""" + cls = self.class_for_k(6) + x = np.array([ + [1.0, 4.0, -1.0, 0.5, 2.1, -0.3], + [2.0, 5.0, 0.5, -1.2, 3.3, 0.8], + [3.0, 6.0, 2.0, 1.4, -2.7, 0.6], + ]) + pose = cls(Rot3(), x) + + np.testing.assert_allclose(pose.xMatrix(), x, atol=1e-12) + for i in range(6): + np.testing.assert_allclose(pose.x(i), x[:, i], atol=1e-12) + + @unittest.skipUnless(hasattr(gtsam.ExtendedPose36, "serialize"), "Serialization not enabled") + def test_serialization_k6(self): + """Serialization works when boost serialization is enabled.""" + cls = self.class_for_k(6) + expected = cls.Expmap(np.array([ + 0.01, -0.02, 0.03, + 0.04, -0.05, 0.06, + 0.07, -0.08, 0.09, + -0.10, 0.11, -0.12, + 0.13, -0.14, 0.15, + -0.16, 0.17, -0.18, + 0.19, -0.20, 0.21, + ])) + actual = cls() + serialized = expected.serialize() + actual.deserialize(serialized) + self.gtsamAssertEquals(expected, actual, 1e-10) + + +if __name__ == "__main__": + unittest.main() From 5b234daea11d28c5c3e63cbb8788184774dc0234 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 11:58:05 -0500 Subject: [PATCH 17/41] Add math --- gtsam/geometry/doc/ExtendedPose3.ipynb | 47 ++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/gtsam/geometry/doc/ExtendedPose3.ipynb b/gtsam/geometry/doc/ExtendedPose3.ipynb index e13a30b04a..b2be902286 100644 --- a/gtsam/geometry/doc/ExtendedPose3.ipynb +++ b/gtsam/geometry/doc/ExtendedPose3.ipynb @@ -71,6 +71,53 @@ "ExtendedPose36 = gtsam.ExtendedPose36\n" ] }, + { + "cell_type": "markdown", + "id": "9b4dc088", + "metadata": {}, + "source": [ + "## Lie Group Math Overview\n", + "\n", + "An element of $SE_K(3)$ is $X=(R, x_1, \\ldots, x_K)$ with $R \\in SO(3)$ and $x_i \\in \\mathbb{R}^3$.\n", + "\n", + "Group composition is:\n", + "\n", + "\\begin{equation}\n", + "(R, x_i) \\cdot (S, y_i) = (RS, x_i + R y_i).\n", + "\\end{equation}\n", + "\n", + "Group inverse is:\n", + "\n", + "\\begin{equation}\n", + "(R, x_i)^{-1} = (R^T, -R^T x_i).\n", + "\\end{equation}\n", + "\n", + "The tangent vector is ordered as $\\xi=[\\omega, \\rho_1, \\ldots, \\rho_K] \\in \\mathbb{R}^{3+3K}$.\n", + "The exponential and logarithm maps are:\n", + "\n", + "\\begin{equation}\n", + "X = \\operatorname{Exp}(\\xi), \\quad \\xi = \\operatorname{Log}(X).\n", + "\\end{equation}\n", + "\n", + "For the matrix-Lie-group view, $\\operatorname{Hat}(\\xi)$ maps tangent vectors to Lie algebra matrices and $\\operatorname{Vee}(\\cdot)$ is the inverse map:\n", + "\n", + "\\begin{equation}\n", + "\\operatorname{Vee}(\\operatorname{Hat}(\\xi)) = \\xi.\n", + "\\end{equation}\n", + "\n", + "The adjoint map transports perturbations between frames:\n", + "\n", + "\\begin{equation}\n", + "\\operatorname{Ad}_X \\eta = \\operatorname{AdjointMap}(X) \\; \\eta.\n", + "\\end{equation}\n", + "\n", + "As a manifold, local updates use retraction and local coordinates:\n", + "\n", + "\\begin{equation}\n", + "X' = X \\oplus \\delta = \\operatorname{Retract}_X(\\delta), \\quad \\delta = \\operatorname{Local}_X(X').\n", + "\\end{equation}\n" + ] + }, { "cell_type": "markdown", "id": "fa791c1f", From 52c0ae18869dde99ff47da803d31c58e8b3d98b3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 12:48:04 -0500 Subject: [PATCH 18/41] Review --- gtsam/geometry/ExtendedPose3-inl.h | 132 +++++---- gtsam/geometry/ExtendedPose3.h | 320 +++++++++++++++------ gtsam/geometry/Pose3.cpp | 94 +++--- gtsam/geometry/Pose3.h | 9 +- gtsam/geometry/tests/testExtendedPose3.cpp | 83 +++--- 5 files changed, 407 insertions(+), 231 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 111de538ee..208eddcaf8 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -86,21 +86,6 @@ ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { x_ = T.block(0, 3, 3, n - 3); } -template -size_t ExtendedPose3::Dimension(size_t k) { - return 3 + 3 * k; -} - -template -size_t ExtendedPose3::k() const { - return static_cast(x_.cols()); -} - -template -size_t ExtendedPose3::dim() const { - return Dimension(k()); -} - template const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { if (H) { @@ -123,7 +108,8 @@ Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { } else { H->setZero(); } - H->block(0, 3 + 3 * i, 3, 3) = R_.matrix(); + const Eigen::Index idx = 3 + 3 * static_cast(i); + H->block(0, idx, 3, 3) = R_.matrix(); } return x_.col(static_cast(i)); } @@ -159,22 +145,22 @@ typename ExtendedPose3::This ExtendedPose3::Identity() { template template -typename ExtendedPose3::This -ExtendedPose3::Identity(size_t k) { +typename ExtendedPose3::This ExtendedPose3::Identity( + size_t k) { return MakeReturn(ExtendedPose3(k)); } template -typename ExtendedPose3::This -ExtendedPose3::inverse() const { +typename ExtendedPose3::This ExtendedPose3::inverse() + const { const Rot3 Rt = R_.inverse(); const Matrix3K x = -(Rt.matrix() * x_); return MakeReturn(ExtendedPose3(Rt, x)); } template -typename ExtendedPose3::This -ExtendedPose3::operator*(const This& other) const { +typename ExtendedPose3::This ExtendedPose3::operator*( + const This& other) const { const ExtendedPose3& otherBase = AsBase(other); if (k() != otherBase.k()) { throw std::invalid_argument("ExtendedPose3: compose requires matching k."); @@ -183,15 +169,36 @@ ExtendedPose3::operator*(const This& other) const { return MakeReturn(ExtendedPose3(R_ * otherBase.R_, x)); } +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. +// See also [this document](doc/Jacobians.md) template typename ExtendedPose3::This ExtendedPose3::Expmap( const TangentVector& xi, ChartJacobian Hxi) { const size_t k = RuntimeK(xi); + // Get angular velocity omega const Vector3 w = xi.template head<3>(); + + // Instantiate functor for Dexp-related operations: const so3::DexpFunctor local(w); + + // Compute rotation using Expmap +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(w); +#else const Rot3 R(local.expmap()); - const Matrix3 Rt = R.transpose(); +#endif + + // The translation t = local.Jacobian().left() * v. + // Below we call local.Jacobian().applyLeft, which is faster if you don't need + // Jacobians, and returns Jacobian of t with respect to w if asked. + // NOTE(Frank): this does the same as the intuitive formulas: + // t_parallel = w * w.dot(v); // translation parallel to axis + // w_cross_v = w.cross(v); // translation orthogonal to axis + // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; + // but Local does not need R, deals automatically with the case where theta2 + // is near zero, and also gives us the machinery for the Jacobians. Matrix3K x; if constexpr (K == Eigen::Dynamic) x.resize(3, static_cast(k)); @@ -200,18 +207,23 @@ typename ExtendedPose3::This ExtendedPose3::Expmap( const size_t d = 3 + 3 * k; ZeroJacobian(Hxi, d); const Matrix3 Jr = local.Jacobian().right(); - Hxi->block(0, 0, 3, 3) = Jr; + Hxi->block(0, 0, 3, 3) = Jr; // Jr here *is* the Jacobian of expmap + const Matrix3 Rt = R.transpose(); for (size_t i = 0; i < k; ++i) { Matrix3 H_xi_w; - const Vector3 rho = xi.template segment<3>(3 + 3 * i); + const Eigen::Index idx = 3 + 3 * static_cast(i); + const Vector3 rho = xi.template segment<3>(idx); x.col(static_cast(i)) = local.Jacobian().applyLeft(rho, &H_xi_w); - Hxi->block(3 + 3 * i, 0, 3, 3) = Rt * H_xi_w; - Hxi->block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jr; + Hxi->block(idx, 0, 3, 3) = Rt * H_xi_w; + Hxi->block(idx, idx, 3, 3) = Jr; + // In the last row, Jr = R^T * Jl, see Barfoot eq. (8.83). + // Jl is the left Jacobian of SO(3) at w. } } else { for (size_t i = 0; i < k; ++i) { - const Vector3 rho = xi.template segment<3>(3 + 3 * i); + const Eigen::Index idx = 3 + 3 * static_cast(i); + const Vector3 rho = xi.template segment<3>(idx); x.col(static_cast(i)) = local.Jacobian().applyLeft(rho); } } @@ -220,8 +232,8 @@ typename ExtendedPose3::This ExtendedPose3::Expmap( } template -typename ExtendedPose3::TangentVector ExtendedPose3::Logmap( - const This& pose, ChartJacobian Hpose) { +typename ExtendedPose3::TangentVector +ExtendedPose3::Logmap(const This& pose, ChartJacobian H) { const ExtendedPose3& poseBase = AsBase(pose); const Vector3 w = Rot3::Logmap(poseBase.R_); const so3::DexpFunctor local(w); @@ -231,11 +243,12 @@ typename ExtendedPose3::TangentVector ExtendedPose3::Log xi.resize(static_cast(poseBase.dim())); xi.template head<3>() = w; for (size_t i = 0; i < poseBase.k(); ++i) { - xi.template segment<3>(3 + 3 * i) = - local.InvJacobian().applyLeft(poseBase.x_.col(static_cast(i))); + const Eigen::Index idx = 3 + 3 * static_cast(i); + xi.template segment<3>(idx) = local.InvJacobian().applyLeft( + poseBase.x_.col(static_cast(i))); } - if (Hpose) *Hpose = LogmapDerivative(xi); + if (H) *H = LogmapDerivative(xi); return xi; } @@ -252,16 +265,19 @@ ExtendedPose3::AdjointMap() const { adj.block(0, 0, 3, 3) = R; for (size_t i = 0; i < k(); ++i) { - adj.block(3 + 3 * i, 0, 3, 3) = + const Eigen::Index idx = 3 + 3 * static_cast(i); + adj.block(idx, 0, 3, 3) = skewSymmetric(x_.col(static_cast(i))) * R; - adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = R; + adj.block(idx, idx, 3, 3) = R; } return adj; } template -typename ExtendedPose3::TangentVector ExtendedPose3::Adjoint( - const TangentVector& xi_b, ChartJacobian H_this, ChartJacobian H_xib) const { +typename ExtendedPose3::TangentVector +ExtendedPose3::Adjoint(const TangentVector& xi_b, + ChartJacobian H_this, + ChartJacobian H_xib) const { const Jacobian Ad = AdjointMap(); if (H_this) *H_this = -Ad * adjointMap(xi_b); if (H_xib) *H_xib = Ad; @@ -269,8 +285,8 @@ typename ExtendedPose3::TangentVector ExtendedPose3::Adj } template -typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap( - const TangentVector& xi) { +typename ExtendedPose3::Jacobian +ExtendedPose3::adjointMap(const TangentVector& xi) { const size_t k = RuntimeK(xi); const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); @@ -283,17 +299,19 @@ typename ExtendedPose3::Jacobian ExtendedPose3::adjointM adj.block(0, 0, 3, 3) = w_hat; for (size_t i = 0; i < k; ++i) { - adj.block(3 + 3 * i, 0, 3, 3) = skewSymmetric( - xi(3 + 3 * i + 0), xi(3 + 3 * i + 1), xi(3 + 3 * i + 2)); - adj.block(3 + 3 * i, 3 + 3 * i, 3, 3) = w_hat; + const Eigen::Index idx = 3 + 3 * static_cast(i); + adj.block(idx, 0, 3, 3) = + skewSymmetric(xi(idx + 0), xi(idx + 1), xi(idx + 2)); + adj.block(idx, idx, 3, 3) = w_hat; } return adj; } template -typename ExtendedPose3::TangentVector ExtendedPose3::adjoint( - const TangentVector& xi, const TangentVector& y, ChartJacobian Hxi, - ChartJacobian H_y) { +typename ExtendedPose3::TangentVector +ExtendedPose3::adjoint(const TangentVector& xi, + const TangentVector& y, ChartJacobian Hxi, + ChartJacobian H_y) { const Jacobian ad_xi = adjointMap(xi); if (Hxi) { if constexpr (dimension == Eigen::Dynamic) { @@ -329,7 +347,10 @@ typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative(const TangentVector& xi) { const size_t k = RuntimeK(xi); const Vector3 w = xi.template head<3>(); + + // Instantiate functor for Dexp-related operations: const so3::DexpFunctor local(w); + const Matrix3 Rt = local.expmap().transpose(); const Matrix3 Jw = Rot3::LogmapDerivative(w); @@ -343,10 +364,11 @@ ExtendedPose3::LogmapDerivative(const TangentVector& xi) { J.block(0, 0, 3, 3) = Jw; for (size_t i = 0; i < k; ++i) { Matrix3 H_xi_w; - local.Jacobian().applyLeft(xi.template segment<3>(3 + 3 * i), H_xi_w); + const Eigen::Index idx = 3 + 3 * static_cast(i); + local.Jacobian().applyLeft(xi.template segment<3>(idx), H_xi_w); const Matrix3 Q = Rt * H_xi_w; - J.block(3 + 3 * i, 0, 3, 3) = -Jw * Q * Jw; - J.block(3 + 3 * i, 3 + 3 * i, 3, 3) = Jw; + J.block(idx, 0, 3, 3) = -Jw * Q * Jw; + J.block(idx, idx, 3, 3) = Jw; } return J; } @@ -367,8 +389,8 @@ ExtendedPose3::ChartAtOrigin::Retract(const TangentVector& xi, template typename ExtendedPose3::TangentVector ExtendedPose3::ChartAtOrigin::Local(const This& pose, - ChartJacobian Hpose) { - return ExtendedPose3::Logmap(pose, Hpose); + ChartJacobian H) { + return ExtendedPose3::Logmap(pose, H); } template @@ -398,14 +420,15 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( } X.block(0, 0, 3, 3) = skewSymmetric(xi(0), xi(1), xi(2)); for (size_t i = 0; i < k; ++i) { - X.block(0, 3 + i, 3, 1) = xi.template segment<3>(3 + 3 * i); + const Eigen::Index idx = 3 + 3 * static_cast(i); + X.block(0, 3 + i, 3, 1) = xi.template segment<3>(idx); } return X; } template -typename ExtendedPose3::TangentVector ExtendedPose3::Vee( - const LieAlgebra& X) { +typename ExtendedPose3::TangentVector +ExtendedPose3::Vee(const LieAlgebra& X) { const Eigen::Index k = X.cols() - 3; TangentVector xi; if constexpr (dimension == Eigen::Dynamic) { @@ -415,7 +438,8 @@ typename ExtendedPose3::TangentVector ExtendedPose3::Vee xi(1) = X(0, 2); xi(2) = X(1, 0); for (Eigen::Index i = 0; i < k; ++i) { - xi.template segment<3>(3 + 3 * i) = X.template block<3, 1>(0, 3 + i); + const Eigen::Index idx = 3 + 3 * i; + xi.template segment<3>(idx) = X.template block<3, 1>(0, idx); } return xi; } diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index 63eb5d9f7f..2ab491625f 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -37,51 +37,27 @@ namespace gtsam { template class ExtendedPose3; -namespace internal { - -/// Compile-time traits for SE_k(3) dimensions. -template -struct ExtendedPose3Traits { - /// Manifold dimensionality for SE_k(3): 3 + 3*k. - static constexpr int Dimension() { - return (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K; - } - - /// Homogeneous matrix size for SE_k(3): (3 + k) x (3 + k). - static constexpr int MatrixDim() { - return (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K; - } -}; - -template -struct ExtendedPose3Class { - using type = Derived; -}; - -template -struct ExtendedPose3Class { - using type = ExtendedPose3; -}; - -} // namespace internal - /** * Lie group SE_k(3): semidirect product of SO(3) with k copies of R^3. - * Tangent ordering is [omega, x1, x2, ..., xk], each xi in R^3. + * State ordering is (R, x_1, ..., x_k) with R in SO(3) and x_i in R^3. + * Tangent ordering is [omega, rho_1, ..., rho_k], each block in R^3. * + * The manifold dimension is 3+3k and the homogeneous matrix size is 3+k. * Template parameter K can be fixed (K >= 1) or Eigen::Dynamic. */ template class GTSAM_EXPORT ExtendedPose3 - : public MatrixLieGroup::type, - internal::ExtendedPose3Traits::Dimension(), - internal::ExtendedPose3Traits::MatrixDim()> { + : public MatrixLieGroup, + ExtendedPose3, Derived>, + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K, + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K> { public: - using This = typename internal::ExtendedPose3Class::type; - inline constexpr static auto dimension = - internal::ExtendedPose3Traits::Dimension(); - inline constexpr static auto matrix_dim = - internal::ExtendedPose3Traits::MatrixDim(); + using This = std::conditional_t, + ExtendedPose3, Derived>; + inline constexpr static int dimension = + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K; + inline constexpr static int matrix_dim = + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K; using Base = MatrixLieGroup; using TangentVector = typename Base::TangentVector; @@ -106,120 +82,259 @@ class GTSAM_EXPORT ExtendedPose3 template using IsFixed = typename std::enable_if= 1, void>::type; - static This MakeReturn(const ExtendedPose3& value) { - if constexpr (std::is_void_v) { - return value; - } else { - return This(value); - } - } - - static const ExtendedPose3& AsBase(const This& value) { - if constexpr (std::is_void_v) { - return value; - } else { - return static_cast(value); - } - } - - static size_t RuntimeK(const TangentVector& xi); - static void ZeroJacobian(ChartJacobian H, size_t d); - public: /// @name Constructors /// @{ - /// Construct fixed-size identity. + /** + * Construct a fixed-size identity element. + * + * For fixed K, this creates R=I and x_i=0 for i=1..k. + * The manifold dimension is 3+3k and matrix size is (3+k)x(3+k). + */ template > ExtendedPose3(); - /// Construct dynamic identity, optionally with runtime k. + /** + * Construct a dynamic-size identity element. + * + * @param k Number of R^3 blocks. + * Creates R=I and x_i=0 for i=1..k. + * The manifold dimension is 3+3k and matrix size is (3+k)x(3+k). + */ template > explicit ExtendedPose3(size_t k = 0); + /** Copy constructor. */ ExtendedPose3(const ExtendedPose3&) = default; + + /** Copy assignment. */ ExtendedPose3& operator=(const ExtendedPose3&) = default; - /// Construct from rotation and 3xK block. + /** + * Construct from rotation and 3xk block. + * + * @param R Rotation in SO(3). + * @param x Matrix in R^(3xk), where column i stores x_i. + */ ExtendedPose3(const Rot3& R, const Matrix3K& x); - /// Construct from homogeneous matrix representation. + /** + * Construct from homogeneous matrix representation. + * + * @param T Homogeneous matrix in R^((3+k)x(3+k)). + * Top-left 3x3 is R, top-right 3xk stores x_1..x_k. + */ explicit ExtendedPose3(const LieAlgebra& T); /// @} /// @name Access /// @{ - static size_t Dimension(size_t k); - - /// Number of R^3 columns. - size_t k() const; - - /// Runtime manifold dimension. - size_t dim() const; - - /// Rotation component. + /** + * Runtime manifold dimension helper. + * + * @param k Number of R^3 blocks. + * @return 3+3k. + */ + static size_t Dimension(size_t k) { return 3 + 3 * k; } + + /** @return Number of R^3 blocks, k. */ + size_t k() const { return static_cast(x_.cols()); } + + /** @return Runtime manifold dimension, 3+3k. */ + size_t dim() const { return Dimension(k()); } + + /** + * Rotation component. + * + * @param H Optional Jacobian in R^(3xdim) for local rotation coordinates. + * @return Rotation R. + */ const Rot3& rotation(ComponentJacobian H = {}) const; - /// i-th R^3 component, returned by value. + /** + * i-th R^3 component, returned by value. + * + * @param i Zero-based block index in [0, k). + * @param H Optional Jacobian in R^(3xdim). + * @return x_i in R^3. + */ Point3 x(size_t i, ComponentJacobian H = {}) const; + /** + * Access all x_i blocks. + * + * @return Matrix in R^(3xk) with columns x_1..x_k. + */ const Matrix3K& xMatrix() const; + + /** + * Mutable access to all x_i blocks. + * + * @return Matrix in R^(3xk) with columns x_1..x_k. + */ Matrix3K& xMatrix(); /// @} /// @name Testable /// @{ + /** + * Print this state. + * + * @param s Optional prefix string. + */ void print(const std::string& s = "") const; + /** + * Equality check with tolerance. + * + * @param other Other state. + * @param tol Absolute tolerance. + * @return True if rotation and all x_i blocks are equal within tol. + */ bool equals(const ExtendedPose3& other, double tol = 1e-9) const; /// @} /// @name Group /// @{ + /** + * Identity element for fixed-size K. + * + * @return Identity with manifold dimension 3+3k and matrix size 3+k. + */ template > static This Identity(); + /** + * Identity element for dynamic-size K. + * + * @param k Number of R^3 blocks. + * @return Identity with manifold dimension 3+3k and matrix size 3+k. + */ template > static This Identity(size_t k = 0); + /** + * Group inverse. + * + * @return X^{-1}. + */ This inverse() const; + /** + * Group composition. + * + * @param other Right-hand operand with the same k. + * @return this * other. + */ This operator*(const This& other) const; /// @} /// @name Lie Group /// @{ + /** + * Exponential map from tangent to group. + * + * @param xi Tangent vector in R^dim. + * @param Hxi Optional Jacobian in R^(dimxdim). + * @return Group element in SE_k(3). + */ static This Expmap(const TangentVector& xi, ChartJacobian Hxi = {}); - static TangentVector Logmap(const This& pose, - ChartJacobian Hpose = {}); - + /** + * Logarithm map from group to tangent. + * + * @param pose Group element in SE_k(3). + * @param Hpose Optional Jacobian in R^(dimxdim). + * @return Tangent vector in R^dim. + */ + static TangentVector Logmap(const This& pose, ChartJacobian Hpose = {}); + + /** + * Adjoint map. + * + * @return Matrix in R^(dimxdim). + */ Jacobian AdjointMap() const; + /** + * Adjoint action on a tangent vector. + * + * @param xi_b Tangent vector in R^dim. + * @param H_this Optional Jacobian in R^(dimxdim). + * @param H_xib Optional Jacobian in R^(dimxdim). + * @return Ad_X xi_b in R^dim. + */ TangentVector Adjoint(const TangentVector& xi_b, ChartJacobian H_this = {}, ChartJacobian H_xib = {}) const; + /** + * Lie algebra adjoint map. + * + * @param xi Tangent vector in R^dim. + * @return ad_xi matrix in R^(dimxdim). + */ static Jacobian adjointMap(const TangentVector& xi); + /** + * Lie bracket action ad_xi(y). + * + * @param xi Tangent vector in R^dim. + * @param y Tangent vector in R^dim. + * @param Hxi Optional Jacobian in R^(dimxdim). + * @param H_y Optional Jacobian in R^(dimxdim). + * @return ad_xi(y) in R^dim. + */ static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, - ChartJacobian Hxi = {}, - ChartJacobian H_y = {}); - + ChartJacobian Hxi = {}, ChartJacobian H_y = {}); + + /** + * Jacobian of Expmap. + * + * @param xi Tangent vector in R^dim. + * @return Matrix in R^(dimxdim). + */ static Jacobian ExpmapDerivative(const TangentVector& xi); + /** + * Jacobian of Logmap evaluated from tangent coordinates. + * + * @param xi Tangent vector in R^dim. + * @return Matrix in R^(dimxdim). + */ static Jacobian LogmapDerivative(const TangentVector& xi); + /** + * Jacobian of Logmap evaluated at a group element. + * + * @param pose Group element in SE_k(3). + * @return Matrix in R^(dimxdim). + */ static Jacobian LogmapDerivative(const This& pose); + /** Chart operations at identity for LieGroup/Manifold compatibility. */ struct ChartAtOrigin { - static This Retract(const TangentVector& xi, - ChartJacobian Hxi = {}); - static TangentVector Local(const This& pose, - ChartJacobian Hpose = {}); + /** + * Retract at identity. + * + * @param xi Tangent vector in R^dim. + * @param Hxi Optional Jacobian in R^(dimxdim). + * @return Expmap(xi). + */ + static This Retract(const TangentVector& xi, ChartJacobian Hxi = {}); + + /** + * Local coordinates at identity. + * + * @param pose Group element in SE_k(3). + * @param Hpose Optional Jacobian in R^(dimxdim). + * @return Logmap(pose) in R^dim. + */ + static TangentVector Local(const This& pose, ChartJacobian Hpose = {}); }; using LieGroup::inverse; @@ -228,10 +343,27 @@ class GTSAM_EXPORT ExtendedPose3 /// @name Matrix Lie Group /// @{ + /** + * Homogeneous matrix representation. + * + * @return Matrix in R^((3+k)x(3+k)). + */ LieAlgebra matrix() const; + /** + * Hat operator from tangent to Lie algebra. + * + * @param xi Tangent vector in R^dim. + * @return Matrix in R^((3+k)x(3+k)). + */ static LieAlgebra Hat(const TangentVector& xi); + /** + * Vee operator from Lie algebra to tangent. + * + * @param X Matrix in R^((3+k)x(3+k)). + * @return Tangent vector in R^dim. + */ static TangentVector Vee(const LieAlgebra& X); /// @} @@ -242,6 +374,26 @@ class GTSAM_EXPORT ExtendedPose3 return os; } + protected: + static This MakeReturn(const ExtendedPose3& value) { + if constexpr (std::is_void_v) { + return value; + } else { + return This(value); + } + } + + static const ExtendedPose3& AsBase(const This& value) { + if constexpr (std::is_void_v) { + return value; + } else { + return static_cast(value); + } + } + + static size_t RuntimeK(const TangentVector& xi); + static void ZeroJacobian(ChartJacobian H, size_t d); + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; @@ -258,13 +410,13 @@ using ExtendedPose3Dynamic = ExtendedPose3; template struct traits> - : public internal::MatrixLieGroup< - ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; + : public internal::MatrixLieGroup, + ExtendedPose3::matrix_dim> {}; template struct traits> - : public internal::MatrixLieGroup< - ExtendedPose3, internal::ExtendedPose3Traits::MatrixDim()> {}; + : public internal::MatrixLieGroup, + ExtendedPose3::matrix_dim> {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 7eea6a2cee..1f9053c59b 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -57,38 +57,6 @@ Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { return Pose3(p); } -/* ************************************************************************* */ -Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { -#ifdef GTSAM_POSE3_EXPMAP - return Expmap(xi, Hxi); -#else - Matrix3 DR; - const Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : nullptr); - if (Hxi) { - Hxi->setIdentity(); - Hxi->block<3, 3>(0, 0) = DR; - } - return Pose3(R, xi.tail<3>()); -#endif -} - -/* ************************************************************************* */ -Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { -#ifdef GTSAM_POSE3_EXPMAP - return Logmap(pose, Hpose); -#else - Matrix3 DR; - Vector6 xi; - xi.head<3>() = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : nullptr); - xi.tail<3>() = pose.translation(); - if (Hpose) { - Hpose->setIdentity(); - Hpose->block<3, 3>(0, 0) = DR; - } - return xi; -#endif -} - /* ************************************************************************* */ /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, @@ -136,8 +104,7 @@ void Pose3::print(const std::string& s) const { /* ************************************************************************* */ bool Pose3::equals(const Pose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && - traits::Equals(x_.col(0), pose.x_.col(0), tol); + return R_.equals(pose.R_, tol) && traits::Equals(x_, pose.x_, tol); } /* ************************************************************************* */ @@ -152,8 +119,8 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t, typename MakeJacobian::type HtPoint; Rot3 Rint = interpolate(R_, T.R_, t, HselfRot, HargRot, HtRot); - Point3 Pint = interpolate(x_.col(0), T.x_.col(0), t, HselfPoint, - HargPoint, HtPoint); + Point3 Pint = + interpolate(x_, T.x_, t, HselfPoint, HargPoint, HtPoint); Pose3 result = Pose3(Rint, Pint); if(Hself) *Hself << HselfRot, Z_3x3, Z_3x3, Rint.transpose() * R_.matrix() * HselfPoint; @@ -164,19 +131,48 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t, return result; } return Pose3(interpolate(R_, T.R_, t), - interpolate(x_.col(0), T.x_.col(0), t)); + interpolate(x_, T.x_, t)); +} +/* ************************************************************************* */ +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, Hxi); +#else + Matrix3 DR; + const Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : nullptr); + if (Hxi) { + Hxi->setIdentity(); + Hxi->block<3, 3>(0, 0) = DR; + } + return Pose3(R, xi.tail<3>()); +#endif } /* ************************************************************************* */ -const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { - t_cache_ = Base::x(0, Hself); - return t_cache_; +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(pose, Hpose); +#else + Matrix3 DR; + Vector6 xi; + xi.head<3>() = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : nullptr); + xi.tail<3>() = pose.translation(); + if (Hpose) { + Hpose->setIdentity(); + Hpose->block<3, 3>(0, 0) = DR; + } + return xi; +#endif } /* ************************************************************************* */ -const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { - return Base::rotation(Hself); +const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { + if (Hself) { + Hself->setZero(); + Hself->block<3, 3>(0, 3) = R_.matrix(); + } + return x_; } /* ************************************************************************* */ @@ -208,7 +204,7 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, if (Hpoint) { *Hpoint = R; } - return R_ * point + x_.col(0); + return R_ * point + x_; } Matrix Pose3::transformFrom(const Matrix& points) const { @@ -216,7 +212,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); } const Matrix3 R = R_.matrix(); - return (R * points).colwise() + x_.col(0); // Eigen broadcasting! + return (R * points).colwise() + x_; // Eigen broadcasting! } /* ************************************************************************* */ @@ -225,7 +221,7 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt * (point - x_.col(0))); + const Point3 q(Rt * (point - x_)); if (Hself) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Hself) << @@ -244,13 +240,13 @@ Matrix Pose3::transformTo(const Matrix& points) const { throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); } const Matrix3 Rt = R_.transpose(); - return Rt * (points.colwise() - x_.col(0)); // Eigen broadcasting! + return Rt * (points.colwise() - x_); // Eigen broadcasting! } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 3> Hpoint) const { - const Vector3 delta = point - x_.col(0); + const Vector3 delta = point - x_; if (!Hself && !Hpoint) return delta.norm(); Matrix13 D_r_point; @@ -269,7 +265,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, /* ************************************************************************* */ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 6> Hpose) const { - const Vector3 delta = pose.x_.col(0) - x_.col(0); + const Vector3 delta = pose.x_ - x_; if (!Hself && !Hpose) return delta.norm(); Matrix13 D_r_point; @@ -295,7 +291,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself, OptionalJacobian<2, 3> Hpoint) const { const Matrix3 Rt = R_.transpose(); - const Point3 local(Rt * (point - x_.col(0))); + const Point3 local(Rt * (point - x_)); if (!Hself && !Hpoint) return Unit3(local); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 00353deec9..7b1544c443 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -66,7 +66,8 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { Pose3(const Base& other) : Base(other) {} /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : Base(R, Vector3(t.x(), t.y(), t.z())) {} + Pose3(const Rot3& R, const Point3& t) + : Base(R, Vector3(t.x(), t.y(), t.z())) {} /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); @@ -147,6 +148,7 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { OptionalJacobian<6, 6> Hxi = {}, OptionalJacobian<6, 6> H_y = {}); + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); @@ -199,9 +201,6 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { /// @name Standard Interface /// @{ - /// get rotation - const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const; - /// get translation const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const; @@ -316,8 +315,6 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { /// @} private: - mutable Point3 t_cache_ = Point3::Zero(); - #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp index e9b5157d88..75272bf141 100644 --- a/gtsam/geometry/tests/testExtendedPose3.cpp +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -14,14 +14,12 @@ * @brief Unit tests for ExtendedPose3 */ -#include - +#include #include #include #include #include - -#include +#include using namespace gtsam; @@ -37,21 +35,13 @@ namespace { const Rot3 kR1 = Rot3::RzRyRx(0.1, -0.2, 0.3); const Rot3 kR2 = Rot3::RzRyRx(-0.3, 0.4, -0.2); -const Matrix33 kX1 = (Matrix33() << 1.0, 4.0, -1.0, 2.0, 5.0, 0.5, 3.0, 6.0, - 2.0) +const Matrix3 kX1 = + (Matrix3() << 1.0, 4.0, -1.0, 2.0, 5.0, 0.5, 3.0, 6.0, 2.0).finished(); +const Matrix3 kX2 = + (Matrix3() << -2.0, 1.0, 0.1, 0.3, -1.5, 2.2, 0.7, -0.9, 1.6).finished(); +const Vector12 kXi = (Vector12() << 0.11, -0.07, 0.05, 0.3, -0.4, 0.1, -0.2, + 0.6, -0.5, 0.7, -0.1, 0.2) .finished(); -const Matrix33 kX2 = (Matrix33() << -2.0, 1.0, 0.1, 0.3, -1.5, 2.2, 0.7, -0.9, - 1.6) - .finished(); -const Vector12 kXi = - (Vector12() << 0.11, -0.07, 0.05, 0.3, -0.4, 0.1, -0.2, 0.6, -0.5, 0.7, - -0.1, 0.2) - .finished(); - -ExtendedPose3d MakeDynamic(const Rot3& R, const Matrix33& X) { - Matrix Xd = X; - return ExtendedPose3d(R, Xd); -} } // namespace @@ -84,14 +74,16 @@ TEST(ExtendedPose3, Dimensions) { //****************************************************************************** TEST(ExtendedPose3, ConstructorsAndAccess) { const ExtendedPose33 fixed(kR1, kX1); - const ExtendedPose3d dynamic = MakeDynamic(kR1, kX1); + const ExtendedPose3d dynamic(kR1, ExtendedPose3d::Matrix3K(kX1)); EXPECT(assert_equal(kR1, fixed.rotation())); EXPECT(assert_equal(kR1, dynamic.rotation())); for (size_t i = 0; i < 3; ++i) { - EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), fixed.x(i))); - EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), dynamic.x(i))); + EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), + fixed.x(i))); + EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), + dynamic.x(i))); } EXPECT(assert_equal(fixed.matrix(), dynamic.matrix())); @@ -100,7 +92,8 @@ TEST(ExtendedPose3, ConstructorsAndAccess) { //****************************************************************************** TEST(ExtendedPose3, GroupOperationsMatch) { const ExtendedPose33 f1(kR1, kX1), f2(kR2, kX2); - const ExtendedPose3d d1 = MakeDynamic(kR1, kX1), d2 = MakeDynamic(kR2, kX2); + const ExtendedPose3d d1(kR1, ExtendedPose3d::Matrix3K(kX1)); + const ExtendedPose3d d2(kR2, ExtendedPose3d::Matrix3K(kX2)); const auto f12 = f1 * f2; const auto d12 = d1 * d2; @@ -108,8 +101,10 @@ TEST(ExtendedPose3, GroupOperationsMatch) { const auto f_id = f1 * f1.inverse(); const auto d_id = d1 * d1.inverse(); - EXPECT(assert_equal(ExtendedPose33::Identity().matrix(), f_id.matrix(), 1e-9)); - EXPECT(assert_equal(ExtendedPose3d::Identity(3).matrix(), d_id.matrix(), 1e-9)); + EXPECT( + assert_equal(ExtendedPose33::Identity().matrix(), f_id.matrix(), 1e-9)); + EXPECT( + assert_equal(ExtendedPose3d::Identity(3).matrix(), d_id.matrix(), 1e-9)); const auto f_between = f1.between(f2); const auto d_between = d1.between(d2); @@ -141,7 +136,7 @@ TEST(ExtendedPose3, ExpmapLogmapRoundTrip) { //****************************************************************************** TEST(ExtendedPose3, AdjointConsistency) { const ExtendedPose33 f(kR1, kX1); - const ExtendedPose3d d = MakeDynamic(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); const Vector xi = kXi; const auto f_conj = f * ExtendedPose33::Expmap(kXi) * f.inverse(); @@ -153,10 +148,12 @@ TEST(ExtendedPose3, AdjointConsistency) { EXPECT(assert_equal(d_conj.matrix(), d_adj.matrix(), 1e-9)); const ExtendedPose33::Jacobian f_generic = - static_cast*>(&f)->AdjointMap(); - const Matrix d_generic = static_cast< - const MatrixLieGroup*>(&d) - ->AdjointMap(); + static_cast*>(&f) + ->AdjointMap(); + const Matrix d_generic = + static_cast*>(&d) + ->AdjointMap(); EXPECT(assert_equal(Matrix(f_generic), Matrix(f.AdjointMap()), 1e-9)); EXPECT(assert_equal(d_generic, d.AdjointMap(), 1e-9)); } @@ -164,31 +161,39 @@ TEST(ExtendedPose3, AdjointConsistency) { //****************************************************************************** TEST(ExtendedPose3, Derivatives) { const ExtendedPose33 f(kR1, kX1); - const ExtendedPose3d d = MakeDynamic(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); const Vector xi = kXi; Matrix Hf; ExtendedPose33::Expmap(kXi, Hf); - const auto f_exp = [](const Vector12& v) { return ExtendedPose33::Expmap(v); }; - const Matrix Hf_num = numericalDerivative11(f_exp, kXi); + const auto f_exp = [](const Vector12& v) { + return ExtendedPose33::Expmap(v); + }; + const Matrix Hf_num = + numericalDerivative11(f_exp, kXi); EXPECT(assert_equal(Hf_num, Hf, 1e-6)); Matrix Hd; ExtendedPose3d::Expmap(xi, Hd); const auto d_exp = [](const Vector& v) { return ExtendedPose3d::Expmap(v); }; - const Matrix Hd_num = numericalDerivative11(d_exp, xi); + const Matrix Hd_num = + numericalDerivative11(d_exp, xi); EXPECT(assert_equal(Hd_num, Hd, 1e-6)); Matrix Lf; ExtendedPose33::Logmap(f, Lf); - const auto f_log = [](const ExtendedPose33& g) { return Vector12(ExtendedPose33::Logmap(g)); }; + const auto f_log = [](const ExtendedPose33& g) { + return Vector12(ExtendedPose33::Logmap(g)); + }; const Matrix Lf_num = numericalDerivative11(f_log, f); EXPECT(assert_equal(Lf_num, Lf, 1e-6)); Matrix Ld; ExtendedPose3d::Logmap(d, Ld); - const auto d_log = [](const ExtendedPose3d& g) { return ExtendedPose3d::Logmap(g); }; + const auto d_log = [](const ExtendedPose3d& g) { + return ExtendedPose3d::Logmap(g); + }; const Matrix Ld_num = numericalDerivative11(d_log, d); EXPECT(assert_equal(Ld_num, Ld, 1e-6)); @@ -197,19 +202,21 @@ TEST(ExtendedPose3, Derivatives) { //****************************************************************************** TEST(ExtendedPose3, VecJacobian) { const ExtendedPose33 f(kR1, kX1); - const ExtendedPose3d d = MakeDynamic(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); Matrix Hf; const Vector vf = f.vec(Hf); const auto fv = [](const ExtendedPose33& g) { return Vector(g.vec()); }; - const Matrix Hf_num = numericalDerivative11(fv, f); + const Matrix Hf_num = + numericalDerivative11(fv, f); EXPECT(assert_equal(Hf_num, Hf, 1e-6)); EXPECT_LONGS_EQUAL(36, vf.size()); Matrix Hd; const Vector vd = d.vec(Hd); const auto dv = [](const ExtendedPose3d& g) { return Vector(g.vec()); }; - const Matrix Hd_num = numericalDerivative11(dv, d); + const Matrix Hd_num = + numericalDerivative11(dv, d); EXPECT(assert_equal(Hd_num, Hd, 1e-6)); EXPECT_LONGS_EQUAL(36, vd.size()); } From 8627df9038154e6330dd48451fc872316dec3f85 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 13:17:22 -0500 Subject: [PATCH 19/41] Harden Vee --- gtsam/geometry/ExtendedPose3-inl.h | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 208eddcaf8..2354c4955c 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -429,17 +429,34 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( template typename ExtendedPose3::TangentVector ExtendedPose3::Vee(const LieAlgebra& X) { - const Eigen::Index k = X.cols() - 3; + if (X.rows() != X.cols() || X.rows() < 3) { + throw std::invalid_argument("ExtendedPose3::Vee: invalid matrix shape."); + } + + const Eigen::Index k = [&]() -> Eigen::Index { + if constexpr (K == Eigen::Dynamic) { + return X.cols() - 3; + } else { + if (X.rows() != matrix_dim) { + throw std::invalid_argument("ExtendedPose3::Vee: invalid matrix shape."); + } + return static_cast(K); + } + }(); + TangentVector xi; if constexpr (dimension == Eigen::Dynamic) { xi.resize(3 + 3 * k); + xi.setZero(); + } else { + xi.setZero(); } xi(0) = X(2, 1); xi(1) = X(0, 2); xi(2) = X(1, 0); for (Eigen::Index i = 0; i < k; ++i) { const Eigen::Index idx = 3 + 3 * i; - xi.template segment<3>(idx) = X.template block<3, 1>(0, idx); + xi.template segment<3>(idx) = X.template block<3, 1>(0, 3 + i); } return xi; } From 502f867a183991145e4661962209ec9565dad280 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 13:32:17 -0500 Subject: [PATCH 20/41] Rename to t_ --- gtsam/geometry/ExtendedPose3-inl.h | 38 ++++++++++++++++-------------- gtsam/geometry/ExtendedPose3.h | 8 +++---- gtsam/geometry/Pose3.cpp | 31 ++++++++++-------------- gtsam/geometry/Pose3.h | 3 ++- gtsam/navigation/NavState.cpp | 18 +++++++------- gtsam/navigation/NavState.h | 2 +- 6 files changed, 49 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 2354c4955c..51825b2816 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -46,18 +46,18 @@ void ExtendedPose3::ZeroJacobian(ChartJacobian H, size_t d) { template template ExtendedPose3::ExtendedPose3() - : R_(Rot3::Identity()), x_(Matrix3K::Zero()) {} + : R_(Rot3::Identity()), t_(Matrix3K::Zero()) {} template template ExtendedPose3::ExtendedPose3(size_t k) - : R_(Rot3::Identity()), x_(3, k) { - x_.setZero(); + : R_(Rot3::Identity()), t_(3, k) { + t_.setZero(); } template ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) - : R_(R), x_(x) {} + : R_(R), t_(x) {} template ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { @@ -67,8 +67,8 @@ ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); } const auto k = n - 3; - x_.resize(3, k); - x_.setZero(); + t_.resize(3, k); + t_.setZero(); } const auto n = T.rows(); @@ -83,7 +83,7 @@ ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { } R_ = Rot3(T.template block<3, 3>(0, 0)); - x_ = T.block(0, 3, 3, n - 3); + t_ = T.block(0, 3, 3, n - 3); } template @@ -111,19 +111,19 @@ Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { const Eigen::Index idx = 3 + 3 * static_cast(i); H->block(0, idx, 3, 3) = R_.matrix(); } - return x_.col(static_cast(i)); + return t_.col(static_cast(i)); } template const typename ExtendedPose3::Matrix3K& ExtendedPose3::xMatrix() const { - return x_; + return t_; } template typename ExtendedPose3::Matrix3K& ExtendedPose3::xMatrix() { - return x_; + return t_; } template @@ -134,7 +134,7 @@ void ExtendedPose3::print(const std::string& s) const { template bool ExtendedPose3::equals(const ExtendedPose3& other, double tol) const { - return R_.equals(other.R_, tol) && equal_with_abs_tol(x_, other.x_, tol); + return R_.equals(other.R_, tol) && equal_with_abs_tol(t_, other.t_, tol); } template @@ -154,7 +154,7 @@ template typename ExtendedPose3::This ExtendedPose3::inverse() const { const Rot3 Rt = R_.inverse(); - const Matrix3K x = -(Rt.matrix() * x_); + const Matrix3K x = -(Rt.matrix() * t_); return MakeReturn(ExtendedPose3(Rt, x)); } @@ -165,7 +165,7 @@ typename ExtendedPose3::This ExtendedPose3::operator*( if (k() != otherBase.k()) { throw std::invalid_argument("ExtendedPose3: compose requires matching k."); } - Matrix3K x = x_ + R_.matrix() * otherBase.x_; + Matrix3K x = t_ + R_.matrix() * otherBase.t_; return MakeReturn(ExtendedPose3(R_ * otherBase.R_, x)); } @@ -185,7 +185,8 @@ typename ExtendedPose3::This ExtendedPose3::Expmap( // Compute rotation using Expmap #ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(w); + // Reuse any quaternion-specific validation inside Rot3::Expmap. + const Rot3 R = Rot3::Expmap(w); #else const Rot3 R(local.expmap()); #endif @@ -245,7 +246,7 @@ ExtendedPose3::Logmap(const This& pose, ChartJacobian H) { for (size_t i = 0; i < poseBase.k(); ++i) { const Eigen::Index idx = 3 + 3 * static_cast(i); xi.template segment<3>(idx) = local.InvJacobian().applyLeft( - poseBase.x_.col(static_cast(i))); + poseBase.t_.col(static_cast(i))); } if (H) *H = LogmapDerivative(xi); @@ -267,7 +268,7 @@ ExtendedPose3::AdjointMap() const { for (size_t i = 0; i < k(); ++i) { const Eigen::Index idx = 3 + 3 * static_cast(i); adj.block(idx, 0, 3, 3) = - skewSymmetric(x_.col(static_cast(i))) * R; + skewSymmetric(t_.col(static_cast(i))) * R; adj.block(idx, idx, 3, 3) = R; } return adj; @@ -404,7 +405,7 @@ ExtendedPose3::matrix() const { M = LieAlgebra::Identity(); } M.template block<3, 3>(0, 0) = R_.matrix(); - M.block(0, 3, 3, static_cast(k())) = x_; + M.block(0, 3, 3, static_cast(k())) = t_; return M; } @@ -438,7 +439,8 @@ ExtendedPose3::Vee(const LieAlgebra& X) { return X.cols() - 3; } else { if (X.rows() != matrix_dim) { - throw std::invalid_argument("ExtendedPose3::Vee: invalid matrix shape."); + throw std::invalid_argument( + "ExtendedPose3::Vee: invalid matrix shape."); } return static_cast(K); } diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index 2ab491625f..6a110dded9 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -75,7 +75,7 @@ class GTSAM_EXPORT ExtendedPose3 protected: Rot3 R_; ///< Rotation component. - Matrix3K x_; ///< K translation-like columns in world frame. + Matrix3K t_; ///< K translation-like columns in world frame. template using IsDynamic = typename std::enable_if::type; @@ -140,7 +140,7 @@ class GTSAM_EXPORT ExtendedPose3 static size_t Dimension(size_t k) { return 3 + 3 * k; } /** @return Number of R^3 blocks, k. */ - size_t k() const { return static_cast(x_.cols()); } + size_t k() const { return static_cast(t_.cols()); } /** @return Runtime manifold dimension, 3+3k. */ size_t dim() const { return Dimension(k()); } @@ -370,7 +370,7 @@ class GTSAM_EXPORT ExtendedPose3 friend std::ostream& operator<<(std::ostream& os, const ExtendedPose3& p) { os << "R: " << p.R_ << "\n"; - os << "x: " << p.x_; + os << "x: " << p.t_; return os; } @@ -400,7 +400,7 @@ class GTSAM_EXPORT ExtendedPose3 template void serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(R_); - ar& BOOST_SERIALIZATION_NVP(x_); + ar& BOOST_SERIALIZATION_NVP(t_); } #endif }; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1f9053c59b..db22342729 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -104,7 +104,7 @@ void Pose3::print(const std::string& s) const { /* ************************************************************************* */ bool Pose3::equals(const Pose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && traits::Equals(x_, pose.x_, tol); + return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); } /* ************************************************************************* */ @@ -119,19 +119,17 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t, typename MakeJacobian::type HtPoint; Rot3 Rint = interpolate(R_, T.R_, t, HselfRot, HargRot, HtRot); - Point3 Pint = - interpolate(x_, T.x_, t, HselfPoint, HargPoint, HtPoint); + Point3 Pint = interpolate(t_, T.t_, t, HselfPoint, HargPoint, HtPoint); Pose3 result = Pose3(Rint, Pint); if(Hself) *Hself << HselfRot, Z_3x3, Z_3x3, Rint.transpose() * R_.matrix() * HselfPoint; - if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, - Rint.transpose() * T.R_.matrix() * HargPoint; + if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, Rint.transpose() * T.R_.matrix() * HargPoint; if(Ht) *Ht << HtRot, Rint.transpose() * HtPoint; return result; } return Pose3(interpolate(R_, T.R_, t), - interpolate(x_, T.x_, t)); + interpolate(t_, T.t_, t)); } /* ************************************************************************* */ @@ -168,11 +166,8 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { - if (Hself) { - Hself->setZero(); - Hself->block<3, 3>(0, 3) = R_.matrix(); - } - return x_; + if (Hself) *Hself << Z_3x3, rotation().matrix(); + return t_; } /* ************************************************************************* */ @@ -204,7 +199,7 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, if (Hpoint) { *Hpoint = R; } - return R_ * point + x_; + return R_ * point + t_; } Matrix Pose3::transformFrom(const Matrix& points) const { @@ -212,7 +207,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); } const Matrix3 R = R_.matrix(); - return (R * points).colwise() + x_; // Eigen broadcasting! + return (R * points).colwise() + t_; // Eigen broadcasting! } /* ************************************************************************* */ @@ -221,7 +216,7 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt * (point - x_)); + const Point3 q(Rt*(point - t_)); if (Hself) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Hself) << @@ -240,13 +235,13 @@ Matrix Pose3::transformTo(const Matrix& points) const { throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); } const Matrix3 Rt = R_.transpose(); - return Rt * (points.colwise() - x_); // Eigen broadcasting! + return Rt * (points.colwise() - t_); // Eigen broadcasting! } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 3> Hpoint) const { - const Vector3 delta = point - x_; + const Vector3 delta = point - t_; if (!Hself && !Hpoint) return delta.norm(); Matrix13 D_r_point; @@ -265,7 +260,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, /* ************************************************************************* */ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 6> Hpose) const { - const Vector3 delta = pose.x_ - x_; + const Vector3 delta = pose.t_ - t_; if (!Hself && !Hpose) return delta.norm(); Matrix13 D_r_point; @@ -291,7 +286,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself, OptionalJacobian<2, 3> Hpoint) const { const Matrix3 Rt = R_.transpose(); - const Point3 local(Rt * (point - x_)); + const Point3 local(Rt * (point - t_)); if (!Hself && !Hpoint) return Unit3(local); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 7b1544c443..02238ea4c2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -320,7 +320,8 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); } #endif /// @} diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index caffd492dc..165b0f022e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -64,7 +64,7 @@ const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { //------------------------------------------------------------------------------ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { const Rot3& nRb = R_; - const Vector3 n_v = x_.col(1); + const Vector3 n_v = t_.col(1); Matrix3 D_bv_nRb; Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0); if (H) @@ -75,7 +75,7 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { //------------------------------------------------------------------------------ double NavState::range(const Point3& point, OptionalJacobian<1, 9> Hself, OptionalJacobian<1, 3> Hpoint) const { - const Vector3 delta = point - x_.col(0); + const Vector3 delta = point - t_.col(0); const double r = delta.norm(); if (!Hself && !Hpoint) return r; @@ -121,15 +121,15 @@ void NavState::print(const std::string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { return R_.equals(other.R_, tol) && - traits::Equals(x_.col(0), other.x_.col(0), tol) && - equal_with_abs_tol(x_.col(1), other.x_.col(1), tol); + traits::Equals(t_.col(0), other.t_.col(0), tol) && + equal_with_abs_tol(t_.col(1), other.t_.col(1), tol); } //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { Rot3 nRb = R_; - Point3 n_t = x_.col(0), n_v = x_.col(1); + Point3 n_t = t_.col(0), n_v = t_.col(1); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); @@ -157,8 +157,8 @@ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - const Point3 dP = R_.unrotate(g.x_.col(0) - x_.col(0), H1 ? &D_dt_R : 0); - const Vector dV = R_.unrotate(g.x_.col(1) - x_.col(1), H1 ? &D_dv_R : 0); + const Point3 dP = R_.unrotate(g.t_.col(0) - t_.col(0), H1 ? &D_dt_R : 0); + const Vector dV = R_.unrotate(g.t_.col(1) - t_.col(1), H1 ? &D_dv_R : 0); Vector9 xi; Matrix3 D_xi_R; @@ -244,7 +244,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { Rot3 nRb = R_; - Point3 n_t = x_.col(0), n_v = x_.col(1); + Point3 n_t = t_.col(0), n_v = t_.col(1); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); @@ -296,7 +296,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; - const Velocity3 n_v = x_.col(1); // derivative is Ri ! + const Velocity3 n_v = t_.col(1); // derivative is Ri ! const double dt22 = 0.5 * dt * dt; Vector9 xi; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index ea9a621c03..5824e6e486 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -126,7 +126,7 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { } /// Return position as Vector3 Vector3 t() const { - return x_.col(0); + return t_.col(0); } /// Return velocity as Vector3. Computation-free. const Vector3& v() const { From 605067d8c41954d3427991d6964a94a636a0325d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 13:46:08 -0500 Subject: [PATCH 21/41] Remove cache hack --- gtsam/navigation/NavState.cpp | 10 ++++------ gtsam/navigation/NavState.h | 11 ++++------- gtsam/navigation/navigation.i | 9 +++++---- 3 files changed, 13 insertions(+), 17 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 165b0f022e..1832a920f6 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -50,15 +50,13 @@ const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -const Point3& NavState::position(OptionalJacobian<3, 9> H) const { - p_cache_ = Base::x(0, H); - return p_cache_; +Point3 NavState::position(OptionalJacobian<3, 9> H) const { + return Base::x(0, H); } //------------------------------------------------------------------------------ -const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { - v_cache_ = Base::x(1, H); - return v_cache_; +Vector3 NavState::velocity(OptionalJacobian<3, 9> H) const { + return Base::x(1, H); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 5824e6e486..3129964f94 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -89,8 +89,8 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { /// @{ const Rot3& attitude(OptionalJacobian<3, 9> H = {}) const; - const Point3& position(OptionalJacobian<3, 9> H = {}) const; - const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const; + Point3 position(OptionalJacobian<3, 9> H = {}) const; + Velocity3 velocity(OptionalJacobian<3, 9> H = {}) const; const Pose3 pose() const { return Pose3(attitude(), position()); @@ -128,8 +128,8 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { Vector3 t() const { return t_.col(0); } - /// Return velocity as Vector3. Computation-free. - const Vector3& v() const { + /// Return velocity as Vector3. + Vector3 v() const { return velocity(); } // Return velocity in body frame @@ -234,9 +234,6 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { /// @} private: - mutable Point3 p_cache_ = Point3::Zero(); - mutable Velocity3 v_cache_ = Velocity3::Zero(); - /// @{ /// serialization #if GTSAM_ENABLE_BOOST_SERIALIZATION /// diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 665abb0842..edbe8bc822 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -45,8 +45,9 @@ class ConstantBias { class NavState { // Constructors NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, gtsam::Vector v); - NavState(const gtsam::Pose3& pose, gtsam::Vector v); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, + const gtsam::Vector3& v); + NavState(const gtsam::Pose3& pose, const gtsam::Vector3& v); // Testable void print(string s = "") const; @@ -55,8 +56,8 @@ class NavState { // Access gtsam::Rot3 attitude() const; gtsam::Point3 position() const; - gtsam::Vector velocity() const; - gtsam::Vector bodyVelocity() const; + gtsam::Vector3 velocity() const; + gtsam::Vector3 bodyVelocity() const; gtsam::Pose3 pose() const; // Standard Interface From 787f8e99b188ae75fde6f460a8970b54fcc731b6 Mon Sep 17 00:00:00 2001 From: Tzvi Strauss Date: Sat, 31 Jan 2026 16:56:00 +0200 Subject: [PATCH 22/41] Refactor rowCounts out of Multifrontal SymbolicJunctionTree. --- gtsam/linear/MultifrontalClique.h | 4 ++-- gtsam/linear/MultifrontalSolver.cpp | 16 ++++++++-------- gtsam/linear/MultifrontalSolver.h | 1 + gtsam/nonlinear/NonlinearMultifrontalSolver.cpp | 12 ++++++++---- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/MultifrontalClique.h b/gtsam/linear/MultifrontalClique.h index dc4f6f7963..012c2dc67e 100644 --- a/gtsam/linear/MultifrontalClique.h +++ b/gtsam/linear/MultifrontalClique.h @@ -51,8 +51,8 @@ class IndexedSymbolicFactor : public SymbolicFactor { public: size_t index_; size_t rows_; - IndexedSymbolicFactor(const KeyVector& keys, size_t index, size_t rows) - : SymbolicFactor(), index_(index), rows_(rows) { + IndexedSymbolicFactor(const KeyVector& keys, size_t index) + : SymbolicFactor(), index_(index) { keys_ = keys; } }; diff --git a/gtsam/linear/MultifrontalSolver.cpp b/gtsam/linear/MultifrontalSolver.cpp index a273e134c3..6a3c48ac61 100644 --- a/gtsam/linear/MultifrontalSolver.cpp +++ b/gtsam/linear/MultifrontalSolver.cpp @@ -134,8 +134,7 @@ PrecomputeScratch precomputeFromGraph(const GaussianFactorGraph& graph) { // Build SymbolicFactorGraph from GaussianFactorGraph SymbolicFactorGraph buildSymbolicGraph( const GaussianFactorGraph& graph, - const std::unordered_set& fixedKeys, - const std::vector& rowCounts) { + const std::unordered_set& fixedKeys) { SymbolicFactorGraph symbolicGraph; symbolicGraph.reserve(graph.size()); for (size_t i = 0; i < graph.size(); ++i) { @@ -149,8 +148,7 @@ SymbolicFactorGraph buildSymbolicGraph( } // Skip factors that are fully constrained away. if (keys.empty()) continue; - symbolicGraph.emplace_shared( - keys, i, rowCounts.at(i)); + symbolicGraph.emplace_shared(keys, i); } return symbolicGraph; } @@ -455,6 +453,7 @@ struct CliqueBuilder { std::vector* cliques; const std::unordered_set* fixedKeys; const MultifrontalParameters* params; + const std::vector* rowCounts; SymbolicJunctionTree::Cluster::KeySetMap separatorCache = {}; BuiltClique build(const SymbolicJunctionTree::sharedNode& cluster, @@ -472,7 +471,7 @@ struct CliqueBuilder { auto indexed = std::static_pointer_cast(factor); factorIndices.push_back(indexed->index_); - vbmRows += indexed->rows_; + vbmRows += rowCounts->at(indexed->index_); } // Create the clique node and cache static structure. @@ -557,7 +556,8 @@ MultifrontalSolver::MultifrontalSolver(PrecomputedData data, } // Build the actual MultifrontalClique structure. - CliqueBuilder builder{dims_, &solution_, &cliques_, &fixedKeys_, ¶ms_}; + CliqueBuilder builder{dims_, &solution_, &cliques_, &fixedKeys_, ¶ms_, + &data.rowCounts}; for (const auto& rootCluster : data.junctionTree.roots()) { if (rootCluster) { roots_.push_back( @@ -583,13 +583,13 @@ MultifrontalSolver::PrecomputedData MultifrontalSolver::Precompute( } SymbolicFactorGraph symbolicGraph = - buildSymbolicGraph(graph, scratch.fixedKeys, scratch.rowCounts); + buildSymbolicGraph(graph, scratch.fixedKeys); SymbolicEliminationTree eliminationTree(symbolicGraph, reducedOrdering); SymbolicJunctionTree junctionTree(eliminationTree); return MultifrontalSolver::PrecomputedData{ std::move(scratch.dims), std::move(scratch.fixedKeys), - std::move(junctionTree)}; + std::move(junctionTree), std::move(scratch.rowCounts)}; } /* ************************************************************************* */ diff --git a/gtsam/linear/MultifrontalSolver.h b/gtsam/linear/MultifrontalSolver.h index 955a5a6e66..77591d459a 100644 --- a/gtsam/linear/MultifrontalSolver.h +++ b/gtsam/linear/MultifrontalSolver.h @@ -86,6 +86,7 @@ class GTSAM_EXPORT MultifrontalSolver std::map dims; ///< Map from variable key to dimension. std::unordered_set fixedKeys; ///< Keys fixed by constrained factors. SymbolicJunctionTree junctionTree; ///< Precomputed symbolic junction tree. + std::vector rowCounts; ///< Row counts indexed by factor index. }; /// Shared pointer to a MultifrontalClique. diff --git a/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp b/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp index 493e991607..930fb52b25 100644 --- a/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp +++ b/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp @@ -65,9 +65,7 @@ SymbolicFactorGraph buildSymbolicGraph( } } if (keys.empty()) continue; - const size_t rows = graph[i]->dim(); - symbolicGraph.emplace_shared(keys, i, - rows); + symbolicGraph.emplace_shared(keys, i); } return symbolicGraph; } @@ -102,9 +100,15 @@ MultifrontalSolver::PrecomputedData NonlinearMultifrontalSolver::Precompute( SymbolicFactorGraph symbolicGraph = buildSymbolicGraph(graph, fixedKeys); SymbolicEliminationTree eliminationTree(symbolicGraph, reducedOrdering); SymbolicJunctionTree junctionTree(eliminationTree); + std::vector rowCounts; + rowCounts.reserve(graph.size()); + for (const auto& factor : graph) { + size_t dim = factor ? factor->dim() : 0; + rowCounts.push_back(dim); + } return MultifrontalSolver::PrecomputedData{ - std::move(dims), std::move(fixedKeys), std::move(junctionTree)}; + std::move(dims), std::move(fixedKeys), std::move(junctionTree), std::move(rowCounts)}; } /* ************************************************************************* */ From ef72cbbc5db904169a567d1b8bc4ee176bfb285c Mon Sep 17 00:00:00 2001 From: Tzvi Strauss Date: Sun, 1 Feb 2026 23:08:20 +0200 Subject: [PATCH 23/41] Optimize multifrontal elimination by caching JunctionTree. Refactor IndexedSymbolicFactor into shared IndexedJunctionTreeBuilder.h, enabling efficient reuse of junction tree structure across multiple nonlinear optimization iterations. Key changes: - Add IndexedJunctionTreeBuilder.h with buildIndexedJunctionTree() template - Add GaussianFactorGraph::optimize() overload using cached junction tree - Cache SymbolicJunctionTree in NonlinearOptimizer for multifrontal solve - Refactor MultifrontalSolver and NonlinearMultifrontalSolver to use shared builder functions Performance improvement: reduces runtime from ~120s to ~90s (~25% faster) in specific test cases by avoiding repeated elimination tree construction. --- .../inference/EliminateableFactorGraph-inst.h | 110 ++++++++++++++++++ gtsam/inference/EliminateableFactorGraph.h | 36 +++++- gtsam/linear/MultifrontalClique.h | 11 -- gtsam/linear/MultifrontalSolver.cpp | 45 ++----- gtsam/linear/MultifrontalSolver.h | 25 ++-- .../linear/tests/testGaussianFactorGraph.cpp | 17 +++ .../nonlinear/NonlinearMultifrontalSolver.cpp | 30 +---- gtsam/nonlinear/NonlinearOptimizer.cpp | 14 ++- gtsam/nonlinear/NonlinearOptimizer.h | 9 ++ gtsam/symbolic/IndexedJunctionTree.h | 106 +++++++++++++++++ .../symbolic/tests/testSymbolicBayesTree.cpp | 13 ++- 11 files changed, 331 insertions(+), 85 deletions(-) create mode 100644 gtsam/symbolic/IndexedJunctionTree.h diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index f6b11f785c..2ce7d03887 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -20,6 +20,10 @@ #include #include +#include +#include + +#include namespace gtsam { @@ -145,6 +149,112 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + IndexedJunctionTree + EliminateableFactorGraph::buildIndexedJunctionTree( + const Ordering& ordering, + const std::unordered_set& fixedKeys) const { + return IndexedJunctionTree(asDerived(), ordering, fixedKeys); + } + + /* ************************************************************************* */ + template + std::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const IndexedJunctionTree& indexedJunctionTree, + const Eliminate& function) const { + gttic(eliminateMultifrontal); + + using BayesTreeNode = typename BayesTreeType::Node; + using SharedFactor = typename FactorGraphType::sharedFactor; + + // Helper struct for cluster elimination traversal. + struct ClusterEliminationData { + ClusterEliminationData* const parentData; + size_t myIndexInParent; + FastVector childFactors; + std::shared_ptr bayesTreeNode; + + explicit ClusterEliminationData(ClusterEliminationData* p) + : parentData(p), bayesTreeNode(std::make_shared()) { + if (parentData) { + myIndexInParent = parentData->childFactors.size(); + parentData->childFactors.push_back(SharedFactor()); + if (parentData->parentData) + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } else { + myIndexInParent = 0; + } + } + }; + + // Post-order visitor functor for elimination. + class EliminationPostOrderVisitor { + const FactorGraphType& graph_; + const Eliminate& function_; + + public: + EliminationPostOrderVisitor(const FactorGraphType& graph, + const Eliminate& function) + : graph_(graph), function_(function) {} + + void operator()(const SymbolicJunctionTree::sharedNode& cluster, + ClusterEliminationData& myData) { + FactorGraphType gatheredFactors; + gatheredFactors.reserve(cluster->factors.size() + myData.childFactors.size()); + + // Gather factors from IndexedSymbolicFactor. + for (const auto& factor : cluster->factors) { + auto indexed = + std::static_pointer_cast(factor); + gatheredFactors.push_back(graph_.at(indexed->index_)); + } + + for (const auto& childFactor : myData.childFactors) + if (childFactor) gatheredFactors.push_back(childFactor); + + auto result = function_(gatheredFactors, cluster->orderedFrontalKeys); + myData.bayesTreeNode->setEliminationResult(result); + + if (myData.parentData && !result.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = result.second; + } + }; + + // Run depth-first traversal. + ClusterEliminationData rootsContainer(nullptr); + EliminationPostOrderVisitor visitorPost(asDerived(), function); + + auto visitorPre = [](const SymbolicJunctionTree::sharedNode& node, + ClusterEliminationData& parentData) { + ClusterEliminationData myData(&parentData); + myData.bayesTreeNode->problemSize_ = node->problemSize_; + return myData; + }; + + treeTraversal::DepthFirstForest(indexedJunctionTree, rootsContainer, visitorPre, + visitorPost); + + // If any factors are remaining, the ordering was incomplete. + KeySet remainingKeys; + for (const auto& factor : rootsContainer.childFactors) { + if (!factor || factor->empty()) continue; + remainingKeys.insert(factor->begin(), factor->end()); + } + if (!remainingKeys.empty()) { + throw InconsistentEliminationRequested(remainingKeys); + } + + // Create BayesTree from roots stored in the dummy BayesTree node. + auto bayesTree = std::make_shared(); + for (const auto& rootClique : rootsContainer.bayesTreeNode->children) + bayesTree->insertRoot(rootClique); + + return bayesTree; + } + /* ************************************************************************* */ template std::pair::BayesNetType>, std::shared_ptr > diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 52e1479a9c..4d3a19e882 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -19,14 +19,17 @@ #pragma once #include -#include #include #include +#include #include #include namespace gtsam { + // Forward declaration + class IndexedJunctionTree; + /// Traits class for eliminateable factor graphs, specifies the types that result from /// elimination, etc. This must be defined for each factor graph that inherits from /// EliminateableFactorGraph. @@ -94,6 +97,20 @@ namespace gtsam { /// Typedef for an optional ordering type typedef std::optional OptionalOrderingType; + /** + * Build an `IndexedJunctionTree` for this factor graph and a fixed ordering. + * + * This structure can be cached and reused for repeated eliminations when the + * factor graph structure and ordering are unchanged. + * + * @param ordering The elimination ordering + * @param fixedKeys Optional set of keys to filter out (e.g., from hard constraints) + * @return An IndexedJunctionTree that can be reused for elimination + */ + IndexedJunctionTree buildIndexedJunctionTree( + const Ordering& ordering, + const std::unordered_set& fixedKeys = {}) const; + /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. * @@ -173,6 +190,23 @@ namespace gtsam { const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; + /** + * Do multifrontal elimination using a pre-built `IndexedJunctionTree`. + * + * This eliminates the factor graph following the cluster structure encoded in + * the indexed junction tree and calls the provided dense elimination function + * on each cluster. The indexed junction tree must have been built from a + * factor graph with the same factor ordering/indices and the same variable + * ordering. + * + * @param indexedJunctionTree Pre-built indexed junction tree + * @param function The elimination function to use for each cluster + * @return A Bayes tree containing the elimination results + */ + std::shared_ptr eliminateMultifrontal( + const IndexedJunctionTree& indexedJunctionTree, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ diff --git a/gtsam/linear/MultifrontalClique.h b/gtsam/linear/MultifrontalClique.h index 012c2dc67e..42afe3484a 100644 --- a/gtsam/linear/MultifrontalClique.h +++ b/gtsam/linear/MultifrontalClique.h @@ -46,17 +46,6 @@ using KeyDimMap = std::map; namespace internal { -/// Helper class to track original factor indices and row counts. -class IndexedSymbolicFactor : public SymbolicFactor { - public: - size_t index_; - size_t rows_; - IndexedSymbolicFactor(const KeyVector& keys, size_t index) - : SymbolicFactor(), index_(index) { - keys_ = keys; - } -}; - /// Sum variable dimensions for a key range, skipping unknown keys. template inline size_t sumDims(const KeyDimMap& dims, const KeyRange& keys) { diff --git a/gtsam/linear/MultifrontalSolver.cpp b/gtsam/linear/MultifrontalSolver.cpp index 6a3c48ac61..c327ea80cc 100644 --- a/gtsam/linear/MultifrontalSolver.cpp +++ b/gtsam/linear/MultifrontalSolver.cpp @@ -26,8 +26,7 @@ #include #include #include -#include -#include +#include #include #include @@ -131,28 +130,6 @@ PrecomputeScratch precomputeFromGraph(const GaussianFactorGraph& graph) { return out; } -// Build SymbolicFactorGraph from GaussianFactorGraph -SymbolicFactorGraph buildSymbolicGraph( - const GaussianFactorGraph& graph, - const std::unordered_set& fixedKeys) { - SymbolicFactorGraph symbolicGraph; - symbolicGraph.reserve(graph.size()); - for (size_t i = 0; i < graph.size(); ++i) { - if (!graph[i]) continue; - KeyVector keys; - keys.reserve(graph[i]->size()); - for (Key key : graph[i]->keys()) { - if (!fixedKeys.count(key)) { - keys.push_back(key); - } - } - // Skip factors that are fully constrained away. - if (keys.empty()) continue; - symbolicGraph.emplace_shared(keys, i); - } - return symbolicGraph; -} - // Sum the dimensions of frontal variables in a symbolic cluster. size_t frontalDimForSymbolicCluster( const SymbolicJunctionTree::sharedNode& cluster, @@ -534,31 +511,31 @@ MultifrontalSolver::MultifrontalSolver(PrecomputedData data, } // Report the symbolic structure before any merge. - reportStructure(data.junctionTree, dims_, "Symbolic cluster structure", + reportStructure(data.indexedJunctionTree, dims_, "Symbolic cluster structure", params_.reportStream); // If applicable, merge leaf children by a separate cap first. if (params_.leafMergeDimCap > 0) { - for (const auto& rootCluster : data.junctionTree.roots()) { + for (const auto& rootCluster : data.indexedJunctionTree.roots()) { mergeLeafChildren(rootCluster, dims_, params_.leafMergeDimCap); } - reportStructure(data.junctionTree, dims_, + reportStructure(data.indexedJunctionTree, dims_, "Clique structure after leaf merge", params_.reportStream); } // If applicable, merge small child cliques bottom-up. if (params_.mergeDimCap > 0) { - for (const auto& rootCluster : data.junctionTree.roots()) { + for (const auto& rootCluster : data.indexedJunctionTree.roots()) { mergeSmallClusters(rootCluster, dims_, params_.mergeDimCap); } - reportStructure(data.junctionTree, dims_, "Clique structure after merge", + reportStructure(data.indexedJunctionTree, dims_, "Clique structure after merge", params_.reportStream); } // Build the actual MultifrontalClique structure. CliqueBuilder builder{dims_, &solution_, &cliques_, &fixedKeys_, ¶ms_, &data.rowCounts}; - for (const auto& rootCluster : data.junctionTree.roots()) { + for (const auto& rootCluster : data.indexedJunctionTree.roots()) { if (rootCluster) { roots_.push_back( builder.build(rootCluster, std::weak_ptr()) @@ -582,14 +559,12 @@ MultifrontalSolver::PrecomputedData MultifrontalSolver::Precompute( } } - SymbolicFactorGraph symbolicGraph = - buildSymbolicGraph(graph, scratch.fixedKeys); - SymbolicEliminationTree eliminationTree(symbolicGraph, reducedOrdering); - SymbolicJunctionTree junctionTree(eliminationTree); + IndexedJunctionTree indexedJunctionTree = + graph.buildIndexedJunctionTree(reducedOrdering, scratch.fixedKeys); return MultifrontalSolver::PrecomputedData{ std::move(scratch.dims), std::move(scratch.fixedKeys), - std::move(junctionTree), std::move(scratch.rowCounts)}; + std::move(indexedJunctionTree), std::move(scratch.rowCounts)}; } /* ************************************************************************* */ diff --git a/gtsam/linear/MultifrontalSolver.h b/gtsam/linear/MultifrontalSolver.h index 77591d459a..a00bb7c17d 100644 --- a/gtsam/linear/MultifrontalSolver.h +++ b/gtsam/linear/MultifrontalSolver.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -82,11 +82,12 @@ class GTSAM_EXPORT MultifrontalSolver /// Tuning parameters for traversal and reporting. using Parameters = MultifrontalParameters; + /// Precomputed symbolic and sizing data for multifrontal solver construction. struct PrecomputedData { - std::map dims; ///< Map from variable key to dimension. - std::unordered_set fixedKeys; ///< Keys fixed by constrained factors. - SymbolicJunctionTree junctionTree; ///< Precomputed symbolic junction tree. - std::vector rowCounts; ///< Row counts indexed by factor index. + std::map dims; ///< Map from variable key to dimension. + std::unordered_set fixedKeys; ///< Keys fixed by constrained factors. + IndexedJunctionTree indexedJunctionTree; ///< Precomputed indexed junction tree. + std::vector rowCounts; ///< Row counts indexed by factor index. }; /// Shared pointer to a MultifrontalClique. @@ -110,7 +111,7 @@ class GTSAM_EXPORT MultifrontalSolver public: /** * Construct the solver from a factor graph and an ordering. - * This builds the symbolic junction tree and pre-allocates all matrices. + * This builds the indexed junction tree and pre-allocates all matrices. * Call load() before eliminating to populate numerical values. * @param graph The factor graph to solve. * Must contain only JacobianFactor instances. @@ -130,8 +131,16 @@ class GTSAM_EXPORT MultifrontalSolver MultifrontalSolver(PrecomputedData data, const Ordering& ordering, const Parameters& params = Parameters{}); - /// Precompute symbolic structure and sizing data from a factor graph. - /// Only JacobianFactor inputs are supported. + /** + * Precompute symbolic structure and sizing data from a factor graph. + * This builds an IndexedJunctionTree that can be reused across multiple + * solver instances when the graph structure and ordering remain unchanged. + * Only JacobianFactor inputs are supported. + * + * @param graph The factor graph (must contain only JacobianFactor instances) + * @param ordering The variable elimination ordering + * @return PrecomputedData containing the indexed junction tree and sizing info + */ static PrecomputedData Precompute(const GaussianFactorGraph& graph, const Ordering& ordering); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index b6b804b6c2..da43d97944 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -21,9 +21,12 @@ #include #include #include +#include #include +#include #include #include +#include #include #include @@ -457,6 +460,20 @@ TEST(GaussianFactorGraph, DenseSolve) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianFactorGraph, optimizeWithIndexedJunctionTree) { + GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); + Ordering ordering{0, 1, 2}; // x2=0, l1=1, x1=2 + + IndexedJunctionTree indexedJunctionTree = fg.buildIndexedJunctionTree(ordering); + + VectorValues expected = fg.optimize(ordering, EliminateQR); + VectorValues actual = + fg.eliminateMultifrontal(indexedJunctionTree, EliminateQR)->optimize(); + + EXPECT(assert_equal(expected, actual, 1e-9)); +} + /* ************************************************************************* */ TEST(GaussianFactorGraph, ProbPrime) { GaussianFactorGraph gfg; diff --git a/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp b/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp index 930fb52b25..9063bafa84 100644 --- a/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp +++ b/gtsam/nonlinear/NonlinearMultifrontalSolver.cpp @@ -21,8 +21,7 @@ #include #include #include -#include -#include +#include #include namespace gtsam { @@ -50,26 +49,6 @@ std::unordered_set collectFixedKeys(const NonlinearFactorGraph& graph) { return fixedKeys; } -SymbolicFactorGraph buildSymbolicGraph( - const NonlinearFactorGraph& graph, - const std::unordered_set& fixedKeys) { - SymbolicFactorGraph symbolicGraph; - symbolicGraph.reserve(graph.size()); - for (size_t i = 0; i < graph.size(); ++i) { - if (!graph[i]) continue; - KeyVector keys; - keys.reserve(graph[i]->size()); - for (Key key : graph[i]->keys()) { - if (!fixedKeys.count(key)) { - keys.push_back(key); - } - } - if (keys.empty()) continue; - symbolicGraph.emplace_shared(keys, i); - } - return symbolicGraph; -} - } // namespace /* ************************************************************************* */ @@ -97,9 +76,8 @@ MultifrontalSolver::PrecomputedData NonlinearMultifrontalSolver::Precompute( } } - SymbolicFactorGraph symbolicGraph = buildSymbolicGraph(graph, fixedKeys); - SymbolicEliminationTree eliminationTree(symbolicGraph, reducedOrdering); - SymbolicJunctionTree junctionTree(eliminationTree); + IndexedJunctionTree indexedJunctionTree(graph, reducedOrdering, fixedKeys); + std::vector rowCounts; rowCounts.reserve(graph.size()); for (const auto& factor : graph) { @@ -108,7 +86,7 @@ MultifrontalSolver::PrecomputedData NonlinearMultifrontalSolver::Precompute( } return MultifrontalSolver::PrecomputedData{ - std::move(dims), std::move(fixedKeys), std::move(junctionTree), std::move(rowCounts)}; + std::move(dims), std::move(fixedKeys), std::move(indexedJunctionTree), std::move(rowCounts)}; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 816789d223..b3f7160c4d 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -25,10 +25,11 @@ #include #include #include +#include #include +#include -#include #include #include #include @@ -139,8 +140,15 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - if (params.ordering) - delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + if (params.ordering) { + if (!indexedJunctionTreeCache_.has_value()) { + indexedJunctionTreeCache_ = gfg.buildIndexedJunctionTree(*params.ordering); + } + + delta = gfg.eliminateMultifrontal(*indexedJunctionTreeCache_, + params.getEliminationFunction()) + ->optimize(); + } else delta = gfg.optimize(params.getEliminationFunction()); } else if (params.isSequential()) { diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 2d6f8afe02..0dc2bd4f2d 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -20,6 +20,10 @@ #include #include +#include + +#include +#include namespace gtsam { @@ -86,6 +90,11 @@ class GTSAM_EXPORT NonlinearOptimizer { mutable std::unique_ptr nonlinearMultifrontalSolver_; + private: + /// Cached indexed junction tree used to avoid rebuilding the symbolic structure + /// across iterations when the ordering remains constant. + mutable std::optional indexedJunctionTreeCache_; + public: /** A shared pointer to this class */ using shared_ptr = std::shared_ptr; diff --git a/gtsam/symbolic/IndexedJunctionTree.h b/gtsam/symbolic/IndexedJunctionTree.h new file mode 100644 index 0000000000..201b26f7f3 --- /dev/null +++ b/gtsam/symbolic/IndexedJunctionTree.h @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + * + * 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 IndexedJunctionTree.h + * @brief Build a symbolic junction tree that stores original factor indices. + * @author Tzvi Strauss + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + +namespace internal { +/// A SymbolicFactor that stores the index of the originating factor. +class IndexedSymbolicFactor : public SymbolicFactor { + public: + size_t index_; + IndexedSymbolicFactor(const KeyVector& keys, size_t index) + : SymbolicFactor(), index_(index) { + keys_ = keys; + } +}; +} // namespace internal + +/** + * A symbolic junction tree whose factors record the original factor indices from + * a corresponding (non-symbolic) factor graph. This allows the junction tree + * structure to be cached and reused for repeated eliminations when the factor + * graph structure and ordering remain unchanged, avoiding the cost of rebuilding + * the symbolic structure. + * + * The underlying implementation uses a `SymbolicJunctionTree` where each factor + * is an `IndexedSymbolicFactor` that stores the original factor index. + * During elimination, these indices are used to retrieve the actual numerical + * factors from the original factor graph. + * + */ +class GTSAM_EXPORT IndexedJunctionTree : public SymbolicJunctionTree { + public: + using SymbolicJunctionTree::SymbolicJunctionTree; + + /** + * Construct an IndexedJunctionTree from any factor graph, ordering, and + * optional set of fixed keys to filter out. + * + * @tparam GRAPH Factor graph type (e.g. GaussianFactorGraph, NonlinearFactorGraph) + * @param graph The input factor graph + * @param ordering The elimination ordering + * @param fixedKeys Keys to filter out (e.g., from hard constraints) + */ + template + IndexedJunctionTree(const GRAPH& graph, const Ordering& ordering, + const std::unordered_set& fixedKeys = {}) + : SymbolicJunctionTree(makeIndexedEliminationTree(graph, ordering, fixedKeys)) {} + + private: + template + static SymbolicFactorGraph buildIndexedSymbolicFactorGraph( + const GRAPH& graph, const std::unordered_set& fixedKeys) { + SymbolicFactorGraph symbolicGraph; + symbolicGraph.reserve(graph.size()); + for (size_t i = 0; i < graph.size(); ++i) { + if (!graph.at(i)) continue; + KeyVector keys; + keys.reserve(graph[i]->size()); + for (Key key : graph[i]->keys()) { + if (!fixedKeys.count(key)) keys.push_back(key); + } + // Skip factors that are fully constrained away. + if (keys.empty()) continue; + symbolicGraph.emplace_shared(keys, i); + } + return symbolicGraph; + } + + template + static SymbolicEliminationTree makeIndexedEliminationTree( + const GRAPH& graph, const Ordering& ordering, + const std::unordered_set& fixedKeys) { + SymbolicFactorGraph symbolicGraph = + buildIndexedSymbolicFactorGraph(graph, fixedKeys); + return SymbolicEliminationTree(std::move(symbolicGraph), ordering); + } +}; +} // namespace gtsam + diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 04fe0434be..b5df33ae42 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -20,11 +20,11 @@ #include #include #include +#include #include #include #include -#include #include using namespace std; @@ -96,6 +96,17 @@ TEST(SymbolicBayesTree, clique_structure) { SymbolicBayesTree actual = *graph.eliminateMultifrontal(order); EXPECT(assert_equal(expected, actual)); + + // Reuse an indexed junction tree built from the same graph and ordering. + IndexedJunctionTree indexedJunctionTree = graph.buildIndexedJunctionTree(order); + + SymbolicBayesTree actualReuse1 = + *graph.eliminateMultifrontal(indexedJunctionTree); + SymbolicBayesTree actualReuse2 = + *graph.eliminateMultifrontal(indexedJunctionTree); + + EXPECT(assert_equal(expected, actualReuse1)); + EXPECT(assert_equal(expected, actualReuse2)); } /* ************************************************************************* * From 4c28bb7a51170081c6aa2b2df8c796d04bd7cc33 Mon Sep 17 00:00:00 2001 From: Tzvi Strauss Date: Wed, 18 Feb 2026 13:48:21 +0200 Subject: [PATCH 24/41] Add parallel elimination in EliminateableFactorGraph. Now aligned with ClusterTree imp. --- .../inference/EliminateableFactorGraph-inst.h | 121 ++++++++++++------ 1 file changed, 80 insertions(+), 41 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 2ce7d03887..6605392925 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -23,6 +23,9 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#endif #include namespace gtsam { @@ -169,73 +172,114 @@ namespace gtsam { using BayesTreeNode = typename BayesTreeType::Node; using SharedFactor = typename FactorGraphType::sharedFactor; - // Helper struct for cluster elimination traversal. + // Elimination traversal data - stores a pointer to the parent data and collects + // the factors resulting from elimination of the children. Also sets up BayesTree + // cliques with parent and child pointers. struct ClusterEliminationData { ClusterEliminationData* const parentData; size_t myIndexInParent; FastVector childFactors; std::shared_ptr bayesTreeNode; - - explicit ClusterEliminationData(ClusterEliminationData* p) - : parentData(p), bayesTreeNode(std::make_shared()) { +#ifdef GTSAM_USE_TBB + std::shared_ptr writeLock; +#endif + + ClusterEliminationData(ClusterEliminationData* _parentData, size_t nChildren) + : parentData(_parentData), bayesTreeNode(std::make_shared()) +#ifdef GTSAM_USE_TBB + , writeLock(std::make_shared()) +#endif + { if (parentData) { +#ifdef GTSAM_USE_TBB + parentData->writeLock->lock(); +#endif myIndexInParent = parentData->childFactors.size(); parentData->childFactors.push_back(SharedFactor()); +#ifdef GTSAM_USE_TBB + parentData->writeLock->unlock(); +#endif + } else { + myIndexInParent = 0; + } + if (parentData) { if (parentData->parentData) bayesTreeNode->parent_ = parentData->bayesTreeNode; parentData->bayesTreeNode->children.push_back(bayesTreeNode); - } else { - myIndexInParent = 0; } } + + static ClusterEliminationData EliminationPreOrderVisitor( + const SymbolicJunctionTree::sharedNode& node, + ClusterEliminationData& parentData) { + assert(node); + ClusterEliminationData myData(&parentData, node->nrChildren()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; + } }; - // Post-order visitor functor for elimination. + // Elimination post-order visitor - gather factors, eliminate, store results. class EliminationPostOrderVisitor { const FactorGraphType& graph_; - const Eliminate& function_; + const Eliminate& eliminationFunction_; public: - EliminationPostOrderVisitor(const FactorGraphType& graph, - const Eliminate& function) - : graph_(graph), function_(function) {} + EliminationPostOrderVisitor( + const FactorGraphType& graph, + const Eliminate& eliminationFunction) + : graph_(graph), eliminationFunction_(eliminationFunction) {} - void operator()(const SymbolicJunctionTree::sharedNode& cluster, + void operator()(const SymbolicJunctionTree::sharedNode& node, ClusterEliminationData& myData) { + assert(node); + FactorGraphType gatheredFactors; - gatheredFactors.reserve(cluster->factors.size() + myData.childFactors.size()); + gatheredFactors.reserve(node->factors.size() + node->nrChildren()); - // Gather factors from IndexedSymbolicFactor. - for (const auto& factor : cluster->factors) { + for (const auto& factor : node->factors) { auto indexed = std::static_pointer_cast(factor); gatheredFactors.push_back(graph_.at(indexed->index_)); } - - for (const auto& childFactor : myData.childFactors) - if (childFactor) gatheredFactors.push_back(childFactor); - - auto result = function_(gatheredFactors, cluster->orderedFrontalKeys); - myData.bayesTreeNode->setEliminationResult(result); - - if (myData.parentData && !result.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = result.second; + gatheredFactors.push_back(myData.childFactors); + + auto eliminationResult = + eliminationFunction_(gatheredFactors, node->orderedFrontalKeys); + + myData.bayesTreeNode->setEliminationResult(eliminationResult); + + if (!eliminationResult.second->empty()) { +#ifdef GTSAM_USE_TBB + myData.parentData->writeLock->lock(); +#endif + myData.parentData->childFactors[myData.myIndexInParent] = + eliminationResult.second; +#ifdef GTSAM_USE_TBB + myData.parentData->writeLock->unlock(); +#endif + } } }; - // Run depth-first traversal. - ClusterEliminationData rootsContainer(nullptr); - EliminationPostOrderVisitor visitorPost(asDerived(), function); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' + // BayesTree node that contains all of the roots as its children. rootsContainer + // also stores the remaining un-eliminated factors passed up from the roots. + std::shared_ptr result = std::make_shared(); - auto visitorPre = [](const SymbolicJunctionTree::sharedNode& node, - ClusterEliminationData& parentData) { - ClusterEliminationData myData(&parentData); - myData.bayesTreeNode->problemSize_ = node->problemSize_; - return myData; - }; + ClusterEliminationData rootsContainer(0, indexedJunctionTree.nrRoots()); + + EliminationPostOrderVisitor visitorPost(asDerived(), function); + { + TbbOpenMPMixedScope threadLimiter; + treeTraversal::DepthFirstForestParallel( + indexedJunctionTree, rootsContainer, + ClusterEliminationData::EliminationPreOrderVisitor, visitorPost, 10); + } - treeTraversal::DepthFirstForest(indexedJunctionTree, rootsContainer, visitorPre, - visitorPost); + // Create BayesTree from roots stored in the dummy BayesTree node. + for (const auto& rootClique : rootsContainer.bayesTreeNode->children) + result->insertRoot(rootClique); // If any factors are remaining, the ordering was incomplete. KeySet remainingKeys; @@ -247,12 +291,7 @@ namespace gtsam { throw InconsistentEliminationRequested(remainingKeys); } - // Create BayesTree from roots stored in the dummy BayesTree node. - auto bayesTree = std::make_shared(); - for (const auto& rootClique : rootsContainer.bayesTreeNode->children) - bayesTree->insertRoot(rootClique); - - return bayesTree; + return result; } /* ************************************************************************* */ From ed1366161da5becce77c5c3494f1db2f65985d6f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 15:57:58 -0500 Subject: [PATCH 25/41] Export --- .github/copilot-instructions.md | 1 + gtsam/geometry/ExtendedPose3.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index 493c4540bf..5998d60cf0 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -4,6 +4,7 @@ For reviewing PRs: * Use meaningful variable names, e.g. `measurement` not `msm`, avoid abbreviations. * Flag overly complex or long/functions: break up in smaller functions * On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition. +* When adding or modifying public classes/methods, always review and follow `Using-GTSAM-EXPORT.md` before finalizing changes, including template specialization/export rules. * If we add a C++ function to a `.i` file to expose it to the wrapper, we must ensure that the parameter names match exactly between the declaration in the header file and the declaration in the `.i`. Similarly, if we change any parameter names in a wrapped function in a header file, or change any parameter names in a `.i` file, we must change the corresponding function in the other file to reflect those changes. * Classes are Uppercase, methods and functions lowerMixedCase. * Public fields in structs keep plain names (no trailing underscore). diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp index 67e7754fe4..535aec8177 100644 --- a/gtsam/geometry/ExtendedPose3.cpp +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -19,11 +19,11 @@ namespace gtsam { -template class ExtendedPose3<1>; -template class ExtendedPose3<2>; -template class ExtendedPose3<3>; -template class ExtendedPose3<4>; -template class ExtendedPose3<6>; -template class ExtendedPose3; +template class GTSAM_EXPORT ExtendedPose3<1>; +template class GTSAM_EXPORT ExtendedPose3<2>; +template class GTSAM_EXPORT ExtendedPose3<3>; +template class GTSAM_EXPORT ExtendedPose3<4>; +template class GTSAM_EXPORT ExtendedPose3<6>; +template class GTSAM_EXPORT ExtendedPose3; } // namespace gtsam From 231f797edc15f537836f782dee8b56ee96793a89 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 16:14:52 -0500 Subject: [PATCH 26/41] Review comments --- gtsam/geometry/ExtendedPose3-inl.h | 83 +++++++++++++----------------- gtsam/geometry/ExtendedPose3.h | 4 +- gtsam/navigation/NavState.cpp | 4 +- 3 files changed, 40 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 51825b2816..113a246a41 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -33,7 +33,7 @@ size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { } template -void ExtendedPose3::ZeroJacobian(ChartJacobian H, size_t d) { +void ExtendedPose3::ZeroJacobian(ChartJacobian H, Eigen::Index d) { if (!H) return; if constexpr (dimension == Eigen::Dynamic) { H->setZero(d, d); @@ -51,7 +51,7 @@ ExtendedPose3::ExtendedPose3() template template ExtendedPose3::ExtendedPose3(size_t k) - : R_(Rot3::Identity()), t_(3, k) { + : R_(Rot3::Identity()), t_(3, static_cast(k)) { t_.setZero(); } @@ -61,23 +61,14 @@ ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) template ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { + const Eigen::Index n = T.rows(); if constexpr (K == Eigen::Dynamic) { - const auto n = T.rows(); if (T.cols() != n || n < 3) { throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); } - const auto k = n - 3; - t_.resize(3, k); - t_.setZero(); - } - - const auto n = T.rows(); - if constexpr (K != Eigen::Dynamic) { - if (n != matrix_dim || T.cols() != matrix_dim) { - throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); - } + t_.resize(3, n - 3); } else { - if (T.cols() != n || n < 3) { + if (n != matrix_dim || T.cols() != matrix_dim) { throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); } } @@ -90,7 +81,7 @@ template const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { if (H) { if constexpr (dimension == Eigen::Dynamic) { - H->setZero(3, dim()); + H->setZero(3, static_cast(dim())); } else { H->setZero(); } @@ -104,7 +95,7 @@ Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { if (i >= k()) throw std::out_of_range("ExtendedPose3: x(i) out of range."); if (H) { if constexpr (dimension == Eigen::Dynamic) { - H->setZero(3, dim()); + H->setZero(3, static_cast(dim())); } else { H->setZero(); } @@ -175,7 +166,7 @@ typename ExtendedPose3::This ExtendedPose3::operator*( template typename ExtendedPose3::This ExtendedPose3::Expmap( const TangentVector& xi, ChartJacobian Hxi) { - const size_t k = RuntimeK(xi); + const Eigen::Index k = static_cast(RuntimeK(xi)); // Get angular velocity omega const Vector3 w = xi.template head<3>(); @@ -202,30 +193,28 @@ typename ExtendedPose3::This ExtendedPose3::Expmap( // is near zero, and also gives us the machinery for the Jacobians. Matrix3K x; - if constexpr (K == Eigen::Dynamic) x.resize(3, static_cast(k)); + if constexpr (K == Eigen::Dynamic) x.resize(3, k); if (Hxi) { - const size_t d = 3 + 3 * k; - ZeroJacobian(Hxi, d); + ZeroJacobian(Hxi, 3 + 3 * k); const Matrix3 Jr = local.Jacobian().right(); Hxi->block(0, 0, 3, 3) = Jr; // Jr here *is* the Jacobian of expmap const Matrix3 Rt = R.transpose(); - for (size_t i = 0; i < k; ++i) { + for (Eigen::Index i = 0; i < k; ++i) { Matrix3 H_xi_w; - const Eigen::Index idx = 3 + 3 * static_cast(i); + const Eigen::Index idx = 3 + 3 * i; const Vector3 rho = xi.template segment<3>(idx); - x.col(static_cast(i)) = - local.Jacobian().applyLeft(rho, &H_xi_w); + x.col(i) = local.Jacobian().applyLeft(rho, &H_xi_w); Hxi->block(idx, 0, 3, 3) = Rt * H_xi_w; Hxi->block(idx, idx, 3, 3) = Jr; // In the last row, Jr = R^T * Jl, see Barfoot eq. (8.83). // Jl is the left Jacobian of SO(3) at w. } } else { - for (size_t i = 0; i < k; ++i) { - const Eigen::Index idx = 3 + 3 * static_cast(i); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; const Vector3 rho = xi.template segment<3>(idx); - x.col(static_cast(i)) = local.Jacobian().applyLeft(rho); + x.col(i) = local.Jacobian().applyLeft(rho); } } @@ -243,10 +232,11 @@ ExtendedPose3::Logmap(const This& pose, ChartJacobian H) { if constexpr (K == Eigen::Dynamic) xi.resize(static_cast(poseBase.dim())); xi.template head<3>() = w; - for (size_t i = 0; i < poseBase.k(); ++i) { - const Eigen::Index idx = 3 + 3 * static_cast(i); - xi.template segment<3>(idx) = local.InvJacobian().applyLeft( - poseBase.t_.col(static_cast(i))); + const Eigen::Index k = static_cast(poseBase.k()); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + xi.template segment<3>(idx) = + local.InvJacobian().applyLeft(poseBase.t_.col(i)); } if (H) *H = LogmapDerivative(xi); @@ -265,10 +255,10 @@ ExtendedPose3::AdjointMap() const { } adj.block(0, 0, 3, 3) = R; - for (size_t i = 0; i < k(); ++i) { - const Eigen::Index idx = 3 + 3 * static_cast(i); - adj.block(idx, 0, 3, 3) = - skewSymmetric(t_.col(static_cast(i))) * R; + const Eigen::Index k = static_cast(this->k()); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + adj.block(idx, 0, 3, 3) = skewSymmetric(t_.col(i)) * R; adj.block(idx, idx, 3, 3) = R; } return adj; @@ -288,7 +278,7 @@ ExtendedPose3::Adjoint(const TangentVector& xi_b, template typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap(const TangentVector& xi) { - const size_t k = RuntimeK(xi); + const Eigen::Index k = static_cast(RuntimeK(xi)); const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Jacobian adj; @@ -299,8 +289,8 @@ ExtendedPose3::adjointMap(const TangentVector& xi) { } adj.block(0, 0, 3, 3) = w_hat; - for (size_t i = 0; i < k; ++i) { - const Eigen::Index idx = 3 + 3 * static_cast(i); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; adj.block(idx, 0, 3, 3) = skewSymmetric(xi(idx + 0), xi(idx + 1), xi(idx + 2)); adj.block(idx, idx, 3, 3) = w_hat; @@ -346,7 +336,7 @@ ExtendedPose3::ExpmapDerivative(const TangentVector& xi) { template typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative(const TangentVector& xi) { - const size_t k = RuntimeK(xi); + const Eigen::Index k = static_cast(RuntimeK(xi)); const Vector3 w = xi.template head<3>(); // Instantiate functor for Dexp-related operations: @@ -363,9 +353,9 @@ ExtendedPose3::LogmapDerivative(const TangentVector& xi) { } J.block(0, 0, 3, 3) = Jw; - for (size_t i = 0; i < k; ++i) { + for (Eigen::Index i = 0; i < k; ++i) { Matrix3 H_xi_w; - const Eigen::Index idx = 3 + 3 * static_cast(i); + const Eigen::Index idx = 3 + 3 * i; local.Jacobian().applyLeft(xi.template segment<3>(idx), H_xi_w); const Matrix3 Q = Rt * H_xi_w; J.block(idx, 0, 3, 3) = -Jw * Q * Jw; @@ -399,20 +389,21 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::matrix() const { LieAlgebra M; if constexpr (matrix_dim == Eigen::Dynamic) { - const Eigen::Index n = 3 + static_cast(k()); + const Eigen::Index k = static_cast(this->k()); + const Eigen::Index n = 3 + k; M = LieAlgebra::Identity(n, n); } else { M = LieAlgebra::Identity(); } M.template block<3, 3>(0, 0) = R_.matrix(); - M.block(0, 3, 3, static_cast(k())) = t_; + M.block(0, 3, 3, static_cast(this->k())) = t_; return M; } template typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( const TangentVector& xi) { - const size_t k = RuntimeK(xi); + const Eigen::Index k = static_cast(RuntimeK(xi)); LieAlgebra X; if constexpr (matrix_dim == Eigen::Dynamic) { X.setZero(3 + k, 3 + k); @@ -420,8 +411,8 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( X.setZero(); } X.block(0, 0, 3, 3) = skewSymmetric(xi(0), xi(1), xi(2)); - for (size_t i = 0; i < k; ++i) { - const Eigen::Index idx = 3 + 3 * static_cast(i); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; X.block(0, 3 + i, 3, 1) = xi.template segment<3>(idx); } return X; diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index 6a110dded9..d2d0103a9f 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -46,7 +46,7 @@ class ExtendedPose3; * Template parameter K can be fixed (K >= 1) or Eigen::Dynamic. */ template -class GTSAM_EXPORT ExtendedPose3 +class ExtendedPose3 : public MatrixLieGroup, ExtendedPose3, Derived>, (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K, @@ -392,7 +392,7 @@ class GTSAM_EXPORT ExtendedPose3 } static size_t RuntimeK(const TangentVector& xi); - static void ZeroJacobian(ChartJacobian H, size_t d); + static void ZeroJacobian(ChartJacobian H, Eigen::Index d); private: #if GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 1832a920f6..384a171b12 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -118,9 +118,7 @@ void NavState::print(const std::string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { - return R_.equals(other.R_, tol) && - traits::Equals(t_.col(0), other.t_.col(0), tol) && - equal_with_abs_tol(t_.col(1), other.t_.col(1), tol); + return Base::equals(other, tol); } //------------------------------------------------------------------------------ From e7a7b52abfc33151fa6f11336402266cb91c0823 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 16:22:57 -0500 Subject: [PATCH 27/41] Try windows fix --- gtsam/geometry/ExtendedPose3.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp index 535aec8177..128233cd18 100644 --- a/gtsam/geometry/ExtendedPose3.cpp +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -24,6 +24,5 @@ template class GTSAM_EXPORT ExtendedPose3<2>; template class GTSAM_EXPORT ExtendedPose3<3>; template class GTSAM_EXPORT ExtendedPose3<4>; template class GTSAM_EXPORT ExtendedPose3<6>; -template class GTSAM_EXPORT ExtendedPose3; } // namespace gtsam From 5aea9987c2cd7928aed5d09e0b50275cf5884ad8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 19:58:08 -0500 Subject: [PATCH 28/41] Fix tests --- gtsam/navigation/tests/testNavState.cpp | 101 +++++++----------------- 1 file changed, 28 insertions(+), 73 deletions(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 51b8956f29..c232a74707 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -353,49 +353,6 @@ TEST(NavState, interpolate) { EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0))); } -/* ************************************************************************* */ -TEST(NavState, Lie) { - NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); - NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); - NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, - {3.0, -1.0, 1.0}); - - // TODO(Varun) - // logmap - // Matrix9 H1, H2; - // auto logmap_b = NavState::Create(Rot3::Identity(), - // Vector3::Zero(), Vector3::Zero()) - // .localCoordinates(nav_state_b, H1, H2); - - // Matrix6 J1, J2; - // auto logmap_pose_b = Pose3::Create(Rot3(), Vector3::Zero()) - // .localCoordinates(nav_state_b.pose(), J1, J2); - - // // Check retraction - // auto retraction_b = NavState().retract(logmap_b); - // CHECK(assert_equal(retraction_b, nav_state_b)); - - // // Test if the sum of the logmap is the same as the logmap of the product - // auto logmap_c = NavState::Create(Rot3::Identity(), - // Vector3::Zero(), Vector3::Zero()) - // .localCoordinates(nav_state_c); - - // auto logmap_bc = NavState::Create( - // gtsam::Rot3::Identity(), Eigen::Vector3d::Zero(), - // Eigen::Vector3d::Zero(), {}, {}, {}) - // .localCoordinates(nav_state_b * nav_state_c); - // Vector9 logmap_bc2 = NavState::Logmap(nav_state_b * nav_state_c); - - // Vector9 logmap_bc_sum = logmap_b + logmap_c; - // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl; - // std::cout << "logmap_bc2 = " << logmap_bc2.transpose() << std::endl; - - // // std::cout << "logmap_bc + logmap_c = " << logmap_bc_sum.transpose() << std::endl; - // // std::cout << "logmap_b + logmap_c = " << (NavState::Logmap(nav_state_b) + NavState::Logmap(nav_state_c)).transpose() << std::endl; - // // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl; - // // CHECK(assert_equal(logmap_bc_sum, logmap_bc)); -} - /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = @@ -428,7 +385,7 @@ TEST(NavState, Coriolis2) { TEST(NavState, Coriolis3) { /** Consider a massless planet with an attached nav frame at - * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', traveling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and * second order Coriolis corrections are as expected. @@ -599,11 +556,10 @@ NavState expected(expectedR, expectedV, expectedP); /* ************************************************************************* */ // Checks correct exponential map (Expmap) with brute force matrix exponential TEST(NavState, Expmap_c_full) { - //TODO(Varun) - // EXPECT(assert_equal(screwNavState::expected, - // expm(screwNavState::xi), 1e-6)); - // EXPECT(assert_equal(screwNavState::expected, - // NavState::Expmap(screwNavState::xi), 1e-6)); + EXPECT(assert_equal(screwNavState::expected, + expm(screwNavState::xi), 1e-6)); + EXPECT(assert_equal(screwNavState::expected, + NavState::Expmap(screwNavState::xi), 1e-6)); } /* ************************************************************************* */ @@ -638,30 +594,29 @@ TEST(NavState, Adjoint_compose_full) { /* ************************************************************************* */ TEST(NavState, expmaps_galore_full) { Vector xi; - //TODO(Varun) - // NavState actual; - // xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); - // actual = NavState::Expmap(xi); - // EXPECT(assert_equal(expm(xi), actual, 1e-6)); - // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-6)); - - // xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9) - // .finished(); - // for (double theta = 1.0; 0.3 * theta <= M_PI; theta *= 2) { - // Vector txi = xi * theta; - // actual = NavState::Expmap(txi); - // EXPECT(assert_equal(expm(txi, 30), actual, 1e-6)); - // Vector log = NavState::Logmap(actual); - // EXPECT(assert_equal(actual, NavState::Expmap(log), 1e-6)); - // EXPECT(assert_equal(txi, log, 1e-6)); // not true once wraps - // } - - // // Works with large v as well, but expm needs 10 iterations! - // xi = - // (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished(); - // actual = NavState::Expmap(xi); - // EXPECT(assert_equal(expm(xi, 10), actual, 1e-5)); - // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-9)); + NavState actual; + xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + actual = NavState::Expmap(xi); + EXPECT(assert_equal(expm(xi), actual, 1e-6)); + EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-6)); + + xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9) + .finished(); + for (double theta = 1.0; 0.3 * theta <= M_PI; theta *= 2) { + Vector txi = xi * theta; + actual = NavState::Expmap(txi); + EXPECT(assert_equal(expm(txi, 30), actual, 1e-6)); + Vector log = NavState::Logmap(actual); + EXPECT(assert_equal(actual, NavState::Expmap(log), 1e-6)); + EXPECT(assert_equal(txi, log, 1e-6)); // not true once wraps + } + + // Works with large v as well, but expm needs 10 iterations! + xi = + (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished(); + actual = NavState::Expmap(xi); + EXPECT(assert_equal(expm(xi, 10), actual, 1e-5)); + EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-9)); } /* ************************************************************************* */ From 49905821910895fdfe424b57233227d63db042df Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 20:17:43 -0500 Subject: [PATCH 29/41] Comment retract [slip ci] --- gtsam/navigation/NavState.cpp | 27 +++------------------------ gtsam/navigation/NavState.h | 13 +++++++++++-- 2 files changed, 14 insertions(+), 26 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 026bcc94a8..e38fde63fb 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -168,6 +168,8 @@ Vector9 NavState::adjointTranspose(const Vector9& xi, const Vector9& y, //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // NOTE: This is an intentional custom chart for NavState manifold updates. + // It differs from the default LieGroup chart based on full Expmap/Logmap. Rot3 nRb = R_; Point3 n_t = t_.col(0), n_v = t_.col(1); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; @@ -195,30 +197,7 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - // return LieGroup::localCoordinates(g, H1, H2); - - //TODO(Varun) Fix so that test on L680 passes - - // Matrix3 D_dR_R, D_dt_R, D_dv_R; - // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); - // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); - - // Vector9 xi; - // Matrix3 D_xi_R; - // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; - // if (H1) { - // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - // D_dt_R, -I_3x3, Z_3x3, // - // D_dv_R, Z_3x3, -I_3x3; - // } - // if (H2) { - // *H2 << D_xi_R, Z_3x3, Z_3x3, // - // Z_3x3, dR.matrix(), Z_3x3, // - // Z_3x3, Z_3x3, dR.matrix(); - // } - // return xi; - + // Inverse of the custom component-wise chart used in retract(). Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_.col(0) - t_.col(0), H1 ? &D_dt_R : 0); diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 49963444bf..7401401745 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -177,12 +177,21 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { return v.segment<3>(6); } - /// retract with optional derivatives + /** + * Manifold retract used by optimization. + * This intentionally uses a component-wise chart (R via Expmap, and p/v via + * world-frame rotation of the tangent increments), not the default LieGroup + * chart based on full Expmap/Logmap. + */ NavState retract(const Vector9& v, // OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; - /// localCoordinates with optional derivatives + /** + * Inverse of the custom manifold chart used by retract. + * Kept consistent with retract for optimization; Lie expmap/logmap remain + * available separately for group operations. + */ Vector9 localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; From 45488d344d03800e8beba1532de5876f38c5d537 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Feb 2026 22:16:02 -0500 Subject: [PATCH 30/41] Tiny perf issues [skip ci] --- gtsam/geometry/ExtendedPose3-inl.h | 11 +++++------ gtsam/geometry/Pose3.h | 5 +++++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 113a246a41..9f7b3d1d46 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -22,10 +22,7 @@ namespace gtsam { template size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { if constexpr (K == Eigen::Dynamic) { - if (xi.size() < 3 || (xi.size() - 3) % 3 != 0) { - throw std::invalid_argument( - "ExtendedPose3: tangent vector size must be 3 + 3*k."); - } + assert(xi.size() >= 3 && (xi.size() - 3) % 3 == 0); return static_cast((xi.size() - 3) / 3); } else { return static_cast(K); @@ -153,8 +150,10 @@ template typename ExtendedPose3::This ExtendedPose3::operator*( const This& other) const { const ExtendedPose3& otherBase = AsBase(other); - if (k() != otherBase.k()) { - throw std::invalid_argument("ExtendedPose3: compose requires matching k."); + if constexpr (K == Eigen::Dynamic) { + if (k() != otherBase.k()) { + throw std::invalid_argument("ExtendedPose3: compose requires matching k."); + } } Matrix3K x = t_ + R_.matrix() * otherBase.t_; return MakeReturn(ExtendedPose3(R_ * otherBase.R_, x)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 02238ea4c2..fdd7160e0b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -126,6 +126,11 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { OptionalJacobian<6, 6> Harg = {}, OptionalJacobian<6, 1> Ht = {}) const; + /// Compose syntactic sugar. + Pose3 operator*(const Pose3& T) const { + return Pose3(R_ * T.R_, t_ + R_ * T.t_); + } + /// @} /// @name Lie Group /// @{ From c37d510a287599e985f89cca6bb91926e52a103a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 23 Feb 2026 08:44:21 -0500 Subject: [PATCH 31/41] Expmap fast path [skip ci] --- gtsam/geometry/ExtendedPose3-inl.h | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 9f7b3d1d46..786473d4e1 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -165,8 +165,6 @@ typename ExtendedPose3::This ExtendedPose3::operator*( template typename ExtendedPose3::This ExtendedPose3::Expmap( const TangentVector& xi, ChartJacobian Hxi) { - const Eigen::Index k = static_cast(RuntimeK(xi)); - // Get angular velocity omega const Vector3 w = xi.template head<3>(); @@ -181,6 +179,22 @@ typename ExtendedPose3::This ExtendedPose3::Expmap( const Rot3 R(local.expmap()); #endif + if constexpr (K == 1) { + Matrix3 H; + const Vector3 v = xi.template tail<3>(); + const Vector3 t = local.Jacobian().applyLeft(v, Hxi ? &H : nullptr); + if (Hxi) { + const Matrix3 Jr = local.Jacobian().right(); + const Matrix3 Rt = R.transpose(); + *Hxi << Jr, Z_3x3, Rt * H, Jr; + } + Matrix3K x; + x.col(0) = t; + return MakeReturn(ExtendedPose3(R, x)); + } + + const Eigen::Index k = static_cast(RuntimeK(xi)); + // The translation t = local.Jacobian().left() * v. // Below we call local.Jacobian().applyLeft, which is faster if you don't need // Jacobians, and returns Jacobian of t with respect to w if asked. @@ -246,6 +260,7 @@ template typename ExtendedPose3::Jacobian ExtendedPose3::AdjointMap() const { const Matrix3 R = R_.matrix(); + Jacobian adj; if constexpr (dimension == Eigen::Dynamic) { adj.setZero(dim(), dim()); @@ -277,9 +292,10 @@ ExtendedPose3::Adjoint(const TangentVector& xi_b, template typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap(const TangentVector& xi) { - const Eigen::Index k = static_cast(RuntimeK(xi)); const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + const Eigen::Index k = static_cast(RuntimeK(xi)); + Jacobian adj; if constexpr (dimension == Eigen::Dynamic) { adj.setZero(3 + 3 * k, 3 + 3 * k); @@ -335,12 +351,12 @@ ExtendedPose3::ExpmapDerivative(const TangentVector& xi) { template typename ExtendedPose3::Jacobian ExtendedPose3::LogmapDerivative(const TangentVector& xi) { - const Eigen::Index k = static_cast(RuntimeK(xi)); const Vector3 w = xi.template head<3>(); // Instantiate functor for Dexp-related operations: const so3::DexpFunctor local(w); + const Eigen::Index k = static_cast(RuntimeK(xi)); const Matrix3 Rt = local.expmap().transpose(); const Matrix3 Jw = Rot3::LogmapDerivative(w); From fc993a2d4ef04534da449ffa132dd5591d9c42b7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 23 Feb 2026 10:41:28 -0500 Subject: [PATCH 32/41] Custom Pose3::Expmap [skip ci] --- gtsam/geometry/ExtendedPose3-inl.h | 14 -------------- gtsam/geometry/Pose3.cpp | 31 ++++++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 3 +++ 3 files changed, 34 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 786473d4e1..f910a7d32a 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -179,20 +179,6 @@ typename ExtendedPose3::This ExtendedPose3::Expmap( const Rot3 R(local.expmap()); #endif - if constexpr (K == 1) { - Matrix3 H; - const Vector3 v = xi.template tail<3>(); - const Vector3 t = local.Jacobian().applyLeft(v, Hxi ? &H : nullptr); - if (Hxi) { - const Matrix3 Jr = local.Jacobian().right(); - const Matrix3 Rt = R.transpose(); - *Hxi << Jr, Z_3x3, Rt * H, Jr; - } - Matrix3K x; - x.col(0) = t; - return MakeReturn(ExtendedPose3(R, x)); - } - const Eigen::Index k = static_cast(RuntimeK(xi)); // The translation t = local.Jacobian().left() * v. diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index db22342729..44288ea79f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -132,6 +132,37 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t, interpolate(t_, T.t_, t)); } +/* ************************************************************************* */ +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { + // Get angular velocity omega and translational velocity v from twist xi + const Vector3 w = xi.head<3>(), v = xi.tail<3>(); + + // Instantiate functor for Dexp-related operations: + const so3::DexpFunctor local(w); + + // Compute rotation using Expmap +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(w); +#else + const Rot3 R(local.expmap()); +#endif + + // The translation t = local.Jacobian().left() * v. + Matrix3 H; + const Vector3 t = local.Jacobian().applyLeft(v, Hxi ? &H : nullptr); + + if (Hxi) { + // The Jacobian of expmap is given by the right Jacobian of SO(3): + const Matrix3 Jr = local.Jacobian().right(); + // Chain H with R^T, the Jacobian of Pose3::Create with respect to t. + const Matrix3 Rt = R.transpose(); + *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap + Rt * H, Jr; // Jr = R^T * Jl, with Jl Jacobian of t in v. + } + + return Pose3(R, t); +} + /* ************************************************************************* */ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { #ifdef GTSAM_POSE3_EXPMAP diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fdd7160e0b..8b31ddbaf1 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -137,6 +137,9 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { using LieAlgebra = Matrix4; + /// Exponential map at identity. + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); + /// The dual version of Adjoint Vector6 AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this = {}, From 9f9ba857ca42a2657c5e3c7f6184e80fbfe0254e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 23 Feb 2026 18:28:16 -0500 Subject: [PATCH 33/41] Address review comments --- gtsam/geometry/ExtendedPose3-inl.h | 43 ++++++------------------- gtsam/geometry/ExtendedPose3.h | 32 +++++++++++------- gtsam/navigation/tests/testNavState.cpp | 2 +- 3 files changed, 31 insertions(+), 46 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index f910a7d32a..7551da2556 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -40,24 +40,12 @@ void ExtendedPose3::ZeroJacobian(ChartJacobian H, Eigen::Index d) { } } -template -template -ExtendedPose3::ExtendedPose3() - : R_(Rot3::Identity()), t_(Matrix3K::Zero()) {} - -template -template -ExtendedPose3::ExtendedPose3(size_t k) - : R_(Rot3::Identity()), t_(3, static_cast(k)) { - t_.setZero(); -} - template ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) : R_(R), t_(x) {} template -ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { +ExtendedPose3::ExtendedPose3(const MatrixRep& T) { const Eigen::Index n = T.rows(); if constexpr (K == Eigen::Dynamic) { if (T.cols() != n || n < 3) { @@ -65,7 +53,7 @@ ExtendedPose3::ExtendedPose3(const LieAlgebra& T) { } t_.resize(3, n - 3); } else { - if (n != matrix_dim || T.cols() != matrix_dim) { + if (n != matrixDim || T.cols() != matrixDim) { throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); } } @@ -125,19 +113,6 @@ bool ExtendedPose3::equals(const ExtendedPose3& other, return R_.equals(other.R_, tol) && equal_with_abs_tol(t_, other.t_, tol); } -template -template -typename ExtendedPose3::This ExtendedPose3::Identity() { - return MakeReturn(ExtendedPose3()); -} - -template -template -typename ExtendedPose3::This ExtendedPose3::Identity( - size_t k) { - return MakeReturn(ExtendedPose3(k)); -} - template typename ExtendedPose3::This ExtendedPose3::inverse() const { @@ -386,15 +361,15 @@ ExtendedPose3::ChartAtOrigin::Local(const This& pose, } template -typename ExtendedPose3::LieAlgebra +typename ExtendedPose3::MatrixRep ExtendedPose3::matrix() const { - LieAlgebra M; - if constexpr (matrix_dim == Eigen::Dynamic) { + MatrixRep M; + if constexpr (matrixDim == Eigen::Dynamic) { const Eigen::Index k = static_cast(this->k()); const Eigen::Index n = 3 + k; - M = LieAlgebra::Identity(n, n); + M = MatrixRep::Identity(n, n); } else { - M = LieAlgebra::Identity(); + M = MatrixRep::Identity(); } M.template block<3, 3>(0, 0) = R_.matrix(); M.block(0, 3, 3, static_cast(this->k())) = t_; @@ -406,7 +381,7 @@ typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( const TangentVector& xi) { const Eigen::Index k = static_cast(RuntimeK(xi)); LieAlgebra X; - if constexpr (matrix_dim == Eigen::Dynamic) { + if constexpr (matrixDim == Eigen::Dynamic) { X.setZero(3 + k, 3 + k); } else { X.setZero(); @@ -430,7 +405,7 @@ ExtendedPose3::Vee(const LieAlgebra& X) { if constexpr (K == Eigen::Dynamic) { return X.cols() - 3; } else { - if (X.rows() != matrix_dim) { + if (X.rows() != matrixDim) { throw std::invalid_argument( "ExtendedPose3::Vee: invalid matrix shape."); } diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index d2d0103a9f..63df5a8556 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -56,10 +56,10 @@ class ExtendedPose3 ExtendedPose3, Derived>; inline constexpr static int dimension = (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K; - inline constexpr static int matrix_dim = + inline constexpr static int matrixDim = (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K; - using Base = MatrixLieGroup; + using Base = MatrixLieGroup; using TangentVector = typename Base::TangentVector; using Jacobian = typename Base::Jacobian; using ChartJacobian = typename Base::ChartJacobian; @@ -67,7 +67,10 @@ class ExtendedPose3 std::conditional_t, OptionalJacobian<3, dimension>>; - using LieAlgebra = Eigen::Matrix; + /// Homogeneous matrix representation in the group. + using MatrixRep = Eigen::Matrix; + /// Lie algebra matrix type used by Hat/Vee. + using LieAlgebra = Eigen::Matrix; using Matrix3K = Eigen::Matrix; static_assert(K == Eigen::Dynamic || K >= 1, @@ -93,7 +96,7 @@ class ExtendedPose3 * The manifold dimension is 3+3k and matrix size is (3+k)x(3+k). */ template > - ExtendedPose3(); + ExtendedPose3() : R_(Rot3::Identity()), t_(Matrix3K::Zero()) {} /** * Construct a dynamic-size identity element. @@ -103,7 +106,10 @@ class ExtendedPose3 * The manifold dimension is 3+3k and matrix size is (3+k)x(3+k). */ template > - explicit ExtendedPose3(size_t k = 0); + explicit ExtendedPose3(size_t k = 0) + : R_(Rot3::Identity()), t_(3, static_cast(k)) { + t_.setZero(); + } /** Copy constructor. */ ExtendedPose3(const ExtendedPose3&) = default; @@ -125,7 +131,7 @@ class ExtendedPose3 * @param T Homogeneous matrix in R^((3+k)x(3+k)). * Top-left 3x3 is R, top-right 3xk stores x_1..x_k. */ - explicit ExtendedPose3(const LieAlgebra& T); + explicit ExtendedPose3(const MatrixRep& T); /// @} /// @name Access @@ -206,7 +212,9 @@ class ExtendedPose3 * @return Identity with manifold dimension 3+3k and matrix size 3+k. */ template > - static This Identity(); + static This Identity() { + return MakeReturn(ExtendedPose3()); + } /** * Identity element for dynamic-size K. @@ -215,7 +223,9 @@ class ExtendedPose3 * @return Identity with manifold dimension 3+3k and matrix size 3+k. */ template > - static This Identity(size_t k = 0); + static This Identity(size_t k = 0) { + return MakeReturn(ExtendedPose3(k)); + } /** * Group inverse. @@ -348,7 +358,7 @@ class ExtendedPose3 * * @return Matrix in R^((3+k)x(3+k)). */ - LieAlgebra matrix() const; + MatrixRep matrix() const; /** * Hat operator from tangent to Lie algebra. @@ -411,12 +421,12 @@ using ExtendedPose3Dynamic = ExtendedPose3; template struct traits> : public internal::MatrixLieGroup, - ExtendedPose3::matrix_dim> {}; + ExtendedPose3::matrixDim> {}; template struct traits> : public internal::MatrixLieGroup, - ExtendedPose3::matrix_dim> {}; + ExtendedPose3::matrixDim> {}; } // namespace gtsam diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index c232a74707..3a6b06134e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -592,7 +592,7 @@ TEST(NavState, Adjoint_compose_full) { } /* ************************************************************************* */ -TEST(NavState, expmaps_galore_full) { +TEST(NavState, ExpmapsGaloreFull) { Vector xi; NavState actual; xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); From 13cc819ac26b7a6f4dfb12c19f6539b600ed2517 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 23 Feb 2026 18:48:26 -0500 Subject: [PATCH 34/41] transposes --- gtsam/geometry/ExtendedPose3-inl.h | 57 ++++++++++++++ gtsam/geometry/ExtendedPose3.h | 26 +++++++ gtsam/geometry/Pose3.cpp | 40 ---------- gtsam/geometry/Pose3.h | 12 --- gtsam/geometry/tests/testExtendedPose3.cpp | 89 ++++++++++++++++++++++ gtsam/navigation/NavState.cpp | 44 ----------- gtsam/navigation/NavState.h | 14 +--- 7 files changed, 173 insertions(+), 109 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index 7551da2556..e4b2973879 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -250,6 +250,35 @@ ExtendedPose3::Adjoint(const TangentVector& xi_b, return Ad * xi_b; } +template +typename ExtendedPose3::TangentVector +ExtendedPose3::AdjointTranspose(const TangentVector& x, + ChartJacobian H_this, + ChartJacobian H_x) const { + const Jacobian Ad = AdjointMap(); + const TangentVector AdTx = Ad.transpose() * x; + + if (H_this) { + if constexpr (dimension == Eigen::Dynamic) { + H_this->setZero(x.size(), x.size()); + } else { + H_this->setZero(); + } + + H_this->block(0, 0, 3, 3) = skewSymmetric(AdTx.template head<3>()); + const Eigen::Index k = static_cast(this->k()); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + const Matrix3 x_i_hat = skewSymmetric(AdTx.template segment<3>(idx)); + H_this->block(0, idx, 3, 3) = x_i_hat; + H_this->block(idx, 0, 3, 3) = x_i_hat; + } + } + + if (H_x) *H_x = Ad.transpose(); + return AdTx; +} + template typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap(const TangentVector& xi) { @@ -301,6 +330,34 @@ ExtendedPose3::adjoint(const TangentVector& xi, return ad_xi * y; } +template +typename ExtendedPose3::TangentVector +ExtendedPose3::adjointTranspose(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi, + ChartJacobian H_y) { + if (Hxi) { + if constexpr (dimension == Eigen::Dynamic) { + Hxi->setZero(xi.size(), xi.size()); + } else { + Hxi->setZero(); + } + for (Eigen::Index i = 0; i < xi.size(); ++i) { + TangentVector dxi; + if constexpr (dimension == Eigen::Dynamic) { + dxi = TangentVector::Zero(xi.size()); + } else { + dxi = TangentVector::Zero(); + } + dxi(i) = 1.0; + Hxi->col(i) = adjointMap(dxi).transpose() * y; + } + } + const Jacobian adT_xi = adjointMap(xi).transpose(); + if (H_y) *H_y = adT_xi; + return adT_xi * y; +} + template typename ExtendedPose3::Jacobian ExtendedPose3::ExpmapDerivative(const TangentVector& xi) { diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index 63df5a8556..c2f6ca43e0 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -282,6 +282,18 @@ class ExtendedPose3 TangentVector Adjoint(const TangentVector& xi_b, ChartJacobian H_this = {}, ChartJacobian H_xib = {}) const; + /** + * Dual adjoint action on a tangent covector. + * + * @param x Tangent vector in R^dim. + * @param H_this Optional Jacobian in R^(dimxdim). + * @param H_x Optional Jacobian in R^(dimxdim). + * @return Ad_X^T x in R^dim. + */ + TangentVector AdjointTranspose(const TangentVector& x, + ChartJacobian H_this = {}, + ChartJacobian H_x = {}) const; + /** * Lie algebra adjoint map. * @@ -302,6 +314,20 @@ class ExtendedPose3 static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, ChartJacobian Hxi = {}, ChartJacobian H_y = {}); + /** + * Dual Lie bracket action ad_xi^T(y). + * + * @param xi Tangent vector in R^dim. + * @param y Tangent vector in R^dim. + * @param Hxi Optional Jacobian in R^(dimxdim). + * @param H_y Optional Jacobian in R^(dimxdim). + * @return ad_xi^T(y) in R^dim. + */ + static TangentVector adjointTranspose(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}); + /** * Jacobian of Expmap. * diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 44288ea79f..846be08064 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -57,46 +57,6 @@ Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { return Pose3(p); } -/* ************************************************************************* */ -/// The dual version of Adjoint -Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, - OptionalJacobian<6, 6> H_x) const { - const Matrix6 Ad = AdjointMap(); - const Vector6 AdTx = Ad.transpose() * x; - - // Jacobians - // See docs/math.pdf for more details. - if (H_pose) { - const auto w_T_hat = skewSymmetric(AdTx.head<3>()), - v_T_hat = skewSymmetric(AdTx.tail<3>()); - *H_pose << w_T_hat, v_T_hat, // - /* */ v_T_hat, Z_3x3; - } - if (H_x) { - *H_x = Ad.transpose(); - } - - return AdTx; -} - -/* ************************************************************************* */ -Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 6; ++i) { - Vector6 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix6 GTi = adjointMap(dxi).transpose(); - Hxi->col(i) = GTi * y; - } - } - const Matrix6& adT_xi = adjointMap(xi).transpose(); - if (H_y) *H_y = adT_xi; - return adT_xi * y; -} - /* ************************************************************************* */ void Pose3::print(const std::string& s) const { std::cout << (s.empty() ? s : s + " ") << *this << std::endl; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8b31ddbaf1..6d4bf0c3bc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -140,22 +140,10 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { /// Exponential map at identity. static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); - /// The dual version of Adjoint - Vector6 AdjointTranspose(const Vector6& x, - OptionalJacobian<6, 6> H_this = {}, - OptionalJacobian<6, 6> H_x = {}) const; - // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);} - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = {}, - OptionalJacobian<6, 6> H_y = {}); - // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp index 75272bf141..32bef7936e 100644 --- a/gtsam/geometry/tests/testExtendedPose3.cpp +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -21,6 +21,8 @@ #include #include +#include + using namespace gtsam; using ExtendedPose33 = ExtendedPose3<3>; @@ -158,6 +160,93 @@ TEST(ExtendedPose3, AdjointConsistency) { EXPECT(assert_equal(d_generic, d.AdjointMap(), 1e-9)); } +//****************************************************************************** +TEST(ExtendedPose3, AdjointTranspose) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); + const Vector12 x = kXi; + const Vector x_dynamic = x; + + EXPECT(assert_equal(Vector(f.AdjointMap().transpose() * x), + Vector(f.AdjointTranspose(x)))); + EXPECT(assert_equal(d.AdjointMap().transpose() * x_dynamic, + d.AdjointTranspose(x_dynamic))); + + std::function + f_adjoint_transpose = [](const ExtendedPose33& g, const Vector12& v) { + return Vector12(g.AdjointTranspose(v)); + }; + Matrix Hf_state, Hf_x; + f.AdjointTranspose(x, Hf_state, Hf_x); + EXPECT(assert_equal(numericalDerivative21(f_adjoint_transpose, f, x), Hf_state, + 1e-8)); + EXPECT( + assert_equal(numericalDerivative22(f_adjoint_transpose, f, x), Hf_x)); + + std::function + d_adjoint_transpose = [](const ExtendedPose3d& g, const Vector& v) { + return g.AdjointTranspose(v); + }; + Matrix Hd_state, Hd_x; + d.AdjointTranspose(x_dynamic, Hd_state, Hd_x); + EXPECT(assert_equal( + numericalDerivative21( + d_adjoint_transpose, d, x_dynamic), + Hd_state, 1e-8)); + EXPECT(assert_equal( + numericalDerivative22( + d_adjoint_transpose, d, x_dynamic), + Hd_x)); +} + +//****************************************************************************** +TEST(ExtendedPose3, adjointTranspose) { + const Vector12 xi = kXi; + const Vector12 y = + (Vector12() << 0.03, -0.07, 0.02, 0.4, 0.1, -0.2, -0.3, 0.5, 0.9, 0.2, + -0.8, 0.6) + .finished(); + const Vector xi_dynamic = xi; + const Vector y_dynamic = y; + + EXPECT(assert_equal(Vector(ExtendedPose33::adjointMap(xi).transpose() * y), + Vector(ExtendedPose33::adjointTranspose(xi, y)))); + EXPECT(assert_equal(ExtendedPose3d::adjointMap(xi_dynamic).transpose() * + y_dynamic, + ExtendedPose3d::adjointTranspose(xi_dynamic, y_dynamic))); + + std::function + f_adjoint_transpose = [](const Vector12& x, const Vector12& v) { + return Vector12(ExtendedPose33::adjointTranspose(x, v)); + }; + Matrix Hf_xi, Hf_y; + EXPECT(assert_equal(Vector(ExtendedPose33::adjointTranspose(xi, y, Hf_xi, Hf_y)), + Vector(f_adjoint_transpose(xi, y)))); + EXPECT( + assert_equal(numericalDerivative21(f_adjoint_transpose, xi, y, 1e-5), Hf_xi, + 1e-5)); + EXPECT( + assert_equal(numericalDerivative22(f_adjoint_transpose, xi, y, 1e-5), Hf_y, + 1e-5)); + + std::function d_adjoint_transpose = + [](const Vector& x, const Vector& v) { + return ExtendedPose3d::adjointTranspose(x, v); + }; + Matrix Hd_xi, Hd_y; + EXPECT(assert_equal(ExtendedPose3d::adjointTranspose(xi_dynamic, y_dynamic, + Hd_xi, Hd_y), + d_adjoint_transpose(xi_dynamic, y_dynamic))); + EXPECT(assert_equal( + numericalDerivative21( + d_adjoint_transpose, xi_dynamic, y_dynamic, 1e-5), + Hd_xi, 1e-5)); + EXPECT(assert_equal( + numericalDerivative22( + d_adjoint_transpose, xi_dynamic, y_dynamic, 1e-5), + Hd_y, 1e-5)); +} + //****************************************************************************** TEST(ExtendedPose3, Derivatives) { const ExtendedPose33 f(kR1, kX1); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index e38fde63fb..a93bfead45 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -121,50 +121,6 @@ bool NavState::equals(const NavState& other, double tol) const { return Base::equals(other, tol); } -//------------------------------------------------------------------------------ -/// The dual version of Adjoint -Vector9 NavState::AdjointTranspose(const Vector9& x, OptionalJacobian<9, 9> H_state, - OptionalJacobian<9, 9> H_x) const { - const Matrix9 Ad = AdjointMap(); - const Vector9 AdTx = Ad.transpose() * x; - - // Jacobians - if (H_state) { - //TODO(Varun) - // const auto w_T_hat = skewSymmetric(AdTx.head<3>()), - // v_T_hat = skewSymmetric(AdTx.segment<3>(3)), - // a_T_hat = skewSymmetric(AdTx.tail<3>()); - // *H_state << w_T_hat, v_T_hat, // - // /* */ v_T_hat, Z_3x3; - throw std::runtime_error( - "AdjointTranpose H_state Jacobian not implemented."); - } - if (H_x) { - *H_x = Ad.transpose(); - } - - return AdTx; -} - -//------------------------------------------------------------------------------ -Vector9 NavState::adjointTranspose(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi, - OptionalJacobian<9, 9> H_y) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 9; ++i) { - Vector9 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix9 GTi = adjointMap(dxi).transpose(); - Hxi->col(i) = GTi * y; - } - } - const Matrix9& adT_xi = adjointMap(xi).transpose(); - if (H_y) *H_y = adT_xi; - return adT_xi * y; -} - //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 7401401745..03f0eee3a1 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -197,21 +197,9 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { {}) const; /// @} - /// @name Lie Group + /// @name Lie Group (all Lie group operations are implemented in ExtendedPose3) /// @{ - /// The dual version of Adjoint - Vector9 AdjointTranspose(const Vector9& x, - OptionalJacobian<9, 9> H_this = {}, - OptionalJacobian<9, 9> H_x = {}) const; - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi = {}, - OptionalJacobian<9, 9> H_y = {}); - /// @} /// @name Dynamics /// @{ From 3ea927fb4d181a616840ad040a0689e42e6736ee Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Feb 2026 08:58:13 -0500 Subject: [PATCH 35/41] Generic adjointTranspose --- gtsam/base/MatrixLieGroup.h | 71 ++++++++++++++++++++++++ gtsam/base/doc/MatrixLieGroup.md | 1 + gtsam/geometry/Pose2.h | 17 +++++- gtsam/geometry/tests/testPose2.cpp | 34 ++++++++++++ gtsam/geometry/tests/testSimilarity2.cpp | 37 ++++++++++++ gtsam/geometry/tests/testSimilarity3.cpp | 37 +++++++++++- 6 files changed, 193 insertions(+), 4 deletions(-) diff --git a/gtsam/base/MatrixLieGroup.h b/gtsam/base/MatrixLieGroup.h index 7653d28f1c..ce1e736f38 100644 --- a/gtsam/base/MatrixLieGroup.h +++ b/gtsam/base/MatrixLieGroup.h @@ -130,7 +130,78 @@ namespace gtsam { return adj; } + /** + * Dual Adjoint action on a tangent covector. + * + * Returns Ad_g^T * x with optional Jacobians with respect to g and x. + */ + TangentVector AdjointTranspose(const TangentVector& x, + ChartJacobian H_this = {}, + ChartJacobian H_x = {}) const { + const auto& m = static_cast(*this); + const Jacobian Ad = m.AdjointMap(); + const TangentVector AdTx = Ad.transpose() * x; + + if (H_this) { + const Eigen::Index d = + (D == Eigen::Dynamic) ? static_cast(m.dim()) : D; + if constexpr (D == Eigen::Dynamic) { + H_this->setZero(d, d); + } else { + H_this->setZero(); + } + for (Eigen::Index i = 0; i < d; ++i) { + H_this->col(i) = Class::adjointTranspose(TangentVector::Unit(d, i), AdTx); + } + } + + if (H_x) *H_x = Ad.transpose(); + return AdTx; + } + + /** + * Dual Lie algebra action ad_xi^T(y), with optional Jacobians. + */ + static TangentVector adjointTranspose(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + const Jacobian adT_xi = AdjointMapFromHatVee(xi).transpose(); + if (Hxi) { + const Eigen::Index d = + (D == Eigen::Dynamic) ? static_cast(xi.size()) : D; + if constexpr (D == Eigen::Dynamic) { + Hxi->setZero(d, d); + } else { + Hxi->setZero(); + } + for (Eigen::Index i = 0; i < d; ++i) { + Hxi->col(i) = AdjointMapFromHatVee(TangentVector::Unit(d, i)).transpose() * y; + } + } + if (H_y) *H_y = adT_xi; + return adT_xi * y; + } + private: + /// Generic Lie algebra adjoint map from Hat/Vee via matrix commutator. + static Jacobian AdjointMapFromHatVee(const TangentVector& xi) { + const Eigen::Index d = + (D == Eigen::Dynamic) ? static_cast(xi.size()) : D; + Jacobian ad; + if constexpr (D == Eigen::Dynamic) { + ad.setZero(d, d); + } else { + ad.setZero(); + } + const auto Xi = Class::Hat(xi); + for (Eigen::Index i = 0; i < d; ++i) { + const auto Ei = Class::Hat(TangentVector::Unit(d, i)); + ad.col(i) = Class::Vee(Xi * Ei - Ei * Xi); + } + return ad; + } + /// Pre-compute and store vectorized generators for fixed-size groups. inline static const Eigen::Matrix& VectorizedGenerators() { diff --git a/gtsam/base/doc/MatrixLieGroup.md b/gtsam/base/doc/MatrixLieGroup.md index f2228fb0db..8699281a35 100644 --- a/gtsam/base/doc/MatrixLieGroup.md +++ b/gtsam/base/doc/MatrixLieGroup.md @@ -30,6 +30,7 @@ You must implement everything from the `LieGroup.md` guide, plus the following: By inheriting from `gtsam::MatrixLieGroup`, you get: * **A default `AdjointMap()` implementation**: This generic version works by repeatedly calling your `Hat` and `Vee` methods. While it is correct, it is often slow. For performance-critical applications, you should still provide a faster, closed-form `AdjointMap()` implementation in your class, which will override the default. +* **Default dual-adjoint helpers**: `AdjointTranspose(x)` and `adjointTranspose(xi, y)` are provided generically, including Jacobians. * **A `vec()` method**: This vectorizes the `N x N` matrix representation of your group element into an `(N*N) x 1` vector. ### 4. Traits and Concept Checking diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 2fd5d34dc5..074436bcab 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -178,8 +178,20 @@ class GTSAM_EXPORT Pose2: public MatrixLieGroup { /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { - return adjointMap(xi).transpose() * y; + static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y, + OptionalJacobian<3, 3> Hxi = {}, + OptionalJacobian<3, 3> H_y = {}) { + const Matrix3 adT = adjointMap(xi).transpose(); + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 3; ++i) { + Vector3 dxi = Vector3::Zero(); + dxi(i) = 1.0; + Hxi->col(i) = adjointMap(dxi).transpose() * y; + } + } + if (H_y) *H_y = adT; + return adT * y; } // temporary fix for wrappers until case issue is resolved @@ -393,4 +405,3 @@ template struct Range : HasRange {}; } // namespace gtsam - diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 00a0ef78ee..7282522527 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -1005,6 +1005,40 @@ TEST(Pose2, AdjointMap) { EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); } +/* ************************************************************************* */ +TEST(Pose2, AdjointTranspose) { + const Pose2 pose(Rot2::fromAngle(0.5), Point2(1.0, 2.0)); + const Vector3 xi(0.2, -0.4, 0.7); + + EXPECT(assert_equal(Vector(pose.AdjointMap().transpose() * xi), + Vector(pose.AdjointTranspose(xi)))); + + Matrix33 actualH1, actualH2; + std::function proxy = + [](const Pose2& g, const Vector3& x) { + return Vector3(g.AdjointTranspose(x)); + }; + pose.AdjointTranspose(xi, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(proxy, pose, xi), actualH1, 1e-8)); + EXPECT(assert_equal(numericalDerivative22(proxy, pose, xi), actualH2)); +} + +/* ************************************************************************* */ +TEST(Pose2, adjointTranspose) { + const Vector3 xi(0.2, -0.4, 0.7); + const Vector3 y(-0.3, 0.5, 0.9); + + Matrix33 Hxi, Hy; + const Vector3 actual = Pose2::adjointTranspose(xi, y, Hxi, Hy); + std::function f = + [](const Vector3& x, const Vector3& v) { + return Pose2::adjointTranspose(x, v); + }; + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index 3ad23cac2d..ef3867f081 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -240,6 +240,43 @@ TEST(Similarity2, AdjointMap) { EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); } +//****************************************************************************** +TEST(Similarity2, AdjointTranspose) { + const Similarity2 sim(Rot2::fromAngle(-0.7), Point2(1.5, -2.3), 0.5); + const Vector4 xi(0.2, -0.4, 0.7, -0.1); + + EXPECT(assert_equal(Vector(sim.AdjointMap().transpose() * xi), + Vector(sim.AdjointTranspose(xi)))); + + Matrix44 actualH1, actualH2; + std::function + adjointTransposeProxy = [](const Similarity2& g, const Vector4& x) { + return Vector4(g.AdjointTranspose(x)); + }; + sim.AdjointTranspose(xi, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(adjointTransposeProxy, sim, xi), + actualH1, 1e-8)); + EXPECT(assert_equal(numericalDerivative22(adjointTransposeProxy, sim, xi), + actualH2)); +} + +//****************************************************************************** +TEST(Similarity2, adjointTranspose) { + const Vector4 xi(0.2, -0.4, 0.7, -0.1); + const Vector4 y(-0.3, 0.5, 0.9, -0.2); + + std::function f = + [](const Vector4& x, const Vector4& v) { + return Vector4(Similarity2::adjointTranspose(x, v)); + }; + + Matrix44 Hxi, Hy; + const Vector4 actual = Similarity2::adjointTranspose(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 42998a86b6..233fba0832 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -653,10 +653,45 @@ TEST(Similarity3, AdjointMap) { EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); } +//****************************************************************************** +TEST(Similarity3, AdjointTranspose) { + const Similarity3 sim(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), + 0.8); + const Vector7 xi(0.2, -0.4, 0.7, -0.1, 0.3, -0.6, 0.5); + + EXPECT(assert_equal(Vector(sim.AdjointMap().transpose() * xi), + Vector(sim.AdjointTranspose(xi)))); + + Matrix77 actualH1, actualH2; + std::function proxy = + [](const Similarity3& g, const Vector7& x) { + return Vector7(g.AdjointTranspose(x)); + }; + sim.AdjointTranspose(xi, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(proxy, sim, xi), actualH1, 1e-8)); + EXPECT(assert_equal(numericalDerivative22(proxy, sim, xi), actualH2)); +} + +//****************************************************************************** +TEST(Similarity3, adjointTranspose) { + const Vector7 xi(0.2, -0.4, 0.7, -0.1, 0.3, -0.6, 0.5); + const Vector7 y(-0.3, 0.5, 0.9, -0.2, 0.4, -0.8, 0.1); + + std::function f = + [](const Vector7& x, const Vector7& v) { + return Vector7(Similarity3::adjointTranspose(x, v)); + }; + + Matrix77 Hxi, Hy; + const Vector7 actual = Similarity3::adjointTranspose(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + //****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } //****************************************************************************** - From c3abd6b01ec8862520d8d42457f1a2f2125b4d45 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Feb 2026 10:13:47 -0500 Subject: [PATCH 36/41] Clean up entire Ad/ad API --- gtsam/base/Lie.h | 1 + gtsam/base/MatrixLieGroup.h | 164 +++++++++++++++++++---- gtsam/base/doc/MatrixLieGroup.md | 4 +- gtsam/geometry/Gal3.cpp | 33 ----- gtsam/geometry/Gal3.h | 11 -- gtsam/geometry/Pose2.h | 31 ----- gtsam/geometry/tests/testSimilarity2.cpp | 17 +++ gtsam/geometry/tests/testSimilarity3.cpp | 17 +++ 8 files changed, 173 insertions(+), 105 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1a6023ee95..96e0efc161 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -209,6 +209,7 @@ struct LieGroupTraits : public GetDimensionImpl { // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj). inline constexpr static auto dimension = Class::dimension; using TangentVector = Eigen::Matrix; + using Jacobian = Eigen::Matrix; using ChartJacobian = OptionalJacobian; static TangentVector Local(const Class& origin, const Class& other, diff --git a/gtsam/base/MatrixLieGroup.h b/gtsam/base/MatrixLieGroup.h index ce1e736f38..89f6732ff9 100644 --- a/gtsam/base/MatrixLieGroup.h +++ b/gtsam/base/MatrixLieGroup.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -56,6 +57,9 @@ namespace gtsam { using Jacobian = typename Base::Jacobian; using TangentVector = typename Base::TangentVector; + /// @name Matrix Lie Group + /// @{ + /** * Vectorize the matrix representation of a Lie group element. * The derivative `H` is the `(N*N) x D` Jacobian of this vectorization map. @@ -130,6 +134,21 @@ namespace gtsam { return adj; } + /** + * Adjoint action on a tangent vector. + * + * Returns Ad_g * xi with optional Jacobians with respect to g and xi. + */ + TangentVector Adjoint(const TangentVector& xi, + ChartJacobian H_this = {}, + ChartJacobian H_xi = {}) const { + const auto& m = static_cast(*this); + const Jacobian Ad = m.AdjointMap(); + if (H_this) *H_this = -Ad * Class::adjointMap(xi); + if (H_xi) *H_xi = Ad; + return Ad * xi; + } + /** * Dual Adjoint action on a tangent covector. * @@ -143,15 +162,18 @@ namespace gtsam { const TangentVector AdTx = Ad.transpose() * x; if (H_this) { - const Eigen::Index d = - (D == Eigen::Dynamic) ? static_cast(m.dim()) : D; + const Eigen::Index d = tangentDim(&m, nullptr); + setZeroJacobian(H_this, d); if constexpr (D == Eigen::Dynamic) { - H_this->setZero(d, d); + for (Eigen::Index i = 0; i < d; ++i) { + H_this->col(i) = + Class::adjointMap(TangentVector::Unit(d, i)).transpose() * AdTx; + } } else { - H_this->setZero(); - } - for (Eigen::Index i = 0; i < d; ++i) { - H_this->col(i) = Class::adjointTranspose(TangentVector::Unit(d, i), AdTx); + const auto& basis = adjointBasis(); + for (Eigen::Index i = 0; i < d; ++i) { + H_this->col(i) = basis[static_cast(i)].transpose() * AdTx; + } } } @@ -159,6 +181,38 @@ namespace gtsam { return AdTx; } + /** + * Lie algebra adjoint map ad_xi, with optional specialization in derived + * classes. + */ + static Jacobian adjointMap(const TangentVector& xi) { + const Eigen::Index d = tangentDim(nullptr, &xi); + Jacobian ad; + if constexpr (D == Eigen::Dynamic) { + ad.setZero(d, d); + } else { + ad.setZero(); + } + const auto Xi = Class::Hat(xi); + for (Eigen::Index i = 0; i < d; ++i) { + const auto Ei = Class::Hat(TangentVector::Unit(d, i)); + ad.col(i) = Class::Vee(Xi * Ei - Ei * Xi); + } + return ad; + } + + /** + * Lie algebra action ad_xi(y), with optional Jacobians. + */ + static TangentVector adjoint(const TangentVector& xi, + const TangentVector& y, ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + const Jacobian ad_xi = Class::adjointMap(xi); + if (Hxi) *Hxi = -Class::adjointMap(y); + if (H_y) *H_y = ad_xi; + return ad_xi * y; + } + /** * Dual Lie algebra action ad_xi^T(y), with optional Jacobians. */ @@ -166,40 +220,61 @@ namespace gtsam { const TangentVector& y, ChartJacobian Hxi = {}, ChartJacobian H_y = {}) { - const Jacobian adT_xi = AdjointMapFromHatVee(xi).transpose(); + const Jacobian adT_xi = Class::adjointMap(xi).transpose(); if (Hxi) { - const Eigen::Index d = - (D == Eigen::Dynamic) ? static_cast(xi.size()) : D; + const Eigen::Index d = tangentDim(nullptr, &xi); + setZeroJacobian(Hxi, d); if constexpr (D == Eigen::Dynamic) { - Hxi->setZero(d, d); + for (Eigen::Index i = 0; i < d; ++i) { + Hxi->col(i) = + Class::adjointMap(TangentVector::Unit(d, i)).transpose() * y; + } } else { - Hxi->setZero(); - } - for (Eigen::Index i = 0; i < d; ++i) { - Hxi->col(i) = AdjointMapFromHatVee(TangentVector::Unit(d, i)).transpose() * y; + const auto& basis = adjointBasis(); + for (Eigen::Index i = 0; i < d; ++i) { + Hxi->col(i) = basis[static_cast(i)].transpose() * y; + } } } if (H_y) *H_y = adT_xi; return adT_xi * y; } + /// @} + private: - /// Generic Lie algebra adjoint map from Hat/Vee via matrix commutator. - static Jacobian AdjointMapFromHatVee(const TangentVector& xi) { - const Eigen::Index d = - (D == Eigen::Dynamic) ? static_cast(xi.size()) : D; - Jacobian ad; + static Eigen::Index tangentDim(const Class* m, const TangentVector* xi) { if constexpr (D == Eigen::Dynamic) { - ad.setZero(d, d); + return m ? static_cast(m->dim()) + : static_cast(xi->size()); } else { - ad.setZero(); + (void)m; + (void)xi; + return D; } - const auto Xi = Class::Hat(xi); - for (Eigen::Index i = 0; i < d; ++i) { - const auto Ei = Class::Hat(TangentVector::Unit(d, i)); - ad.col(i) = Class::Vee(Xi * Ei - Ei * Xi); + } + + static void setZeroJacobian(ChartJacobian H, Eigen::Index d) { + if constexpr (D == Eigen::Dynamic) { + H->setZero(d, d); + } else { + (void)d; + H->setZero(); } - return ad; + } + + /// Basis maps ad_{e_i}, cached for fixed-size groups. + template = 0> + static const std::array& adjointBasis() { + static const std::array basis = []() { + std::array B{}; + for (int i = 0; i < DD; ++i) { + B[static_cast(i)] = + Class::adjointMap(TangentVector::Unit(DD, i)); + } + return B; + }(); + return basis; } /// Pre-compute and store vectorized generators for fixed-size groups. @@ -213,10 +288,12 @@ namespace gtsam { namespace internal { - /// Adds LieAlgebra, Hat, Vee, and Vec to LieGroupTraits + /// Adds MatrixLieGroup methods to LieGroupTraits template struct MatrixLieGroupTraits : LieGroupTraits { using LieAlgebra = typename Class::LieAlgebra; using TangentVector = typename LieGroupTraits::TangentVector; + using Jacobian = typename LieGroupTraits::Jacobian; + using ChartJacobian = typename LieGroupTraits::ChartJacobian; static LieAlgebra Hat(const TangentVector& v) { return Class::Hat(v); @@ -233,6 +310,37 @@ namespace gtsam { LieGroupTraits::dimension> H = {}) { return m.vec(H); } + + static TangentVector AdjointTranspose(const Class& m, + const TangentVector& x, + ChartJacobian Hm = {}, + ChartJacobian Hx = {}) { + return m.AdjointTranspose(x, Hm, Hx); + } + + static TangentVector Adjoint(const Class& m, const TangentVector& x, + ChartJacobian Hm = {}, + ChartJacobian Hx = {}) { + return m.Adjoint(x, Hm, Hx); + } + + static Jacobian adjointMap(const TangentVector& xi) { + return Class::adjointMap(xi); + } + + static TangentVector adjoint(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + return Class::adjoint(xi, y, Hxi, H_y); + } + + static TangentVector adjointTranspose(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + return Class::adjointTranspose(xi, y, Hxi, H_y); + } }; /// Both LieGroupTraits and Testable diff --git a/gtsam/base/doc/MatrixLieGroup.md b/gtsam/base/doc/MatrixLieGroup.md index 8699281a35..0ae188ef1f 100644 --- a/gtsam/base/doc/MatrixLieGroup.md +++ b/gtsam/base/doc/MatrixLieGroup.md @@ -29,8 +29,8 @@ You must implement everything from the `LieGroup.md` guide, plus the following: By inheriting from `gtsam::MatrixLieGroup`, you get: -* **A default `AdjointMap()` implementation**: This generic version works by repeatedly calling your `Hat` and `Vee` methods. While it is correct, it is often slow. For performance-critical applications, you should still provide a faster, closed-form `AdjointMap()` implementation in your class, which will override the default. -* **Default dual-adjoint helpers**: `AdjointTranspose(x)` and `adjointTranspose(xi, y)` are provided generically, including Jacobians. +* **Default group adjoint methods**: `AdjointMap()`, `Adjoint(xi)`, and `AdjointTranspose(x)` are provided generically, including Jacobians for the latter two. +* **Default Lie algebra methods**: `adjointMap(xi)`, `adjoint(xi, y)`, and `adjointTranspose(xi, y)` are provided generically, including Jacobians for the latter two. * **A `vec()` method**: This vectorizes the `N x N` matrix representation of your group element into an `(N*N) x 1` vector. ### 4. Traits and Concept Checking diff --git a/gtsam/geometry/Gal3.cpp b/gtsam/geometry/Gal3.cpp index 6e5be3b7c6..abcc05eeb4 100644 --- a/gtsam/geometry/Gal3.cpp +++ b/gtsam/geometry/Gal3.cpp @@ -389,29 +389,6 @@ Gal3::Jacobian Gal3::AdjointMap() const { return Ad; } -//------------------------------------------------------------------------------ -Gal3::TangentVector Gal3::Adjoint(const TangentVector& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const { - Jacobian Ad = AdjointMap(); - TangentVector y = Ad * xi; - - if (H_xi) { - *H_xi = Ad; - } - - if (H_g) { - // NOTE: Using numerical derivative for the Jacobian with respect to - // the group element instead of deriving the analytical expression. - // Future work to use analytical instead. - std::function adjoint_action_wrt_g = - [&](const Gal3& g_in, const TangentVector& xi_in) { - return g_in.Adjoint(xi_in); - }; - *H_g = numericalDerivative21(adjoint_action_wrt_g, *this, xi, 1e-7); - } - return y; -} - -//------------------------------------------------------------------------------ Gal3::Jacobian Gal3::adjointMap(const TangentVector& xi) { // Implements adjoint representation as in Equation 28, Page 10 const Matrix3 Omega = skewSymmetric(xi_w(xi)); @@ -428,16 +405,6 @@ Gal3::Jacobian Gal3::adjointMap(const TangentVector& xi) { return ad; } -//------------------------------------------------------------------------------ -Gal3::TangentVector Gal3::adjoint(const TangentVector& xi, const TangentVector& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) { - Jacobian ad_xi = adjointMap(xi); - if (Hy) *Hy = ad_xi; - if (Hxi) { - *Hxi = -adjointMap(y); - } - return ad_xi * y; -} - //------------------------------------------------------------------------------ Gal3::Jacobian Gal3::ExpmapDerivative(const TangentVector& xi) { Gal3::Jacobian J; diff --git a/gtsam/geometry/Gal3.h b/gtsam/geometry/Gal3.h index a03689ba34..f824898007 100644 --- a/gtsam/geometry/Gal3.h +++ b/gtsam/geometry/Gal3.h @@ -188,17 +188,6 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup { /// Calculate Adjoint map Ad_g Jacobian AdjointMap() const; - /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at - /// identity - TangentVector Adjoint(const TangentVector& xi_base, - OptionalJacobian<10, 10> H_g = {}, - OptionalJacobian<10, 10> H_xi = {}) const; - - /// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y` - static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, - OptionalJacobian<10, 10> Hxi = {}, - OptionalJacobian<10, 10> Hy = {}); - /// Compute the adjoint map `ad(xi)` associated with tangent vector xi static Jacobian adjointMap(const TangentVector& xi); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 074436bcab..5da5c24ab4 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -158,42 +158,11 @@ class GTSAM_EXPORT Pose2: public MatrixLieGroup { */ Matrix3 AdjointMap() const; - /// Apply AdjointMap to twist xi - inline Vector3 Adjoint(const Vector3& xi) const { - return AdjointMap()*xi; - } - /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ static Matrix3 adjointMap(const Vector3& v); - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector3 adjoint(const Vector3& xi, const Vector3& y) { - return adjointMap(xi) * y; - } - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y, - OptionalJacobian<3, 3> Hxi = {}, - OptionalJacobian<3, 3> H_y = {}) { - const Matrix3 adT = adjointMap(xi).transpose(); - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 3; ++i) { - Vector3 dxi = Vector3::Zero(); - dxi(i) = 1.0; - Hxi->col(i) = adjointMap(dxi).transpose() * y; - } - } - if (H_y) *H_y = adT; - return adT * y; - } - // temporary fix for wrappers until case issue is resolved static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);} static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);} diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index ef3867f081..483f64a8ad 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -277,6 +277,23 @@ TEST(Similarity2, adjointTranspose) { EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); } +//****************************************************************************** +TEST(Similarity2, adjoint) { + const Vector4 xi(0.2, -0.4, 0.7, -0.1); + const Vector4 y(-0.3, 0.5, 0.9, -0.2); + + std::function f = + [](const Vector4& x, const Vector4& v) { + return Vector4(Similarity2::adjoint(x, v)); + }; + + Matrix44 Hxi, Hy; + const Vector4 actual = Similarity2::adjoint(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 233fba0832..9ab493ce49 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -689,6 +689,23 @@ TEST(Similarity3, adjointTranspose) { EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); } +//****************************************************************************** +TEST(Similarity3, adjoint) { + const Vector7 xi(0.2, -0.4, 0.7, -0.1, 0.3, -0.6, 0.5); + const Vector7 y(-0.3, 0.5, 0.9, -0.2, 0.4, -0.8, 0.1); + + std::function f = + [](const Vector7& x, const Vector7& v) { + return Vector7(Similarity3::adjoint(x, v)); + }; + + Matrix77 Hxi, Hy; + const Vector7 actual = Similarity3::adjoint(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + //****************************************************************************** int main() { TestResult tr; From 303c38a0b4f78eec7043129340389fb8ace1cb68 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Feb 2026 12:29:25 -0500 Subject: [PATCH 37/41] Wrapper --- gtsam/geometry/Rot3.h | 39 +++++++++++++++ gtsam/geometry/geometry.i | 92 +++++++++++++++++++++++++++++------ gtsam/navigation/navigation.i | 8 ++- 3 files changed, 121 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 31947c6491..c9f6d0d8d1 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -396,6 +396,30 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /** Calculate Adjoint map */ Matrix3 AdjointMap() const { return matrix(); } + /// Apply this element's AdjointMap to a tangent vector. + Vector3 Adjoint(const Vector3& xi, OptionalJacobian<3, 3> H_this = {}, + OptionalJacobian<3, 3> H_xi = {}) const { + const Matrix3 Ad = AdjointMap(); + if (H_this) *H_this = -Ad * adjointMap(xi); + if (H_xi) *H_xi = Ad; + return Ad * xi; + } + + /// Apply the dual adjoint action to a tangent covector. + Vector3 AdjointTranspose(const Vector3& x, + OptionalJacobian<3, 3> H_this = {}, + OptionalJacobian<3, 3> H_x = {}) const { + const Matrix3 AdT = AdjointMap().transpose(); + if (H_this) { + H_this->setZero(); + for (int i = 0; i < 3; ++i) { + H_this->col(i) = adjointMap(Vector3::Unit(i)).transpose() * (AdT * x); + } + } + if (H_x) *H_x = AdT; + return AdT * x; + } + /// Matrix representation of the Lie-algebra adjoint operator ad_xi on so(3). static Matrix3 adjointMap(const Vector3& xi); @@ -404,6 +428,21 @@ class GTSAM_EXPORT Rot3 : public LieGroup { OptionalJacobian<3, 3> Hxi = {}, OptionalJacobian<3, 3> Hy = {}); + /// Apply the dual Lie-algebra adjoint map to y with optional derivatives. + static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y, + OptionalJacobian<3, 3> Hxi = {}, + OptionalJacobian<3, 3> H_y = {}) { + const Matrix3 adT = adjointMap(xi).transpose(); + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 3; ++i) { + Hxi->col(i) = adjointMap(Vector3::Unit(i)).transpose() * y; + } + } + if (H_y) *H_y = adT; + return adT * y; + } + // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE struct GTSAM_EXPORT ChartAtOrigin { static Rot3 Retract(const Vector3& v, ChartJacobian H = {}); diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 28ab12079f..d559ae54a7 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -177,6 +177,12 @@ class Rot2 { gtsam::Vector logmap(const gtsam::Rot2& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -293,6 +299,12 @@ class SO3 { gtsam::Vector3 logmap(const gtsam::SO3& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -332,6 +344,12 @@ class SO4 { gtsam::Vector logmap(const gtsam::SO4& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -373,6 +391,12 @@ class SOn { gtsam::Vector logmap(const gtsam::SOn& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -450,6 +474,12 @@ class Rot3 { gtsam::Vector logmap(const gtsam::Rot3& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -530,13 +560,16 @@ class Pose2 { gtsam::Vector logmap(const gtsam::Pose2& g, Eigen::Ref H1, Eigen::Ref H2); static gtsam::Matrix ExpmapDerivative(gtsam::Vector v); static gtsam::Matrix LogmapDerivative(const gtsam::Pose2& v); + + // Matrix Lie Group gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); static gtsam::Matrix adjointMap_(gtsam::Vector xi); static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y); - static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); - - // Matrix Lie Group gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -624,20 +657,15 @@ class Pose3 { gtsam::Vector logmap(const gtsam::Pose3& g); gtsam::Vector logmap(const gtsam::Pose3& g, Eigen::Ref H1, Eigen::Ref H2); + // Matrix Lie Group gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi_b) const; - gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref H_this, - Eigen::Ref H_xib) const; gtsam::Vector AdjointTranspose(gtsam::Vector x) const; - gtsam::Vector AdjointTranspose(gtsam::Vector x, Eigen::Ref H_this, - Eigen::Ref H_x) const; static gtsam::Matrix adjointMap(gtsam::Vector xi); static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); static gtsam::Matrix adjointMap_(gtsam::Vector xi); static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y); - static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); - - // Matrix Lie Group gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -723,15 +751,17 @@ class ExtendedPose3 { // Lie Group static This Expmap(gtsam::Vector xi); static gtsam::Vector Logmap(const This& pose); - gtsam::Matrix AdjointMap() const; - gtsam::Vector Adjoint(gtsam::Vector xi_b) const; - static gtsam::Matrix adjointMap(gtsam::Vector xi); - static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi); static gtsam::Matrix LogmapDerivative(gtsam::Vector xi); static gtsam::Matrix LogmapDerivative(const This& pose); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi_b) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -782,6 +812,12 @@ class SL4 { gtsam::Vector logmap(const gtsam::SL4& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -1467,13 +1503,22 @@ class Similarity2 { static gtsam::Vector Logmap(const gtsam::Similarity2& S); gtsam::Similarity2 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Similarity2& g); + + // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); + gtsam::Vector vec() const; + gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); static gtsam::Vector Vee(const gtsam::Matrix& X); // Standard Interface bool equals(const gtsam::Similarity2& sim, double tol) const; void print(string s = "") const; - gtsam::Matrix matrix() const; gtsam::Rot2& rotation(); gtsam::Point2& translation(); double scale() const; @@ -1514,13 +1559,22 @@ class Similarity3 { static gtsam::Vector Logmap(const gtsam::Similarity3& s); gtsam::Similarity3 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Similarity3& g); + + // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); + gtsam::Vector vec() const; + gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); static gtsam::Vector Vee(const gtsam::Matrix& X); // Standard Interface bool equals(const gtsam::Similarity3& sim, double tol) const; void print(string s = "") const; - gtsam::Matrix matrix() const; gtsam::Rot3& rotation(); gtsam::Point3& translation(); double scale() const; @@ -1584,6 +1638,12 @@ class Gal3 { gtsam::Vector10 logmap(const gtsam::Gal3& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index edbe8bc822..3530d0e62c 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -90,10 +90,14 @@ class NavState { gtsam::NavState expmap(gtsam::Vector v, Eigen::Ref H1, Eigen::Ref H2); gtsam::Vector logmap(const gtsam::NavState& p); gtsam::Vector logmap(const gtsam::NavState& p, Eigen::Ref H1, Eigen::Ref H2); - gtsam::Matrix AdjointMap() const; - gtsam::Vector Adjoint(gtsam::Vector xi_b) const; // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi_b) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); From fd665716cb1f1da98074b2262517eaf54e17ca08 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Feb 2026 14:44:56 -0500 Subject: [PATCH 38/41] Rot3 -> MatrixLieGroup --- gtsam/geometry/Rot3.cpp | 13 ----------- gtsam/geometry/Rot3.h | 48 ++--------------------------------------- 2 files changed, 2 insertions(+), 59 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index c02c74c067..0d82e5bdb2 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -245,19 +245,6 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { return SO3::LogmapDerivative(x); } -/* ************************************************************************* */ -Matrix3 Rot3::adjointMap(const Vector3& xi) { return Hat(xi); } - -/* ************************************************************************* */ -Vector3 Rot3::adjoint(const Vector3& xi, const Vector3& y, - OptionalJacobian<3, 3> Hxi, - OptionalJacobian<3, 3> Hy) { - const Matrix3 ad_xi = adjointMap(xi); - if (Hxi) *Hxi = -Hat(y); - if (Hy) *Hy = ad_xi; - return ad_xi * y; -} - /* ************************************************************************* */ pair RQ(const Matrix3& A, OptionalJacobian<3, 9> H) { const double x = -atan2(-A(2, 1), A(2, 2)); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index c9f6d0d8d1..5c5b0496cd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -55,7 +55,7 @@ namespace gtsam { * if it is defined. * @ingroup geometry */ -class GTSAM_EXPORT Rot3 : public LieGroup { +class GTSAM_EXPORT Rot3 : public MatrixLieGroup { public: static constexpr size_t MatrixM = 3; private: @@ -396,52 +396,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /** Calculate Adjoint map */ Matrix3 AdjointMap() const { return matrix(); } - /// Apply this element's AdjointMap to a tangent vector. - Vector3 Adjoint(const Vector3& xi, OptionalJacobian<3, 3> H_this = {}, - OptionalJacobian<3, 3> H_xi = {}) const { - const Matrix3 Ad = AdjointMap(); - if (H_this) *H_this = -Ad * adjointMap(xi); - if (H_xi) *H_xi = Ad; - return Ad * xi; - } - - /// Apply the dual adjoint action to a tangent covector. - Vector3 AdjointTranspose(const Vector3& x, - OptionalJacobian<3, 3> H_this = {}, - OptionalJacobian<3, 3> H_x = {}) const { - const Matrix3 AdT = AdjointMap().transpose(); - if (H_this) { - H_this->setZero(); - for (int i = 0; i < 3; ++i) { - H_this->col(i) = adjointMap(Vector3::Unit(i)).transpose() * (AdT * x); - } - } - if (H_x) *H_x = AdT; - return AdT * x; - } - /// Matrix representation of the Lie-algebra adjoint operator ad_xi on so(3). - static Matrix3 adjointMap(const Vector3& xi); - - /// Apply the Lie-algebra adjoint map to y with optional derivatives. - static Vector3 adjoint(const Vector3& xi, const Vector3& y, - OptionalJacobian<3, 3> Hxi = {}, - OptionalJacobian<3, 3> Hy = {}); - - /// Apply the dual Lie-algebra adjoint map to y with optional derivatives. - static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y, - OptionalJacobian<3, 3> Hxi = {}, - OptionalJacobian<3, 3> H_y = {}) { - const Matrix3 adT = adjointMap(xi).transpose(); - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 3; ++i) { - Hxi->col(i) = adjointMap(Vector3::Unit(i)).transpose() * y; - } - } - if (H_y) *H_y = adT; - return adT * y; - } + static Matrix3 adjointMap(const Vector3& xi) { return Hat(xi); } // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE struct GTSAM_EXPORT ChartAtOrigin { From 152d65d3788b9d91918a1a1c3d7c7b9186dd86ad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Feb 2026 14:45:27 -0500 Subject: [PATCH 39/41] Remove generics --- gtsam/geometry/ExtendedPose3-inl.h | 95 ------------------------------ gtsam/geometry/ExtendedPose3.h | 49 --------------- 2 files changed, 144 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h index e4b2973879..8cadbccb52 100644 --- a/gtsam/geometry/ExtendedPose3-inl.h +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -239,46 +239,6 @@ ExtendedPose3::AdjointMap() const { return adj; } -template -typename ExtendedPose3::TangentVector -ExtendedPose3::Adjoint(const TangentVector& xi_b, - ChartJacobian H_this, - ChartJacobian H_xib) const { - const Jacobian Ad = AdjointMap(); - if (H_this) *H_this = -Ad * adjointMap(xi_b); - if (H_xib) *H_xib = Ad; - return Ad * xi_b; -} - -template -typename ExtendedPose3::TangentVector -ExtendedPose3::AdjointTranspose(const TangentVector& x, - ChartJacobian H_this, - ChartJacobian H_x) const { - const Jacobian Ad = AdjointMap(); - const TangentVector AdTx = Ad.transpose() * x; - - if (H_this) { - if constexpr (dimension == Eigen::Dynamic) { - H_this->setZero(x.size(), x.size()); - } else { - H_this->setZero(); - } - - H_this->block(0, 0, 3, 3) = skewSymmetric(AdTx.template head<3>()); - const Eigen::Index k = static_cast(this->k()); - for (Eigen::Index i = 0; i < k; ++i) { - const Eigen::Index idx = 3 + 3 * i; - const Matrix3 x_i_hat = skewSymmetric(AdTx.template segment<3>(idx)); - H_this->block(0, idx, 3, 3) = x_i_hat; - H_this->block(idx, 0, 3, 3) = x_i_hat; - } - } - - if (H_x) *H_x = Ad.transpose(); - return AdTx; -} - template typename ExtendedPose3::Jacobian ExtendedPose3::adjointMap(const TangentVector& xi) { @@ -303,61 +263,6 @@ ExtendedPose3::adjointMap(const TangentVector& xi) { return adj; } -template -typename ExtendedPose3::TangentVector -ExtendedPose3::adjoint(const TangentVector& xi, - const TangentVector& y, ChartJacobian Hxi, - ChartJacobian H_y) { - const Jacobian ad_xi = adjointMap(xi); - if (Hxi) { - if constexpr (dimension == Eigen::Dynamic) { - Hxi->setZero(xi.size(), xi.size()); - } else { - Hxi->setZero(); - } - for (Eigen::Index i = 0; i < xi.size(); ++i) { - TangentVector dxi; - if constexpr (dimension == Eigen::Dynamic) { - dxi = TangentVector::Zero(xi.size()); - } else { - dxi = TangentVector::Zero(); - } - dxi(i) = 1.0; - Hxi->col(i) = adjointMap(dxi) * y; - } - } - if (H_y) *H_y = ad_xi; - return ad_xi * y; -} - -template -typename ExtendedPose3::TangentVector -ExtendedPose3::adjointTranspose(const TangentVector& xi, - const TangentVector& y, - ChartJacobian Hxi, - ChartJacobian H_y) { - if (Hxi) { - if constexpr (dimension == Eigen::Dynamic) { - Hxi->setZero(xi.size(), xi.size()); - } else { - Hxi->setZero(); - } - for (Eigen::Index i = 0; i < xi.size(); ++i) { - TangentVector dxi; - if constexpr (dimension == Eigen::Dynamic) { - dxi = TangentVector::Zero(xi.size()); - } else { - dxi = TangentVector::Zero(); - } - dxi(i) = 1.0; - Hxi->col(i) = adjointMap(dxi).transpose() * y; - } - } - const Jacobian adT_xi = adjointMap(xi).transpose(); - if (H_y) *H_y = adT_xi; - return adT_xi * y; -} - template typename ExtendedPose3::Jacobian ExtendedPose3::ExpmapDerivative(const TangentVector& xi) { diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index c2f6ca43e0..0d121a94b1 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -271,29 +271,6 @@ class ExtendedPose3 */ Jacobian AdjointMap() const; - /** - * Adjoint action on a tangent vector. - * - * @param xi_b Tangent vector in R^dim. - * @param H_this Optional Jacobian in R^(dimxdim). - * @param H_xib Optional Jacobian in R^(dimxdim). - * @return Ad_X xi_b in R^dim. - */ - TangentVector Adjoint(const TangentVector& xi_b, ChartJacobian H_this = {}, - ChartJacobian H_xib = {}) const; - - /** - * Dual adjoint action on a tangent covector. - * - * @param x Tangent vector in R^dim. - * @param H_this Optional Jacobian in R^(dimxdim). - * @param H_x Optional Jacobian in R^(dimxdim). - * @return Ad_X^T x in R^dim. - */ - TangentVector AdjointTranspose(const TangentVector& x, - ChartJacobian H_this = {}, - ChartJacobian H_x = {}) const; - /** * Lie algebra adjoint map. * @@ -302,32 +279,6 @@ class ExtendedPose3 */ static Jacobian adjointMap(const TangentVector& xi); - /** - * Lie bracket action ad_xi(y). - * - * @param xi Tangent vector in R^dim. - * @param y Tangent vector in R^dim. - * @param Hxi Optional Jacobian in R^(dimxdim). - * @param H_y Optional Jacobian in R^(dimxdim). - * @return ad_xi(y) in R^dim. - */ - static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, - ChartJacobian Hxi = {}, ChartJacobian H_y = {}); - - /** - * Dual Lie bracket action ad_xi^T(y). - * - * @param xi Tangent vector in R^dim. - * @param y Tangent vector in R^dim. - * @param Hxi Optional Jacobian in R^(dimxdim). - * @param H_y Optional Jacobian in R^(dimxdim). - * @return ad_xi^T(y) in R^dim. - */ - static TangentVector adjointTranspose(const TangentVector& xi, - const TangentVector& y, - ChartJacobian Hxi = {}, - ChartJacobian H_y = {}); - /** * Jacobian of Expmap. * From c12a51fef10b7296bda5f2c1870a21fe7756c6e3 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 25 Feb 2026 00:04:42 +0000 Subject: [PATCH 40/41] Initial plan From 5515f78a457f3c5c9372caf62a7d4ae61da99ca9 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 25 Feb 2026 00:07:52 +0000 Subject: [PATCH 41/41] Apply review feedback: restore Jacobian overloads, use trait-based dim, add perf note Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com> --- gtsam/base/MatrixLieGroup.h | 2 +- gtsam/base/doc/MatrixLieGroup.md | 2 ++ gtsam/geometry/geometry.i | 4 ++++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/base/MatrixLieGroup.h b/gtsam/base/MatrixLieGroup.h index 89f6732ff9..6987d36406 100644 --- a/gtsam/base/MatrixLieGroup.h +++ b/gtsam/base/MatrixLieGroup.h @@ -245,7 +245,7 @@ namespace gtsam { private: static Eigen::Index tangentDim(const Class* m, const TangentVector* xi) { if constexpr (D == Eigen::Dynamic) { - return m ? static_cast(m->dim()) + return m ? static_cast(traits::GetDimension(*m)) : static_cast(xi->size()); } else { (void)m; diff --git a/gtsam/base/doc/MatrixLieGroup.md b/gtsam/base/doc/MatrixLieGroup.md index 0ae188ef1f..0788b1da5e 100644 --- a/gtsam/base/doc/MatrixLieGroup.md +++ b/gtsam/base/doc/MatrixLieGroup.md @@ -33,6 +33,8 @@ By inheriting from `gtsam::MatrixLieGroup`, you get: * **Default Lie algebra methods**: `adjointMap(xi)`, `adjoint(xi, y)`, and `adjointTranspose(xi, y)` are provided generically, including Jacobians for the latter two. * **A `vec()` method**: This vectorizes the `N x N` matrix representation of your group element into an `(N*N) x 1` vector. +**Performance Note:** The generic implementation of `AdjointMap()` is correct but may be slower because it is derived via the `Hat`/`Vee` mappings. If a closed-form expression for `AdjointMap()` is available for your group, consider overriding the default implementation for better performance. + ### 4. Traits and Concept Checking Finally, the traits specialization in your header file must be updated to reflect that your class is now a `MatrixLieGroup`. diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index d559ae54a7..f282d53c73 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -660,7 +660,11 @@ class Pose3 { // Matrix Lie Group gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi_b) const; + gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref H_this, + Eigen::Ref H_xib) const; gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x, Eigen::Ref H_this, + Eigen::Ref H_x) const; static gtsam::Matrix adjointMap(gtsam::Vector xi); static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);