diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam/geometry/Event.cpp similarity index 96% rename from gtsam_unstable/geometry/Event.cpp rename to gtsam/geometry/Event.cpp index 1262823da..d4fa8632c 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam/geometry/Event.cpp @@ -17,7 +17,7 @@ * @date December 2014 */ -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/geometry/Event.h b/gtsam/geometry/Event.h similarity index 97% rename from gtsam_unstable/geometry/Event.h rename to gtsam/geometry/Event.h index 471ada35b..1bdc39f34 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam/geometry/Event.h @@ -20,7 +20,7 @@ #pragma once #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace gtsam { * SLAM, where we have "time of arrival" measurements at a set of sensors. The * TOA functor below provides a measurement function for those applications. */ -class GTSAM_UNSTABLE_EXPORT Event { +class GTSAM_EXPORT Event { double time_; ///< Time event was generated Point3 location_; ///< Location at time event was generated @@ -84,7 +84,7 @@ template <> struct traits : internal::Manifold {}; /// Time of arrival to given sensor -class TimeOfArrival { +class GTSAM_EXPORT TimeOfArrival { const double speed_; ///< signal speed public: diff --git a/gtsam/geometry/Gal3.cpp b/gtsam/geometry/Gal3.cpp new file mode 100644 index 000000000..081ab2659 --- /dev/null +++ b/gtsam/geometry/Gal3.cpp @@ -0,0 +1,493 @@ +/* ---------------------------------------------------------------------------- + * 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 Gal3.cpp + * @brief Implementation of 3D Galilean Group SGal(3) state + * @authors Matt Kielo, Scott Baker, Frank Dellaert + * @date April 30, 2025 + * + * This implementation is based on the paper: + * Kelly, J. (2023). "All About the Galilean Group SGal(3)" + * arXiv:2312.07555 + * + * All section, equation, and page references in comments throughout this file + * refer to the aforementioned paper. + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +//------------------------------------------------------------------------------ +// Constants and Helper function for Expmap/Logmap +//------------------------------------------------------------------------------ +namespace { // Anonymous namespace for internal linkage + constexpr double kSmallAngleThreshold = 1e-10; + + // The type of the Lie algebra (matrix representation) + using LieAlgebra = Matrix5; + + // Helper functions for accessing tangent vector components + Eigen::Block rho(Vector10& v) { return v.block<3, 1>(0, 0); } + Eigen::Block nu(Vector10& v) { return v.block<3, 1>(3, 0); } + Eigen::Block theta(Vector10& v) { return v.block<3, 1>(6, 0); } + Eigen::Block t_tan(Vector10& v) { return v.block<1, 1>(9, 0); } + // Const versions + Eigen::Block rho(const Vector10& v) { return v.block<3, 1>(0, 0); } + Eigen::Block nu(const Vector10& v) { return v.block<3, 1>(3, 0); } + Eigen::Block theta(const Vector10& v) { return v.block<3, 1>(6, 0); } + Eigen::Block t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); } + +} // end anonymous namespace + +//------------------------------------------------------------------------------ +// Static Constructor/Create functions +//------------------------------------------------------------------------------ +Gal3 Gal3::Create(const Rot3& R, const Point3& r, const Velocity3& v, double t, + OptionalJacobian<10, 3> H1, OptionalJacobian<10, 3> H2, + OptionalJacobian<10, 3> H3, OptionalJacobian<10, 1> H4) { + if (H1) { + H1->setZero(); + H1->block<3, 3>(6, 0) = Matrix3::Identity(); + } + if (H2) { + H2->setZero(); + H2->block<3, 3>(0, 0) = R.transpose(); + } + if (H3) { + H3->setZero(); + H3->block<3, 3>(3, 0) = R.transpose(); + } + if (H4) { + H4->setZero(); + Vector3 drho_dt = -R.transpose() * v; + H4->block<3, 1>(0, 0) = drho_dt; + (*H4)(9, 0) = 1.0; + } + return Gal3(R, r, v, t); +} + +//------------------------------------------------------------------------------ +Gal3 Gal3::FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t, + OptionalJacobian<10, 6> H1, OptionalJacobian<10, 3> H2, + OptionalJacobian<10, 1> H3) { + const Rot3& R = pose.rotation(); + const Point3& r = pose.translation(); + if (H1) { + H1->setZero(); + H1->block<3, 3>(6, 0) = Matrix3::Identity(); + H1->block<3, 3>(0, 3) = Matrix3::Identity(); + } + if (H2) { + H2->setZero(); + H2->block<3, 3>(3, 0) = R.transpose(); + } + if (H3) { + H3->setZero(); + Vector3 drho_dt = -R.transpose() * v; + H3->block<3, 1>(0, 0) = drho_dt; + (*H3)(9, 0) = 1.0; + } + return Gal3(R, r, v, t); +} + +//------------------------------------------------------------------------------ +// Constructors +//------------------------------------------------------------------------------ +Gal3::Gal3(const Matrix5& M) { + // Constructor from 5x5 matrix representation (Equation 9, Page 5) + if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 || + M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) { + throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones."); + } + R_ = Rot3(M.block<3, 3>(0, 0)); + v_ = M.block<3, 1>(0, 3); + r_ = Point3(M.block<3, 1>(0, 4)); + t_ = M(3, 4); +} + +//------------------------------------------------------------------------------ +// Component Access +//------------------------------------------------------------------------------ +const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const { + if (H) { + H->setZero(); + H->block<3, 3>(0, 6) = Matrix3::Identity(); + } + return R_; +} + +//------------------------------------------------------------------------------ +const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const { + if (H) { + H->setZero(); + H->block<3,3>(0, 0) = R_.matrix(); + H->block<3,1>(0, 9) = v_; + } + return r_; +} + +//------------------------------------------------------------------------------ +const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const { + if (H) { + H->setZero(); + H->block<3, 3>(0, 3) = R_.matrix(); + } + return v_; +} + +//------------------------------------------------------------------------------ +const double& Gal3::time(OptionalJacobian<1, 10> H) const { + if (H) { + H->setZero(); + (*H)(0, 9) = 1.0; + } + return t_; +} + +//------------------------------------------------------------------------------ +// Matrix Representation +//------------------------------------------------------------------------------ +Matrix5 Gal3::matrix() const { + // Returns 5x5 matrix representation as in Equation 9, Page 5 + Matrix5 M = Matrix5::Identity(); + M.block<3, 3>(0, 0) = R_.matrix(); + M.block<3, 1>(0, 3) = v_; + M.block<3, 1>(0, 4) = Vector3(r_); + M(3, 4) = t_; + M.block<1,3>(3,0).setZero(); + M.block<1,4>(4,0).setZero(); + return M; +} + +//------------------------------------------------------------------------------ +// Stream operator +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const Gal3& state) { + os << "R: " << state.R_ << "\n"; + os << "r: " << state.r_.transpose() << "\n"; + os << "v: " << state.v_.transpose() << "\n"; + os << "t: " << state.t_; + return os; +} + +//------------------------------------------------------------------------------ +// Testable Requirements +//------------------------------------------------------------------------------ +void Gal3::print(const std::string& s) const { + std::cout << (s.empty() ? "" : s + " "); + std::cout << *this << std::endl; +} + +//------------------------------------------------------------------------------ +bool Gal3::equals(const Gal3& other, double tol) const { + return R_.equals(other.R_, tol) && + traits::Equals(r_, other.r_, tol) && + traits::Equals(v_, other.v_, tol) && + std::abs(t_ - other.t_) < tol; +} + +//------------------------------------------------------------------------------ +// Group Operations +//------------------------------------------------------------------------------ +Gal3 Gal3::inverse() const { + // Implements inverse formula from Equation 10, Page 5 + const Rot3 Rinv = R_.inverse(); + const Velocity3 v_inv = -(Rinv.rotate(v_)); + const Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_)); + const double t_inv = -t_; + return Gal3(Rinv, r_inv, v_inv, t_inv); +} + +//------------------------------------------------------------------------------ +Gal3 Gal3::operator*(const Gal3& other) const { + // Implements group composition through matrix multiplication + const Gal3& g1 = *this; + const Gal3& g2 = other; + + const Rot3 R_comp = g1.R_.compose(g2.R_); + const Vector3 r1_vec(g1.r_); + const Vector3 r2_vec(g2.r_); + const Vector3 r_comp_vec = g1.R_.rotate(r2_vec) + g2.t_ * g1.v_ + r1_vec; + const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_; + const double t_comp = g1.t_ + g2.t_; + + return Gal3(R_comp, Point3(r_comp_vec), v_comp, t_comp); +} + +//------------------------------------------------------------------------------ +// Lie Group Static Functions +//------------------------------------------------------------------------------ +gtsam::Gal3 gtsam::Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) { + // Implements exponential map from Equations 16-19, Pages 7-8 + const Vector3 rho_tan = rho(xi); + const Vector3 nu_tan = nu(xi); + const Vector3 theta_tan = theta(xi); + const double t_tan_val = t_tan(xi)(0); + + const gtsam::so3::DexpFunctor dexp_functor(theta_tan); + const Rot3 R = Rot3::Expmap(theta_tan); + const Matrix3 Jl_theta = dexp_functor.leftJacobian(); + + Matrix3 E; + if (dexp_functor.nearZero) { + // Small angle approximation for E matrix (from Equation 19, Page 8) + E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor.W + (1.0 / 24.0) * dexp_functor.WW; + } else { + // Closed form for E matrix (from Equation 19, Page 8) + const double B_E = (1.0 - 2.0 * dexp_functor.B) / (2.0 * dexp_functor.theta2); + E = 0.5 * Matrix3::Identity() + dexp_functor.C * dexp_functor.W + B_E * dexp_functor.WW; + } + + const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan)); + const Velocity3 v_final = Jl_theta * nu_tan; + const double t_final = t_tan_val; + + Gal3 result(R, r_final, v_final, t_final); + + if (Hxi) { + *Hxi = Gal3::ExpmapDerivative(xi); + } + + return result; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) { + // Implements logarithmic map from Equations 20-23, Page 8 + const Vector3 theta_vec = Rot3::Logmap(g.R_); + const gtsam::so3::DexpFunctor dexp_functor_log(theta_vec); + const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse(); + + Matrix3 E; + if (dexp_functor_log.nearZero) { + // Small angle approximation for E matrix + E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor_log.W + (1.0 / 24.0) * dexp_functor_log.WW; + } else { + // Closed form for E matrix (from Equation 19, Page 8) + const double B_E = (1.0 - 2.0 * dexp_functor_log.B) / (2.0 * dexp_functor_log.theta2); + E = 0.5 * Matrix3::Identity() + dexp_functor_log.C * dexp_functor_log.W + B_E * dexp_functor_log.WW; + } + + const Vector3 r_vec = Vector3(g.r_); + const Velocity3& v_vec = g.v_; + const double& t_val = g.t_; + + // Implementation of Equation 23, Page 8 + const Vector3 nu_tan = Jl_theta_inv * v_vec; + const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan)); + const double t_tan_val = t_val; + + Vector10 xi; + rho(xi) = rho_tan; + nu(xi) = nu_tan; + theta(xi) = theta_vec; + t_tan(xi)(0) = t_tan_val; + + if (Hg) { + *Hg = Gal3::LogmapDerivative(g); + } + + return xi; +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::AdjointMap() const { + // Implements adjoint map as in Equation 26, Page 9 + const Matrix3 Rmat = R_.matrix(); + const Vector3 v_vec = v_; + const Vector3 r_minus_tv = Vector3(r_) - t_ * v_; + + Matrix10 Ad = Matrix10::Zero(); + + Ad.block<3,3>(0,0) = Rmat; + Ad.block<3,3>(0,3) = -t_ * Rmat; + Ad.block<3,3>(0,6) = skewSymmetric(r_minus_tv) * Rmat; + Ad.block<3,1>(0,9) = v_vec; + + Ad.block<3,3>(3,3) = Rmat; + Ad.block<3,3>(3,6) = skewSymmetric(v_vec) * Rmat; + + Ad.block<3,3>(6,6) = Rmat; + + Ad(9,9) = 1.0; + + return Ad; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const { + Matrix10 Ad = AdjointMap(); + Vector10 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 Vector10& xi_in) { + return g_in.Adjoint(xi_in); + }; + *H_g = numericalDerivative21(adjoint_action_wrt_g, *this, xi, 1e-7); + } + return y; +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::adjointMap(const Vector10& xi) { + // Implements adjoint representation as in Equation 28, Page 10 + const Matrix3 Theta_hat = skewSymmetric(theta(xi)); + const Matrix3 Nu_hat = skewSymmetric(nu(xi)); + const Matrix3 Rho_hat = skewSymmetric(rho(xi)); + const double t_val = t_tan(xi)(0); + const Vector3 nu_vec = nu(xi); + + Matrix10 ad = Matrix10::Zero(); + + ad.block<3,3>(0,0) = Theta_hat; + ad.block<3,3>(0,3) = -t_val * Matrix3::Identity(); + ad.block<3,3>(0,6) = Rho_hat; + ad.block<3,1>(0,9) = nu_vec; + + ad.block<3,3>(3,3) = Theta_hat; + ad.block<3,3>(3,6) = Nu_hat; + + ad.block<3,3>(6,6) = Theta_hat; + + return ad; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) { + Matrix10 ad_xi = adjointMap(xi); + if (Hy) *Hy = ad_xi; + if (Hxi) { + *Hxi = -adjointMap(y); + } + return ad_xi * y; +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) { + // Related to the left Jacobian in Equations 31-36, Pages 10-11 + // NOTE: Using numerical approximation instead of implementing the analytical + // expression for the Jacobian. Future work to replace this + // with analytical derivative. + if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); + std::function fn = + [](const Vector10& v) { return Gal3::Expmap(v); }; + return numericalDerivative11(fn, xi, 1e-5); +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::LogmapDerivative(const Gal3& g) { + // Related to the inverse of left Jacobian in Equations 31-36, Pages 10-11 + // NOTE: Using numerical approximation instead of implementing the analytical + // expression for the inverse Jacobian. Future work to replace this + // with analytical derivative. + Vector10 xi = Gal3::Logmap(g); + if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); + std::function fn = + [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + return numericalDerivative11(fn, g, 1e-5); +} + +//------------------------------------------------------------------------------ +// Lie Algebra (Hat/Vee maps) +//------------------------------------------------------------------------------ +Matrix5 Gal3::Hat(const Vector10& xi) { + // Implements hat operator as in Equation 13, Page 6 + const Vector3 rho_tan = rho(xi); + const Vector3 nu_tan = nu(xi); + const Vector3 theta_tan = theta(xi); + const double t_tan_val = t_tan(xi)(0); + + Matrix5 X = Matrix5::Zero(); + X.block<3, 3>(0, 0) = skewSymmetric(theta_tan); + X.block<3, 1>(0, 3) = nu_tan; + X.block<3, 1>(0, 4) = rho_tan; + X(3, 4) = t_tan_val; + return X; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::Vee(const Matrix5& X) { + // Implements vee operator (inverse of hat operator in Equation 13, Page 6) + if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9 || std::abs(X(3,3)) > 1e-9) { + throw std::invalid_argument("Matrix is not in sgal(3)"); + } + + Vector10 xi; + rho(xi) = X.block<3, 1>(0, 4); + nu(xi) = X.block<3, 1>(0, 3); + const Matrix3& S = X.block<3, 3>(0, 0); + theta(xi) << S(2, 1), S(0, 2), S(1, 0); + t_tan(xi)(0) = X(3, 4); + return xi; +} + +//------------------------------------------------------------------------------ +// ChartAtOrigin +//------------------------------------------------------------------------------ +Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) { + return Gal3::Expmap(xi, Hxi); +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) { + return Gal3::Logmap(g, Hg); +} + +//------------------------------------------------------------------------------ +Event Gal3::act(const Event& e, OptionalJacobian<4, 10> Hself, + OptionalJacobian<4, 4> He) const { + // Implements group action on events (spacetime points) as described in Section 4.1, Page 3-4 + const double& t_in = e.time(); + const Point3& p_in = e.location(); + + const double t_out = t_in + t_; + const Point3 p_out = R_.rotate(p_in) + v_ * t_in + r_; + + if (He) { + He->setZero(); + (*He)(0, 0) = 1.0; + He->block<3, 1>(1, 0) = v_; + He->block<3, 3>(1, 1) = R_.matrix(); + } + + if (Hself) { + Hself->setZero(); + const Matrix3 Rmat = R_.matrix(); + + (*Hself)(0, 9) = 1.0; + Hself->block<3, 3>(1, 0) = Rmat; + Hself->block<3, 3>(1, 3) = Rmat * t_in; + Hself->block<3, 3>(1, 6) = -Rmat * skewSymmetric(p_in); + Hself->block<3, 1>(1, 9) = v_; + } + + return Event(t_out, p_out); +} + +} // namespace gtsam diff --git a/gtsam/geometry/Gal3.h b/gtsam/geometry/Gal3.h new file mode 100644 index 000000000..2234562b5 --- /dev/null +++ b/gtsam/geometry/Gal3.h @@ -0,0 +1,235 @@ +/* ---------------------------------------------------------------------------- + * 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 Gal3.h + * @brief 3D Galilean Group SGal(3) state (attitude, position, velocity, time) + * @authors Matt Kielo, Scott Baker, Frank Dellaert + * @date April 30, 2025 + */ + +#pragma once + +#include // Includes Rot3, Point3 +#include +#include // For LieGroup base class and traits +#include // For Manifold traits + +#include // For std::sqrt, std::cos, std::sin +#include // For std::function used in numerical derivatives + +namespace gtsam { + +// Forward declaration +class Gal3; + +// Use Vector3 for velocity for consistency with NavState +using Velocity3 = Vector3; +// Define Vector10 for tangent space +using Vector10 = Eigen::Matrix; +// Define Matrix5 for Lie Algebra matrix representation +using Matrix5 = Eigen::Matrix; +// Define Matrix10 for Jacobians +using Matrix10 = Eigen::Matrix; + +/** + * Represents an element of the 3D Galilean group SGal(3). + * Internal state: rotation, translation, velocity, time. + */ +class GTSAM_EXPORT Gal3 : public LieGroup { + private: + Rot3 R_; ///< Rotation world R body + Point3 r_; ///< Position in world frame, n_r_b + Velocity3 v_; ///< Velocity in world frame, n_v_b + double t_; ///< Time component + + public: + + /// The dimension of the tangent space + inline static constexpr size_t dimension = 10; + + /// @name Constructors + /// @{ + + /// Default constructor: Identity element + Gal3() : R_(Rot3::Identity()), r_(Point3::Zero()), v_(Velocity3::Zero()), t_(0.0) {} + + /// Construct from attitude, position, velocity, time + Gal3(const Rot3& R, const Point3& r, const Velocity3& v, double t) : + R_(R), r_(r), v_(v), t_(t) {} + + /// Construct from a 5x5 matrix representation + explicit Gal3(const Matrix5& M); + + /// Named constructor from components with derivatives + static Gal3 Create(const Rot3& R, const Point3& r, const Velocity3& v, double t, + OptionalJacobian<10, 3> H1 = {}, + OptionalJacobian<10, 3> H2 = {}, + OptionalJacobian<10, 3> H3 = {}, + OptionalJacobian<10, 1> H4 = {}); + + /// Named constructor from Pose3, velocity, and time with derivatives + static Gal3 FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t, + OptionalJacobian<10, 6> H1 = {}, + OptionalJacobian<10, 3> H2 = {}, + OptionalJacobian<10, 1> H3 = {}); + + /// @} + /// @name Component Access + /// @{ + + /// Access rotation component (Rot3) + const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const; + + /// Access translation component (Point3) + const Point3& translation(OptionalJacobian<3, 10> H = {}) const; + + /// Access velocity component (Vector3) + const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const; + + /// Access time component (double) + const double& time(OptionalJacobian<1, 10> H = {}) const; + + // Convenience accessors matching NavState names + const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const { return rotation(H); } + const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); } + + /// @} + /// @name Derived quantities + /// @{ + + /// Return rotation matrix (Matrix3) + Matrix3 R() const { return R_.matrix(); } + + /// Return position as Vector3 + Vector3 r() const { return Vector3(r_); } // Conversion from Point3 + + /// Return velocity as Vector3 + const Vector3& v() const { return v_; } + + /// Return time scalar + const double& t() const { return t_; } + + /// Return 5x5 homogeneous matrix representation + Matrix5 matrix() const; + + /// @} + /// @name Testable + /// @{ + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const Gal3& state); + + /// Print with optional string prefix + void print(const std::string& s = "") const; + + /// Check equality within tolerance + bool equals(const Gal3& other, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// Return the identity element + static Gal3 Identity() { return Gal3(); } + + /// Return the inverse of this element + Gal3 inverse() const; + + // Bring LieGroup::inverse() into scope (version with derivative) + using LieGroup::inverse; + + /// Group composition operator + Gal3 operator*(const Gal3& other) const; + + /// @} + /// @name Group Action + /// @{ + + /** + * Apply Galilean transform to a spacetime Event + * @param e Input event (location, time) + * @param Hself Optional Jacobian wrt this Gal3 element's tangent space + * @param He Optional Jacobian wrt the input event's tangent space + * @return Transformed event + */ + Event act(const Event& e, OptionalJacobian<4, 10> Hself = {}, + OptionalJacobian<4, 4> He = {}) const; + + /// @} + /// @name Lie Group Static Functions + /// @{ + + /// Exponential map at identity: tangent vector xi -> manifold element g + static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {}); + + /// Logarithmic map at identity: manifold element g -> tangent vector xi + static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {}); + + /// Calculate Adjoint map Ad_g + Matrix10 AdjointMap() const; + + /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at identity + Vector10 Adjoint(const Vector10& xi_base, OptionalJacobian<10, 10> H_g = {}, + OptionalJacobian<10, 10> H_xi = {}) const; + + /// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y` + static Vector10 adjoint(const Vector10& xi, const Vector10& y, + OptionalJacobian<10, 10> Hxi = {}, + OptionalJacobian<10, 10> Hy = {}); + + /// Compute the adjoint map `ad(xi)` associated with tangent vector xi + static Matrix10 adjointMap(const Vector10& xi); + + /// Derivative of Expmap(xi) w.r.t. xi evaluated at xi + static Matrix10 ExpmapDerivative(const Vector10& xi); + + /// Derivative of Logmap(g) w.r.t. g + static Matrix10 LogmapDerivative(const Gal3& g); + + /// Chart at origin, uses Expmap/Logmap for Retract/Local + struct ChartAtOrigin { + static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {}); + static Vector10 Local(const Gal3& g, ChartJacobian Hg = {}); + }; + + /// Hat operator: maps tangent vector xi to Lie algebra matrix + static Matrix5 Hat(const Vector10& xi); + + /// Vee operator: maps Lie algebra matrix to tangent vector xi + static Vector10 Vee(const Matrix5& X); + + /// @} + + private: + /// @name 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(r_); + ar & BOOST_SERIALIZATION_NVP(v_); + ar & BOOST_SERIALIZATION_NVP(t_); + } +#endif + /// @} + +}; // class Gal3 + +/// Traits specialization for Gal3 +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +} // namespace gtsam diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 87bdde7c9..1193087ed 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -213,7 +213,7 @@ namespace so3 { ExpmapFunctor(const gtsam::Vector3& axis, double angle); gtsam::Matrix3 expmap() const; }; - + virtual class DexpFunctor : gtsam::so3::ExpmapFunctor { const gtsam::Vector3 omega; const double C; // (1 - A) / theta^2 @@ -228,7 +228,7 @@ namespace so3 { gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const; gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const; gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const; - }; + }; } class SO3 { @@ -427,7 +427,7 @@ class Rot3 { Eigen::Ref HR, Eigen::Ref Hp) const; gtsam::Unit3 unrotate(const gtsam::Unit3& p) const; - + // Standard Interface gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; @@ -522,7 +522,7 @@ class Pose2 { // enabling serialization functionality void serialize() const; }; - + #include class Pose3 { // Standard Constructors @@ -573,12 +573,12 @@ class Pose3 { static gtsam::Matrix6 LogmapDerivative(const gtsam::Pose3& xi); static gtsam::Pose3 Expmap(gtsam::Vector xi, Eigen::Ref Hxi); static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref Hpose); - + gtsam::Pose3 expmap(gtsam::Vector v); gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref H1, Eigen::Ref H2); gtsam::Vector logmap(const gtsam::Pose3& p); gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref H1, Eigen::Ref H2); - + gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi_b) const; gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref H_this, @@ -718,7 +718,7 @@ class OrientedPlane3 { static size_t Dim(); size_t dim() const; - + gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const; gtsam::OrientedPlane3 retract(const gtsam::Vector3& v, Eigen::Ref H) const; @@ -1491,7 +1491,7 @@ gtsam::TriangulationResult triangulateSafe( const gtsam::Point2Vector& measurements, const gtsam::TriangulationParameters& params); -// Cal3Unified versions +// Cal3Unified versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, @@ -1515,6 +1515,17 @@ gtsam::TriangulationResult triangulateSafe( const gtsam::TriangulationParameters& params); +#include +class Event { + Event(); + Event(double t, const gtsam::Point3& p); + Event(double t, double x, double y, double z); + double time() const; + gtsam::Point3 location() const; + double height() const; + void print(string s) const; +}; + #include template diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam/geometry/tests/testEvent.cpp similarity index 98% rename from gtsam_unstable/geometry/tests/testEvent.cpp rename to gtsam/geometry/tests/testEvent.cpp index 25a05793d..07c24a12c 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam/geometry/tests/testEvent.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/gtsam/geometry/tests/testGal3.cpp b/gtsam/geometry/tests/testGal3.cpp new file mode 100644 index 000000000..8235186b9 --- /dev/null +++ b/gtsam/geometry/tests/testGal3.cpp @@ -0,0 +1,1178 @@ +/* ---------------------------------------------------------------------------- + * 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 testGal3.cpp + * @brief Unit tests for Gal3 class + * @authors Matt Kielo, Scott Baker, Frank Dellaert + * @date April 29, 2025 + */ + +#include + +#include +#include +#include // For CHECK_LIE_GROUP_DERIVATIVES, etc. +#include +#include +#include // Included for kTestPose definition + +#include +#include +#include // For std::bind if using numerical derivatives + +using namespace std; +using namespace gtsam; + +// Define tolerance +static const double kTol = 1e-8; + +// Instantiate Testable and Lie concepts for Gal3 +GTSAM_CONCEPT_TESTABLE_INST(gtsam::Gal3) +GTSAM_CONCEPT_LIE_INST(gtsam::Gal3) + +// Define common test values +const Rot3 kTestRot = Rot3::RzRyRx(0.1, -0.2, 0.3); +const Point3 kTestPos(1.0, -2.0, 3.0); +const Velocity3 kTestVel(0.5, 0.6, -0.7); +const double kTestTime = 1.5; +const Pose3 kTestPose(kTestRot, kTestPos); +const Gal3 kTestGal3(kTestRot, kTestPos, kTestVel, kTestTime); + + +/* ************************************************************************* */ +TEST(Gal3, Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); +} + +/* ************************************************************************* */ +// Define test instances for Lie group checks +const Gal3 kTestGal3_Lie1(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, -2.0, 3.0), Velocity3(0.5, 0.6, -0.7), 1.5); +const Gal3 kTestGal3_Lie2(Rot3::RzRyRx(-0.2, 0.3, 0.1), Point3(-2.0, 3.0, 1.0), Velocity3(0.6, -0.7, 0.5), 2.0); +const Gal3 kIdentity_Lie = Gal3::Identity(); + +/* ************************************************************************* */ +// Use GTSAM's Lie Group Test Macros +TEST(Gal3, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(kIdentity_Lie, kIdentity_Lie); + CHECK_LIE_GROUP_DERIVATIVES(kIdentity_Lie, kTestGal3_Lie1); + CHECK_LIE_GROUP_DERIVATIVES(kTestGal3_Lie1, kIdentity_Lie); + CHECK_LIE_GROUP_DERIVATIVES(kTestGal3_Lie1, kTestGal3_Lie2); +} + +/* ************************************************************************* */ +// Check derivatives of retract and localCoordinates +TEST(Gal3, ChartDerivatives) { + CHECK_CHART_DERIVATIVES(kIdentity_Lie, kIdentity_Lie); + CHECK_CHART_DERIVATIVES(kIdentity_Lie, kTestGal3_Lie1); + CHECK_CHART_DERIVATIVES(kTestGal3_Lie1, kIdentity_Lie); + CHECK_CHART_DERIVATIVES(kTestGal3_Lie1, kTestGal3_Lie2); +} + + +/* ************************************************************************* */ +TEST(Gal3, StaticConstructorsValue) { + Gal3 g1 = Gal3::Create(kTestRot, kTestPos, kTestVel, kTestTime); + EXPECT(assert_equal(kTestGal3, g1, kTol)); + + Gal3 g2 = Gal3::FromPoseVelocityTime(kTestPose, kTestVel, kTestTime); + EXPECT(assert_equal(kTestGal3, g2, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, ComponentAccessorsValue) { + EXPECT(assert_equal(kTestRot, kTestGal3.rotation(), kTol)); + EXPECT(assert_equal(kTestRot, kTestGal3.attitude(), kTol)); // Alias + EXPECT(assert_equal(kTestPos, kTestGal3.translation(), kTol)); + EXPECT(assert_equal(kTestPos, kTestGal3.position(), kTol)); // Alias + EXPECT(assert_equal(kTestVel, kTestGal3.velocity(), kTol)); + EXPECT_DOUBLES_EQUAL(kTestTime, kTestGal3.time(), kTol); + EXPECT(assert_equal(kTestRot.matrix(), kTestGal3.R(), kTol)); + EXPECT(assert_equal(kTestPos, kTestGal3.r(), kTol)); + EXPECT(assert_equal(kTestVel, kTestGal3.v(), kTol)); + EXPECT_DOUBLES_EQUAL(kTestTime, kTestGal3.t(), kTol); +} + +/* ************************************************************************* */ +TEST(Gal3, MatrixConstructorValue) { + Matrix5 M_known = kTestGal3.matrix(); + Gal3 g_from_matrix(M_known); + EXPECT(assert_equal(kTestGal3, g_from_matrix, kTol)); + + // Test invalid matrix construction (violating zero structure) + Matrix5 M_invalid_zero = M_known; + M_invalid_zero(4, 0) = 1e-5; + try { + Gal3 g_invalid(M_invalid_zero); + FAIL("Constructor should have thrown for invalid matrix structure (zero violation)."); + } catch (const std::invalid_argument& e) { + // Expected exception + } catch (...) { + FAIL("Constructor threw unexpected exception type for invalid matrix."); + } + + // Test invalid matrix construction (violating M(3,3) == 1) + Matrix5 M_invalid_diag = M_known; + M_invalid_diag(3, 3) = 0.9; + try { + Gal3 g_invalid(M_invalid_diag); + FAIL("Constructor should have thrown for invalid matrix structure (M(3,3) != 1)."); + } catch (const std::invalid_argument& e) { + // Expected exception + } catch (...) { + FAIL("Constructor threw unexpected exception type for invalid matrix."); + } +} + +/* ************************************************************************* */ +/* ************************************************************************* */ +TEST(Gal3, Identity) { + // Original hardcoded expected data (kept for functional equivalence) + const Matrix3 expected_R_mat = Matrix3::Identity(); + const Vector3 expected_r_vec = Vector3::Zero(); + const Vector3 expected_v_vec = Vector3::Zero(); + const double expected_t_val = 0.0; + + Gal3 custom_ident = Gal3::Identity(); + + // Original component checks (kept for functional equivalence) + EXPECT(assert_equal(expected_R_mat, custom_ident.rotation().matrix(), kTol)); + EXPECT(assert_equal(Point3(expected_r_vec), custom_ident.translation(), kTol)); + EXPECT(assert_equal(expected_v_vec, custom_ident.velocity(), kTol)); + EXPECT_DOUBLES_EQUAL(expected_t_val, custom_ident.time(), kTol); + + // Original full object check (kept for functional equivalence) + EXPECT(assert_equal(Gal3(Rot3(expected_R_mat), Point3(expected_r_vec), expected_v_vec, expected_t_val), custom_ident, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, HatVee) { + // Test Case 1 + const Vector10 tau_vec_1 = (Vector10() << + -0.9919387548344049, 0.41796965721894275, -0.08567669832855362, // rho + -0.24728318780816563, 0.42470896104182426, 0.37654216726012074, // nu + -0.4665439537974297, -0.46391731412948783, -0.46638346398428376, // theta + -0.2703091399101363 // t_tan + ).finished(); + const Matrix5 expected_xi_matrix_1 = (Matrix5() << + 0.0, 0.46638346398428376, -0.46391731412948783, -0.24728318780816563, -0.9919387548344049, + -0.46638346398428376, 0.0, 0.4665439537974297, 0.42470896104182426, 0.41796965721894275, + 0.46391731412948783, -0.4665439537974297, 0.0, 0.37654216726012074, -0.08567669832855362, + 0.0, 0.0, 0.0, 0.0, -0.2703091399101363, + 0.0, 0.0, 0.0, 0.0, 0.0 + ).finished(); + + Matrix5 custom_Xi_1 = Gal3::Hat(tau_vec_1); + EXPECT(assert_equal(expected_xi_matrix_1, custom_Xi_1, kTol)); + + Vector10 custom_tau_rec_1 = Gal3::Vee(expected_xi_matrix_1); + EXPECT(assert_equal(tau_vec_1, custom_tau_rec_1, kTol)); + + // Round trip check + Vector10 custom_tau_rec_roundtrip_1 = Gal3::Vee(custom_Xi_1); + EXPECT(assert_equal(tau_vec_1, custom_tau_rec_roundtrip_1, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Expmap) { + // Test Case 1 + const Vector10 tau_vec_1 = (Vector10() << + -0.6659680127970163, 0.0801034296770802, -0.005425197747099379, // rho + -0.24823309993679712, -0.3282613511681929, -0.3614305580886979, // nu + 0.3267045270397072, -0.21318895514136532, -0.1810111529904679, // theta + -0.11137002094819903 // t_tan + ).finished(); + const Matrix3 expected_R_mat_1 = (Matrix3() << + 0.961491754653074, 0.14119138670272793, -0.23579354964696073, + -0.20977429976081094, 0.9313179826319476, -0.297727322203087, + 0.17756223949368974, 0.33572579219851445, 0.925072885538575 + ).finished(); + const Point3 expected_r_vec_1(-0.637321673031978, 0.16116104619552254, -0.03248254605908951); + const Velocity3 expected_v_vec_1(-0.22904001980446076, -0.23988916442951638, -0.4308710747620977); + const double expected_t_val_1 = -0.11137002094819903; + const Gal3 expected_exp_1(Rot3(expected_R_mat_1), expected_r_vec_1, expected_v_vec_1, expected_t_val_1); + + Gal3 custom_exp_1 = Gal3::Expmap(tau_vec_1); + EXPECT(assert_equal(expected_exp_1, custom_exp_1, kTol)); + + // Check derivatives + Matrix10 aH; + Gal3::Expmap(tau_vec_1, aH); + std::function expmap_func = [](const Vector10& v){ return Gal3::Expmap(v); }; + Matrix expectedH = numericalDerivative11(expmap_func, tau_vec_1); + EXPECT(assert_equal(expectedH, aH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(Gal3, Logmap) { + // Test case 1: Logmap(GroupElement) + const Matrix3 input_R_mat_1 = (Matrix3() << + -0.8479395778141634, 0.26601628670932354, -0.4585126035145831, + 0.5159883846729487, 0.612401478276553, -0.5989327310201821, + 0.1214679351060988, -0.744445944720015, -0.6565407650184298 + ).finished(); + const Point3 input_r_vec_1(0.6584021866593519, -0.3393257406257678, -0.5420636579124554); + const Velocity3 input_v_vec_1(0.1772802663861217, 0.3146080790621266, 0.7173535084539808); + const double input_t_val_1 = -0.12088016100268817; + const Gal3 custom_g_1(Rot3(input_R_mat_1), input_r_vec_1, input_v_vec_1, input_t_val_1); + + const Vector10 expected_log_tau_1 = (Vector10() << + -0.6366686897004876, -0.2565186503803428, -1.1108185946230884, // rho + 1.122213550821757, -0.21828427331226408, 0.03100839182220047, // nu + -0.6312616056853186, -2.516056355068653, 1.0844223965979727, // theta + -0.12088016100268817 // t_tan + ).finished(); + + Vector10 custom_log_tau_1 = Gal3::Logmap(custom_g_1); + // Note: Logmap can have higher errors near singularities + EXPECT(assert_equal(expected_log_tau_1, custom_log_tau_1, kTol * 1e3)); + + // Test Log(Exp(tau)) round trip (using data from Expmap test) + const Vector10 tau_vec_orig_rt1 = (Vector10() << + -0.6659680127970163, 0.0801034296770802, -0.005425197747099379, + -0.24823309993679712, -0.3282613511681929, -0.3614305580886979, + 0.3267045270397072, -0.21318895514136532, -0.1810111529904679, + -0.11137002094819903 + ).finished(); + Gal3 g_exp_rt1 = Gal3::Expmap(tau_vec_orig_rt1); + Vector10 tau_log_exp_rt1 = Gal3::Logmap(g_exp_rt1); + EXPECT(assert_equal(tau_vec_orig_rt1, tau_log_exp_rt1, kTol * 10)); + + // Test Exp(Log(g)) round trip (using data from first Logmap test) + Gal3 custom_g_orig_rt2 = custom_g_1; + Vector10 tau_log_rt2 = Gal3::Logmap(custom_g_orig_rt2); + Gal3 g_exp_log_rt2 = Gal3::Expmap(tau_log_rt2); + EXPECT(assert_equal(custom_g_orig_rt2, g_exp_log_rt2, kTol * 10)); +} + +/* ************************************************************************* */ +TEST(Gal3, Compose) { + // Test Case 1 + const Matrix3 g1_R_mat_1 = (Matrix3() << + -0.5427153955878299, 0.8391431164114453, 0.03603927817303032, + 0.8040805715986894, 0.5314810596281534, -0.2664250694549225, + -0.24272295682417533, -0.11561450357036887, -0.963181630220753 + ).finished(); + const Point3 g1_r_vec_1(0.9978490360071179, -0.5634861893781862, 0.025864788808796835); + const Velocity3 g1_v_vec_1(0.04857438013356852, -0.012834026018545996, 0.945550047767139); + const double g1_t_val_1 = -0.41496643117394594; + const Gal3 c1_1(Rot3(g1_R_mat_1), g1_r_vec_1, g1_v_vec_1, g1_t_val_1); + + const Matrix3 g2_R_mat_1 = (Matrix3() << + -0.3264538540162394, 0.24276278793202133, -0.9135064914894779, + 0.9430076454867458, 0.1496317101600385, -0.2972321178273404, + 0.06453264097716288, -0.9584761760784951, -0.27777501352435885 + ).finished(); + const Point3 g2_r_vec_1(-0.1995427558196442, 0.7830589040103644, -0.433370507989717); + const Velocity3 g2_v_vec_1(0.8986541507293722, 0.051990700444202176, -0.8278883042875157); + const double g2_t_val_1 = -0.6155723080111539; + const Gal3 c2_1(Rot3(g2_R_mat_1), g2_r_vec_1, g2_v_vec_1, g2_t_val_1); + + const Matrix3 expected_R_mat_1 = (Matrix3() << + 0.9708156167565788, -0.04073147244077803, 0.23634294026763253, + 0.22150238776832248, 0.5300893429357028, -0.8184998355032984, + -0.09194417042137741, 0.8469629482207595, 0.5236411308011658 + ).finished(); + const Point3 expected_r_vec_1(1.7177230471349891, -0.18439262777314758, -0.18087448280827323); + const Velocity3 expected_v_vec_1(-0.4253479212754224, 0.9579585887030762, 1.5188219826819997); + const double expected_t_val_1 = -1.0305387391850998; + const Gal3 expected_comp_1(Rot3(expected_R_mat_1), expected_r_vec_1, expected_v_vec_1, expected_t_val_1); + + Gal3 custom_comp_1 = c1_1 * c2_1; // Or c1_1.compose(c2_1) + EXPECT(assert_equal(expected_comp_1, custom_comp_1, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Inverse) { + Gal3 expected_identity = Gal3::Identity(); + + // Test Case 1 + const Matrix3 g_R_mat_1 = (Matrix3() << + 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, + 0.6729369985913887, -0.6193062871756463, 0.4044941514923666, + -0.31758898858193396, -0.7357676057205693, -0.5981498680963873 + ).finished(); + const Point3 g_r_vec_1(0.06321286832132045, -0.9214393132931736, -0.12472480681013542); + const Velocity3 g_v_vec_1(0.4770686298036335, 0.2799576331302327, -0.29190264050471715); + const double g_t_val_1 = 0.3757227805330059; + const Gal3 custom_g_1(Rot3(g_R_mat_1), g_r_vec_1, g_v_vec_1, g_t_val_1); + + const Matrix3 expected_inv_R_mat_1 = (Matrix3() << + 0.6680516673568877, 0.6729369985913887, -0.31758898858193396, + 0.2740542884848495, -0.6193062871756463, -0.7357676057205693, + -0.6918101016209183, 0.4044941514923666, -0.5981498680963873 + ).finished(); + const Point3 expected_inv_r_vec_1(0.7635904739613719, -0.6150700906051861, 0.32598918251792036); + const Velocity3 expected_inv_v_vec_1(-0.5998054073176801, -0.17213568846657853, 0.042198146082895516); + const double expected_inv_t_val_1 = -0.3757227805330059; + const Gal3 expected_inv_1(Rot3(expected_inv_R_mat_1), expected_inv_r_vec_1, expected_inv_v_vec_1, expected_inv_t_val_1); + + Gal3 custom_inv_1 = custom_g_1.inverse(); + EXPECT(assert_equal(expected_inv_1, custom_inv_1, kTol)); + + // Check g * g.inverse() == Identity + Gal3 ident_c_1 = custom_g_1 * custom_inv_1; + EXPECT(assert_equal(expected_identity, ident_c_1, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Between) { + // Test Case 1 + const Matrix3 g1_R_mat_1 = (Matrix3() << + -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, + -0.6694062876067332, -0.572463082520484, 0.47347781496466923, + 0.6967527259484512, -0.26267274676776586, 0.6674868290752107 + ).finished(); + const Point3 g1_r_vec_1(0.680375434309419, -0.21123414636181392, 0.5661984475172117); + const Velocity3 g1_v_vec_1(0.536459189623808, -0.44445057839362445, 0.10793991159086103); + const double g1_t_val_1 = -0.0452058962756795; + const Gal3 c1_1(Rot3(g1_R_mat_1), g1_r_vec_1, g1_v_vec_1, g1_t_val_1); + + const Matrix3 g2_R_mat_1 = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + const Point3 g2_r_vec_1(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + const Velocity3 g2_v_vec_1(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + const double g2_t_val_1 = -0.6866418214918308; + const Gal3 c2_1(Rot3(g2_R_mat_1), g2_r_vec_1, g2_v_vec_1, g2_t_val_1); + + // Expected: c1.inverse() * c2 + const Matrix3 expected_R_mat_1 = (Matrix3() << + -0.6566377699430239, 0.753549894443112, -0.0314546605295386, + -0.4242286152401611, -0.3345440819370167, 0.8414929228771536, + 0.6235839326792096, 0.5659000033801861, 0.5393517081447288 + ).finished(); + const Point3 expected_r_vec_1(-0.8113603292280276, 0.06632931940009146, 0.535144598419476); + const Velocity3 expected_v_vec_1(0.8076311151757332, -0.7866187376416414, -0.4028976707214998); + const double expected_t_val_1 = -0.6414359252161513; + const Gal3 expected_btw_1(Rot3(expected_R_mat_1), expected_r_vec_1, expected_v_vec_1, expected_t_val_1); + + Gal3 custom_btw_1 = c1_1.between(c2_1); + EXPECT(assert_equal(expected_btw_1, custom_btw_1, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, MatrixComponents) { + // Test Case 1 + const Matrix3 source_R_mat_1 = (Matrix3() << + 0.43788117516687186, -0.12083239518241493, -0.8908757538001356, + 0.4981128609611659, 0.8575347951217139, 0.12852102124027118, + 0.7484274541861499, -0.5000336063006573, 0.43568651389548174 + ).finished(); + const Point3 source_r_vec_1(0.3684370505476542, 0.8219440615838134, -0.03501868668711683); + const Velocity3 source_v_vec_1(0.7621243390078305, 0.282161192634218, -0.13609316346053646); + const double source_t_val_1 = 0.23919296788014144; + const Gal3 c1(Rot3(source_R_mat_1), source_r_vec_1, source_v_vec_1, source_t_val_1); + + Matrix5 expected_Mc; + expected_Mc << source_R_mat_1, source_v_vec_1, source_r_vec_1, + Vector3::Zero().transpose(), 1.0, source_t_val_1, + Vector4::Zero().transpose(), 1.0; + + Matrix5 Mc = c1.matrix(); + EXPECT(assert_equal(expected_Mc, Mc, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Associativity) { + // Test Case 1 + const Vector10 tau1_1 = (Vector10() << + 0.14491869060866264, -0.21431172810692092, -0.4956042914756127, + -0.13411549788475238, 0.44534636811395767, -0.33281648500962074, + -0.012095339359589743, -0.20104157502639808, 0.08197507996416392, + -0.006285789459736368 + ).finished(); + const Vector10 tau2_1 = (Vector10() << + 0.29551108535517384, 0.2938672197508704, 0.0788811297200055, + -0.07107463852205165, 0.22379088709954872, -0.26508231911443875, + -0.11625916165500054, 0.04661407377867886, 0.1788274027858556, + -0.015243024791797033 + ).finished(); + const Vector10 tau3_1 = (Vector10() << + 0.37646834040234084, 0.3782542871960659, -0.6525520272351378, + 0.005426723127791683, 0.09951505587485733, 0.3813252061414707, + -0.09289643299325814, -0.0017149201262141199, -0.08507973168896384, + -0.1109049754985476 + ).finished(); + + Gal3 g1_1 = Gal3::Expmap(tau1_1); + Gal3 g2_1 = Gal3::Expmap(tau2_1); + Gal3 g3_1 = Gal3::Expmap(tau3_1); + + Gal3 left_assoc_1 = (g1_1 * g2_1) * g3_1; + Gal3 right_assoc_1 = g1_1 * (g2_1 * g3_1); + + // Use slightly larger tolerance for composed operations + EXPECT(assert_equal(left_assoc_1, right_assoc_1, kTol * 10)); +} + +/* ************************************************************************* */ +TEST(Gal3, IdentityProperties) { + Gal3 custom_identity = Gal3::Identity(); + + // Test data + const Matrix3 g_R_mat_1 = (Matrix3() << + -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, + 0.773189425449982, 0.15205374379417114, 0.6156766776243058, + 0.3622989004266723, 0.6908978584436601, -0.6256194178153267 + ).finished(); + const Point3 g_r_vec_1(-0.8716573584227159, -0.9599539022706234, -0.08459652545144625); + const Velocity3 g_v_vec_1(0.7018395735425127, -0.4666685012479632, 0.07952068144433233); + const double g_t_val_1 = -0.24958604725524136; + const Gal3 custom_g_1(Rot3(g_R_mat_1), g_r_vec_1, g_v_vec_1, g_t_val_1); + + // g * g.inverse() == identity + Gal3 g_inv_1 = custom_g_1.inverse(); + Gal3 g_times_inv_1 = custom_g_1 * g_inv_1; + EXPECT(assert_equal(custom_identity, g_times_inv_1, kTol * 10)); // Use slightly larger tol for round trip + + // identity * g == g + Gal3 id_times_g_1 = custom_identity * custom_g_1; + EXPECT(assert_equal(custom_g_1, id_times_g_1, kTol)); + + // g * identity == g + Gal3 g_times_id_1 = custom_g_1 * custom_identity; + EXPECT(assert_equal(custom_g_1, g_times_id_1, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Adjoint) { + // Test data + const Matrix3 g_R_mat_1 = (Matrix3() << + -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, + -0.6694062876067332, -0.572463082520484, 0.47347781496466923, + 0.6967527259484512, -0.26267274676776586, 0.6674868290752107 + ).finished(); + const Point3 g_r_vec_1(0.680375434309419, -0.21123414636181392, 0.5661984475172117); + const Velocity3 g_v_vec_1(0.536459189623808, -0.44445057839362445, 0.10793991159086103); + const double g_t_val_1 = -0.0452058962756795; + const Gal3 test_g(Rot3(g_R_mat_1), g_r_vec_1, g_v_vec_1, g_t_val_1); + + // Expected Adjoint Map + Matrix10 expected_adj_matrix = (Matrix10() << + -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, -0.011651451315476903, 0.03511218083817791, 0.025979828658358354, 0.22110620799336958, 0.3876840721487304, -0.42479976201969083, 0.536459189623808, + -0.6694062876067332, -0.572463082520484, 0.47347781496466923, -0.03026111120383766, -0.025878706730076754, 0.021403988992128208, -0.6381411631187845, 0.6286520658991062, -0.14213043434767314, -0.44445057839362445, + 0.6967527259484512, -0.26267274676776586, 0.6674868290752107, 0.03149733145902263, -0.011874356944831452, 0.03017434036055619, -0.5313038186955878, -0.22369794100291154, 0.4665680546909384, 0.10793991159086103, + 0.0, 0.0, 0.0, -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, -0.23741649654248634, 0.1785366687454684, -0.3477720607401242, 0.0, + 0.0, 0.0, 0.0, -0.6694062876067332, -0.572463082520484, 0.47347781496466923, -0.40160003518135456, 0.22475195574939733, -0.2960463760548867, 0.0, + 0.0, 0.0, 0.0, 0.6967527259484512, -0.26267274676776586, 0.6674868290752107, -0.47366266867570694, 0.03810916679440729, 0.5094272729993004, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6694062876067332, -0.572463082520484, 0.47347781496466923, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.6967527259484512, -0.26267274676776586, 0.6674868290752107, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 + ).finished(); + + Matrix10 computed_adj = test_g.AdjointMap(); + EXPECT(assert_equal(expected_adj_matrix, computed_adj, kTol * 10)); // Slightly larger tolerance + + // Test tangent vector for adjoint action + Vector10 test_tangent = (Vector10() << + -0.28583171387804845, -0.7221038902918132, -0.09516831208249353, + -0.13619637934919504, -0.05432836001072756, 0.1629798883384306, + -0.20194877118636279, -0.18928645162443278, 0.07312685929426145, + -0.042327821984942095 + ).finished(); + + // Expected result after applying adjoint to tangent vector + Vector10 expected_adj_tau = (Vector10() << + -0.7097860882934639, 0.5869666797222274, 0.10746143202899403, + 0.07529021542994252, 0.21635024626679053, 0.15385717129791027, + -0.05294531834013589, 0.27816922833676766, -0.042176749221369034, + -0.042327821984942095 + ).finished(); + + Vector10 computed_adj_tau = test_g.Adjoint(test_tangent); + EXPECT(assert_equal(expected_adj_tau, computed_adj_tau, kTol * 10)); // Slightly larger tolerance + + // Test the adjoint property: Log(g * Exp(tau) * g^-1) = Ad(g) * tau + Gal3 exp_tau = Gal3::Expmap(test_tangent); + Gal3 g_exp_tau_ginv = test_g * exp_tau * test_g.inverse(); + Vector10 log_result = Gal3::Logmap(g_exp_tau_ginv); + + // Expected result for Adjoint(test_tangent) (should match expected_adj_tau) + // Recalculated here from Python code to ensure consistency for this specific check + Vector10 expected_log_result = (Vector10() << + -0.7097860882934638, 0.5869666797222274, 0.10746143202899389, + 0.07529021542994252, 0.21635024626679047, 0.15385717129791018, + -0.05294531834013579, 0.27816922833676755, -0.04217674922136877, + -0.04232782198494209 + ).finished(); + + // Compare Log(g*Exp(tau)*g^-1) with Ad(g)*tau + EXPECT(assert_equal(expected_log_result, log_result, kTol * 10)); // Use larger tolerance for Exp/Log round trip + // Also check against previously calculated Adjoint result + EXPECT(assert_equal(computed_adj_tau, log_result, kTol * 10)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Compose) { + // Test data + Matrix3 g1_R_mat = (Matrix3() << + -0.3264538540162394, 0.24276278793202133, -0.9135064914894779, + 0.9430076454867458, 0.1496317101600385, -0.2972321178273404, + 0.06453264097716288, -0.9584761760784951, -0.27777501352435885 + ).finished(); + Point3 g1_r_vec(-0.1995427558196442, 0.7830589040103644, -0.433370507989717); + Velocity3 g1_v_vec(0.8986541507293722, 0.051990700444202176, -0.8278883042875157); + double g1_t_val = -0.6155723080111539; + Gal3 g1(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + Matrix3 g2_R_mat = (Matrix3() << + -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, + 0.773189425449982, 0.15205374379417114, 0.6156766776243058, + 0.3622989004266723, 0.6908978584436601, -0.6256194178153267 + ).finished(); + Point3 g2_r_vec(-0.8716573584227159, -0.9599539022706234, -0.08459652545144625); + Velocity3 g2_v_vec(0.7018395735425127, -0.4666685012479632, 0.07952068144433233); + double g2_t_val = -0.24958604725524136; + Gal3 g2(Rot3(g2_R_mat), g2_r_vec, g2_v_vec, g2_t_val); + + // Expected Jacobians + Matrix10 expected_H1 = (Matrix10() << + -0.5204974727334908, 0.773189425449982, 0.3622989004266723, 0.12990890682589473, -0.19297729247761214, -0.09042475048241341, -0.2823811043440715, 0.3598327802048799, -1.1736098322206205, 0.6973186192002845, + 0.7067813015326174, 0.15205374379417114, 0.6908978584436601, -0.17640275132344085, -0.03795049288394836, -0.17243846554606443, -0.650366876876537, 0.542434959867202, 0.5459387038042347, -0.4800290630417764, + 0.4791060140322894, 0.6156766776243058, -0.6256194178153267, -0.11957817625853331, -0.15366430835548997, 0.15614587757865273, 0.652649909196605, -0.5858564732208887, -0.07673941868885611, 0.0008110342596663322, + 0.0, 0.0, 0.0, -0.5204974727334908, 0.773189425449982, 0.3622989004266723, -0.23055803486323456, -0.29566601949219695, 0.29975516112150496, 0.0, + 0.0, 0.0, 0.0, 0.7067813015326174, 0.15205374379417114, 0.6908978584436601, -0.3345116854380048, -0.42869572760154795, 0.4365499053963549, 0.0, + 0.0, 0.0, 0.0, 0.4791060140322894, 0.6156766776243058, -0.6256194178153267, 0.24299784710943456, 0.47718330211934956, 0.6556899423712481, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5204974727334908, 0.773189425449982, 0.3622989004266723, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7067813015326174, 0.15205374379417114, 0.6908978584436601, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4791060140322894, 0.6156766776243058, -0.6256194178153267, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 + ).finished(); + Matrix10 expected_H2 = Matrix10::Identity(); + + Matrix10 H1, H2; + g1.compose(g2, H1, H2); + + EXPECT(assert_equal(expected_H1, H1, kTol)); + EXPECT(assert_equal(expected_H2, H2, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Inverse) { + // Test data + Matrix3 g_R_mat = (Matrix3() << + 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, + 0.6729369985913887, -0.6193062871756463, 0.4044941514923666, + -0.31758898858193396, -0.7357676057205693, -0.5981498680963873 + ).finished(); + Point3 g_r_vec(0.06321286832132045, -0.9214393132931736, -0.12472480681013542); + Velocity3 g_v_vec(0.4770686298036335, 0.2799576331302327, -0.29190264050471715); + double g_t_val = 0.3757227805330059; + Gal3 g(Rot3(g_R_mat), g_r_vec, g_v_vec, g_t_val); + + // Expected Jacobian + Matrix10 expected_H = (Matrix10() << + -0.6680516673568877, -0.2740542884848495, 0.6918101016209183, 0.2510022299990406, 0.10296843928652219, -0.2599288149818328, -0.33617296841685484, -0.7460372203093872, -0.6201638436054394, -0.4770686298036335, + -0.6729369985913887, 0.6193062871756463, -0.4044941514923666, 0.252837760234292, -0.23268748021920607, 0.1519776673080509, 0.04690510412308051, 0.0894976987957895, 0.05899296065652178, -0.2799576331302327, + 0.31758898858193396, 0.7357676057205693, 0.5981498680963873, -0.1193254178566693, -0.27644465064744467, -0.22473853161662533, -0.6077563738775408, -0.3532109665151345, 0.7571646227598928, 0.29190264050471715, + -0.0, -0.0, -0.0, -0.6680516673568877, -0.2740542884848495, 0.6918101016209183, -0.10752062523052273, 0.3867608979391727, 0.049383710440088574, -0.0, + -0.0, -0.0, -0.0, -0.6729369985913887, 0.6193062871756463, -0.4044941514923666, 0.04349430207154942, -0.271014473064643, -0.48729973338095034, -0.0, + -0.0, -0.0, -0.0, 0.31758898858193396, 0.7357676057205693, 0.5981498680963873, -0.1340109682602236, 0.3721751918050696, -0.38664898924142477, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.6680516673568877, -0.2740542884848495, 0.6918101016209183, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.6729369985913887, 0.6193062871756463, -0.4044941514923666, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, 0.31758898858193396, 0.7357676057205693, 0.5981498680963873, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -1.0 + ).finished(); + + Matrix10 H; + g.inverse(H); + + EXPECT(assert_equal(expected_H, H, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Logmap) { + const double jac_tol = 1e-5; // Tolerance for Jacobian checks + + // Test Case 1 + const Matrix3 R1_mat = (Matrix3() << + -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, + 0.773189425449982, 0.15205374379417114, 0.6156766776243058, + 0.3622989004266723, 0.6908978584436601, -0.6256194178153267 + ).finished(); + const Point3 r1_vec(-0.8716573584227159, -0.9599539022706234, -0.08459652545144625); + const Velocity3 v1_vec(0.7018395735425127, -0.4666685012479632, 0.07952068144433233); + const double t1_val = -0.24958604725524136; + const Gal3 g1(Rot3(R1_mat), r1_vec, v1_vec, t1_val); + + const Vector10 expected_log_tau1 = (Vector10() << + -1.2604528322799349, -0.3898722924179116, -0.6402392791385879, + -0.34870126525214656, -0.4153032457886797, 1.1791315551702946, + 1.4969846781977756, 2.324590726540746, 1.321595100333433, + -0.24958604725524136 + ).finished(); + + Matrix10 Hg1_gtsam; + Vector10 log1_gtsam = Gal3::Logmap(g1, Hg1_gtsam); + EXPECT(assert_equal(expected_log_tau1, log1_gtsam, kTol)); + + // Verify Jacobian against numerical derivative + std::function logmap_func1 = + [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + Matrix H_numerical1 = numericalDerivative11(logmap_func1, g1, jac_tol); + EXPECT(assert_equal(H_numerical1, Hg1_gtsam, jac_tol)); + + // Test Case 2 + const Matrix3 R2_mat = (Matrix3() << + 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, + 0.6729369985913887,-0.6193062871756463, 0.4044941514923666, + -0.31758898858193396,-0.7357676057205693, -0.5981498680963873 + ).finished(); + const Point3 r2_vec(0.06321286832132045, -0.9214393132931736, -0.12472480681013542); + const Velocity3 v2_vec(0.4770686298036335, 0.2799576331302327, -0.29190264050471715); + const double t2_val = 0.3757227805330059; + const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); + + const Vector10 expected_log_tau2 = (Vector10() << + -0.5687147057967125, -0.3805406510759017, -1.063343079044753, + 0.5179342682694047, 0.3616924279678234, -0.0984011207117694, + -2.215366977027571, -0.72705858167113, 0.7749725693135466, + 0.3757227805330059 + ).finished(); + + Matrix10 Hg2_gtsam; + Vector10 log2_gtsam = Gal3::Logmap(g2, Hg2_gtsam); + EXPECT(assert_equal(expected_log_tau2, log2_gtsam, kTol)); + + // Verify Jacobian against numerical derivative + std::function logmap_func2 = + [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + Matrix H_numerical2 = numericalDerivative11(logmap_func2, g2, jac_tol); + EXPECT(assert_equal(H_numerical2, Hg2_gtsam, jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Expmap) { + // Test data + Vector10 tau_vec = (Vector10() << + 0.1644755309744135, -0.212580759622502, 0.027598787765563664, + 0.06314401798217141, -0.07707725418679535, -0.26078036994807674, + 0.3779854061644677, -0.09638288751073966, -0.2917351793587256, + -0.49338105141355293 + ).finished(); + + // Expected Jacobian + Matrix10 expected_H = (Matrix10() << + 0.9844524218735122, -0.1490063714302354, 0.02908427677999133, -0.24094497482047258, 0.04906744369191897, -0.008766606502586993, 0.0010755746403492512, 0.020896120374367614, 0.09422195803906698, -0.032194210151797825, + 0.13700585416694203, 0.9624511801109918, 0.18991633864490795, -0.04463269623239319, -0.23281449603592955, -0.06241249224896669, -0.05013517822202011, -0.011608679508659724, 0.08214064896800377, 0.05141115662809599, + -0.06540787266535304, -0.1806541475679301, 0.9749387339968734, 0.0221898591040537, 0.05898968326106501, -0.23742922610598202, -0.09935687017740953, -0.06570748233856251, -0.025136525844073204, 0.1253312320983055, + 0.0, 0.0, 0.0, 0.9844524218735122, -0.1490063714302354, 0.02908427677999133, -0.02734050131352483, -0.1309992318097018, 0.01786013841817754, 0.0, + 0.0, 0.0, 0.0, 0.13700585416694203, 0.9624511801109918, 0.18991633864490795, 0.1195266279722227, -0.032519018758919625, 0.035417068913109986, 0.0, + 0.0, 0.0, 0.0, -0.06540787266535304, -0.1806541475679301, 0.9749387339968734, -0.0560073503522914, -0.019830198590275447, -0.010039835763395311, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9844524218735122, -0.1490063714302354, 0.02908427677999133, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13700585416694203, 0.9624511801109918, 0.18991633864490795, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.06540787266535304, -0.1806541475679301, 0.9749387339968734, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 + ).finished(); + + Matrix10 H; + Gal3::Expmap(tau_vec, H); + + EXPECT(assert_equal(expected_H, H, kTol)); +} + +/* ************************************************************************* */ +// Test Between Jacobian against numerical derivatives +TEST(Gal3, Jacobian_Between) { + // Test data + Matrix3 g1_R_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + Point3 g1_r_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + Velocity3 g1_v_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + double g1_t_val = -0.6866418214918308; + Gal3 g1(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + Matrix3 g2_R_mat = (Matrix3() << + -0.5427153955878299, 0.8391431164114453, 0.03603927817303032, + 0.8040805715986894, 0.5314810596281534, -0.2664250694549225, + -0.24272295682417533, -0.11561450357036887, -0.963181630220753 + ).finished(); + Point3 g2_r_vec(0.9978490360071179, -0.5634861893781862, 0.025864788808796835); + Velocity3 g2_v_vec(0.04857438013356852, -0.012834026018545996, 0.945550047767139); + double g2_t_val = -0.41496643117394594; + Gal3 g2(Rot3(g2_R_mat), g2_r_vec, g2_v_vec, g2_t_val); + + Matrix10 H1_analytical, H2_analytical; + g1.between(g2, H1_analytical, H2_analytical); + + std::function between_func = + [](const Gal3& g1_in, const Gal3& g2_in) { return g1_in.between(g2_in); }; + + // Verify H1 + Matrix H1_numerical = numericalDerivative21(between_func, g1, g2, 1e-6); + EXPECT(assert_equal(H1_numerical, H1_analytical, 1e-5)); // Tolerance for numerical comparison + + // Verify H2 + Matrix H2_numerical = numericalDerivative22(between_func, g1, g2, 1e-6); + EXPECT(assert_equal(H2_numerical, H2_analytical, 1e-5)); // Tolerance for numerical comparison +} + + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_AdjointMap) { + // Test data + Matrix3 g1_R_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + Point3 g1_r_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + Velocity3 g1_v_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + double g1_t_val = -0.6866418214918308; + Gal3 g = Gal3(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + Vector10 xi = (Vector10() << 0.1, -0.2, 0.3, 0.4, -0.5, 0.6, -0.1, 0.15, -0.25, 0.9).finished(); + + // Analytical Adjoint Map + Matrix10 Ad_analytical = g.AdjointMap(); + + // Numerical derivative of Adjoint action Ad_g(xi) w.r.t xi should equal Adjoint Map + std::function adjoint_action = + [](const Gal3& g_in, const Vector10& xi_in) { + return g_in.Adjoint(xi_in); + }; + Matrix H_xi_numerical = numericalDerivative22(adjoint_action, g, xi); + EXPECT(assert_equal(Ad_analytical, H_xi_numerical, 1e-7)); // Tolerance for numerical diff + + // Verify derivative of Adjoint action Ad_g(xi) w.r.t g + Matrix10 H_g_analytical; + g.Adjoint(xi, H_g_analytical); // Calculate analytical H_g + + // Need wrapper that only returns value for numericalDerivative21 + std::function adjoint_action_wrt_g_val = + [](const Gal3& g_in, const Vector10& xi_in) { + return g_in.Adjoint(xi_in); // Return only value + }; + Matrix H_g_numerical = numericalDerivative21(adjoint_action_wrt_g_val, g, xi); + EXPECT(assert_equal(H_g_analytical, H_g_numerical, 1e-7)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Inverse2) { + // Test data + Matrix3 g1_R_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + Point3 g1_r_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + Velocity3 g1_v_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + double g1_t_val = -0.6866418214918308; + Gal3 g = Gal3(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + // Analytical Jacobian H_inv = d(g.inverse()) / d(g) + Matrix10 H_inv_analytical; + g.inverse(H_inv_analytical); + + // Numerical Jacobian + std::function inverse_func = + [](const Gal3& g_in) { return g_in.inverse(); }; + Matrix H_inv_numerical = numericalDerivative11(inverse_func, g); + + EXPECT(assert_equal(H_inv_numerical, H_inv_analytical, 1e-5)); + + Matrix10 expected_adjoint = -g.AdjointMap(); // Check against -Ad(g) for right perturbations + EXPECT(assert_equal(expected_adjoint, H_inv_analytical, 1e-8)); + +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Compose2) { + // Test data + Matrix3 g1_R_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + Point3 g1_r_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + Velocity3 g1_v_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + double g1_t_val = -0.6866418214918308; + Gal3 g1(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + Matrix3 g2_R_mat = (Matrix3() << + -0.5427153955878299, 0.8391431164114453, 0.03603927817303032, + 0.8040805715986894, 0.5314810596281534, -0.2664250694549225, + -0.24272295682417533, -0.11561450357036887, -0.963181630220753 + ).finished(); + Point3 g2_r_vec(0.9978490360071179, -0.5634861893781862, 0.025864788808796835); + Velocity3 g2_v_vec(0.04857438013356852, -0.012834026018545996, 0.945550047767139); + double g2_t_val = -0.41496643117394594; + Gal3 g2(Rot3(g2_R_mat), g2_r_vec, g2_v_vec, g2_t_val); + + // Analytical Jacobians + Matrix10 H1_analytical, H2_analytical; + g1.compose(g2, H1_analytical, H2_analytical); + + // Numerical Jacobians + std::function compose_func = + [](const Gal3& g1_in, const Gal3& g2_in) { return g1_in.compose(g2_in); }; + Matrix H1_numerical = numericalDerivative21(compose_func, g1, g2); + Matrix H2_numerical = numericalDerivative22(compose_func, g1, g2); + + EXPECT(assert_equal(H1_numerical, H1_analytical, 1e-5)); + EXPECT(assert_equal(H2_numerical, H2_analytical, 1e-5)); + + // Check analytical Jacobians against theoretical formulas + EXPECT(assert_equal(g2.inverse().AdjointMap(), H1_analytical, 1e-8)); + EXPECT(assert_equal(gtsam::Matrix10(Matrix10::Identity()), H2_analytical, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Gal3, Act) { + const double kTol = 1e-9; // Tolerance for value checks + const double jac_tol = 1e-6; // Tolerance for Jacobian checks + + // Test Case 1 Data + const Matrix3 R1_mat = (Matrix3() << + -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, + -0.6694062876067332, -0.572463082520484, 0.47347781496466923, + 0.6967527259484512, -0.26267274676776586, 0.6674868290752107 + ).finished(); + const Point3 r1_vec(0.680375434309419, -0.21123414636181392, 0.5661984475172117); + const Velocity3 v1_vec(0.536459189623808, -0.44445057839362445, 0.10793991159086103); + const double t1_val = -0.0452058962756795; + const Gal3 g1(Rot3(R1_mat), r1_vec, v1_vec, t1_val); + const Point3 point_in1(4.967141530112327, -1.3826430117118464, 6.476885381006925); + const double time_in1 = 0.0; + const Event e_in1(time_in1, point_in1); + const Point3 expected_p_out1 = g1.rotation().rotate(point_in1) + g1.velocity() * time_in1 + g1.translation(); + const double expected_t_out1 = time_in1 + t1_val; + + Matrix H1g_gtsam = Matrix::Zero(4, 10); + Matrix H1e_gtsam = Matrix::Zero(4, 4); + Event e_out1_gtsam = g1.act(e_in1, H1g_gtsam, H1e_gtsam); + + EXPECT(assert_equal(expected_p_out1, e_out1_gtsam.location(), kTol)); + EXPECT_DOUBLES_EQUAL(expected_t_out1, e_out1_gtsam.time(), kTol); + + // Verify H1g: Derivative of output Event wrt Gal3 g1 + std::function act_func1_loc_wrt_g = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).location(); }; + Matrix H1g_loc_numerical = numericalDerivative21(act_func1_loc_wrt_g, g1, e_in1, jac_tol); + EXPECT(assert_equal(H1g_loc_numerical, H1g_gtsam.block<3, 10>(1, 0), jac_tol)); + + std::function act_func1_time_wrt_g = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).time(); }; + Matrix H1g_time_numerical = numericalDerivative21(act_func1_time_wrt_g, g1, e_in1, jac_tol); + EXPECT(assert_equal(H1g_time_numerical, H1g_gtsam.row(0), jac_tol)); + + // Verify H1e: Derivative of output Event wrt Event e1 + std::function act_func1_loc_wrt_e = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).location(); }; + Matrix H1e_loc_numerical = numericalDerivative22(act_func1_loc_wrt_e, g1, e_in1, jac_tol); + EXPECT(assert_equal(H1e_loc_numerical, H1e_gtsam.block<3, 4>(1, 0), jac_tol)); + + std::function act_func1_time_wrt_e = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).time(); }; + Matrix H1e_time_numerical = numericalDerivative22(act_func1_time_wrt_e, g1, e_in1, jac_tol); + EXPECT(assert_equal(H1e_time_numerical, H1e_gtsam.row(0), jac_tol)); + + // Test Case 2 Data + const Matrix3 R2_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + const Point3 r2_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + const Velocity3 v2_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + const double t2_val = -0.6866418214918308; + const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); + const Point3 point_in2(15.230298564080254, -2.3415337472333597, -2.3413695694918055); + const double time_in2 = 0.0; + const Event e_in2(time_in2, point_in2); + const Point3 expected_p_out2 = g2.rotation().rotate(point_in2) + g2.velocity() * time_in2 + g2.translation(); + const double expected_t_out2 = time_in2 + t2_val; + + Matrix H2g_gtsam = Matrix::Zero(4, 10); + Matrix H2e_gtsam = Matrix::Zero(4, 4); + Event e_out2_gtsam = g2.act(e_in2, H2g_gtsam, H2e_gtsam); + + EXPECT(assert_equal(expected_p_out2, e_out2_gtsam.location(), kTol)); + EXPECT_DOUBLES_EQUAL(expected_t_out2, e_out2_gtsam.time(), kTol); + + // Verify H2g: Derivative of output Event wrt Gal3 g2 + std::function act_func2_loc_wrt_g = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).location(); }; + Matrix H2g_loc_numerical = numericalDerivative21(act_func2_loc_wrt_g, g2, e_in2, jac_tol); + EXPECT(assert_equal(H2g_loc_numerical, H2g_gtsam.block<3, 10>(1, 0), jac_tol)); + + std::function act_func2_time_wrt_g = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).time(); }; + Matrix H2g_time_numerical = numericalDerivative21(act_func2_time_wrt_g, g2, e_in2, jac_tol); + EXPECT(assert_equal(H2g_time_numerical, H2g_gtsam.row(0), jac_tol)); + + // Verify H2e: Derivative of output Event wrt Event e2 + std::function act_func2_loc_wrt_e = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).location(); }; + Matrix H2e_loc_numerical = numericalDerivative22(act_func2_loc_wrt_e, g2, e_in2, jac_tol); + EXPECT(assert_equal(H2e_loc_numerical, H2e_gtsam.block<3, 4>(1, 0), jac_tol)); + + std::function act_func2_time_wrt_e = + [](const Gal3& g_in, const Event& e_in) { return g_in.act(e_in).time(); }; + Matrix H2e_time_numerical = numericalDerivative22(act_func2_time_wrt_e, g2, e_in2, jac_tol); + EXPECT(assert_equal(H2e_time_numerical, H2e_gtsam.row(0), jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Interpolate) { + const double interp_tol = 1e-6; // Tolerance for interpolation value comparison + + // Test data + const Matrix3 R1_mat = (Matrix3() << + -0.5427153955878299, 0.8391431164114453, 0.03603927817303032, + 0.8040805715986894, 0.5314810596281534, -0.2664250694549225, + -0.24272295682417533,-0.11561450357036887,-0.963181630220753 + ).finished(); + const Point3 r1_vec(0.9978490360071179, -0.5634861893781862, 0.025864788808796835); + const Velocity3 v1_vec(0.04857438013356852, -0.012834026018545996, 0.945550047767139); + const double t1_val = -0.41496643117394594; + const Gal3 g1(Rot3(R1_mat), r1_vec, v1_vec, t1_val); + + const Matrix3 R2_mat = (Matrix3() << + -0.3264538540162394, 0.24276278793202133, -0.9135064914894779, + 0.9430076454867458, 0.1496317101600385, -0.2972321178273404, + 0.06453264097716288,-0.9584761760784951, -0.27777501352435885 + ).finished(); + const Point3 r2_vec(-0.1995427558196442, 0.7830589040103644, -0.433370507989717); + const Velocity3 v2_vec(0.8986541507293722, 0.051990700444202176, -0.8278883042875157); + const double t2_val = -0.6155723080111539; + const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); + + // Expected results for different alphas + const Gal3 expected_alpha0_00 = g1; + const Gal3 expected_alpha0_25(Rot3(-0.5305293740379828, 0.8049951577356123, -0.26555861745588893, + 0.8357380608208966, 0.444360967494815, -0.32262241748272774, + -0.14170559967126867, -0.39309811318459514,-0.9085116380281089), + Point3(0.7319725410358775, -0.24509083842031143, -0.20526665070473993), + Velocity3(0.447233815335834, 0.08156238532481315, 0.6212732810121263), + -0.46511790038324796); + const Gal3 expected_alpha0_50(Rot3(-0.4871812472793253, 0.6852678695634308, -0.5413523614461815, + 0.8717937932763143, 0.34522257149684665,-0.34755856791913387, + -0.051283665082120816,-0.6412716453057169, -0.7655982383878919), + Point3(0.4275876127256323, 0.0806397132393819, -0.3622648631703127), + Velocity3(0.7397753319939113, 0.12027591889377548, 0.19009330402180313), + -0.5152693695925499); + const Gal3 expected_alpha0_75(Rot3(-0.416880477991759, 0.49158776471130083,-0.7645600935541361, + 0.9087464582621175, 0.24369303223618222,-0.338812013712018, + 0.019762127046961175,-0.8360353913714909, -0.5483195078682666), + Point3(0.10860773480095642, 0.42140693978775756, -0.4380637787537704), + Velocity3(0.895925123398753, 0.10738792759782673, -0.3083508669060544), + -0.5654208388018519); + const Gal3 expected_alpha1_00 = g2; + + // Compare values (requires Gal3::interpolate implementation) + EXPECT(assert_equal(expected_alpha0_00, gtsam::interpolate(g1, g2, 0.0), interp_tol)); + EXPECT(assert_equal(expected_alpha0_25, gtsam::interpolate(g1, g2, 0.25), interp_tol)); + EXPECT(assert_equal(expected_alpha0_50, gtsam::interpolate(g1, g2, 0.5), interp_tol)); + EXPECT(assert_equal(expected_alpha0_75, gtsam::interpolate(g1, g2, 0.75), interp_tol)); + EXPECT(assert_equal(expected_alpha1_00, gtsam::interpolate(g1, g2, 1.0), interp_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_StaticConstructors) { + // Verify analytical Jacobians of constructors against numerical derivatives. + // Assumes Gal3::Create and Gal3::FromPoseVelocityTime implement these Jacobians. + const double jac_tol = 1e-7; // Tolerance for Jacobian checks + + // Test Gal3::Create + gtsam::Matrix H1_ana, H2_ana, H3_ana, H4_ana; + Gal3::Create(kTestRot, kTestPos, kTestVel, kTestTime, H1_ana, H2_ana, H3_ana, H4_ana); + + std::function create_func = + [](const Rot3& R, const Point3& r, const Velocity3& v, const double& t){ + return Gal3::Create(R, r, v, t); // Call without Jacobians for numerical diff + }; + + const double& time_ref = kTestTime; // Needed for lambda capture if using C++11/14 + Matrix H1_num = numericalDerivative41(create_func, kTestRot, kTestPos, kTestVel, time_ref, jac_tol); + Matrix H2_num = numericalDerivative42(create_func, kTestRot, kTestPos, kTestVel, time_ref, jac_tol); + Matrix H3_num = numericalDerivative43(create_func, kTestRot, kTestPos, kTestVel, time_ref, jac_tol); + Matrix H4_num = numericalDerivative44(create_func, kTestRot, kTestPos, kTestVel, time_ref, jac_tol); + + EXPECT(assert_equal(H1_num, H1_ana, jac_tol)); + EXPECT(assert_equal(H2_num, H2_ana, jac_tol)); + EXPECT(assert_equal(H3_num, H3_ana, jac_tol)); + EXPECT(assert_equal(H4_num, H4_ana, jac_tol)); + + // Test Gal3::FromPoseVelocityTime + gtsam::Matrix Hpv1_ana, Hpv2_ana, Hpv3_ana; + Gal3::FromPoseVelocityTime(kTestPose, kTestVel, kTestTime, Hpv1_ana, Hpv2_ana, Hpv3_ana); + + std::function fromPoseVelTime_func = + [](const Pose3& pose, const Velocity3& v, const double& t){ + return Gal3::FromPoseVelocityTime(pose, v, t); // Call without Jacobians + }; + + Matrix Hpv1_num = numericalDerivative31(fromPoseVelTime_func, kTestPose, kTestVel, time_ref, jac_tol); + Matrix Hpv2_num = numericalDerivative32(fromPoseVelTime_func, kTestPose, kTestVel, time_ref, jac_tol); + Matrix Hpv3_num = numericalDerivative33(fromPoseVelTime_func, kTestPose, kTestVel, time_ref, jac_tol); + + EXPECT(assert_equal(Hpv1_num, Hpv1_ana, jac_tol)); + EXPECT(assert_equal(Hpv2_num, Hpv2_ana, jac_tol)); + EXPECT(assert_equal(Hpv3_num, Hpv3_ana, jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Accessors) { + // Verify analytical Jacobians of accessors against numerical derivatives. + // Assumes accessors implement these Jacobians. + const double jac_tol = 1e-7; + + // Test rotation() / attitude() Jacobian + gtsam::Matrix Hrot_ana; + kTestGal3.rotation(Hrot_ana); + std::function rot_func = + [](const Gal3& g) { return g.rotation(); }; + Matrix Hrot_num = numericalDerivative11(rot_func, kTestGal3, jac_tol); + EXPECT(assert_equal(Hrot_num, Hrot_ana, jac_tol)); + + // Test translation() / position() Jacobian + gtsam::Matrix Hpos_ana; + kTestGal3.translation(Hpos_ana); + std::function pos_func = + [](const Gal3& g) { return g.translation(); }; + Matrix Hpos_num = numericalDerivative11(pos_func, kTestGal3, jac_tol); + EXPECT(assert_equal(Hpos_num, Hpos_ana, jac_tol)); + + // Test velocity() Jacobian + gtsam::Matrix Hvel_ana; + kTestGal3.velocity(Hvel_ana); + std::function vel_func = + [](const Gal3& g) { return g.velocity(); }; + Matrix Hvel_num = numericalDerivative11(vel_func, kTestGal3, jac_tol); + EXPECT(assert_equal(Hvel_num, Hvel_ana, jac_tol)); + + // Test time() Jacobian + gtsam::Matrix Htime_ana; + kTestGal3.time(Htime_ana); + std::function time_func = + [](const Gal3& g) { return g.time(); }; + Matrix Htime_num = numericalDerivative11(time_func, kTestGal3, jac_tol); + EXPECT(assert_equal(Htime_num, Htime_ana, jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Interpolate) { + // *** CIRCULARITY WARNING *** + // Gal3::interpolate computes its Jacobians using a chain rule involving + // Logmap/Expmap Jacobians. Those are currently numerical, so this test verifies + // the chain rule implementation assuming the underlying derivatives are correct. + const double jac_tol = 1e-7; + const Gal3 g1 = kTestGal3_Lie1; + const Gal3 g2 = kTestGal3_Lie2; + const double alpha = 0.3; + + gtsam::Matrix H1_ana, H2_ana, Ha_ana; + gtsam::interpolate(g1, g2, alpha, H1_ana, H2_ana, Ha_ana); + + std::function interp_func = + [](const Gal3& g1_in, const Gal3& g2_in, const double& a){ + return gtsam::interpolate(g1_in, g2_in, a); // Call without Jacobians + }; + + const double& alpha_ref = alpha; // Needed for lambda capture if using C++11/14 + Matrix H1_num = numericalDerivative31(interp_func, g1, g2, alpha_ref, jac_tol); + Matrix H2_num = numericalDerivative32(interp_func, g1, g2, alpha_ref, jac_tol); + Matrix Ha_num = numericalDerivative33(interp_func, g1, g2, alpha_ref, jac_tol); + + EXPECT(assert_equal(H1_num, H1_ana, jac_tol)); + EXPECT(assert_equal(H2_num, H2_ana, jac_tol)); + EXPECT(assert_equal(Ha_num, Ha_ana, jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, StaticAdjoint) { + // Verify static adjointMap and adjoint Jacobians against numerical derivatives + // and analytical properties. Assumes static adjoint provides analytical Jacobians. + const double jac_tol = 1e-7; + Vector10 xi = (Vector10() << 0.1, -0.2, 0.3, 0.4, -0.5, 0.6, -0.1, 0.15, -0.25, 0.9).finished(); + Vector10 y = (Vector10() << -0.5, 0.4, -0.3, 0.2, -0.1, 0.0, 0.3, -0.35, 0.45, -0.1).finished(); + + // Test static adjointMap + Matrix10 ad_xi_map = Gal3::adjointMap(xi); + Vector10 ad_xi_map_times_y = ad_xi_map * y; + Vector10 ad_xi_y_direct = Gal3::adjoint(xi, y); + EXPECT(assert_equal(ad_xi_map_times_y, ad_xi_y_direct, kTol)); // Check ad(xi)*y == [xi,y] + + // Verify d(adjoint(xi,y))/dy == adjointMap(xi) numerically + std::function static_adjoint_func = + [](const Vector10& xi_in, const Vector10& y_in){ + return Gal3::adjoint(xi_in, y_in); // Call without Jacobians + }; + Matrix Hy_num = numericalDerivative22(static_adjoint_func, xi, y, jac_tol); + EXPECT(assert_equal(ad_xi_map, Hy_num, jac_tol)); + + // Test static adjoint Jacobians + gtsam::Matrix Hxi_ana, Hy_ana; + Gal3::adjoint(xi, y, Hxi_ana, Hy_ana); // Get analytical Jacobians + + EXPECT(assert_equal(ad_xi_map, Hy_ana, kTol)); // Check analytical Hy vs ad(xi) + EXPECT(assert_equal(-Gal3::adjointMap(y), Hxi_ana, kTol)); // Check analytical Hxi vs -ad(y) + + // Verify analytical Hxi against numerical derivative + Matrix Hxi_num = numericalDerivative21(static_adjoint_func, xi, y, jac_tol); + EXPECT(assert_equal(Hxi_num, Hxi_ana, jac_tol)); + + // Verify Jacobi Identity: [x, [y, z]] + [y, [z, x]] + [z, [x, y]] = 0 + Vector10 z = (Vector10() << 0.7, 0.8, 0.9, -0.1, -0.2, -0.3, 0.5, 0.6, 0.7, 0.1).finished(); + Vector10 term1 = Gal3::adjoint(xi, Gal3::adjoint(y, z)); + Vector10 term2 = Gal3::adjoint(y, Gal3::adjoint(z, xi)); + Vector10 term3 = Gal3::adjoint(z, Gal3::adjoint(xi, y)); + Vector10 sum_terms = term1 + term2 + term3; + EXPECT(assert_equal(Vector10(Vector10::Zero()), sum_terms, jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, ExpLog_NearZero) { + const double kTolRoundtrip = kTol * 100; // Adjusted tolerance for round-trip + + // Small non-zero tangent vector 1 + Vector10 xi_small1; + xi_small1 << 1e-5, -2e-5, 1.5e-5, -3e-5, 4e-5, -2.5e-5, 1e-7, -2e-7, 1.5e-7, -5e-5; + Gal3 g_small1 = Gal3::Expmap(xi_small1); + Vector10 xi_recovered1 = Gal3::Logmap(g_small1); + EXPECT(assert_equal(xi_small1, xi_recovered1, kTolRoundtrip)); + + // Small non-zero tangent vector 2 + Vector10 xi_small2; + xi_small2 << -5e-6, 1e-6, -4e-6, 2e-6, -6e-6, 1e-6, -5e-8, 8e-8, -2e-8, 9e-6; + Gal3 g_small2 = Gal3::Expmap(xi_small2); + Vector10 xi_recovered2 = Gal3::Logmap(g_small2); + EXPECT(assert_equal(xi_small2, xi_recovered2, kTolRoundtrip)); + + // Even smaller theta magnitude + Vector10 xi_small3; + xi_small3 << 1e-9, 2e-9, 3e-9, -4e-9,-5e-9,-6e-9, 1e-10, 2e-10, 3e-10, 7e-9; + Gal3 g_small3 = Gal3::Expmap(xi_small3); + Vector10 xi_recovered3 = Gal3::Logmap(g_small3); + EXPECT(assert_equal(xi_small3, xi_recovered3, kTol)); // Tighter tolerance near zero + + // Zero tangent vector (Strict Identity case) + Vector10 xi_zero = Vector10::Zero(); + Gal3 g_identity = Gal3::Expmap(xi_zero); + Vector10 xi_recovered_zero = Gal3::Logmap(g_identity); + EXPECT(assert_equal(Gal3::Identity(), g_identity, kTol)); + EXPECT(assert_equal(xi_zero, xi_recovered_zero, kTol)); + EXPECT(assert_equal(xi_zero, Gal3::Logmap(Gal3::Expmap(xi_zero)), kTol)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index a97aa27be..79dda3c3d 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 5a791e9ed..dff099e24 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -162,7 +162,7 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; - + #include class SimWall2D { SimWall2D(); @@ -379,17 +379,6 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { typedef gtsam::RangeFactor RangeFactorRTV; -#include -class Event { - Event(); - Event(double t, const gtsam::Point3& p); - Event(double t, double x, double y, double z); - double time() const; - gtsam::Point3 location() const; - double height() const; - void print(string s) const; -}; - class TimeOfArrival { TimeOfArrival(); TimeOfArrival(double speed); diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index ae4fa874f..2dc8d8648 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -18,4 +18,4 @@ #endif -#include +#include \ No newline at end of file diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index caf6b7234..f58b4205d 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -18,4 +18,4 @@ #endif -#include \ No newline at end of file +#include diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 784bb03ca..e041f9d97 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -20,7 +20,7 @@ #pragma once #include -#include +#include namespace gtsam { diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index b6628c138..22b403342 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/python/gtsam_unstable/examples/TimeOfArrivalExample.py b/python/gtsam_unstable/examples/TimeOfArrivalExample.py index 59f008a05..66a258491 100644 --- a/python/gtsam_unstable/examples/TimeOfArrivalExample.py +++ b/python/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -12,8 +12,9 @@ Author: Frank Dellaert # pylint: disable=invalid-name, no-name-in-module from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Point3, Values, noiseModel) -from gtsam_unstable import Event, TimeOfArrival, TOAFactor + NonlinearFactorGraph, Point3, Values, noiseModel, Event, + TimeOfArrival) +from gtsam_unstable import TOAFactor # units MS = 1e-3