diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam/geometry/Event.cpp similarity index 100% rename from gtsam_unstable/geometry/Event.cpp rename to gtsam/geometry/Event.cpp diff --git a/gtsam_unstable/geometry/Event.h b/gtsam/geometry/Event.h similarity index 100% rename from gtsam_unstable/geometry/Event.h rename to gtsam/geometry/Event.h diff --git a/gtsam/geometry/Gal3.cpp b/gtsam/geometry/Gal3.cpp index f7e250f57..bc694e3b8 100644 --- a/gtsam/geometry/Gal3.cpp +++ b/gtsam/geometry/Gal3.cpp @@ -11,29 +11,35 @@ * @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 // 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 // For GtsamLieTypeChecks @TODO - check + +#include +#include +#include +#include +#include +#include +#include #include #include -#include // For std::function +#include namespace gtsam { //------------------------------------------------------------------------------ -// Constants and Helper function for Expmap/Logmap (Implementation Detail) +// Constants and Helper function for Expmap/Logmap //------------------------------------------------------------------------------ namespace { // Anonymous namespace for internal linkage - - // Define the threshold locally within the cpp file constexpr double kSmallAngleThreshold = 1e-10; - } // end anonymous namespace //------------------------------------------------------------------------------ @@ -42,30 +48,23 @@ namespace { // Anonymous namespace for internal linkage 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) { - // H = d Logmap(Create(R,r,v,t)) / d [w, r_vec, v_vec, t] - // Note: Derivatives are w.r.t tangent space elements at the identity - // for inputs R, r, v, t. However, the standard way is derivative - // w.r.t minimal tangent spaces of inputs. Let's assume standard approach. - if (H1) { // d xi_out / d omega_R (3x3) + if (H1) { H1->setZero(); - H1->block<3, 3>(6, 0) = Matrix3::Identity(); // d theta / d omega_R = I + H1->block<3, 3>(6, 0) = Matrix3::Identity(); } - if (H2) { // d xi_out / d delta_r (3x3) + if (H2) { H2->setZero(); - H2->block<3, 3>(0, 0) = R.transpose(); // d rho / d delta_r = R^T + H2->block<3, 3>(0, 0) = R.transpose(); } - if (H3) { // d xi_out / d delta_v (3x3) + if (H3) { H3->setZero(); - H3->block<3, 3>(3, 0) = R.transpose(); // d nu / d delta_v = R^T + H3->block<3, 3>(3, 0) = R.transpose(); } - if (H4) { // d xi_out / d delta_t (10x1) + if (H4) { H4->setZero(); - // As derived: d xi / dt = [-R^T v; 0; 0; 1] - Vector3 drho_dt = -R.transpose() * v; // Corrected derivative for rho - H4->block<3, 1>(0, 0) = drho_dt; // Assign to rho rows (0-2) - // d nu / dt = 0 - // d theta / dt = 0 - (*H4)(9, 0) = 1.0; // d t_tan / dt = 1 + 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); } @@ -75,29 +74,22 @@ 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(); // Get r for H3 calculation if needed below - if (H1) { // d xi_out / d xi_pose (10x6), xi_pose = [omega_R; v_r] + const Point3& r = pose.translation(); + if (H1) { H1->setZero(); - // d theta / d omega_pose = I (rows 6-8, cols 0-2) H1->block<3, 3>(6, 0) = Matrix3::Identity(); - // d rho / d v_r = I (rows 0-2, cols 3-5) - Corrected H1->block<3, 3>(0, 3) = Matrix3::Identity(); - // Other blocks (d_rho/d_omega, d_nu/d_omega, d_nu/d_vr, d_tt/d_omega, d_tt/d_vr) are zero. } - if (H2) { // d xi_out / d v_in (10x3) + if (H2) { H2->setZero(); - H2->block<3, 3>(3, 0) = R.transpose(); // d(nu_out)/d(v_in) = R^T + H2->block<3, 3>(3, 0) = R.transpose(); } - if (H3) { // d xi_out / d t_in (10x1) + if (H3) { H3->setZero(); - // As derived: d xi / dt = [-R^T v; 0; 0; 1] - Vector3 drho_dt = -R.transpose() * v; // Corrected derivative for rho - H3->block<3, 1>(0, 0) = drho_dt; // Assign to rho rows (0-2) - // d nu / dt = 0 - // d theta / dt = 0 - (*H3)(9, 0) = 1.0; // d t_tan / dt = 1 + Vector3 drho_dt = -R.transpose() * v; + H3->block<3, 1>(0, 0) = drho_dt; + (*H3)(9, 0) = 1.0; } - // Pass r (translation from pose) to the Gal3 constructor return Gal3(R, r, v, t); } @@ -105,15 +97,15 @@ Gal3 Gal3::FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t, // Constructors //------------------------------------------------------------------------------ Gal3::Gal3(const Matrix5& M) { - // Ensure matrix adheres to SGal(3) structure + // 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) { // Row 4 first 4 = 0; Row 3 first 3 = 0 + 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); // v is in column 3 - r_ = Point3(M.block<3, 1>(0, 4)); // r is in column 4 - t_ = M(3, 4); // t is at (3, 4) + v_ = M.block<3, 1>(0, 3); + r_ = Point3(M.block<3, 1>(0, 4)); + t_ = M(3, 4); } //------------------------------------------------------------------------------ @@ -122,23 +114,16 @@ Gal3::Gal3(const Matrix5& M) { const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const { if (H) { H->setZero(); - H->block<3, 3>(0, 6) = Matrix3::Identity(); // Derivative w.r.t theta + H->block<3, 3>(0, 6) = Matrix3::Identity(); } return R_; } //------------------------------------------------------------------------------ const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const { - // H = d r / d xi = [dr/drho, dr/dnu, dr/dtheta, dr/dt_tan] if (H) { H->setZero(); - // dr/drho = R (local tangent space) H->block<3,3>(0, 0) = R_.matrix(); - // dr/dnu = 0 - // H->block<3,3>(0, 3) = Matrix3::Zero(); // Already zero - // dr/dtheta = 0 - // H->block<3,3>(0, 6) = Matrix3::Zero(); // Already zero - // dr/dt_tan = v (Corrected) H->block<3,1>(0, 9) = v_; } return r_; @@ -148,7 +133,7 @@ const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const { const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const { if (H) { H->setZero(); - H->block<3, 3>(0, 3) = R_.matrix(); // d(v)/d(nu) + H->block<3, 3>(0, 3) = R_.matrix(); } return v_; } @@ -157,7 +142,7 @@ const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const { const double& Gal3::time(OptionalJacobian<1, 10> H) const { if (H) { H->setZero(); - (*H)(0, 9) = 1.0; // Derivative w.r.t t_tan + (*H)(0, 9) = 1.0; } return t_; } @@ -166,13 +151,14 @@ const double& Gal3::time(OptionalJacobian<1, 10> H) const { // 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(); // Zero out row 3, cols 0-2 - M.block<1,4>(4,0).setZero(); // Zero out row 4, cols 0-3 + M.block<1,3>(3,0).setZero(); + M.block<1,4>(4,0).setZero(); return M; } @@ -207,7 +193,7 @@ bool Gal3::equals(const Gal3& other, double tol) const { // Group Operations //------------------------------------------------------------------------------ Gal3 Gal3::inverse() const { - // Formula: R_inv = R', v_inv = -R'*v, r_inv = -R'*(r - t*v), t_inv = -t + // 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_)); @@ -216,119 +202,71 @@ Gal3 Gal3::inverse() const { } //------------------------------------------------------------------------------ - Gal3 Gal3::operator*(const Gal3& other) const { - // This function provides the fundamental math for g1 * g2 - // Formula: R = R1*R2, v = R1*v2 + v1, r = R1*r2 + t2*v1 + r1, t = t1+t2 - const Gal3& g1 = *this; // g1 is the current object (*this) - const Gal3& g2 = other; // g2 is the argument + // Implements group composition through matrix multiplication + const Gal3& g1 = *this; + const Gal3& g2 = other; const Rot3 R_comp = g1.R_.compose(g2.R_); - - // Ensure correct types for addition (using Vector3 temporarily if r_ is Point3) 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_; - // Construct the result using the computed components 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) { - // Extract tangent vector components + // 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); - // Use the SO(3) DexpFunctor. This computes theta, W, WW, sin/cos (or Taylor series), - // and coefficients A, B, C efficiently, handling the near-zero case. const gtsam::so3::DexpFunctor dexp_functor(theta_tan); - - // Get Rotation matrix R = Exp(theta) - // We can use the functor's result directly if it provides it, - // or call the standard Rot3::Expmap which might be slightly clearer. - // Let's assume Rot3::Expmap is fine, as DexpFunctor primarily helps with Jacobians and coefficients. - const Rot3 R = Rot3::Expmap(theta_tan); // or const Rot3 R(dexp_functor.expmap()); - - // Get Left Jacobian of SO(3) Expmap: J_R(theta) + const Rot3 R = Rot3::Expmap(theta_tan); const Matrix3 Jl_theta = dexp_functor.leftJacobian(); - // Calculate the E matrix using coefficients from dexp_functor to avoid redundant calculations. - // E = 0.5 * I + C * W + B_E * W2 - // where C = dexp_functor.C = (theta - sin(theta)) / theta^3 - // and B_E = (1 - 2 * dexp_functor.B) / (2 * dexp_functor.theta2) - // where B = dexp_functor.B = (1 - cos(theta)) / theta^2 - // Note: DexpFunctor handles the small angle case (nearZero==true) internally - // by using Taylor approximations for B and C, which results in the - // correct Taylor series for E. Matrix3 E; if (dexp_functor.nearZero) { - // Although the formula below *should* work due to Taylor series in B,C, - // we can be explicit using the known Taylor series for E for clarity/safety. - // E = 0.5*I + (1/6)*W + (1/24)*WW + // 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 { - // Use the derived formula. theta2 cannot be zero here. + // 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; } - // Calculate final state components using the computed parts 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; - // Construct the final Gal3 object Gal3 result(R, r_final, v_final, t_final); - // If Jacobian is requested, compute it numerically. - // The lambda should call this Expmap function *without* the Jacobian flag - // to avoid infinite recursion. if (Hxi) { - std::function expmap_wrapper = - [](const Vector10& tangent_vector) -> Gal3 { - // Call the Expmap function itself, but without requesting Jacobian - return Gal3::Expmap(tangent_vector); - }; - - // Compute numerical derivative using the wrapper *Hxi = Gal3::ExpmapDerivative(xi); - // Note: The numerical derivative computes the Right Jacobian Jr(xi) for Expmap. - // Jr(xi) = d Expmap(xi o eps) / d eps |_{eps=0} - // = d (Expmap(xi) * Expmap(Dr(xi) * eps)) / d eps |_{eps=0} - // = d (Expmap(xi) * (I + (Dr(xi)*eps)^ )) / d eps |_{eps=0} * Dr(xi) ??? No, this is not right. - // The numerical derivative directly computes dExpmap(xi)/dxi where the perturbation is additive in xi. - // This corresponds to the Right Jacobian by definition in GTSAM's manifold context. - // Retract(x, v) = x.compose(Expmap(v)) (Right chart) - // Retract(Identity, xi) = Expmap(xi) - // Derivative d Retract(Identity, xi) / d xi = Jr_SO3(xi) for SO3. - // Let's assume numericalDerivative11 gives the correct Jacobian expected by the retract/localCoordinates framework. } return result; } -// Also update the numerical derivative lambda within Logmap if it re-implements logic: +//------------------------------------------------------------------------------ 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_); - - // Use DexpFunctor to get Jacobians and coefficients efficiently const gtsam::so3::DexpFunctor dexp_functor_log(theta_vec); - const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse(); // Inverse Left Jacobian + const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse(); - // Calculate E using the same logic as in Expmap, but based on theta_vec 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; } @@ -337,10 +275,8 @@ Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) { 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; - // Corrected formula for rho_tan based on Expmap structure: r = Jl*rho + E*(t*nu) - // => Jl*rho = r - E*(t*nu) - // => rho = Jl_inv * (r - E*(t*nu)) const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan)); const double t_tan_val = t_val; @@ -351,47 +287,31 @@ Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) { t_tan(xi)(0) = t_tan_val; if (Hg) { - // Define a lambda that calls Logmap *without* the Jacobian argument - std::function logmap_wrapper = - [](const Gal3& state) -> Vector10 { - return Gal3::Logmap(state); // Call Logmap without Hg - }; - // Compute numerical derivative *Hg = Gal3::LogmapDerivative(g); - // This numerical derivative gives dLogmap(g)/dg where perturbation is on g. - // This corresponds to the derivative of the Local chart. - // Local(x,y) = Logmap(x.between(y)) - // Local(Identity, g) = Logmap(Identity.between(g)) = Logmap(g) - // Derivative d Local(Identity, g) / d g should be Jl_inv(Logmap(g)). Let's assume numericalDerivative works as expected. } 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(); - // Row block 1: drho' output 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; // [(r-t*v)]x * R + Ad.block<3,3>(0,6) = skewSymmetric(r_minus_tv) * Rmat; Ad.block<3,1>(0,9) = v_vec; - // Row block 2: dnu' output Ad.block<3,3>(3,3) = Rmat; - Ad.block<3,3>(3,6) = skewSymmetric(v_vec) * Rmat; // [v]x * R + Ad.block<3,3>(3,6) = skewSymmetric(v_vec) * Rmat; - // Row block 3: dtheta' output Ad.block<3,3>(6,6) = Rmat; - // Row block 4: dt' output Ad(9,9) = 1.0; return Ad; @@ -399,18 +319,19 @@ Matrix10 Gal3::AdjointMap() const { //------------------------------------------------------------------------------ Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const { - Matrix10 Ad = AdjointMap(); // Ad = Ad(g) - Vector10 y = Ad * xi; // y = Ad_g(xi) + Matrix10 Ad = AdjointMap(); + Vector10 y = Ad * xi; if (H_xi) { - *H_xi = Ad; // Jacobian w.r.t xi is Ad(g) + *H_xi = Ad; } if (H_g) { - // Jacobian H_g = d(Ad_g(xi))/d(g) -> use numerical derivative + // 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) { - // Call Adjoint *without* asking for its Jacobians for numerical diff return g_in.Adjoint(xi_in); }; *H_g = numericalDerivative21(adjoint_action_wrt_g, *this, xi, 1e-7); @@ -419,40 +340,34 @@ Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, Optiona } //------------------------------------------------------------------------------ -Matrix10 Gal3::adjointMap(const Vector10& xi) { // Lowercase 'a' - ad(xi) - const Matrix3 Theta_hat = skewSymmetric(theta(xi)); // =[theta_xi]x - const Matrix3 Nu_hat = skewSymmetric(nu(xi)); // =[nu_xi]x - const Matrix3 Rho_hat = skewSymmetric(rho(xi)); // =[rho_xi]x - const double t_val = t_tan(xi)(0); // =t_xi +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(); - // Structure based on Lie bracket [xi, y] = ad(xi)y - // Row block 1: maps input rho_y, nu_y, theta_y, t_y to output rho_z 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; - // Row block 2: maps input nu_y, theta_y to output nu_z ad.block<3,3>(3,3) = Theta_hat; ad.block<3,3>(3,6) = Nu_hat; - // Row block 3: maps input theta_y to output theta_z ad.block<3,3>(6,6) = Theta_hat; - // Row block 4: maps input t_y to output t_z (which is 0) - 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 w.r.t y is ad(xi) + if (Hy) *Hy = ad_xi; if (Hxi) { - // Jacobian w.r.t xi is -ad(y) *Hxi = -adjointMap(y); } return ad_xi * y; @@ -460,8 +375,10 @@ Vector10 Gal3::adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<1 //------------------------------------------------------------------------------ Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) { - // Use numerical derivative for Expmap Jacobian - // Use locally defined constant + // 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); }; @@ -470,9 +387,11 @@ Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) { //------------------------------------------------------------------------------ Matrix10 Gal3::LogmapDerivative(const Gal3& g) { - // Use numerical derivative for Logmap Jacobian + // 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); - // Use locally defined constant if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); std::function fn = [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; @@ -483,6 +402,7 @@ Matrix10 Gal3::LogmapDerivative(const Gal3& g) { // 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); @@ -490,25 +410,25 @@ Matrix5 Gal3::Hat(const Vector10& xi) { 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) + 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) { - // Check Lie algebra structure: Row 4 all=0; Row 3 first 3=0, last = t. + // 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); // rho from column 4 - nu(xi) = X.block<3, 1>(0, 3); // nu from column 3 + 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); // Extract theta from skew-symmetric part - t_tan(xi)(0) = X(3, 4); // t from element (3, 4) + theta(xi) << S(2, 1), S(0, 2), S(1, 0); + t_tan(xi)(0) = X(3, 4); return xi; } @@ -516,38 +436,43 @@ Vector10 Gal3::Vee(const Matrix5& X) { // ChartAtOrigin //------------------------------------------------------------------------------ Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) { - // Retraction at origin is Expmap return Gal3::Expmap(xi, Hxi); } //------------------------------------------------------------------------------ Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) { - // Local coordinates at origin is Logmap return Gal3::Logmap(g, Hg); } -Point3 Gal3::act(const Point3& p, OptionalJacobian<3, 10> Hself, OptionalJacobian<3, 3> Hp) const { - // Implementation assumes instantaneous action: p_out = R_.rotate(p) + r_ - Point3 r_out = R_.rotate(p) + r_; +//------------------------------------------------------------------------------ +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(); - if (Hp) { - *Hp = R_.matrix(); // Jacobian d(R*p+r)/dp = R - } - if (Hself) { - // Jacobian d(act(g,p))/d(xi) where g = this, xi is tangent at identity - // Corresponds to columns for [rho, nu, theta, t_tan] - Hself->setZero(); - const Matrix3 Rmat = R_.matrix(); - // Derivative w.r.t. rho (cols 0-2) -> d(r)/drho = R - Hself->block<3,3>(0, 0) = Rmat; - // Derivative w.r.t. nu (cols 3-5) -> d(R*p+r)/dnu = 0 - // Block remains zero. - // Derivative w.r.t. theta (cols 6-8) -> d(R*p)/dtheta = -R*skew(p) - Hself->block<3,3>(0, 6) = -Rmat * skewSymmetric(p); - // Derivative w.r.t. t_tan (col 9) -> Based on retract analysis, should be v_ - Hself->block<3,1>(0, 9) = v_; - } - return r_out; + 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 index fb6d0a0ef..0b1a0ea79 100644 --- a/gtsam/geometry/Gal3.h +++ b/gtsam/geometry/Gal3.h @@ -1,15 +1,15 @@ /* ---------------------------------------------------------------------------- * 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 - * -------------------------------------------------------------------------- */ + +* 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_tan] * @authors Matt Kielo, Scott Baker, Frank Dellaert * @date April 30, 2025 */ @@ -17,6 +17,7 @@ #pragma once #include // Includes Rot3, Point3 +#include #include // For LieGroup base class and traits #include // For Manifold traits @@ -32,36 +33,14 @@ class Gal3; using Velocity3 = Vector3; // Define Vector10 for tangent space using Vector10 = Eigen::Matrix; -// Define Matrix5 for Lie Algebra matrix representation (Hat map output) +// Define Matrix5 for Lie Algebra matrix representation using Matrix5 = Eigen::Matrix; -// Define Matrix10 for Jacobians (AdjointMap, Expmap/Logmap derivatives) +// 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 + * Internal state: rotation, translation, velocity, time. */ class GTSAM_EXPORT Gal3 : public LieGroup { private: @@ -81,11 +60,11 @@ class GTSAM_EXPORT Gal3 : public LieGroup { /// 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) + /// 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 (manif convention) + /// Construct from a 5x5 matrix representation explicit Gal3(const Matrix5& M); /// Named constructor from components with derivatives @@ -117,28 +96,27 @@ class GTSAM_EXPORT Gal3 : public LieGroup { /// Access time component (double) const double& time(OptionalJacobian<1, 10> H = {}) const; - // Convenience accessors matching NavState names where they overlap + // 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). + /// Return rotation matrix (Matrix3) Matrix3 R() const { return R_.matrix(); } - /// Return position as Vector3. + /// Return position as Vector3 Vector3 r() const { return Vector3(r_); } // Conversion from Point3 - /// Return velocity as Vector3. Computation-free. + /// Return velocity as Vector3 const Vector3& v() const { return v_; } - /// Return time scalar. Computation-free. + /// Return time scalar const double& t() const { return t_; } - /// Return 5x5 homogeneous matrix representation (manif convention). + /// Return 5x5 homogeneous matrix representation Matrix5 matrix() const; /// @} @@ -168,13 +146,7 @@ class GTSAM_EXPORT Gal3 : public LieGroup { // Bring LieGroup::inverse() into scope (version with derivative) using LieGroup::inverse; - /// Compose with another element: `this * other` - // Gal3 compose(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, - // OptionalJacobian<10, 10> H2 = {}) const; - - // /// Group composition operator: `this * other` - // Gal3 operator*(const Gal3& other) const { return compose(other); } - // Gal3 compose(const Gal3& other) const; + /// Group composition operator Gal3 operator*(const Gal3& other) const; /// @} @@ -182,21 +154,14 @@ class GTSAM_EXPORT Gal3 : public LieGroup { /// @{ /** - * Action of the group element on a 3D point. - * Following GTSAM convention for Pose3::transformFrom, this applies - * the instantaneous rotation and translation: p' = R*p + r. - * If time evolution is needed, use compose or specific dynamics model. + * 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 */ - Point3 act(const Point3& p, - OptionalJacobian<3, 10> Hself = {}, - OptionalJacobian<3, 3> Hp = {}) const; - - // Alias for consistency if used elsewhere - Point3 transformFrom(const Point3& p, - OptionalJacobian<3, 10> Hself = {}, - OptionalJacobian<3, 3> Hp = {}) const { - return act(p, Hself, Hp); - } + Event act(const Event& e, OptionalJacobian<4, 10> Hself = {}, + OptionalJacobian<4, 4> He = {}) const; /// @} /// @name Lie Group Static Functions @@ -205,9 +170,7 @@ class GTSAM_EXPORT Gal3 : public LieGroup { /// The type of the Lie algebra (matrix representation) using LieAlgebra = Matrix5; - // Helper functions for accessing tangent vector components by name - // Input xi is [rho(3), nu(3), theta(3), t_tan(1)] - // *** CORRECTED to use .block(startRow, startCol) syntax *** + // Helper functions for accessing tangent vector components static Eigen::Block rho(Vector10& v) { return v.block<3, 1>(0, 0); } static Eigen::Block nu(Vector10& v) { return v.block<3, 1>(3, 0); } static Eigen::Block theta(Vector10& v) { return v.block<3, 1>(6, 0); } @@ -219,36 +182,30 @@ class GTSAM_EXPORT Gal3 : public LieGroup { static Eigen::Block t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); } /// Exponential map at identity: tangent vector xi -> manifold element g - /// xi = [rho, nu, theta, t_tan] (10x1) static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {}); /// Logarithmic map at identity: manifold element g -> tangent vector xi - /// Returns xi = [rho, nu, theta, t_tan] (10x1) static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {}); - /// Calculate Adjoint map Ad_g, transforming a tangent vector in the identity frame - /// to the frame of this element g. Ad_g = [Ad_R, 0; Ad_r*Ad_R, Ad_R] structure adapted for Gal3. + /// Calculate Adjoint map Ad_g Matrix10 AdjointMap() const; - /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at identity. - /// Result is xi_this = Ad_g * xi_base. - /// H_g = d(Ad_g * xi_base)/dg, H_xi = d(Ad_g * xi_base)/dxi_base = Ad_g - Vector10 Adjoint(const Vector10& xi_base, OptionalJacobian<10, 10> H_g = {}, OptionalJacobian<10, 10> H_xi = {}) 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`. - /// This is the Lie bracket [xi, y] = ad_xi(y). - static Vector10 adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi = {}, OptionalJacobian<10, 10> Hy = {}); + /// 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 10x10 adjoint map `ad(xi)` associated with tangent vector xi. - /// Acts on a tangent vector y: `ad(xi, y) = ad(xi) * y`. + /// 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. - /// Currently implemented numerically. + /// 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 (represented as tangent vector at identity). - /// Currently implemented numerically. + /// Derivative of Logmap(g) w.r.t. g static Matrix10 LogmapDerivative(const Gal3& g); /// Chart at origin, uses Expmap/Logmap for Retract/Local @@ -257,12 +214,10 @@ class GTSAM_EXPORT Gal3 : public LieGroup { 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_tan] -> [ skew(theta) nu rho ; 0 0 t_tan ; 0 0 0 ] + /// 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 (Manif convention). - /// [ S N R ; 0 0 T ; 0 0 0 ] -> [ R ; N ; vee(S) ; T ] = [rho; nu; theta; t_tan] + /// Vee operator: maps Lie algebra matrix to tangent vector xi static Vector10 Vee(const Matrix5& X); /// @} @@ -274,11 +229,10 @@ class GTSAM_EXPORT Gal3 : public LieGroup { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - // Serialize member variables - ar& BOOST_SERIALIZATION_NVP(R_); - ar& BOOST_SERIALIZATION_NVP(r_); - ar& BOOST_SERIALIZATION_NVP(v_); - ar& BOOST_SERIALIZATION_NVP(t_); + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(r_); + ar & BOOST_SERIALIZATION_NVP(v_); + ar & BOOST_SERIALIZATION_NVP(t_); } #endif /// @} diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam/geometry/tests/testEvent.cpp similarity index 100% rename from gtsam_unstable/geometry/tests/testEvent.cpp rename to gtsam/geometry/tests/testEvent.cpp diff --git a/gtsam/geometry/tests/testGal3.cpp b/gtsam/geometry/tests/testGal3.cpp index b798f9234..8235186b9 100644 --- a/gtsam/geometry/tests/testGal3.cpp +++ b/gtsam/geometry/tests/testGal3.cpp @@ -8,7 +8,7 @@ /** * @file testGal3.cpp - * @brief Unit tests for Gal3 class: self-consistency and recon to Manif + * @brief Unit tests for Gal3 class * @authors Matt Kielo, Scott Baker, Frank Dellaert * @date April 29, 2025 */ @@ -17,9 +17,10 @@ #include #include -#include // Include for Lie group concepts if needed later +#include // For CHECK_LIE_GROUP_DERIVATIVES, etc. #include -#include // Point3 is already included via Gal3.h usually +#include +#include // Included for kTestPose definition #include #include @@ -28,14 +29,14 @@ using namespace std; using namespace gtsam; -// Define tolerance matching Python TOL +// 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) -// 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) +// 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); @@ -44,30 +45,21 @@ const Pose3 kTestPose(kTestRot, kTestPos); const Gal3 kTestGal3(kTestRot, kTestPos, kTestVel, kTestTime); -/* ************************************************************************* */ -// Test GTSAM Concept Compliance /* ************************************************************************* */ TEST(Gal3, Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - // Since its LieAlgebra is Matrix5, it also technically fits IsMatrixLieGroup, - // but IsLieGroup is sufficient here. GTSAM_CONCEPT_ASSERT(IsLieGroup); } /* ************************************************************************* */ -// Use GTSAM's Lie Group Test Macros -/* ************************************************************************* */ - -// Define some test instances if needed, or reuse existing ones like kTestGal3 +// 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(); -// Check derivatives of compose, inverse, between, Expmap, Logmap -// compose, inverse, between are currently calculated analytically. -// @TODO: Expmap/Logmap/retract/localCoordinates currently rely on -// numerical derivative, so check is circular +/* ************************************************************************* */ +// 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); @@ -75,6 +67,7 @@ TEST(Gal3, LieGroupDerivatives) { CHECK_LIE_GROUP_DERIVATIVES(kTestGal3_Lie1, kTestGal3_Lie2); } +/* ************************************************************************* */ // Check derivatives of retract and localCoordinates TEST(Gal3, ChartDerivatives) { CHECK_CHART_DERIVATIVES(kIdentity_Lie, kIdentity_Lie); @@ -84,110 +77,64 @@ TEST(Gal3, ChartDerivatives) { } -/* ************************************************************************* */ -// Test 1: Static Constructors (`Create`, `FromPoseVelocityTime`) Value Tests /* ************************************************************************* */ TEST(Gal3, StaticConstructorsValue) { - // --- Test Gal3::Create --- Gal3 g1 = Gal3::Create(kTestRot, kTestPos, kTestVel, kTestTime); - - // Verify components match inputs - EXPECT(assert_equal(kTestRot, g1.rotation(), kTol)); - EXPECT(assert_equal(kTestPos, g1.translation(), kTol)); - EXPECT(assert_equal(kTestVel, g1.velocity(), kTol)); - EXPECT_DOUBLES_EQUAL(kTestTime, g1.time(), kTol); - // Verify whole object matches reference (which was constructed the same way) EXPECT(assert_equal(kTestGal3, g1, kTol)); - - // --- Test Gal3::FromPoseVelocityTime --- Gal3 g2 = Gal3::FromPoseVelocityTime(kTestPose, kTestVel, kTestTime); - - // Verify components match inputs - EXPECT(assert_equal(kTestPose.rotation(), g2.rotation(), kTol)); - EXPECT(assert_equal(kTestPose.translation(), g2.translation(), kTol)); - EXPECT(assert_equal(kTestVel, g2.velocity(), kTol)); - EXPECT_DOUBLES_EQUAL(kTestTime, g2.time(), kTol); - // Verify whole object matches reference EXPECT(assert_equal(kTestGal3, g2, kTol)); } -/* ************************************************************************* */ -// Test 2: Component Accessor Tests /* ************************************************************************* */ TEST(Gal3, ComponentAccessorsValue) { - // Use the pre-defined kTestGal3 constructed with known values - - // Test rotation() / attitude() EXPECT(assert_equal(kTestRot, kTestGal3.rotation(), kTol)); EXPECT(assert_equal(kTestRot, kTestGal3.attitude(), kTol)); // Alias - - // Test translation() / position() EXPECT(assert_equal(kTestPos, kTestGal3.translation(), kTol)); EXPECT(assert_equal(kTestPos, kTestGal3.position(), kTol)); // Alias - - // Test velocity() EXPECT(assert_equal(kTestVel, kTestGal3.velocity(), kTol)); - - // Test time() EXPECT_DOUBLES_EQUAL(kTestTime, kTestGal3.time(), kTol); - - // Test derived accessors EXPECT(assert_equal(kTestRot.matrix(), kTestGal3.R(), kTol)); - // Corrected line: Removed .vector() EXPECT(assert_equal(kTestPos, kTestGal3.r(), kTol)); EXPECT(assert_equal(kTestVel, kTestGal3.v(), kTol)); EXPECT_DOUBLES_EQUAL(kTestTime, kTestGal3.t(), kTol); } -/* ************************************************************************* */ -// Test 3: `Gal3(const Matrix5& M)` Constructor Test /* ************************************************************************* */ TEST(Gal3, MatrixConstructorValue) { - // Use the pre-defined kTestGal3 constructed with known values - Matrix5 M_known = kTestGal3.matrix(); // Assumes .matrix() is tested/correct - - // Construct from the known valid matrix + Matrix5 M_known = kTestGal3.matrix(); Gal3 g_from_matrix(M_known); - - // Verify the constructed object is equal to the original EXPECT(assert_equal(kTestGal3, g_from_matrix, kTol)); - // Optional: Test invalid matrix construction (if constructor has checks) - Matrix5 M_invalid = M_known; - M_invalid(4, 0) = 1e-5; // Violate the zero structure in the last row - // Assuming your Gal3(Matrix5) constructor throws std::invalid_argument - // or similar for invalid structure. Adjust exception type if needed. + // 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); - FAIL("Constructor should have thrown for invalid matrix structure."); + 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, test passes. - // Optionally check e.what() for specific message if desired. + // Expected exception } catch (...) { FAIL("Constructor threw unexpected exception type for invalid matrix."); } - // Test another invalid case - M_invalid = M_known; - M_invalid(3, 3) = 0.9; // Violate M(3,3) == 1 + // 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); + 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, test passes. + // Expected exception } catch (...) { FAIL("Constructor threw unexpected exception type for invalid matrix."); } } -// ======================================================================== -// Test Cases Section (mirroring Python's TestGal3 class) -// ======================================================================== - +/* ************************************************************************* */ /* ************************************************************************* */ TEST(Gal3, Identity) { - // Hardcoded expected data + // 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(); @@ -195,21 +142,19 @@ TEST(Gal3, Identity) { Gal3 custom_ident = Gal3::Identity(); - // Check components individually + // Original component checks (kept for functional equivalence) 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 + // 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) { - // Hardcoded test cases for Hat/Vee (from Python script) - // Case 1 + // Test Case 1 const Vector10 tau_vec_1 = (Vector10() << -0.9919387548344049, 0.41796965721894275, -0.08567669832855362, // rho -0.24728318780816563, 0.42470896104182426, 0.37654216726012074, // nu @@ -224,25 +169,20 @@ TEST(Gal3, HatVee) { 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 + // 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)); - - // 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 + // Test Case 1 const Vector10 tau_vec_1 = (Vector10() << -0.6659680127970163, 0.0801034296770802, -0.005425197747099379, // rho -0.24823309993679712, -0.3282613511681929, -0.3614305580886979, // nu @@ -254,38 +194,30 @@ TEST(Gal3, Expmap) { -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 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 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 + // 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) { - // --- Part 1: Test Logmap(GroupElement) --- - // Hardcoded test case 1 (from Python script) + // 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(); - // 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; @@ -299,12 +231,10 @@ TEST(Gal3, Logmap) { ).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 + // Note: Logmap can have higher errors near singularities + EXPECT(assert_equal(expected_log_tau_1, custom_log_tau_1, kTol * 1e3)); - - // --- Part 2: Test Log(Exp(tau)) round trip --- - // Using data from the Expmap test case + // 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, @@ -313,22 +243,18 @@ TEST(Gal3, Logmap) { ).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 + // 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); - // 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) + // Test Case 1 const Matrix3 g1_R_mat_1 = (Matrix3() << -0.5427153955878299, 0.8391431164114453, 0.03603927817303032, 0.8040805715986894, 0.5314810596281534, -0.2664250694549225, @@ -349,7 +275,6 @@ TEST(Gal3, Compose) { 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, @@ -361,19 +286,14 @@ TEST(Gal3, Compose) { 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) + // Test Case 1 const Matrix3 g_R_mat_1 = (Matrix3() << 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, 0.6729369985913887, -0.6193062871756463, 0.4044941514923666, @@ -384,7 +304,6 @@ TEST(Gal3, Inverse) { 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, @@ -395,20 +314,17 @@ TEST(Gal3, Inverse) { 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) + // Test Case 1 const Matrix3 g1_R_mat_1 = (Matrix3() << -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, -0.6694062876067332, -0.572463082520484, 0.47347781496466923, @@ -429,7 +345,7 @@ TEST(Gal3, Between) { 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 + // Expected: c1.inverse() * c2 const Matrix3 expected_R_mat_1 = (Matrix3() << -0.6566377699430239, 0.753549894443112, -0.0314546605295386, -0.4242286152401611, -0.3345440819370167, 0.8414929228771536, @@ -440,16 +356,13 @@ TEST(Gal3, Between) { 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) + // Test Case 1 const Matrix3 source_R_mat_1 = (Matrix3() << 0.43788117516687186, -0.12083239518241493, -0.8908757538001356, 0.4981128609611659, 0.8575347951217139, 0.12852102124027118, @@ -460,31 +373,18 @@ TEST(Gal3, MatrixComponents) { 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) + // Test Case 1 const Vector10 tau1_1 = (Vector10() << 0.14491869060866264, -0.21431172810692092, -0.4956042914756127, -0.13411549788475238, 0.44534636811395767, -0.33281648500962074, @@ -511,17 +411,15 @@ TEST(Gal3, Associativity) { 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 + // 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(); - // Hardcoded test case 1 (from Python script) + // Test data const Matrix3 g_R_mat_1 = (Matrix3() << -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, 0.773189425449982, 0.15205374379417114, 0.6156766776243058, @@ -532,29 +430,24 @@ TEST(Gal3, IdentityProperties) { 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 + // 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 + EXPECT(assert_equal(custom_identity, g_times_inv_1, kTol * 10)); // Use slightly larger tol for round trip - // Test identity * g == g + // 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 + // 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 + EXPECT(assert_equal(custom_g_1, g_times_id_1, kTol)); } - -/* ************************************************************************* */ - /* ************************************************************************* */ TEST(Gal3, Adjoint) { - // Hardcoded test case for adjoint from Python code - const Matrix3 g_R_mat_1 = (Matrix3() << // Added << + // 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 @@ -562,32 +455,27 @@ TEST(Gal3, Adjoint) { 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 matrix (from Python test) - Matrix10 expected_adj_matrix; - expected_adj_matrix = (Matrix10() << // Added << + // 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 + 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(); - // Compute adjoint map Matrix10 computed_adj = test_g.AdjointMap(); - - // Compare with expected result - EXPECT(assert_equal(expected_adj_matrix, computed_adj, kTol * 10)); + EXPECT(assert_equal(expected_adj_matrix, computed_adj, kTol * 10)); // Slightly larger tolerance // Test tangent vector for adjoint action - Vector10 test_tangent = (Vector10() << // Added << + Vector10 test_tangent = (Vector10() << -0.28583171387804845, -0.7221038902918132, -0.09516831208249353, -0.13619637934919504, -0.05432836001072756, 0.1629798883384306, -0.20194877118636279, -0.18928645162443278, 0.07312685929426145, @@ -595,43 +483,39 @@ TEST(Gal3, Adjoint) { ).finished(); // Expected result after applying adjoint to tangent vector - Vector10 expected_adj_tau = (Vector10() << // Added << + Vector10 expected_adj_tau = (Vector10() << -0.7097860882934639, 0.5869666797222274, 0.10746143202899403, - 0.07529021542994252, 0.21635024626679053, 0.15385717129791027, + 0.07529021542994252, 0.21635024626679053, 0.15385717129791027, -0.05294531834013589, 0.27816922833676766, -0.042176749221369034, -0.042327821984942095 ).finished(); - // Apply adjoint to tangent vector Vector10 computed_adj_tau = test_g.Adjoint(test_tangent); - - // Compare with expected result - EXPECT(assert_equal(expected_adj_tau, computed_adj_tau, kTol * 10)); + 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); - // Compare with expected result for the adjoint property - Vector10 expected_log_result = (Vector10() << // Added << + // 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.07529021542994252, 0.21635024626679047, 0.15385717129791018, -0.05294531834013579, 0.27816922833676755, -0.04217674922136877, -0.04232782198494209 ).finished(); - EXPECT(assert_equal(expected_log_result, log_result, kTol * 10)); + // 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)); } -// ======================================================================== -// Jacobian Tests Section (mirroring Python's TestGal3Jacobians class) -// ======================================================================== - - /* ************************************************************************* */ TEST(Gal3, Jacobian_Compose) { - // Hardcoded data from Python test test_compose_jacobians + // Test data Matrix3 g1_R_mat = (Matrix3() << -0.3264538540162394, 0.24276278793202133, -0.9135064914894779, 0.9430076454867458, 0.1496317101600385, -0.2972321178273404, @@ -652,6 +536,7 @@ TEST(Gal3, Jacobian_Compose) { 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, @@ -664,21 +549,18 @@ TEST(Gal3, Jacobian_Compose) { 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 expected_H2 = Matrix10::Identity(); // From Python test - - // Calculate analytical Jacobians Matrix10 H1, H2; g1.compose(g2, H1, H2); - // Compare EXPECT(assert_equal(expected_H1, H1, kTol)); EXPECT(assert_equal(expected_H2, H2, kTol)); } /* ************************************************************************* */ TEST(Gal3, Jacobian_Inverse) { - // Hardcoded data from Python test test_inverse_jacobian + // Test data Matrix3 g_R_mat = (Matrix3() << 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, 0.6729369985913887, -0.6193062871756463, 0.4044941514923666, @@ -689,6 +571,7 @@ TEST(Gal3, Jacobian_Inverse) { 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, @@ -702,21 +585,17 @@ TEST(Gal3, Jacobian_Inverse) { -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -1.0 ).finished(); - // Calculate analytical Jacobian Matrix10 H; g.inverse(H); - // Compare EXPECT(assert_equal(expected_H, H, kTol)); } -/* ************************************************************************* */ -// Test Logmap Value and Jacobian (vs numerical) using captured data /* ************************************************************************* */ TEST(Gal3, Jacobian_Logmap) { const double jac_tol = 1e-5; // Tolerance for Jacobian checks - // --- Test Case 1 --- + // Test Case 1 const Matrix3 R1_mat = (Matrix3() << -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, 0.773189425449982, 0.15205374379417114, 0.6156766776243058, @@ -728,25 +607,23 @@ TEST(Gal3, Jacobian_Logmap) { const Gal3 g1(Rot3(R1_mat), r1_vec, v1_vec, t1_val); const Vector10 expected_log_tau1 = (Vector10() << - -1.2604528322799349, -0.3898722924179116, -0.6402392791385879, // rho - -0.34870126525214656, -0.4153032457886797, 1.1791315551702946, // nu - 1.4969846781977756, 2.324590726540746, 1.321595100333433, // theta - -0.24958604725524136 // t_tan + -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); - - // Verify value EXPECT(assert_equal(expected_log_tau1, log1_gtsam, kTol)); - // Verify Jacobian (calculated by Gal3::Logmap) against numerical derivative + + // Verify Jacobian against numerical derivative std::function logmap_func1 = - [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + [](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 --- + // Test Case 2 const Matrix3 R2_mat = (Matrix3() << 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, 0.6729369985913887,-0.6193062871756463, 0.4044941514923666, @@ -758,19 +635,18 @@ TEST(Gal3, Jacobian_Logmap) { const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); const Vector10 expected_log_tau2 = (Vector10() << - -0.5687147057967125, -0.3805406510759017, -1.063343079044753, // rho - 0.5179342682694047, 0.3616924279678234, -0.0984011207117694, // nu - -2.215366977027571, -0.72705858167113, 0.7749725693135466, // theta - 0.3757227805330059 // t_tan + -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); - - // Verify value EXPECT(assert_equal(expected_log_tau2, log2_gtsam, kTol)); - // Verify Jacobian - std::function logmap_func2 = + + // 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)); @@ -778,14 +654,15 @@ TEST(Gal3, Jacobian_Logmap) { /* ************************************************************************* */ TEST(Gal3, Jacobian_Expmap) { - // Hardcoded data from Python test test_expmap_jacobian + // Test data Vector10 tau_vec = (Vector10() << - 0.1644755309744135, -0.212580759622502, 0.027598787765563664, // rho - 0.06314401798217141, -0.07707725418679535, -0.26078036994807674, // nu - 0.3779854061644677, -0.09638288751073966, -0.2917351793587256, // theta - -0.49338105141355293 // t_tan + 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, @@ -799,66 +676,16 @@ TEST(Gal3, Jacobian_Expmap) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 ).finished(); - // Calculate analytical Jacobian Matrix10 H; Gal3::Expmap(tau_vec, H); - // Compare EXPECT(assert_equal(expected_H, H, kTol)); } /* ************************************************************************* */ -// From Manif -- but doesnt match. Unclear why. -// TEST(Gal3, Jacobian_Between) { -// // Hardcoded data from Python test test_between_jacobians -// 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 expected_H1 = (Matrix10() << -// -0.6615763028739445, 0.20669033144286447, 0.7208300093390132, -0.1797340003083421, 0.0561526764696732, 0.19583177414002104, -0.3638953521017815, 0.5514557580523199, -0.4921064751586784, 0.18578447025511585, -// -0.6777438034544028, 0.24655572954989036, -0.6927291022508597, -0.18412631233900273, 0.06698312406057733, -0.1881974492385603, 0.16072606484418983, -1.0875528521598636, -0.544330359681567, 0.8120736895446168, -// 0.3209051765783825, 0.9468312305338058, 0.02303233663866666, 0.08718203910196184, 0.25723074412043495, 0.006257319046242687, -0.41075512891439026, 0.1628186824736606, -0.9703039103797245, -0.4943804748805436, -// -0.0, -0.0, -0.0, -0.6615763028739445, 0.20669033144286447, 0.7208300093390132, 0.07446465266115959, -0.890789069415051, 0.32376778794135697, -0.0, -// -0.0, -0.0, -0.0, -0.6777438034544028, 0.24655572954989036, -0.6927291022508597, -0.2674512085517958, 0.2780902027976629, 0.3606433327863202, -0.0, -// -0.0, -0.0, -0.0, 0.3209051765783825, 0.9468312305338058, 0.02303233663866666, -0.41133443569666744, 0.12204155444521667, 0.714065394510391, -0.0, -// -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.6615763028739445, 0.20669033144286447, 0.7208300093390132, -0.0, -// -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.6777438034544028, 0.24655572954989036, -0.6927291022508597, -0.0, -// -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, 0.3209051765783825, 0.9468312305338058, 0.02303233663866666, -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(); // From Python test - -// // Calculate analytical Jacobians -// Matrix10 H1, H2; -// g1.between(g2, H1, H2); - -// // Compare -// EXPECT(assert_equal(expected_H1, H1, kTol)); -// EXPECT(assert_equal(expected_H2, H2, kTol)); -// } -// -// Using numerical derivative as a reference instead of Manif. -// +// Test Between Jacobian against numerical derivatives TEST(Gal3, Jacobian_Between) { - // Define test points g1, g2 (using the same data as before for context) + // Test data Matrix3 g1_R_mat = (Matrix3() << 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, @@ -879,42 +706,25 @@ TEST(Gal3, Jacobian_Between) { double g2_t_val = -0.41496643117394594; Gal3 g2(Rot3(g2_R_mat), g2_r_vec, g2_v_vec, g2_t_val); - // --- Verify H1 --- - Matrix10 H1_analytical, H2_analytical; // Placeholders - // Calculate analytical Jacobians using your Gal3::between implementation + Matrix10 H1_analytical, H2_analytical; g1.between(g2, H1_analytical, H2_analytical); - // Define the 'between' function for numerical derivatives std::function between_func = [](const Gal3& g1_in, const Gal3& g2_in) { return g1_in.between(g2_in); }; - // Calculate numerical derivative w.r.t first arg - Matrix H1_numerical = numericalDerivative21(between_func, g1, g2, 1e-6); // Adjust epsilon if needed - - // Compare analytical H1 with numerical H1 + // 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 --- - // Analytical H2 is already calculated in H2_analytical - - // Calculate numerical derivative w.r.t second arg - Matrix H2_numerical = numericalDerivative22(between_func, g1, g2, 1e-6); // Adjust epsilon if needed - - // Compare analytical H2 with numerical H2 + // 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 } -/* ************************************************************************* */ -// @TODO: Add test_act_jacobians from Python as Gal3::act is not defined. -/* ************************************************************************* */ - - -/* ************************************************************************* */ -// Test AdjointMap against numerical derivatives /* ************************************************************************* */ TEST(Gal3, Jacobian_AdjointMap) { - // Use a sample Gal3 object (e.g., g1 from the between test) + // Test data Matrix3 g1_R_mat = (Matrix3() << 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, @@ -925,52 +735,35 @@ TEST(Gal3, Jacobian_AdjointMap) { double g1_t_val = -0.6866418214918308; Gal3 g = Gal3(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); - // Pick a sample tangent vector Vector10 xi = (Vector10() << 0.1, -0.2, 0.3, 0.4, -0.5, 0.6, -0.1, 0.15, -0.25, 0.9).finished(); - // Calculate the analytical Adjoint Map + // Analytical Adjoint Map Matrix10 Ad_analytical = g.AdjointMap(); - // Define the Adjoint action function for numerical differentiation - // Calculates Ad_g(xi) + // 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); // Assuming g.Adjoint(xi) uses g.AdjointMap() * xi + return g_in.Adjoint(xi_in); }; - - // Calculate numerical derivative of Ad_g(xi) w.r.t xi - // This numerical derivative should be equal to the Adjoint Map itself Matrix H_xi_numerical = numericalDerivative22(adjoint_action, g, xi); + EXPECT(assert_equal(Ad_analytical, H_xi_numerical, 1e-7)); // Tolerance for numerical diff - // Compare analytical AdjointMap with the numerical derivative w.r.t xi - EXPECT(assert_equal(Ad_analytical, H_xi_numerical, 1e-7)); // Use a tolerance suitable for numerical diff - - // --- Optional: Also check the derivative w.r.t g --- - // Define Adjoint action function for numerical derivative w.r.t g - std::function adjoint_action_wrt_g = - [](const Gal3& g_in, const Vector10& xi_in) { - Matrix10 H_g; // Analytical H_g from Adjoint - Vector10 result = g_in.Adjoint(xi_in, H_g); // Need Adjoint to compute its H_g - return result; // Only return value for numericalDerivative - }; - - // Calculate numerical derivative of Ad_g(xi) w.r.t g - Matrix H_g_numerical = numericalDerivative21(adjoint_action_wrt_g, g, xi); - - // Calculate analytical derivative of Ad_g(xi) w.r.t g + // Verify derivative of Adjoint action Ad_g(xi) w.r.t g Matrix10 H_g_analytical; - g.Adjoint(xi, H_g_analytical); // Call Adjoint to get analytical H_g + g.Adjoint(xi, H_g_analytical); // Calculate analytical H_g - // Compare analytical H_g with numerical 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 inverse Jacobian against numerical derivatives /* ************************************************************************* */ TEST(Gal3, Jacobian_Inverse2) { - // Use a sample Gal3 object (e.g., g1 from the between test) + // Test data Matrix3 g1_R_mat = (Matrix3() << 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, @@ -981,30 +774,26 @@ TEST(Gal3, Jacobian_Inverse2) { double g1_t_val = -0.6866418214918308; Gal3 g = Gal3(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); - // Calculate analytical Jacobian H_inv = d(g.inverse()) / d(g) + // Analytical Jacobian H_inv = d(g.inverse()) / d(g) Matrix10 H_inv_analytical; - g.inverse(H_inv_analytical); // Assuming inverse calculates its H + g.inverse(H_inv_analytical); - // Define inverse function wrapper + // Numerical Jacobian std::function inverse_func = [](const Gal3& g_in) { return g_in.inverse(); }; - - // Calculate numerical Jacobian Matrix H_inv_numerical = numericalDerivative11(inverse_func, g); - // Compare analytical H_inv with numerical H_inv EXPECT(assert_equal(H_inv_numerical, H_inv_analytical, 1e-5)); - // Optional: Check against theoretical formula H_inv = -Ad(g.inverse()) - // EXPECT(assert_equal(-g.inverse().AdjointMap(), H_inv_analytical, 1e-8)); + Matrix10 expected_adjoint = -g.AdjointMap(); // Check against -Ad(g) for right perturbations + EXPECT(assert_equal(expected_adjoint, H_inv_analytical, 1e-8)); + } -/* ************************************************************************* */ -// Test compose Jacobians against numerical derivatives /* ************************************************************************* */ TEST(Gal3, Jacobian_Compose2) { - // Use g1 and g2 from the between test - Matrix3 g1_R_mat = (Matrix3() << + // 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 @@ -1024,107 +813,129 @@ TEST(Gal3, Jacobian_Compose2) { double g2_t_val = -0.41496643117394594; Gal3 g2(Rot3(g2_R_mat), g2_r_vec, g2_v_vec, g2_t_val); - // Calculate analytical Jacobians H1=d(g1*g2)/dg1, H2=d(g1*g2)/dg2 + // Analytical Jacobians Matrix10 H1_analytical, H2_analytical; g1.compose(g2, H1_analytical, H2_analytical); - // Define compose function wrapper + // Numerical Jacobians std::function compose_func = [](const Gal3& g1_in, const Gal3& g2_in) { return g1_in.compose(g2_in); }; - - // Calculate numerical Jacobians Matrix H1_numerical = numericalDerivative21(compose_func, g1, g2); Matrix H2_numerical = numericalDerivative22(compose_func, g1, g2); - // Compare analytical vs numerical EXPECT(assert_equal(H1_numerical, H1_analytical, 1e-5)); EXPECT(assert_equal(H2_numerical, H2_analytical, 1e-5)); - // Optional: Check analytical Jacobians against theoretical formulas (assuming GTSAM conventions) - // H1 = Ad(g2.inverse()) - // H2 = Identity() (for right compose convention) - // EXPECT(assert_equal(g2.inverse().AdjointMap(), H1_analytical, 1e-8)); - // EXPECT(assert_equal(Matrix10::Identity(), H2_analytical, 1e-8)); + // 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 act method Value and Jacobians (vs numerical) using captured data /* ************************************************************************* */ TEST(Gal3, Act) { - const double jac_tol = 1e-6; // Tolerance for Jacobian checks + const double kTol = 1e-9; // Tolerance for value checks + const double jac_tol = 1e-6; // Tolerance for Jacobian checks - // --- Test Case 1 --- - 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 p_in1(4.967141530112327, -1.3826430117118464, 6.476885381006925); - const Point3 expected_p_out1(2.048479118929923, 0.321903700434202, 8.713486171732242); + // 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(3, 10); - Matrix H1p_gtsam = Matrix::Zero(3, 3); - Point3 p_out1_gtsam = g1.act(p_in1, H1g_gtsam, H1p_gtsam); // Requires Gal3::act to be implemented + 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); - // Verify value - EXPECT(assert_equal(expected_p_out1, p_out1_gtsam, kTol)); + EXPECT(assert_equal(expected_p_out1, e_out1_gtsam.location(), kTol)); + EXPECT_DOUBLES_EQUAL(expected_t_out1, e_out1_gtsam.time(), kTol); - // Verify Jacobians vs numerical - std::function act_func1 = - [](const Gal3& g_in, const Point3& p_in) { return g_in.act(p_in); }; // Requires Gal3::act - Matrix H1g_numerical = numericalDerivative21(act_func1, g1, p_in1, jac_tol); - Matrix H1p_numerical = numericalDerivative22(act_func1, g1, p_in1, jac_tol); - EXPECT(assert_equal(H1g_numerical, H1g_gtsam, jac_tol)); - EXPECT(assert_equal(H1p_numerical, H1p_gtsam, jac_tol)); + // 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)); - // --- Test Case 2 --- - const Matrix3 R2_mat = (Matrix3() << + // 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 p_in2(15.230298564080254, -2.3415337472333597, -2.3413695694918055); - const Point3 expected_p_out2(1.9483976916013555, 16.30852024491054, -1.2538227216376843); + ).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(3, 10); - Matrix H2p_gtsam = Matrix::Zero(3, 3); - Point3 p_out2_gtsam = g2.act(p_in2, H2g_gtsam, H2p_gtsam); // Requires Gal3::act to be implemented + 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); - // Verify value - EXPECT(assert_equal(expected_p_out2, p_out2_gtsam, kTol)); + EXPECT(assert_equal(expected_p_out2, e_out2_gtsam.location(), kTol)); + EXPECT_DOUBLES_EQUAL(expected_t_out2, e_out2_gtsam.time(), kTol); - // Verify Jacobians vs numerical - std::function act_func2 = - [](const Gal3& g_in, const Point3& p_in) { return g_in.act(p_in); }; // Requires Gal3::act - Matrix H2g_numerical = numericalDerivative21(act_func2, g2, p_in2, jac_tol); - Matrix H2p_numerical = numericalDerivative22(act_func2, g2, p_in2, jac_tol); - EXPECT(assert_equal(H2g_numerical, H2g_gtsam, jac_tol)); - EXPECT(assert_equal(H2p_numerical, H2p_gtsam, jac_tol)); + // 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 interpolate method against manif data (value check only) /* ************************************************************************* */ TEST(Gal3, Interpolate) { const double interp_tol = 1e-6; // Tolerance for interpolation value comparison - // --- Test Case 1 --- + // 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 + -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); @@ -1132,9 +943,9 @@ TEST(Gal3, Interpolate) { 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 + -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); @@ -1142,29 +953,28 @@ TEST(Gal3, Interpolate) { const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); // Expected results for different alphas - const Gal3 expected_alpha0_00 = g1; // Should be identical to g1 + 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), + 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_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; // Should be identical to g2 + 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 to be implemented + // 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)); @@ -1172,101 +982,82 @@ TEST(Gal3, Interpolate) { EXPECT(assert_equal(expected_alpha1_00, gtsam::interpolate(g1, g2, 1.0), interp_tol)); } - - -/* ************************************************************************* */ -// Added Test: Jacobians for Static Constructors /* ************************************************************************* */ TEST(Gal3, Jacobian_StaticConstructors) { - // Note: This test verifies analytical Jacobians against numerical derivatives - // of the constructor functions. Gal3::Create and Gal3::FromPoseVelocityTime - // implement analytical Jacobians, so this test is NOT circular. + // 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; // Use dynamic Matrix type + // 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); - // Numerical derivatives - // Explicitly use const double& for the double argument in lambda 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 }; - // Pass kTestTime by const reference to match lambda - const double& time_ref = kTestTime; + 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); - - // Compare 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; // Use dynamic Matrix type + // Test Gal3::FromPoseVelocityTime + gtsam::Matrix Hpv1_ana, Hpv2_ana, Hpv3_ana; Gal3::FromPoseVelocityTime(kTestPose, kTestVel, kTestTime, Hpv1_ana, Hpv2_ana, Hpv3_ana); - // Numerical derivatives - // Explicitly use const double& for the double argument in lambda std::function fromPoseVelTime_func = [](const Pose3& pose, const Velocity3& v, const double& t){ return Gal3::FromPoseVelocityTime(pose, v, t); // Call without Jacobians }; - // Pass kTestTime by const reference to match lambda 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); - // Compare 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)); } - -/* ************************************************************************* */ -// Added Test: Jacobians for Component Accessors /* ************************************************************************* */ TEST(Gal3, Jacobian_Accessors) { - // Note: This test verifies analytical Jacobians against numerical derivatives - // of the accessor functions. The accessors in Gal3 implement analytical - // Jacobians, so this test is NOT circular. + // 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; // Use dynamic Matrix type + // 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; // Use dynamic Matrix type + // 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; // Use dynamic Matrix type + // 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; // Use dynamic Matrix type + // Test time() Jacobian + gtsam::Matrix Htime_ana; kTestGal3.time(Htime_ana); std::function time_func = [](const Gal3& g) { return g.time(); }; @@ -1274,93 +1065,65 @@ TEST(Gal3, Jacobian_Accessors) { EXPECT(assert_equal(Htime_num, Htime_ana, jac_tol)); } -/* ************************************************************************* */ -// Added Test: Jacobians for interpolate /* ************************************************************************* */ TEST(Gal3, Jacobian_Interpolate) { // *** CIRCULARITY WARNING *** - // Gal3::interpolate computes its Jacobians (H1, H2, Ha) using a chain rule - // that involves the Jacobians of Logmap and Expmap. Since Gal3::Logmap and - // Gal3::Expmap compute their Jacobians numerically (via LogmapDerivative - // and ExpmapDerivative in Gal3.cpp), comparing the interpolate Jacobians - // against numerical derivatives here is circular. It verifies the chain rule - // implementation assuming the underlying numerical derivatives are correct. + // 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; // Use pre-defined states + const Gal3 g1 = kTestGal3_Lie1; const Gal3 g2 = kTestGal3_Lie2; - const double alpha = 0.3; // Example interpolation factor + const double alpha = 0.3; - gtsam::Matrix H1_ana, H2_ana, Ha_ana; // Use dynamic Matrix type + gtsam::Matrix H1_ana, H2_ana, Ha_ana; gtsam::interpolate(g1, g2, alpha, H1_ana, H2_ana, Ha_ana); - // Numerical derivatives - // Explicitly use const double& for the double argument in lambda std::function interp_func = [](const Gal3& g1_in, const Gal3& g2_in, const double& a){ - // Call interpolate *without* asking for its Jacobians for numerical diff - return gtsam::interpolate(g1_in, g2_in, a); + return gtsam::interpolate(g1_in, g2_in, a); // Call without Jacobians }; - // Pass alpha by const reference to match lambda - const double& alpha_ref = alpha; + 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); - - // Compare analytical (numerically derived) vs numerical 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)); } /* ************************************************************************* */ -// Added Test: Static Adjoint functions -/* ************************************************************************* */ -TEST(Gal3, StaticAdjoint) { // Removed duplicate TEST macro here - // Note: This test verifies the analytical Jacobians (Hxi, Hy) provided by - // the static Gal3::adjoint function against expected analytical properties - // (e.g., Hy = adjointMap(xi), Hxi = -adjointMap(y)) and also against - // numerical derivatives of the static adjoint function itself. - // Since the Jacobians in static Gal3::adjoint are implemented analytically - // using adjointMap, this test is NOT circular regarding those Jacobians. +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; - // Create two tangent vectors 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 --- + // Test static adjointMap Matrix10 ad_xi_map = Gal3::adjointMap(xi); - - // Check if ad_xi_map * y equals adjoint(xi, y) - // Evaluate the product explicitly into a Vector10 for assert_equal 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)); + EXPECT(assert_equal(ad_xi_map_times_y, ad_xi_y_direct, kTol)); // Check ad(xi)*y == [xi,y] - - // Verify Jacobian of adjoint(xi,y) w.r.t y equals adjointMap(xi) - std::function static_adjoint_func = + // Verify d(adjoint(xi,y))/dy == adjointMap(xi) numerically + std::function static_adjoint_func = [](const Vector10& xi_in, const Vector10& y_in){ - // Call static adjoint *without* asking for its Jacobians for numerical diff - return Gal3::adjoint(xi_in, 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 --- - gtsam::Matrix Hxi_ana, Hy_ana; // Use dynamic Matrix type + // Test static adjoint Jacobians + gtsam::Matrix Hxi_ana, Hy_ana; Gal3::adjoint(xi, y, Hxi_ana, Hy_ana); // Get analytical Jacobians - // Check analytical H_y matches ad_xi_map (which was checked numerically above) - EXPECT(assert_equal(ad_xi_map, Hy_ana, kTol)); + 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) - // Check analytical H_xi matches -ad(y) - Matrix10 minus_ad_y_map = -Gal3::adjointMap(y); - EXPECT(assert_equal(minus_ad_y_map, Hxi_ana, kTol)); - - // Verify analytical H_xi against numerical derivative + // 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)); @@ -1369,71 +1132,44 @@ TEST(Gal3, StaticAdjoint) { // Removed duplicate TEST macro here 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)); - // Evaluate the sum explicitly into a Vector10 for assert_equal Vector10 sum_terms = term1 + term2 + term3; - // Explicitly cast Vector10::Zero() to Vector10 to resolve ambiguity EXPECT(assert_equal(Vector10(Vector10::Zero()), sum_terms, jac_tol)); } -/* ************************************************************************* */ -// Test Expmap/Logmap self-consistency for near-zero tangent vectors /* ************************************************************************* */ TEST(Gal3, ExpLog_NearZero) { - // Tolerance for round-trip checks often needs to be slightly looser - const double kTolRoundtrip = kTol * 100; // Adjusted tolerance + const double kTolRoundtrip = kTol * 100; // Adjusted tolerance for round-trip - // --- Test Case 1: Small non-zero tangent vector --- + // Small non-zero tangent vector 1 Vector10 xi_small1; - xi_small1 << 1e-5, -2e-5, 1.5e-5, // rho - -3e-5, 4e-5, -2.5e-5, // nu - 1e-7, -2e-7, 1.5e-7, // theta (norm ~ 2.69e-7 radians) - -5e-5; // t_tan - + 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); - - // Check if the recovered tangent vector is close to the original small vector. - // Compare Vector10 directly using assert_equal's Eigen overload. EXPECT(assert_equal(xi_small1, xi_recovered1, kTolRoundtrip)); - - // --- Test Case 2: Another small non-zero tangent vector --- - Vector10 xi_small2; - xi_small2 << -5e-6, 1e-6, -4e-6, // rho - 2e-6, -6e-6, 1e-6, // nu - -5e-8, 8e-8, -2e-8, // theta (norm ~ 9.64e-8 radians) - 9e-6; // t_tan - + // 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)); - - // --- Test Case 3: Even smaller theta magnitude --- - Vector10 xi_small3; - xi_small3 << 1e-9, 2e-9, 3e-9, // rho - -4e-9,-5e-9,-6e-9, // nu - 1e-10, 2e-10, 3e-10, // theta (norm ~ 3.74e-10 radians) - 7e-9; // t_tan - + // 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); - // Use a tighter tolerance as we get closer to zero - EXPECT(assert_equal(xi_small3, xi_recovered3, kTol)); + EXPECT(assert_equal(xi_small3, xi_recovered3, kTol)); // Tighter tolerance near zero - - // --- Test Case 4: Zero tangent vector (Strict Identity case) --- + // 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)); // Compare vectors directly - EXPECT(assert_equal(xi_zero, Gal3::Logmap(Gal3::Expmap(xi_zero)), kTol)); // Compare vectors directly + 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;