diff --git a/gtsam/navigation/Gal3.cpp b/gtsam/navigation/Gal3.cpp new file mode 100644 index 000000000..339cff9ec --- /dev/null +++ b/gtsam/navigation/Gal3.cpp @@ -0,0 +1,546 @@ +/* ---------------------------------------------------------------------------- + * 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 + * @author Based on Python implementation by User + * @date April 29, 2025 + */ + +#include // Note: Adjust path if needed +#include // For so3::DexpFunctor if needed, and Rot3 Log/Exp +#include // For derivative checking if desired +#include // For Z_3x3 etc. +#include // For constructing expressions + +#include +#include +#include // For std::function + +namespace gtsam { + +//------------------------------------------------------------------------------ +// Static Helper Functions Implementation +//------------------------------------------------------------------------------ + +Matrix3 Gal3::SO3_LeftJacobian(const Vector3& theta) { + using std::cos; + using std::sin; + using std::sqrt; + + const double angle_sq = theta.squaredNorm(); + + // Use Taylor series expansion for small angles + if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { + // Jl(w) approx = I + 0.5*[w]x + (1/6)*[w]x^2 + const Matrix3 W = skewSymmetric(theta); + return Matrix3::Identity() + 0.5 * W + (1.0/6.0) * W * W; + } + + const double angle = sqrt(angle_sq); + const Matrix3 W = skewSymmetric(theta); + const Matrix3 W2 = W * W; + + const double inv_angle_sq = 1.0 / angle_sq; + const double sin_angle = sin(angle); + const double cos_angle = cos(angle); + + // coeff1 = (1 - cos(theta)) / theta^2 + const double coeff1 = (1.0 - cos_angle) * inv_angle_sq; + // coeff2 = (theta - sin(theta)) / theta^3 + const double coeff2 = (angle - sin_angle) / (angle_sq * angle); + + return Matrix3::Identity() + coeff1 * W + coeff2 * W2; +} + +//------------------------------------------------------------------------------ +Matrix3 Gal3::SO3_LeftJacobianInverse(const Vector3& theta) { + using std::cos; + using std::sin; + using std::sqrt; + using std::tan; + + const double angle_sq = theta.squaredNorm(); + const Matrix3 W = skewSymmetric(theta); + + // Use Taylor series expansion for small angles + if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { + // Jl(w)^-1 approx = I - 0.5*W + (1/12)*W^2 + return Matrix3::Identity() - 0.5 * W + (1.0/12.0) * W * W; + } + + const double angle = sqrt(angle_sq); + const double half_angle = 0.5 * angle; + + // Formula: I - 0.5*W + (1/theta^2 - cot(theta/2)/(2*theta)) * W^2 + // which is I - 0.5*W + (1 - 0.5*theta*cot(0.5*theta))/theta^2 * W^2 + double cot_half_angle; + // Avoid division by zero/very small numbers for tan(half_angle) + if (std::abs(sin(half_angle)) < kSmallAngleThreshold) { + // If sin(half_angle) is near zero, theta is near multiples of 2*pi. + // cot(x) ~ 1/x for small x. Let x = half_angle. + // If half_angle is very small, use Taylor expansion of the coefficient: + // (1 - 0.5*theta*cot(0.5*theta))/theta^2 -> 1/12 + O(theta^2) + if (std::abs(half_angle) < kSmallAngleThreshold) { + // Use Taylor approx for the coefficient directly + const double coeff = 1.0 / 12.0; // + angle_sq / 720.0 + ...; // Higher order if needed + return Matrix3::Identity() - 0.5 * W + coeff * (W * W); + } else { + // theta is near non-zero multiples of 2*pi. tan(half_angle) is near zero. + // cot becomes large. Use limit or high-precision calculation if needed. + // For now, use standard tan. May need robustness improvements. + cot_half_angle = cos(half_angle) / sin(half_angle); // Avoid 1.0/tan() + } + } else { + cot_half_angle = cos(half_angle) / sin(half_angle); // Avoid 1.0/tan() + } + + const double coeff = (1.0 - 0.5 * angle * cot_half_angle) / angle_sq; + return Matrix3::Identity() - 0.5 * W + coeff * (W * W); + + // Alternative using GTSAM SO3 properties (verify this matches J_l^-1): + // J_l(theta)^-1 = J_r(-theta). + // return gtsam::so3::DexpFunctor(-theta).rightJacobian(); +} + + +//------------------------------------------------------------------------------ +Matrix3 Gal3::Calculate_E(const Vector3& theta) { + using std::cos; + using std::sin; + using std::sqrt; + + const double angle_sq = theta.squaredNorm(); + const Matrix3 W = skewSymmetric(theta); + const Matrix3 W2 = W * W; + + Matrix3 E = 0.5 * Matrix3::Identity(); + + // Small angle approximation (from manif SGal3Tangent_base.h / Expmap formula) + // E approx = 0.5*I + (1/6)*W + (1/24)*W^2 + if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { + E += (1.0 / 6.0) * W + (1.0 / 24.0) * W2; + return E; + } + + const double angle = sqrt(angle_sq); + const double sin_angle = sin(angle); + const double cos_angle = cos(angle); + + // Coefficients A and B from manif::SGal3TangentBase::exp() + // A = (theta - sin(theta)) / theta^3 + const double A = (angle - sin_angle) / (angle_sq * angle); + // B = (theta^2 + 2*cos(theta) - 2) / (2 * theta^4) + const double B = (angle_sq + 2.0 * cos_angle - 2.0) / (2.0 * angle_sq * angle_sq); + + E += A * W + B * W2; + return E; +} + +//------------------------------------------------------------------------------ +// Constructors +//------------------------------------------------------------------------------ +Gal3::Gal3(const Matrix5& M) { + // CORRECTED: Use head(N) with correct sizes for checks + 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) { // Row 4 first 4 = 0; Row 3 first 3 = 0 + 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); // Manif: v is in column 3 (0-indexed) + r_ = Point3(M.block<3, 1>(0, 4)); // Manif: r is in column 4 + t_ = M(3, 4); // Manif: t is at (3, 4) +} + +//------------------------------------------------------------------------------ +// Component Access +//------------------------------------------------------------------------------ +const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const { + if (H) { + *H = Matrix::Zero(3, 10); + H->block<3, 3>(0, 6) = Matrix3::Identity(); // Derivative w.r.t theta + } + return R_; +} + +//------------------------------------------------------------------------------ +const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const { + if (H) { + *H = Matrix::Zero(3, 10); + H->block<3, 3>(0, 0) = R_.matrix(); // Derivative w.r.t rho (local tangent space) + } + return r_; +} + +//------------------------------------------------------------------------------ +const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const { + if (H) { + *H = Matrix::Zero(3, 10); + H->block<3, 3>(0, 3) = R_.matrix(); // d(v)/d(nu) + } + return v_; +} + +//------------------------------------------------------------------------------ +const double& Gal3::time(OptionalJacobian<1, 10> H) const { + if (H) { + *H = Matrix::Zero(1, 10); + (*H)(0, 9) = 1.0; // Derivative w.r.t t_tan + } + return t_; +} + +//------------------------------------------------------------------------------ +// Matrix Representation +//------------------------------------------------------------------------------ +Matrix5 Gal3::matrix() const { + Matrix5 M = Matrix5::Identity(); + M.block<3, 3>(0, 0) = R_.matrix(); + M.block<3, 1>(0, 3) = v_; // Manif: v in col 3 + M.block<3, 1>(0, 4) = Vector3(r_); // Manif: r in col 4 // CORRECTED: Assign Vector3 from Point3 + M(3, 4) = t_; // Manif: t at (3, 4) + M.block<1,3>(3,0).setZero(); // Zero out row 3, cols 0-2 + M.block<1,4>(4,0).setZero(); // Zero out row 4, cols 0-3 + return M; +} + +//------------------------------------------------------------------------------ +// 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 { + // Formula: R_inv = R', v_inv = -R'*v, r_inv = -R'*(r - t*v), t_inv = -t + const Rot3 Rinv = R_.inverse(); + const Velocity3 v_inv = -(Rinv.rotate(v_)); + // CORRECTED: Cast r_ to Vector3 for subtraction + 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::compose(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { + // Formula: R = R1*R2, v = R1*v2 + v1, r = R1*r2 + t2*v1 + r1, t = t1+t2 + // where this = g1, other = g2 + const Gal3& g1 = *this; + const Gal3& g2 = other; + + const Rot3 R_comp = g1.R_.compose(g2.R_); + // CORRECTED: Cast g1.r_ to Vector3 for addition + const Vector3 r_comp_vec = g1.R_.rotate(g2.r_) + g2.t_ * g1.v_ + Vector3(g1.r_); + const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_; + const double t_comp = g1.t_ + g2.t_; + + Gal3 result(R_comp, Point3(r_comp_vec), v_comp, t_comp); + + // Jacobians require correct AdjointMap implementation + if (H1) { + *H1 = g2.inverse().AdjointMap(); + } + if (H2) { + *H2 = Matrix10::Identity(); + } + + return result; +} + +//------------------------------------------------------------------------------ +Gal3 Gal3::between(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { + const Gal3& g1 = *this; + const Gal3& g2 = other; + Gal3 g1_inv = g1.inverse(); + Gal3 result = g1_inv.compose(g2); // g1_inv * g2 + + // Jacobians require correct AdjointMap implementation + if (H1) { + *H1 = -result.AdjointMap(); + } + if (H2) { + *H2 = g1_inv.AdjointMap(); + } + + return result; +} + + +//------------------------------------------------------------------------------ +// Manifold Operations (Retract/Local) +//------------------------------------------------------------------------------ +Gal3 Gal3::retract(const Vector10& xi, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { + Gal3 g_exp = Gal3::Expmap(xi); + Matrix10 H_exp_xi = Matrix10::Zero(); // Placeholder + if (H2) { + H_exp_xi = Gal3::ExpmapDerivative(xi); // Needs ExpmapDerivative implemented + } + + Matrix10 H_comp_this, H_comp_gexp; + Gal3 result = this->compose(g_exp, H_comp_this, H_comp_gexp); + + if (H1) { + *H1 = H_comp_this; // Requires AdjointMap + } + if (H2) { + // Needs ExpmapDerivative and potentially AdjointMap if H_comp_gexp is not Identity + *H2 = H_comp_gexp * H_exp_xi; // H_comp_gexp is Identity here + } + return result; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { + Matrix10 H_between_this, H_between_other; + Gal3 result_g = this->between(other, H_between_this, H_between_other); + + Matrix10 H_log_g = Matrix10::Zero(); // Placeholder + if (H1 || H2) { + H_log_g = Gal3::LogmapDerivative(result_g); // Needs LogmapDerivative implemented + } + Vector10 result_xi = Gal3::Logmap(result_g); + + // Jacobians require LogmapDerivative and AdjointMap (via between Jacobians) + if (H1) { + *H1 = H_log_g * H_between_this; + } + if (H2) { + *H2 = H_log_g * H_between_other; + } + return result_xi; +} + +//------------------------------------------------------------------------------ +// Lie Group Static Functions +//------------------------------------------------------------------------------ + +Matrix5 Gal3::Hat(const Vector10& xi) { + 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; // nu in column 3 + X.block<3, 1>(0, 4) = rho_tan; // rho in column 4 + X(3, 4) = t_tan_val; // t in element (3, 4) + return X; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::Vee(const Matrix5& X) { + // CORRECTED: Check X.row(3).head(N) for Lie algebra structure + // Lie algebra row 3 is [0 0 0 0 t], so first 3 elements should be 0. + // Lie algebra row 4 is [0 0 0 0 0], so all 5 elements should be 0. + if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9) { // Row 4 all=0; Row 3 first 3=0 + throw std::invalid_argument("Matrix is not in sgal(3)"); + } + + Vector10 xi; + rho(xi) = X.block<3, 1>(0, 4); // rho from column 4 + nu(xi) = X.block<3, 1>(0, 3); // nu from column 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); // t from element (3, 4) + return xi; +} + +//------------------------------------------------------------------------------ +Gal3 Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) { + 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 Rot3 R = Rot3::Expmap(theta_tan); + const Matrix3 Jl_theta = SO3_LeftJacobian(theta_tan); + const Matrix3 E = Calculate_E(theta_tan); + + 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; + + if (Hxi) { + // Jacobian placeholder - requires derivation + *Hxi = ExpmapDerivative(xi); + } + + return Gal3(R, r_final, v_final, t_final); +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) { + const Vector3 theta_vec = Rot3::Logmap(g.R_); + const Matrix3 Jl_theta_inv = SO3_LeftJacobianInverse(theta_vec); + const Matrix3 E = Calculate_E(theta_vec); + + // CORRECTED: Cast r_ to Vector3 for subtraction + const Vector3 r_vec = Vector3(g.r_); + const Velocity3& v_vec = g.v_; + const double& t_val = g.t_; + + 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; + + // Jacobian placeholder - requires derivation + if (Hg) { + *Hg = LogmapDerivative(g); + } + + Vector10 xi; + rho(xi) = rho_tan; + nu(xi) = nu_tan; + theta(xi) = theta_vec; + t_tan(xi)(0) = t_tan_val; + return xi; +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::AdjointMap() const { + const Matrix3 Rmat = R_.matrix(); + const Matrix3 Vhat = skewSymmetric(v_); + // CORRECTED: Cast r_ to Vector3 for subtraction + const Vector3 r_minus_tv = Vector3(r_) - t_ * v_; + const Matrix3 S_r_minus_tv_hat = skewSymmetric(r_minus_tv); + + Matrix10 Ad = Matrix10::Zero(); + + // Row block 1: drho' output (maps input drho, dnu, dtheta, dt) + Ad.block<3,3>(0,0) = Rmat; + Ad.block<3,3>(0,3) = t_ * Rmat; + Ad.block<3,3>(0,6) = -Rmat * S_r_minus_tv_hat; + Ad.block<3,1>(0,9) = Rmat * v_; + + // Row block 2: dnu' output + Ad.block<3,3>(3,3) = Rmat; + Ad.block<3,3>(3,6) = -Rmat * Vhat; + + // Row block 3: dtheta' output + Ad.block<3,3>(6,6) = Rmat; + + // Row block 4: dt' output + 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(); + if (H_xi) { + *H_xi = Ad; + } + // Jacobian H_g requires adjointMap + if (H_g) { + *H_g = -adjointMap(Ad * xi); + } + return Ad * xi; +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::adjointMap(const Vector10& xi) { + // Based on Manif SGal3Tangent::adj() / Lie bracket [xi, y] + 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); + + Matrix10 ad = Matrix10::Zero(); + + // Row block 1: rho_out = Theta_hat*m + Rho_hat*eta + t_val*p + ad.block<3,3>(0,0) = Theta_hat; // d/dm + ad.block<3,3>(0,3) = t_val * Matrix3::Identity(); // d/dp + ad.block<3,3>(0,6) = Rho_hat; // d/deta + + // Row block 2: nu_out = Theta_hat*p + Nu_hat*eta + ad.block<3,3>(3,3) = Theta_hat; // d/dp + ad.block<3,3>(3,6) = Nu_hat; // d/deta + + // Row block 3: theta_out = Theta_hat*eta + ad.block<3,3>(6,6) = Theta_hat; // d/deta + + // Row block 4: t_out = 0 (already zero) + + 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; + // Jacobian Hxi requires adjointMap + if (Hxi) { + *Hxi = -adjointMap(y); + } + return ad_xi * y; +} + + +//------------------------------------------------------------------------------ +Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) { + if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); + // CORRECTED: Explicitly create std::function to resolve ambiguity + std::function fn = + [](const Vector10& v) { return Gal3::Expmap(v); }; + // Note: Default delta for numericalDerivative might be too large or small + return numericalDerivative11(fn, xi, 1e-5); // Pass function object +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::LogmapDerivative(const Gal3& g) { + Vector10 xi = Gal3::Logmap(g); + if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); + // CORRECTED: Explicitly create std::function to resolve ambiguity + std::function fn = + [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + // Note: Default delta for numericalDerivative might be too large or small + return numericalDerivative11(fn, g, 1e-5); // Pass function object +} + + +//------------------------------------------------------------------------------ +// 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); +} + +//------------------------------------------------------------------------------ +// 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; +} + + +} // namespace gtsam diff --git a/gtsam/navigation/Gal3.h b/gtsam/navigation/Gal3.h new file mode 100644 index 000000000..d210c1813 --- /dev/null +++ b/gtsam/navigation/Gal3.h @@ -0,0 +1,282 @@ +/* ---------------------------------------------------------------------------- + * 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) + * Based on manif convention: [R, r, v, t], tangent [rho, nu, theta, t] + * @author Based on Python implementation by User + * @date April 29, 2025 + */ + +#pragma once + +#include // Includes Rot3, Point3 +#include +#include +#include // For LieGroup base class + +#include // For std::sqrt, std::cos, std::sin, std::tan +#include // For std::numeric_limits +#include // For std::function + +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). + * Aligned with manif conventions: state (R, r, v, t), tangent (rho, nu, theta, t_tan). + * Internal state: + * R_: Rotation (Rot3) - attitude (world R body) + * r_: Translation (Point3) - position in world frame + * v_: Velocity Boost (Velocity3) - velocity in world frame + * t_: Time (double) + * + * Homogeneous Matrix Representation (manif convention): + * [ R v r ] + * [ 0 1 t ] + * [ 0 0 1 ] (Where R is 3x3, v,r are 3x1, t is scalar, 0 are row vectors) + * + * Lie Algebra Matrix Representation (manif convention): + * [ skew(theta) nu rho ] + * [ 0 0 t ] + * [ 0 0 0 ] (Where skew(theta) is 3x3, nu,rho are 3x1, t is scalar) + * + * Tangent Vector xi (Vector10): [rho; nu; theta; t_tan] + * rho (3x1): Position tangent component + * nu (3x1): Velocity tangent component + * theta(3x1): Rotation tangent component (so(3)) + * t_tan (1x1): Time tangent component + */ +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 + + // Small number epsilon for checking small angles, etc. + static constexpr double kSmallAngleThreshold = 1e-10; + + public: + + inline constexpr static 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 (manif order) + 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 (manif convention) + explicit Gal3(const Matrix5& M); + + /// @} + /// @name Component Access + /// @{ + + const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const; + const Point3& translation(OptionalJacobian<3, 10> H = {}) const; + const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const; + const double& time(OptionalJacobian<1, 10> H = {}) const; + + // Convenience accessors matching NavState where names overlap + 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 R() const { return R_.matrix(); } + + /// Return position as Vector3 + Vector3 r() const { return Vector3(r_); } + + /// Return velocity as Vector3. Computation-free. + const Vector3& v() const { return v_; } + + /// Return time scalar. Computation-free. + const double& t() const { return t_; } + + /// Return 5x5 homogeneous matrix representation (manif convention). + Matrix5 matrix() const; + + /// @} + /// @name Testable + /// @{ + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const Gal3& state); + + /// print + void print(const std::string& s = "") const; + + /// equals + bool equals(const Gal3& other, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Gal3 Identity() { return Gal3(); } + + /// inverse transformation (Manif formula from SGal3_base.h) + Gal3 inverse() const; + + // Bring LieGroup::inverse() into scope (version with derivative) + using LieGroup::inverse; + + /// compose (Manif formula from SGal3_base.h) + /// Returns this * other + Gal3 compose(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, + OptionalJacobian<10, 10> H2 = {}) const; + + /// compose syntactic sugar + Gal3 operator*(const Gal3& other) const { + return compose(other); + } + + /// Calculate the relative transformation: this->inverse() * other + Gal3 between(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, + OptionalJacobian<10, 10> H2 = {}) const; + + + /// @} + /// @name Manifold Operations (Retract/Local) + /// @{ + + /// Default retract uses Expmap + Gal3 retract(const Vector10& xi, OptionalJacobian<10, 10> H1 = {}, + OptionalJacobian<10, 10> H2 = {}) const; + + /// Default localCoordinates uses Logmap + Vector10 localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, + OptionalJacobian<10, 10> H2 = {}) const; + + /// @} + /// @name Lie Group + using LieAlgebra = Matrix5; + /// @{ + + // Helper functions for accessing tangent vector components + // Input xi is [rho(3), nu(3), theta(3), t_tan(1)] + static Eigen::Block rho(Vector10& v) { return v.segment<3>(0); } + static Eigen::Block nu(Vector10& v) { return v.segment<3>(3); } + static Eigen::Block theta(Vector10& v) { return v.segment<3>(6); } + static Eigen::Block t_tan(Vector10& v) { return v.segment<1>(9); } + // Const versions too + static const Eigen::Block rho(const Vector10& v) { return v.segment<3>(0); } + static const Eigen::Block nu(const Vector10& v) { return v.segment<3>(3); } + static const Eigen::Block theta(const Vector10& v) { return v.segment<3>(6); } + static const Eigen::Block t_tan(const Vector10& v) { return v.segment<1>(9); } + + /// Exponential map at identity - create a Gal3 element from Lie algebra tangent vector xi. + /// xi = [rho, nu, theta, t_tan] (10x1) + static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {}); + // REMOVED using LieGroup::Expmap; + + /// Logarithmic map at identity - return the Lie algebra tangent vector xi for this element. + /// Returns xi = [rho, nu, theta, t_tan] (10x1) + static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {}); + // REMOVED using LieGroup::Logmap; + + /// Calculate Adjoint map, transforming a twist in the identity frame to the frame of this element. + Matrix10 AdjointMap() const; + + /// Apply this element's AdjointMap Ad_g to a tangent vector xi, H_g = d(Ad_g * xi)/dg, H_xi = d(Ad_g * xi)/dxi = Ad_g + Vector10 Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g = {}, OptionalJacobian<10, 10> H_xi = {}) const; + + /// The adjoint action of xi on y `ad(xi, y)` + static Vector10 adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi = {}, OptionalJacobian<10, 10> Hy = {}); + + /// Compute the 10x10 adjoint map `ad(xi)` that acts on a tangent vector y `ad(xi, y) = ad(xi) * y` + static Matrix10 adjointMap(const Vector10& xi); + + /// Derivative of Expmap(xi) w.r.t. xi + static Matrix10 ExpmapDerivative(const Vector10& xi); + + /// Derivative of Logmap(g) w.r.t. g (represented as tangent vector at identity) + static Matrix10 LogmapDerivative(const Gal3& g); + + + // Chart at origin, depends on compile-time flag GTSAM_USE_MANIFOLD_MARKERS + 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 (Manif convention). + /// xi = [rho, nu, theta, t] -> [ skew(theta) nu rho ; 0 0 t ; 0 0 0 ] + static Matrix5 Hat(const Vector10& xi); + + /// Vee operator: maps Lie algebra matrix to tangent vector xi (Manif convention). + /// [ S N R ; 0 0 T ; 0 0 0 ] -> [ R ; N ; vee(S) ; T ] = [rho; nu; theta; t] + static Vector10 Vee(const Matrix5& X); + + + /// @} + /// @name Helper Functions (moved inside class for context, could be private or in detail namespace) + /// @{ + + /// Calculate the SO(3) Left Jacobian J_l(theta) + static Matrix3 SO3_LeftJacobian(const Vector3& theta); + + /// Calculate the inverse SO(3) Left Jacobian J_l(theta)^-1 + static Matrix3 SO3_LeftJacobianInverse(const Vector3& theta); + + /// Calculate the 3x3 E matrix used in SGal3 Expmap/Logmap + static Matrix3 Calculate_E(const Vector3& theta); + + /// @} + + +private: + /// @{ + /// 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 + +// // Specialize Gal3 traits +template <> +struct traits : public internal::MatrixLieGroup {}; + +template <> +struct traits : public internal::MatrixLieGroup {}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testGal3.cpp b/gtsam/navigation/tests/testGal3.cpp new file mode 100644 index 000000000..7000fcc61 --- /dev/null +++ b/gtsam/navigation/tests/testGal3.cpp @@ -0,0 +1,412 @@ +/* ---------------------------------------------------------------------------- + * 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, mirroring custom_gal3_tests.py + * @author Based on Python tests by User and NavState tests + * @date April 29, 2025 // Updated Date + */ + +#include + +#include +#include +#include // Include for Lie group concepts if needed later +#include +#include // Point3 is already included via Gal3.h usually + +#include +#include +#include // For std::bind if using numerical derivatives + +using namespace std; +using namespace gtsam; + +// Define tolerance matching Python TOL +static const double kTol = 1e-8; +GTSAM_CONCEPT_TESTABLE_INST(gtsam::Gal3) +GTSAM_CONCEPT_LIE_INST(gtsam::Gal3) + +// Instantiate Testable concept for Gal3 to allow assert_equal(Gal3, Gal3) +// Instantiate Lie group concepts if you plan to use CHECK_LIE_GROUP_DERIVATIVES etc. +// GTSAM_CONCEPT_LIE_INST(Gal3) + +// ======================================================================== +// Test Cases Section (mirroring Python's TestGal3 class) +// ======================================================================== + +/* ************************************************************************* */ +TEST(Gal3, Identity) { + // Hardcoded expected data + 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(); + + // Check components individually + EXPECT(assert_equal(expected_R_mat, custom_ident.rotation().matrix(), kTol)); + // Ensure translation() returns Point3, then convert if needed or compare Point3 + 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); + + // Alternative: Check whole object if Testable concept works as expected + EXPECT(assert_equal(Gal3(Rot3(expected_R_mat), Point3(expected_r_vec), expected_v_vec, expected_t_val), custom_ident, kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, HatVee) { + // Hardcoded test cases for Hat/Vee (from Python script) + // 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(); + + // Test Hat operation + Matrix5 custom_Xi_1 = Gal3::Hat(tau_vec_1); + EXPECT(assert_equal(expected_xi_matrix_1, custom_Xi_1, kTol)); + + // Test Vee operation (using the expected Xi matrix as input) + Vector10 custom_tau_rec_1 = Gal3::Vee(expected_xi_matrix_1); + EXPECT(assert_equal(tau_vec_1, custom_tau_rec_1, kTol)); + + // Test Vee(Hat(tau)) round trip + Vector10 custom_tau_rec_roundtrip_1 = Gal3::Vee(custom_Xi_1); + EXPECT(assert_equal(tau_vec_1, custom_tau_rec_roundtrip_1, kTol)); + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +TEST(Gal3, Expmap) { + // Hardcoded test case for Expmap (from Python script) + // 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 Vector3 expected_r_vec_1 = (Vector3() << -0.637321673031978, 0.16116104619552254, -0.03248254605908951).finished(); + const Vector3 expected_v_vec_1 = (Vector3() << -0.22904001980446076, -0.23988916442951638, -0.4308710747620977).finished(); + const double expected_t_val_1 = -0.11137002094819903; + + Gal3 custom_exp_1 = Gal3::Expmap(tau_vec_1); + + // Check components individually + EXPECT(assert_equal(expected_R_mat_1, custom_exp_1.rotation().matrix(), kTol)); + EXPECT(assert_equal(Point3(expected_r_vec_1), custom_exp_1.translation(), kTol)); + EXPECT(assert_equal(expected_v_vec_1, custom_exp_1.velocity(), kTol)); + EXPECT_DOUBLES_EQUAL(expected_t_val_1, custom_exp_1.time(), kTol); + + // Check derivatives (optional, but good practice like in testNavState) + // Matrix9 aH; + // Gal3::Expmap(tau_vec_1, aH); + // std::function expmap_func = [](const Vector9& v){ return Gal3::Expmap(v); }; + // Matrix expectedH = numericalDerivative11(expmap_func, tau_vec_1); + // EXPECT(assert_equal(expectedH, aH, 1e-6)); // Use appropriate tolerance + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +TEST(Gal3, Logmap) { + // --- Part 1: Test Logmap(GroupElement) --- + // Hardcoded test case 1 (from Python script) + 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(); + // Use Point3/Velocity3 directly for construction + 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); + EXPECT(assert_equal(expected_log_tau_1, custom_log_tau_1, kTol*1e3)); + // Add more test cases here if they exist in the Python script + + + // --- Part 2: Test Log(Exp(tau)) round trip --- + // Using data from the Expmap test case + 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); + // Use slightly larger tolerance for round trip + EXPECT(assert_equal(tau_vec_orig_rt1, tau_log_exp_rt1, kTol * 10)); + + + // --- Part 3: Test Exp(Log(g)) round trip --- + // Using data from the first Logmap test case + Gal3 custom_g_orig_rt2 = custom_g_1; // Reuse from Part 1 + Vector10 tau_log_rt2 = Gal3::Logmap(custom_g_orig_rt2); + Gal3 g_exp_log_rt2 = Gal3::Expmap(tau_log_rt2); + // Compare the reconstructed g against the original using assert_equal for Gal3 + EXPECT(assert_equal(custom_g_orig_rt2, g_exp_log_rt2, kTol * 10)); +} + +/* ************************************************************************* */ +TEST(Gal3, Compose) { + // Hardcoded test case 1 (from Python script) + 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); + + // Expected result of composition + 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) + + // Compare the whole Gal3 object + EXPECT(assert_equal(expected_comp_1, custom_comp_1, kTol)); + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +TEST(Gal3, Inverse) { + // Hardcoded identity for comparison + Gal3 expected_identity = Gal3::Identity(); + + // Hardcoded test case 1 (from Python script) + 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); + + // Expected inverse + 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); + + // Test inverse calculation + 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)); + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +TEST(Gal3, Between) { + // Hardcoded test case 1 (from Python script) + 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 result of c1.between(c2) which is 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)); + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +TEST(Gal3, MatrixComponents) { + // Hardcoded test case 1 (from Python script) + 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); + + // Expected matrix representation + 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(); + + // Compare the whole matrix + EXPECT(assert_equal(expected_Mc, Mc, kTol)); + + // Optional: Individual component checks (redundant if the above passes) + // EXPECT(assert_equal(source_R_mat_1, Mc.block<3,3>(0,0), kTol)); + // EXPECT(assert_equal(source_v_vec_1, Mc.block<3,1>(0,3), kTol)); + // EXPECT(assert_equal(source_r_vec_1.vector(), Mc.block<3,1>(0,4), kTol)); // .vector() for Point3 + // EXPECT_DOUBLES_EQUAL(source_t_val_1, Mc(3,4), kTol); + // EXPECT_DOUBLES_EQUAL(1.0, Mc(3,3), kTol); + // EXPECT_DOUBLES_EQUAL(1.0, Mc(4,4), kTol); + // EXPECT(assert_equal(Vector3::Zero(), Mc.block<1,3>(3,0).transpose(), kTol)); + // EXPECT(assert_equal(Vector4::Zero(), Mc.block<1,4>(4,0).transpose(), kTol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Associativity) { + // Hardcoded test case 1 (from Python script) + 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); + + // Compare the results directly using assert_equal for Gal3 + EXPECT(assert_equal(left_assoc_1, right_assoc_1, kTol * 10)); // Slightly larger tol for composed operations + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +TEST(Gal3, IdentityProperties) { + Gal3 custom_identity = Gal3::Identity(); + + // Hardcoded test case 1 (from Python script) + 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); + + // Test 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)); // Slightly larger tol + + // Test identity * g == g + Gal3 id_times_g_1 = custom_identity * custom_g_1; + EXPECT(assert_equal(custom_g_1, id_times_g_1, kTol)); + + // Test g * identity == g + Gal3 g_times_id_1 = custom_g_1 * custom_identity; + EXPECT(assert_equal(custom_g_1, g_times_id_1, kTol)); + + // Add more test cases here if they exist in the Python script +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */