diff --git a/.gitignore b/.gitignore index bca6a479f..946378312 100644 --- a/.gitignore +++ b/.gitignore @@ -23,3 +23,4 @@ xcode/ # MyST build outputs _build +examples/evaluatePreintegrationNavState.cpp diff --git a/gtsam/navigation/Gal3.cpp b/gtsam/navigation/Gal3.cpp index 339cff9ec..dd223381d 100644 --- a/gtsam/navigation/Gal3.cpp +++ b/gtsam/navigation/Gal3.cpp @@ -10,7 +10,7 @@ * @file Gal3.cpp * @brief Implementation of 3D Galilean Group SGal(3) state * @author Based on Python implementation by User - * @date April 29, 2025 + * @date April 30, 2025 // Updated Date */ #include // Note: Adjust path if needed @@ -26,140 +26,125 @@ namespace gtsam { //------------------------------------------------------------------------------ -// Static Helper Functions Implementation +// Constants and Helper function for Expmap/Logmap (Implementation Detail) //------------------------------------------------------------------------------ +namespace { // Anonymous namespace for internal linkage -Matrix3 Gal3::SO3_LeftJacobian(const Vector3& theta) { - using std::cos; - using std::sin; - using std::sqrt; + // Define the threshold locally within the cpp file + constexpr double kSmallAngleThreshold = 1e-10; - const double angle_sq = theta.squaredNorm(); + Matrix3 Calculate_E(const Vector3& theta) { + using std::cos; + using std::sin; + using std::sqrt; - // Use Taylor series expansion for small angles - if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { - // Jl(w) approx = I + 0.5*[w]x + (1/6)*[w]x^2 - const Matrix3 W = skewSymmetric(theta); - return Matrix3::Identity() + 0.5 * W + (1.0/6.0) * W * W; - } + const double angle_sq = theta.squaredNorm(); + const Matrix3 W = skewSymmetric(theta); + const Matrix3 W2 = W * W; - const double angle = sqrt(angle_sq); - const Matrix3 W = skewSymmetric(theta); - const Matrix3 W2 = W * W; + Matrix3 E = 0.5 * Matrix3::Identity(); - const double inv_angle_sq = 1.0 / angle_sq; - const double sin_angle = sin(angle); - const double cos_angle = cos(angle); + // Small angle approximation - Use locally defined constant + if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { + E += (1.0 / 6.0) * W + (1.0 / 24.0) * W2; + return E; + } - // coeff1 = (1 - cos(theta)) / theta^2 - const double coeff1 = (1.0 - cos_angle) * inv_angle_sq; - // coeff2 = (theta - sin(theta)) / theta^3 - const double coeff2 = (angle - sin_angle) / (angle_sq * angle); + const double angle = sqrt(angle_sq); + const double sin_angle = sin(angle); + const double cos_angle = cos(angle); - return Matrix3::Identity() + coeff1 * W + coeff2 * W2; + // Coefficients from manif::SGal3TangentBase::exp() + const double A = (angle - sin_angle) / (angle_sq * angle); // A = (theta - sin(theta)) / theta^3 + const double B = (angle_sq + 2.0 * cos_angle - 2.0) / (2.0 * angle_sq * angle_sq); // B = (theta^2 + 2*cos(theta) - 2) / (2 * theta^4) + + E += A * W + B * W2; + return E; + } +} // end anonymous namespace + +//------------------------------------------------------------------------------ +// Static Constructor/Create functions (NavState order: 1) +//------------------------------------------------------------------------------ +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) + *H1 = Matrix::Zero(10, 3); + H1->block<3, 3>(6, 0) = Matrix3::Identity(); // d theta / d omega_R = I + } + if (H2) { // d xi_out / d delta_r (3x3) + *H2 = Matrix::Zero(10, 3); + H2->block<3, 3>(0, 0) = R.transpose(); // d rho / d delta_r = R^T + } + if (H3) { // d xi_out / d delta_v (3x3) + *H3 = Matrix::Zero(10, 3); + H3->block<3, 3>(3, 0) = R.transpose(); // d nu / d delta_v = R^T + } + if (H4) { // d xi_out / d delta_t (10x1) + *H4 = Matrix::Zero(10, 1); + // 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 + } + return Gal3(R, r, v, t); } //------------------------------------------------------------------------------ -Matrix3 Gal3::SO3_LeftJacobianInverse(const Vector3& theta) { - using std::cos; - using std::sin; - using std::sqrt; - using std::tan; - - const double angle_sq = theta.squaredNorm(); - const Matrix3 W = skewSymmetric(theta); - - // Use Taylor series expansion for small angles - if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { - // Jl(w)^-1 approx = I - 0.5*W + (1/12)*W^2 - return Matrix3::Identity() - 0.5 * W + (1.0/12.0) * W * W; +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] + *H1 = Matrix::Zero(10, 6); + // 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. } - - const double angle = sqrt(angle_sq); - const double half_angle = 0.5 * angle; - - // Formula: I - 0.5*W + (1/theta^2 - cot(theta/2)/(2*theta)) * W^2 - // which is I - 0.5*W + (1 - 0.5*theta*cot(0.5*theta))/theta^2 * W^2 - double cot_half_angle; - // Avoid division by zero/very small numbers for tan(half_angle) - if (std::abs(sin(half_angle)) < kSmallAngleThreshold) { - // If sin(half_angle) is near zero, theta is near multiples of 2*pi. - // cot(x) ~ 1/x for small x. Let x = half_angle. - // If half_angle is very small, use Taylor expansion of the coefficient: - // (1 - 0.5*theta*cot(0.5*theta))/theta^2 -> 1/12 + O(theta^2) - if (std::abs(half_angle) < kSmallAngleThreshold) { - // Use Taylor approx for the coefficient directly - const double coeff = 1.0 / 12.0; // + angle_sq / 720.0 + ...; // Higher order if needed - return Matrix3::Identity() - 0.5 * W + coeff * (W * W); - } else { - // theta is near non-zero multiples of 2*pi. tan(half_angle) is near zero. - // cot becomes large. Use limit or high-precision calculation if needed. - // For now, use standard tan. May need robustness improvements. - cot_half_angle = cos(half_angle) / sin(half_angle); // Avoid 1.0/tan() - } - } else { - cot_half_angle = cos(half_angle) / sin(half_angle); // Avoid 1.0/tan() + if (H2) { // d xi_out / d v_in (10x3) + *H2 = Matrix::Zero(10, 3); + H2->block<3, 3>(3, 0) = R.transpose(); // d(nu_out)/d(v_in) = R^T } - - const double coeff = (1.0 - 0.5 * angle * cot_half_angle) / angle_sq; - return Matrix3::Identity() - 0.5 * W + coeff * (W * W); - - // Alternative using GTSAM SO3 properties (verify this matches J_l^-1): - // J_l(theta)^-1 = J_r(-theta). - // return gtsam::so3::DexpFunctor(-theta).rightJacobian(); -} - - -//------------------------------------------------------------------------------ -Matrix3 Gal3::Calculate_E(const Vector3& theta) { - using std::cos; - using std::sin; - using std::sqrt; - - const double angle_sq = theta.squaredNorm(); - const Matrix3 W = skewSymmetric(theta); - const Matrix3 W2 = W * W; - - Matrix3 E = 0.5 * Matrix3::Identity(); - - // Small angle approximation (from manif SGal3Tangent_base.h / Expmap formula) - // E approx = 0.5*I + (1/6)*W + (1/24)*W^2 - if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) { - E += (1.0 / 6.0) * W + (1.0 / 24.0) * W2; - return E; + if (H3) { // d xi_out / d t_in (10x1) + *H3 = Matrix::Zero(10, 1); + // 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 } - - const double angle = sqrt(angle_sq); - const double sin_angle = sin(angle); - const double cos_angle = cos(angle); - - // Coefficients A and B from manif::SGal3TangentBase::exp() - // A = (theta - sin(theta)) / theta^3 - const double A = (angle - sin_angle) / (angle_sq * angle); - // B = (theta^2 + 2*cos(theta) - 2) / (2 * theta^4) - const double B = (angle_sq + 2.0 * cos_angle - 2.0) / (2.0 * angle_sq * angle_sq); - - E += A * W + B * W2; - return E; + // Pass r (translation from pose) to the Gal3 constructor + return Gal3(R, r, v, t); } //------------------------------------------------------------------------------ -// Constructors +// Constructors (NavState order: 2 - Included in Constructor section) //------------------------------------------------------------------------------ Gal3::Gal3(const Matrix5& M) { - // CORRECTED: Use head(N) with correct sizes for checks + // Ensure matrix adheres to SGal(3) structure if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 || M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) { // Row 4 first 4 = 0; Row 3 first 3 = 0 throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones."); } R_ = Rot3(M.block<3, 3>(0, 0)); - v_ = M.block<3, 1>(0, 3); // Manif: v is in column 3 (0-indexed) - r_ = Point3(M.block<3, 1>(0, 4)); // Manif: r is in column 4 - t_ = M(3, 4); // Manif: t is at (3, 4) + 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) } //------------------------------------------------------------------------------ -// Component Access +// Component Access (NavState order: 3) //------------------------------------------------------------------------------ const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const { if (H) { @@ -171,9 +156,17 @@ const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const { //------------------------------------------------------------------------------ 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 = Matrix::Zero(3, 10); - H->block<3, 3>(0, 0) = R_.matrix(); // Derivative w.r.t rho (local tangent space) + // 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_; } @@ -197,21 +190,32 @@ const double& Gal3::time(OptionalJacobian<1, 10> H) const { } //------------------------------------------------------------------------------ -// Matrix Representation +// Matrix Representation (NavState order: 4) //------------------------------------------------------------------------------ Matrix5 Gal3::matrix() const { Matrix5 M = Matrix5::Identity(); M.block<3, 3>(0, 0) = R_.matrix(); - M.block<3, 1>(0, 3) = v_; // Manif: v in col 3 - M.block<3, 1>(0, 4) = Vector3(r_); // Manif: r in col 4 // CORRECTED: Assign Vector3 from Point3 - M(3, 4) = t_; // Manif: t at (3, 4) + M.block<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 return M; } //------------------------------------------------------------------------------ -// Testable Requirements +// Stream operator (NavState order: 5) +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const Gal3& state) { + os << "R: " << state.R_ << "\n"; + os << "r: " << state.r_.transpose() << "\n"; + os << "v: " << state.v_.transpose() << "\n"; + os << "t: " << state.t_; + return os; +} + +//------------------------------------------------------------------------------ +// Testable Requirements (NavState order: 6) //------------------------------------------------------------------------------ void Gal3::print(const std::string& s) const { std::cout << (s.empty() ? "" : s + " "); @@ -227,13 +231,12 @@ bool Gal3::equals(const Gal3& other, double tol) const { } //------------------------------------------------------------------------------ -// Group Operations +// Group Operations (NavState order: 7) //------------------------------------------------------------------------------ Gal3 Gal3::inverse() const { // Formula: R_inv = R', v_inv = -R'*v, r_inv = -R'*(r - t*v), t_inv = -t const Rot3 Rinv = R_.inverse(); const Velocity3 v_inv = -(Rinv.rotate(v_)); - // CORRECTED: Cast r_ to Vector3 for subtraction const Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_)); const double t_inv = -t_; return Gal3(Rinv, r_inv, v_inv, t_inv); @@ -242,24 +245,21 @@ Gal3 Gal3::inverse() const { //------------------------------------------------------------------------------ Gal3 Gal3::compose(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { // Formula: R = R1*R2, v = R1*v2 + v1, r = R1*r2 + t2*v1 + r1, t = t1+t2 - // where this = g1, other = g2 const Gal3& g1 = *this; const Gal3& g2 = other; const Rot3 R_comp = g1.R_.compose(g2.R_); - // CORRECTED: Cast g1.r_ to Vector3 for addition const Vector3 r_comp_vec = g1.R_.rotate(g2.r_) + g2.t_ * g1.v_ + Vector3(g1.r_); const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_; const double t_comp = g1.t_ + g2.t_; Gal3 result(R_comp, Point3(r_comp_vec), v_comp, t_comp); - // Jacobians require correct AdjointMap implementation if (H1) { - *H1 = g2.inverse().AdjointMap(); + *H1 = g2.inverse().AdjointMap(); // Jacobian w.r.t this is Ad(g2.inverse()) } if (H2) { - *H2 = Matrix10::Identity(); + *H2 = Matrix10::Identity(); // Jacobian w.r.t other is Identity } return result; @@ -269,69 +269,203 @@ Gal3 Gal3::compose(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacob Gal3 Gal3::between(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { const Gal3& g1 = *this; const Gal3& g2 = other; - Gal3 g1_inv = g1.inverse(); - Gal3 result = g1_inv.compose(g2); // g1_inv * g2 - // Jacobians require correct AdjointMap implementation + // Step 1: Calculate g1.inverse() and its Jacobian w.r.t. g1 + // H_inv_g1 = -Ad(g1_inv) + Matrix10 H_inv_g1; + // NOTE: This internal call to inverse *does* take the Jacobian argument + Gal3 g1_inv = g1.inverse(H_inv_g1); + + // Step 2: Calculate g2.inverse() needed for compose Jacobian + Gal3 g2_inv = g2.inverse(); // This calls the public inverse() without Jacobian + + // Step 3: Calculate compose(g1_inv, g2) and its Jacobians w.r.t. g1_inv and g2 + // H_comp_g1inv = Ad(g2_inv) + // H_comp_g2 = Identity + Matrix10 H_comp_g1inv, H_comp_g2; + Gal3 result = g1_inv.compose(g2, H_comp_g1inv, H_comp_g2); + + // Step 4: Apply chain rule explicitly for H1 if (H1) { - *H1 = -result.AdjointMap(); + // H1 = d(compose)/d(g1_inv) * d(g1_inv)/d(g1) = H_comp_g1inv * H_inv_g1 + *H1 = H_comp_g1inv * H_inv_g1; } + + // Step 5: Assign H2 if (H2) { - *H2 = g1_inv.AdjointMap(); + // H2 = d(compose)/d(g2) = H_comp_g2 = Identity + *H2 = H_comp_g2; // Should be Identity() } return result; } - //------------------------------------------------------------------------------ -// Manifold Operations (Retract/Local) +// Lie Group Static Functions (NavState order: 8) //------------------------------------------------------------------------------ -Gal3 Gal3::retract(const Vector10& xi, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { - Gal3 g_exp = Gal3::Expmap(xi); - Matrix10 H_exp_xi = Matrix10::Zero(); // Placeholder - if (H2) { - H_exp_xi = Gal3::ExpmapDerivative(xi); // Needs ExpmapDerivative implemented - } +Gal3 Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) { + const Vector3 rho_tan = rho(xi); + const Vector3 nu_tan = nu(xi); + const Vector3 theta_tan = theta(xi); + const double t_tan_val = t_tan(xi)(0); - Matrix10 H_comp_this, H_comp_gexp; - Gal3 result = this->compose(g_exp, H_comp_this, H_comp_gexp); + const Rot3 R = Rot3::Expmap(theta_tan); + const gtsam::so3::DexpFunctor dexp_functor(theta_tan); + const Matrix3 Jl_theta = dexp_functor.leftJacobian(); // Left Jacobian of SO(3) Expmap - if (H1) { - *H1 = H_comp_this; // Requires AdjointMap - } - if (H2) { - // Needs ExpmapDerivative and potentially AdjointMap if H_comp_gexp is not Identity - *H2 = H_comp_gexp * H_exp_xi; // H_comp_gexp is Identity here - } - return result; + const Matrix3 E = Calculate_E(theta_tan); // Use helper matrix E(theta) + + const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan)); + const Velocity3 v_final = Jl_theta * nu_tan; + const double t_final = t_tan_val; + + if (Hxi) { + *Hxi = ExpmapDerivative(xi); // Use numerical derivative for Jacobian + } + + return Gal3(R, r_final, v_final, t_final); } //------------------------------------------------------------------------------ -Vector10 Gal3::localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { - Matrix10 H_between_this, H_between_other; - Gal3 result_g = this->between(other, H_between_this, H_between_other); +Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) { + const Vector3 theta_vec = Rot3::Logmap(g.R_); + const gtsam::so3::DexpFunctor dexp_functor_log(theta_vec); + const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse(); // Inverse Left Jacobian - Matrix10 H_log_g = Matrix10::Zero(); // Placeholder - if (H1 || H2) { - H_log_g = Gal3::LogmapDerivative(result_g); // Needs LogmapDerivative implemented - } - Vector10 result_xi = Gal3::Logmap(result_g); + const Matrix3 E = Calculate_E(theta_vec); // Use helper matrix E(theta) - // Jacobians require LogmapDerivative and AdjointMap (via between Jacobians) - if (H1) { - *H1 = H_log_g * H_between_this; + const Vector3 r_vec = Vector3(g.r_); + const Velocity3& v_vec = g.v_; + const double& t_val = g.t_; + + const Vector3 nu_tan = Jl_theta_inv * v_vec; + const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan)); + const double t_tan_val = t_val; + + if (Hg) { + *Hg = LogmapDerivative(g); // Use numerical derivative for Jacobian } - if (H2) { - *H2 = H_log_g * H_between_other; - } - return result_xi; + + Vector10 xi; + rho(xi) = rho_tan; + nu(xi) = nu_tan; + theta(xi) = theta_vec; + t_tan(xi)(0) = t_tan_val; + return xi; } //------------------------------------------------------------------------------ -// Lie Group Static Functions -//------------------------------------------------------------------------------ +Matrix10 Gal3::AdjointMap() const { + 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,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 + + // Row block 3: dtheta' output + Ad.block<3,3>(6,6) = Rmat; + + // Row block 4: dt' output + Ad(9,9) = 1.0; + + return Ad; +} + +//------------------------------------------------------------------------------ +Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const { + Matrix10 Ad = AdjointMap(); // Ad = Ad(g) + Vector10 y = Ad * xi; // y = Ad_g(xi) + + if (H_xi) { + *H_xi = Ad; // Jacobian w.r.t xi is Ad(g) + } + + if (H_g) { + // Jacobian H_g = d(Ad_g(xi))/d(g) -> use numerical derivative + 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); + } + return y; +} + +//------------------------------------------------------------------------------ +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 + 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 (Hxi) { + // Jacobian w.r.t xi is -ad(y) + *Hxi = -adjointMap(y); + } + return ad_xi * y; +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) { + // Use numerical derivative for Expmap Jacobian + // Use locally defined constant + if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); + std::function fn = + [](const Vector10& v) { return Gal3::Expmap(v); }; + return numericalDerivative11(fn, xi, 1e-5); +} + +//------------------------------------------------------------------------------ +Matrix10 Gal3::LogmapDerivative(const Gal3& g) { + // Use numerical derivative for Logmap Jacobian + 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); }; + return numericalDerivative11(fn, g, 1e-5); +} + +//------------------------------------------------------------------------------ +// Lie Algebra (Hat/Vee maps) (NavState order: 9) +//------------------------------------------------------------------------------ Matrix5 Gal3::Hat(const Vector10& xi) { const Vector3 rho_tan = rho(xi); const Vector3 nu_tan = nu(xi); @@ -348,10 +482,8 @@ Matrix5 Gal3::Hat(const Vector10& xi) { //------------------------------------------------------------------------------ Vector10 Gal3::Vee(const Matrix5& X) { - // CORRECTED: Check X.row(3).head(N) for Lie algebra structure - // Lie algebra row 3 is [0 0 0 0 t], so first 3 elements should be 0. - // Lie algebra row 4 is [0 0 0 0 0], so all 5 elements should be 0. - if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9) { // Row 4 all=0; Row 3 first 3=0 + // Check Lie algebra structure: Row 4 all=0; Row 3 first 3=0, last = t. + 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)"); } @@ -359,188 +491,155 @@ Vector10 Gal3::Vee(const Matrix5& X) { rho(xi) = X.block<3, 1>(0, 4); // rho from column 4 nu(xi) = X.block<3, 1>(0, 3); // nu from column 3 const Matrix3& S = X.block<3, 3>(0, 0); - theta(xi) << S(2, 1), S(0, 2), S(1, 0); + 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) return xi; } //------------------------------------------------------------------------------ -Gal3 Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) { - const Vector3 rho_tan = rho(xi); - const Vector3 nu_tan = nu(xi); - const Vector3 theta_tan = theta(xi); - const double t_tan_val = t_tan(xi)(0); - - const Rot3 R = Rot3::Expmap(theta_tan); - const Matrix3 Jl_theta = SO3_LeftJacobian(theta_tan); - const Matrix3 E = Calculate_E(theta_tan); - - const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan)); - const Velocity3 v_final = Jl_theta * nu_tan; - const double t_final = t_tan_val; - - if (Hxi) { - // Jacobian placeholder - requires derivation - *Hxi = ExpmapDerivative(xi); - } - - return Gal3(R, r_final, v_final, t_final); -} - -//------------------------------------------------------------------------------ -Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) { - const Vector3 theta_vec = Rot3::Logmap(g.R_); - const Matrix3 Jl_theta_inv = SO3_LeftJacobianInverse(theta_vec); - const Matrix3 E = Calculate_E(theta_vec); - - // CORRECTED: Cast r_ to Vector3 for subtraction - const Vector3 r_vec = Vector3(g.r_); - const Velocity3& v_vec = g.v_; - const double& t_val = g.t_; - - const Vector3 nu_tan = Jl_theta_inv * v_vec; - const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan)); - const double t_tan_val = t_val; - - // Jacobian placeholder - requires derivation - if (Hg) { - *Hg = LogmapDerivative(g); - } - - Vector10 xi; - rho(xi) = rho_tan; - nu(xi) = nu_tan; - theta(xi) = theta_vec; - t_tan(xi)(0) = t_tan_val; - return xi; -} - -//------------------------------------------------------------------------------ -Matrix10 Gal3::AdjointMap() const { - const Matrix3 Rmat = R_.matrix(); - const Matrix3 Vhat = skewSymmetric(v_); - // CORRECTED: Cast r_ to Vector3 for subtraction - const Vector3 r_minus_tv = Vector3(r_) - t_ * v_; - const Matrix3 S_r_minus_tv_hat = skewSymmetric(r_minus_tv); - - Matrix10 Ad = Matrix10::Zero(); - - // Row block 1: drho' output (maps input drho, dnu, dtheta, dt) - Ad.block<3,3>(0,0) = Rmat; - Ad.block<3,3>(0,3) = t_ * Rmat; - Ad.block<3,3>(0,6) = -Rmat * S_r_minus_tv_hat; - Ad.block<3,1>(0,9) = Rmat * v_; - - // Row block 2: dnu' output - Ad.block<3,3>(3,3) = Rmat; - Ad.block<3,3>(3,6) = -Rmat * Vhat; - - // Row block 3: dtheta' output - Ad.block<3,3>(6,6) = Rmat; - - // Row block 4: dt' output - Ad(9,9) = 1.0; - - return Ad; -} - - -//------------------------------------------------------------------------------ -Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const { - Matrix10 Ad = AdjointMap(); - if (H_xi) { - *H_xi = Ad; - } - // Jacobian H_g requires adjointMap - if (H_g) { - *H_g = -adjointMap(Ad * xi); - } - return Ad * xi; -} - -//------------------------------------------------------------------------------ -Matrix10 Gal3::adjointMap(const Vector10& xi) { - // Based on Manif SGal3Tangent::adj() / Lie bracket [xi, y] - const Matrix3 Theta_hat = skewSymmetric(theta(xi)); - const Matrix3 Nu_hat = skewSymmetric(nu(xi)); - const Matrix3 Rho_hat = skewSymmetric(rho(xi)); - const double t_val = t_tan(xi)(0); - - Matrix10 ad = Matrix10::Zero(); - - // Row block 1: rho_out = Theta_hat*m + Rho_hat*eta + t_val*p - ad.block<3,3>(0,0) = Theta_hat; // d/dm - ad.block<3,3>(0,3) = t_val * Matrix3::Identity(); // d/dp - ad.block<3,3>(0,6) = Rho_hat; // d/deta - - // Row block 2: nu_out = Theta_hat*p + Nu_hat*eta - ad.block<3,3>(3,3) = Theta_hat; // d/dp - ad.block<3,3>(3,6) = Nu_hat; // d/deta - - // Row block 3: theta_out = Theta_hat*eta - ad.block<3,3>(6,6) = Theta_hat; // d/deta - - // Row block 4: t_out = 0 (already zero) - - return ad; -} - -//------------------------------------------------------------------------------ -Vector10 Gal3::adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) { - Matrix10 ad_xi = adjointMap(xi); - if (Hy) *Hy = ad_xi; - // Jacobian Hxi requires adjointMap - if (Hxi) { - *Hxi = -adjointMap(y); - } - return ad_xi * y; -} - - -//------------------------------------------------------------------------------ -Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) { - if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); - // CORRECTED: Explicitly create std::function to resolve ambiguity - std::function fn = - [](const Vector10& v) { return Gal3::Expmap(v); }; - // Note: Default delta for numericalDerivative might be too large or small - return numericalDerivative11(fn, xi, 1e-5); // Pass function object -} - -//------------------------------------------------------------------------------ -Matrix10 Gal3::LogmapDerivative(const Gal3& g) { - Vector10 xi = Gal3::Logmap(g); - if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity(); - // CORRECTED: Explicitly create std::function to resolve ambiguity - std::function fn = - [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; - // Note: Default delta for numericalDerivative might be too large or small - return numericalDerivative11(fn, g, 1e-5); // Pass function object -} - - -//------------------------------------------------------------------------------ -// ChartAtOrigin +// ChartAtOrigin (NavState order: 10) //------------------------------------------------------------------------------ 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); } //------------------------------------------------------------------------------ -// Stream operator +// Manifold Operations (Instance) (NavState order: 11 - retract/local/interpolate) //------------------------------------------------------------------------------ -std::ostream& operator<<(std::ostream& os, const Gal3& state) { - os << "R: " << state.R_ << "\n"; - os << "r: " << state.r_.transpose() << "\n"; - os << "v: " << state.v_.transpose() << "\n"; - os << "t: " << state.t_; - return os; +Gal3 Gal3::retract(const Vector10& xi, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { + // Default retract is Expmap applied via composition: this->compose(Expmap(xi)) + Matrix10 H_exp_xi; + Gal3 g_exp = Gal3::Expmap(xi, H2 ? &H_exp_xi : nullptr); // Expmap(xi) and its Jacobian H2' + + Matrix10 H_comp_this, H_comp_gexp; + Gal3 result = this->compose(g_exp, H1 ? &H_comp_this : nullptr, H2 ? &H_comp_gexp : nullptr); + + if (H1) { + // H1 = d(compose)/d(this) = H_comp_this = Ad(g_exp.inverse()) + *H1 = H_comp_this; + } + if (H2) { + // H2 = d(compose)/d(g_exp) * d(g_exp)/d(xi) = H_comp_gexp * H_exp_xi + // H_comp_gexp is Identity, so H2 = H_exp_xi + *H2 = H_comp_gexp * H_exp_xi; + } + return result; } +//------------------------------------------------------------------------------ +Vector10 Gal3::localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const { + // Default localCoords is Logmap of the relative transformation: Logmap(this->between(other)) + Matrix10 H_between_this, H_between_other; + Gal3 result_g = this->between(other, H1 ? &H_between_this : nullptr, H2 ? &H_between_other : nullptr); // g = between(this, other) and its Jacobians + + Matrix10 H_log_g; + Vector10 result_xi = Gal3::Logmap(result_g, (H1 || H2) ? &H_log_g : nullptr); // xi = Logmap(g) and its Jacobian + + if (H1) { + // H1 = d(Logmap)/d(g) * d(g)/d(this) = H_log_g * H_between_this + *H1 = H_log_g * H_between_this; + } + if (H2) { + // H2 = d(Logmap)/d(g) * d(g)/d(other) = H_log_g * H_between_other + *H2 = H_log_g * H_between_other; + } + return result_xi; +} + +//------------------------------------------------------------------------------ +Gal3 Gal3::interpolate(const Gal3& other, double alpha, + OptionalJacobian<10, 10> H1, + OptionalJacobian<10, 10> H2, + OptionalJacobian<10, 1> Ha) const { + // Formula: g_interp = g1 * Exp(alpha * Log(g1.inverse * g2)) + // g_interp = g1 * Exp(alpha * Log(g1.between(g2))) + // g_interp = g1.compose(Gal3::Expmap(alpha * Gal3::Logmap(g1.between(g2)))) + + // Step 1: Calculate between and Logmap + Matrix10 H_between_g1, H_between_g2; // Jacobians of between + Gal3 g_between = this->between(other, + (H1 || H2 || Ha) ? &H_between_g1 : nullptr, + (H1 || H2 || Ha) ? &H_between_g2 : nullptr); + + Matrix10 H_log_between; // Jacobian of Logmap wrt g_between + Vector10 xi_between = Gal3::Logmap(g_between, + (H1 || H2 || Ha) ? &H_log_between : nullptr); + + // Step 2: Scale tangent vector + Vector10 xi_scaled = alpha * xi_between; + + // Step 3: Calculate Expmap + Matrix10 H_exp_scaled; // Jacobian of Expmap wrt xi_scaled + Gal3 g_exp_scaled = Gal3::Expmap(xi_scaled, + (H1 || H2 || Ha) ? &H_exp_scaled : nullptr); + + // Step 4: Compose with g1 (this) + Matrix10 H_comp_g1, H_comp_gexp; // Jacobians of compose + Gal3 g_interp = this->compose(g_exp_scaled, + (H1 || Ha) ? &H_comp_g1 : nullptr, + (H1 || H2 || Ha) ? &H_comp_gexp : nullptr); + + // --- Calculate Jacobians using Chain Rule --- + if (H1 || H2 || Ha) { + const Matrix10 d_xi_sc_d_xi_btw = alpha * Matrix10::Identity(); + const Matrix10 chain_gexp_gbtw = H_exp_scaled * d_xi_sc_d_xi_btw * H_log_between; + const Matrix10 chain_gexp_g1 = chain_gexp_gbtw * H_between_g1; + const Matrix10 chain_gexp_g2 = chain_gexp_gbtw * H_between_g2; + const Vector10 d_xi_sc_d_alpha = xi_between; + const Vector10 chain_gexp_alpha = H_exp_scaled * d_xi_sc_d_alpha; + + if (H1) { + // d(g_interp)/d(g1) = d(f5)/d(g1) + d(f5)/d(g_exp) * d(g_exp)/d(g1) + *H1 = H_comp_g1 + H_comp_gexp * chain_gexp_g1; + } + if (H2) { + // d(g_interp)/d(g2) = d(f5)/d(g_exp) * d(g_exp)/d(g2) + *H2 = H_comp_gexp * chain_gexp_g2; + } + if (Ha) { + // d(g_interp)/d(alpha) = d(f5)/d(g_exp) * d(g_exp)/d(alpha) + *Ha = H_comp_gexp * chain_gexp_alpha; + } + } + return g_interp; +} + +//------------------------------------------------------------------------------ +// Group Action (NavState order: After Manifold Instance methods, before Dynamics) +//------------------------------------------------------------------------------ +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_; + + 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 = Matrix::Zero(3, 10); + 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; +} + +//------------------------------------------------------------------------------ +// Dynamics (NavState order: 12) - Gal3 currently has no dynamics methods. +//------------------------------------------------------------------------------ } // namespace gtsam diff --git a/gtsam/navigation/Gal3.h b/gtsam/navigation/Gal3.h index d210c1813..418d36956 100644 --- a/gtsam/navigation/Gal3.h +++ b/gtsam/navigation/Gal3.h @@ -9,21 +9,19 @@ /** * @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] + * Based on manif convention: [R, r, v, t], tangent [rho, nu, theta, t_tan] * @author Based on Python implementation by User - * @date April 29, 2025 + * @date April 30, 2025 // Updated Date */ #pragma once #include // Includes Rot3, Point3 -#include -#include -#include // For LieGroup base class +#include // For LieGroup base class and traits +#include // For Manifold traits -#include // For std::sqrt, std::cos, std::sin, std::tan -#include // For std::numeric_limits -#include // For std::function +#include // For std::sqrt, std::cos, std::sin +#include // For std::function used in numerical derivatives namespace gtsam { @@ -34,9 +32,9 @@ class Gal3; using Velocity3 = Vector3; // Define Vector10 for tangent space using Vector10 = Eigen::Matrix; -// Define Matrix5 for Lie Algebra matrix representation +// Define Matrix5 for Lie Algebra matrix representation (Hat map output) using Matrix5 = Eigen::Matrix; -// Define Matrix10 for Jacobians +// Define Matrix10 for Jacobians (AdjointMap, Expmap/Logmap derivatives) using Matrix10 = Eigen::Matrix; @@ -72,12 +70,10 @@ class GTSAM_EXPORT Gal3 : public LieGroup { Velocity3 v_; ///< Velocity in world frame, n_v_b double t_; ///< Time component - // Small number epsilon for checking small angles, etc. - static constexpr double kSmallAngleThreshold = 1e-10; - public: - inline constexpr static size_t dimension = 10; + /// The dimension of the tangent space + inline static constexpr size_t dimension = 10; /// @name Constructors /// @{ @@ -92,28 +88,49 @@ class GTSAM_EXPORT Gal3 : public LieGroup { /// Construct from a 5x5 matrix representation (manif convention) explicit Gal3(const Matrix5& M); + /// Named constructor from components with derivatives + static Gal3 Create(const Rot3& R, const Point3& r, const Velocity3& v, double t, + OptionalJacobian<10, 3> H1 = {}, + OptionalJacobian<10, 3> H2 = {}, + OptionalJacobian<10, 3> H3 = {}, + OptionalJacobian<10, 1> H4 = {}); + + /// Named constructor from Pose3, velocity, and time with derivatives + static Gal3 FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t, + OptionalJacobian<10, 6> H1 = {}, + OptionalJacobian<10, 3> H2 = {}, + OptionalJacobian<10, 1> H3 = {}); + /// @} /// @name Component Access /// @{ + /// Access rotation component (Rot3) const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const; + + /// Access translation component (Point3) const Point3& translation(OptionalJacobian<3, 10> H = {}) const; + + /// Access velocity component (Vector3) const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const; + + /// Access time component (double) const double& time(OptionalJacobian<1, 10> H = {}) const; - // Convenience accessors matching NavState where names overlap + // Convenience accessors matching NavState names where they overlap const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const { return rotation(H); } const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); } + /// @} /// @name Derived quantities /// @{ - /// Return rotation matrix. + /// Return rotation matrix (Matrix3). Matrix3 R() const { return R_.matrix(); } - /// Return position as Vector3 - Vector3 r() const { return Vector3(r_); } + /// Return position as Vector3. + Vector3 r() const { return Vector3(r_); } // Conversion from Point3 /// Return velocity as Vector3. Computation-free. const Vector3& v() const { return v_; } @@ -132,137 +149,158 @@ class GTSAM_EXPORT Gal3 : public LieGroup { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Gal3& state); - /// print + /// Print with optional string prefix void print(const std::string& s = "") const; - /// equals + /// Check equality within tolerance bool equals(const Gal3& other, double tol = 1e-9) const; /// @} /// @name Group /// @{ - /// identity for group operation + /// Return the identity element static Gal3 Identity() { return Gal3(); } - /// inverse transformation (Manif formula from SGal3_base.h) + /// Return the inverse of this element Gal3 inverse() const; // Bring LieGroup::inverse() into scope (version with derivative) using LieGroup::inverse; - /// compose (Manif formula from SGal3_base.h) - /// Returns this * other + /// Compose with another element: `this * other` Gal3 compose(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, OptionalJacobian<10, 10> H2 = {}) const; - /// compose syntactic sugar - Gal3 operator*(const Gal3& other) const { - return compose(other); - } + /// Group composition operator: `this * other` + Gal3 operator*(const Gal3& other) const { return compose(other); } - /// Calculate the relative transformation: this->inverse() * other + /// Calculate the relative transformation: `this->inverse() * other` Gal3 between(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, OptionalJacobian<10, 10> H2 = {}) const; - /// @} - /// @name Manifold Operations (Retract/Local) + /// @name Manifold Operations /// @{ - /// Default retract uses Expmap + /// Retract from tangent space xi to manifold element (this + xi) Gal3 retract(const Vector10& xi, OptionalJacobian<10, 10> H1 = {}, OptionalJacobian<10, 10> H2 = {}) const; - /// Default localCoordinates uses Logmap + /// Compute local coordinates (tangent vector) from this to other (logmap(this.inverse * other)) Vector10 localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1 = {}, OptionalJacobian<10, 10> H2 = {}) const; + + /** + * Interpolate between two Gal3 elements. + * @param other The other Gal3 element. + * @param alpha Interpolation fraction (0=this, 1=other). + * @return Interpolated Gal3 element: this->compose(Expmap(alpha * Logmap(this->between(other)))) + */ + Gal3 interpolate(const Gal3& other, double alpha, + OptionalJacobian<10, 10> H1 = {}, + OptionalJacobian<10, 10> H2 = {}, + OptionalJacobian<10, 1> Ha = {}) const; // Added Jacobian args to match NavState/typical patterns + /// @} - /// @name Lie Group - using LieAlgebra = Matrix5; + /// @name Group Action /// @{ - // Helper functions for accessing tangent vector components - // Input xi is [rho(3), nu(3), theta(3), t_tan(1)] - static Eigen::Block rho(Vector10& v) { return v.segment<3>(0); } - static Eigen::Block nu(Vector10& v) { return v.segment<3>(3); } - static Eigen::Block theta(Vector10& v) { return v.segment<3>(6); } - static Eigen::Block t_tan(Vector10& v) { return v.segment<1>(9); } - // Const versions too - static const Eigen::Block rho(const Vector10& v) { return v.segment<3>(0); } - static const Eigen::Block nu(const Vector10& v) { return v.segment<3>(3); } - static const Eigen::Block theta(const Vector10& v) { return v.segment<3>(6); } - static const Eigen::Block t_tan(const Vector10& v) { return v.segment<1>(9); } + /** + * 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. + */ + Point3 act(const Point3& p, + OptionalJacobian<3, 10> Hself = {}, + OptionalJacobian<3, 3> Hp = {}) const; - /// Exponential map at identity - create a Gal3 element from Lie algebra tangent vector xi. + // 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); + } + + /// @} + /// @name Lie Group Static Functions + /// @{ + + /// 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 *** + 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); } + static Eigen::Block t_tan(Vector10& v) { return v.block<1, 1>(9, 0); } + // Const versions + static Eigen::Block rho(const Vector10& v) { return v.block<3, 1>(0, 0); } + static Eigen::Block nu(const Vector10& v) { return v.block<3, 1>(3, 0); } + static Eigen::Block theta(const Vector10& v) { return v.block<3, 1>(6, 0); } + 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 = {}); - // REMOVED using LieGroup::Expmap; - /// Logarithmic map at identity - return the Lie algebra tangent vector xi for this element. + /// 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 = {}); - // REMOVED using LieGroup::Logmap; - /// Calculate Adjoint map, transforming a twist in the identity frame to the frame of this element. + /// 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. Matrix10 AdjointMap() const; - /// Apply this element's AdjointMap Ad_g to a tangent vector xi, H_g = d(Ad_g * xi)/dg, H_xi = d(Ad_g * xi)/dxi = Ad_g - Vector10 Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g = {}, OptionalJacobian<10, 10> H_xi = {}) const; + /// 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; - /// The adjoint action of xi on y `ad(xi, y)` + /// 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 = {}); - /// Compute the 10x10 adjoint map `ad(xi)` that acts on a tangent vector y `ad(xi, y) = ad(xi) * y` + /// Compute the 10x10 adjoint map `ad(xi)` associated with tangent vector xi. + /// Acts on a tangent vector y: `ad(xi, y) = ad(xi) * y`. static Matrix10 adjointMap(const Vector10& xi); - /// Derivative of Expmap(xi) w.r.t. xi + /// Derivative of Expmap(xi) w.r.t. xi evaluated at xi. + /// Currently implemented numerically. static Matrix10 ExpmapDerivative(const Vector10& xi); - /// Derivative of Logmap(g) w.r.t. g (represented as tangent vector at identity) + /// Derivative of Logmap(g) w.r.t. g (represented as tangent vector at identity). + /// Currently implemented numerically. static Matrix10 LogmapDerivative(const Gal3& g); - - // Chart at origin, depends on compile-time flag GTSAM_USE_MANIFOLD_MARKERS + /// Chart at origin, uses Expmap/Logmap for Retract/Local struct ChartAtOrigin { static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {}); static Vector10 Local(const Gal3& g, ChartJacobian Hg = {}); }; - /// Hat operator: maps tangent vector xi to Lie algebra matrix (Manif convention). - /// xi = [rho, nu, theta, t] -> [ skew(theta) nu rho ; 0 0 t ; 0 0 0 ] + /// xi = [rho, nu, theta, t_tan] -> [ skew(theta) nu rho ; 0 0 t_tan ; 0 0 0 ] static Matrix5 Hat(const Vector10& xi); /// Vee operator: maps Lie algebra matrix to tangent vector xi (Manif convention). - /// [ S N R ; 0 0 T ; 0 0 0 ] -> [ R ; N ; vee(S) ; T ] = [rho; nu; theta; t] + /// [ S N R ; 0 0 T ; 0 0 0 ] -> [ R ; N ; vee(S) ; T ] = [rho; nu; theta; t_tan] static Vector10 Vee(const Matrix5& X); - - /// @} - /// @name Helper Functions (moved inside class for context, could be private or in detail namespace) - /// @{ - - /// Calculate the SO(3) Left Jacobian J_l(theta) - static Matrix3 SO3_LeftJacobian(const Vector3& theta); - - /// Calculate the inverse SO(3) Left Jacobian J_l(theta)^-1 - static Matrix3 SO3_LeftJacobianInverse(const Vector3& theta); - - /// Calculate the 3x3 E matrix used in SGal3 Expmap/Logmap - static Matrix3 Calculate_E(const Vector3& theta); - /// @} - -private: + private: + /// @name Serialization /// @{ - /// serialization #if GTSAM_ENABLE_BOOST_SERIALIZATION 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_); @@ -270,13 +308,14 @@ private: } #endif /// @} + }; // class Gal3 -// // Specialize Gal3 traits +/// Traits specialization for Gal3 template <> -struct traits : public internal::MatrixLieGroup {}; +struct traits : public internal::LieGroup {}; template <> -struct traits : public internal::MatrixLieGroup {}; +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/navigation/tests/testGal3.cpp b/gtsam/navigation/tests/testGal3.cpp index 7000fcc61..10be53b76 100644 --- a/gtsam/navigation/tests/testGal3.cpp +++ b/gtsam/navigation/tests/testGal3.cpp @@ -36,6 +36,150 @@ 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) +const Rot3 kTestRot = Rot3::RzRyRx(0.1, -0.2, 0.3); +const Point3 kTestPos(1.0, -2.0, 3.0); +const Velocity3 kTestVel(0.5, 0.6, -0.7); +const double kTestTime = 1.5; +const Pose3 kTestPose(kTestRot, kTestPos); +const Gal3 kTestGal3(kTestRot, kTestPos, kTestVel, kTestTime); + + +/* ************************************************************************* */ +// Test 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 +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 +TEST(Gal3, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(kIdentity_Lie, kIdentity_Lie); + CHECK_LIE_GROUP_DERIVATIVES(kIdentity_Lie, kTestGal3_Lie1); + CHECK_LIE_GROUP_DERIVATIVES(kTestGal3_Lie1, kIdentity_Lie); + CHECK_LIE_GROUP_DERIVATIVES(kTestGal3_Lie1, kTestGal3_Lie2); +} + +// Check derivatives of retract and localCoordinates +TEST(Gal3, ChartDerivatives) { + CHECK_CHART_DERIVATIVES(kIdentity_Lie, kIdentity_Lie); + CHECK_CHART_DERIVATIVES(kIdentity_Lie, kTestGal3_Lie1); + CHECK_CHART_DERIVATIVES(kTestGal3_Lie1, kIdentity_Lie); + CHECK_CHART_DERIVATIVES(kTestGal3_Lie1, kTestGal3_Lie2); +} + + +/* ************************************************************************* */ +// Test 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 + 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. + try { + Gal3 g_invalid(M_invalid); + FAIL("Constructor should have thrown for invalid matrix structure."); + } catch (const std::invalid_argument& e) { + // Expected exception, test passes. + // Optionally check e.what() for specific message if desired. + } 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 + try { + Gal3 g_invalid(M_invalid); + FAIL("Constructor should have thrown for invalid matrix structure (M(3,3) != 1)."); + } catch (const std::invalid_argument& e) { + // Expected exception, test passes. + } catch (...) { + FAIL("Constructor threw unexpected exception type for invalid matrix."); + } +} // ======================================================================== // Test Cases Section (mirroring Python's TestGal3 class) @@ -404,6 +548,834 @@ TEST(Gal3, IdentityProperties) { // Add more test cases here if they exist in the Python script } + +/* ************************************************************************* */ + +/* ************************************************************************* */ +TEST(Gal3, Adjoint) { + // Hardcoded test case for adjoint from Python code + const Matrix3 g_R_mat_1 = (Matrix3() << // Added << + -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, + -0.6694062876067332, -0.572463082520484, 0.47347781496466923, + 0.6967527259484512, -0.26267274676776586, 0.6674868290752107 + ).finished(); + const Point3 g_r_vec_1(0.680375434309419, -0.21123414636181392, 0.5661984475172117); + const Velocity3 g_v_vec_1(0.536459189623808, -0.44445057839362445, 0.10793991159086103); + const double g_t_val_1 = -0.0452058962756795; + + const Gal3 test_g(Rot3(g_R_mat_1), g_r_vec_1, g_v_vec_1, g_t_val_1); + + // Expected adjoint matrix (from Python test) + Matrix10 expected_adj_matrix; + expected_adj_matrix = (Matrix10() << // Added << + -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, -0.011651451315476903, 0.03511218083817791, 0.025979828658358354, 0.22110620799336958, 0.3876840721487304, -0.42479976201969083, 0.536459189623808, + -0.6694062876067332, -0.572463082520484, 0.47347781496466923, -0.03026111120383766, -0.025878706730076754, 0.021403988992128208, -0.6381411631187845, 0.6286520658991062, -0.14213043434767314, -0.44445057839362445, + 0.6967527259484512, -0.26267274676776586, 0.6674868290752107, 0.03149733145902263, -0.011874356944831452, 0.03017434036055619, -0.5313038186955878, -0.22369794100291154, 0.4665680546909384, 0.10793991159086103, + 0.0, 0.0, 0.0, -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, -0.23741649654248634, 0.1785366687454684, -0.3477720607401242, 0.0, + 0.0, 0.0, 0.0, -0.6694062876067332, -0.572463082520484, 0.47347781496466923, -0.40160003518135456, 0.22475195574939733, -0.2960463760548867, 0.0, + 0.0, 0.0, 0.0, 0.6967527259484512, -0.26267274676776586, 0.6674868290752107, -0.47366266867570694, 0.03810916679440729, 0.5094272729993004, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2577418495238488, 0.7767168385303765, 0.5747000015202739, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6694062876067332, -0.572463082520484, 0.47347781496466923, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.6967527259484512, -0.26267274676776586, 0.6674868290752107, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 + ).finished(); + + // Compute adjoint map + Matrix10 computed_adj = test_g.AdjointMap(); + + // Compare with expected result + EXPECT(assert_equal(expected_adj_matrix, computed_adj, kTol * 10)); + + // Test tangent vector for adjoint action + Vector10 test_tangent = (Vector10() << // Added << + -0.28583171387804845, -0.7221038902918132, -0.09516831208249353, + -0.13619637934919504, -0.05432836001072756, 0.1629798883384306, + -0.20194877118636279, -0.18928645162443278, 0.07312685929426145, + -0.042327821984942095 + ).finished(); + + // Expected result after applying adjoint to tangent vector + Vector10 expected_adj_tau = (Vector10() << // Added << + -0.7097860882934639, 0.5869666797222274, 0.10746143202899403, + 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)); + + // 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 << + -0.7097860882934638, 0.5869666797222274, 0.10746143202899389, + 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)); +} + +// ======================================================================== +// Jacobian Tests Section (mirroring Python's TestGal3Jacobians class) +// ======================================================================== + + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Compose) { + // Hardcoded data from Python test test_compose_jacobians + Matrix3 g1_R_mat = (Matrix3() << + -0.3264538540162394, 0.24276278793202133, -0.9135064914894779, + 0.9430076454867458, 0.1496317101600385, -0.2972321178273404, + 0.06453264097716288, -0.9584761760784951, -0.27777501352435885 + ).finished(); + Point3 g1_r_vec(-0.1995427558196442, 0.7830589040103644, -0.433370507989717); + Velocity3 g1_v_vec(0.8986541507293722, 0.051990700444202176, -0.8278883042875157); + double g1_t_val = -0.6155723080111539; + Gal3 g1(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + Matrix3 g2_R_mat = (Matrix3() << + -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, + 0.773189425449982, 0.15205374379417114, 0.6156766776243058, + 0.3622989004266723, 0.6908978584436601, -0.6256194178153267 + ).finished(); + Point3 g2_r_vec(-0.8716573584227159, -0.9599539022706234, -0.08459652545144625); + Velocity3 g2_v_vec(0.7018395735425127, -0.4666685012479632, 0.07952068144433233); + double g2_t_val = -0.24958604725524136; + Gal3 g2(Rot3(g2_R_mat), g2_r_vec, g2_v_vec, g2_t_val); + + Matrix10 expected_H1 = (Matrix10() << + -0.5204974727334908, 0.773189425449982, 0.3622989004266723, 0.12990890682589473, -0.19297729247761214, -0.09042475048241341, -0.2823811043440715, 0.3598327802048799, -1.1736098322206205, 0.6973186192002845, + 0.7067813015326174, 0.15205374379417114, 0.6908978584436601, -0.17640275132344085, -0.03795049288394836, -0.17243846554606443, -0.650366876876537, 0.542434959867202, 0.5459387038042347, -0.4800290630417764, + 0.4791060140322894, 0.6156766776243058, -0.6256194178153267, -0.11957817625853331, -0.15366430835548997, 0.15614587757865273, 0.652649909196605, -0.5858564732208887, -0.07673941868885611, 0.0008110342596663322, + 0.0, 0.0, 0.0, -0.5204974727334908, 0.773189425449982, 0.3622989004266723, -0.23055803486323456, -0.29566601949219695, 0.29975516112150496, 0.0, + 0.0, 0.0, 0.0, 0.7067813015326174, 0.15205374379417114, 0.6908978584436601, -0.3345116854380048, -0.42869572760154795, 0.4365499053963549, 0.0, + 0.0, 0.0, 0.0, 0.4791060140322894, 0.6156766776243058, -0.6256194178153267, 0.24299784710943456, 0.47718330211934956, 0.6556899423712481, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5204974727334908, 0.773189425449982, 0.3622989004266723, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7067813015326174, 0.15205374379417114, 0.6908978584436601, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4791060140322894, 0.6156766776243058, -0.6256194178153267, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 + ).finished(); + + Matrix10 expected_H2 = Matrix10::Identity(); // 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 + Matrix3 g_R_mat = (Matrix3() << + 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, + 0.6729369985913887, -0.6193062871756463, 0.4044941514923666, + -0.31758898858193396, -0.7357676057205693, -0.5981498680963873 + ).finished(); + Point3 g_r_vec(0.06321286832132045, -0.9214393132931736, -0.12472480681013542); + Velocity3 g_v_vec(0.4770686298036335, 0.2799576331302327, -0.29190264050471715); + double g_t_val = 0.3757227805330059; + Gal3 g(Rot3(g_R_mat), g_r_vec, g_v_vec, g_t_val); + + Matrix10 expected_H = (Matrix10() << + -0.6680516673568877, -0.2740542884848495, 0.6918101016209183, 0.2510022299990406, 0.10296843928652219, -0.2599288149818328, -0.33617296841685484, -0.7460372203093872, -0.6201638436054394, -0.4770686298036335, + -0.6729369985913887, 0.6193062871756463, -0.4044941514923666, 0.252837760234292, -0.23268748021920607, 0.1519776673080509, 0.04690510412308051, 0.0894976987957895, 0.05899296065652178, -0.2799576331302327, + 0.31758898858193396, 0.7357676057205693, 0.5981498680963873, -0.1193254178566693, -0.27644465064744467, -0.22473853161662533, -0.6077563738775408, -0.3532109665151345, 0.7571646227598928, 0.29190264050471715, + -0.0, -0.0, -0.0, -0.6680516673568877, -0.2740542884848495, 0.6918101016209183, -0.10752062523052273, 0.3867608979391727, 0.049383710440088574, -0.0, + -0.0, -0.0, -0.0, -0.6729369985913887, 0.6193062871756463, -0.4044941514923666, 0.04349430207154942, -0.271014473064643, -0.48729973338095034, -0.0, + -0.0, -0.0, -0.0, 0.31758898858193396, 0.7357676057205693, 0.5981498680963873, -0.1340109682602236, 0.3721751918050696, -0.38664898924142477, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.6680516673568877, -0.2740542884848495, 0.6918101016209183, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.6729369985913887, 0.6193062871756463, -0.4044941514923666, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, 0.31758898858193396, 0.7357676057205693, 0.5981498680963873, -0.0, + -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -1.0 + ).finished(); + + // 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 --- + const Matrix3 R1_mat = (Matrix3() << + -0.5204974727334908, 0.7067813015326174, 0.4791060140322894, + 0.773189425449982, 0.15205374379417114, 0.6156766776243058, + 0.3622989004266723, 0.6908978584436601, -0.6256194178153267 + ).finished(); + const Point3 r1_vec(-0.8716573584227159, -0.9599539022706234, -0.08459652545144625); + const Velocity3 v1_vec(0.7018395735425127, -0.4666685012479632, 0.07952068144433233); + const double t1_val = -0.24958604725524136; + const Gal3 g1(Rot3(R1_mat), r1_vec, v1_vec, t1_val); + + const Vector10 expected_log_tau1 = (Vector10() << + -1.2604528322799349, -0.3898722924179116, -0.6402392791385879, // rho + -0.34870126525214656, -0.4153032457886797, 1.1791315551702946, // nu + 1.4969846781977756, 2.324590726540746, 1.321595100333433, // theta + -0.24958604725524136 // t_tan + ).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 + std::function logmap_func1 = + [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + Matrix H_numerical1 = numericalDerivative11(logmap_func1, g1, jac_tol); + EXPECT(assert_equal(H_numerical1, Hg1_gtsam, jac_tol)); + + + // --- Test Case 2 --- + const Matrix3 R2_mat = (Matrix3() << + 0.6680516673568877, 0.2740542884848495, -0.6918101016209183, + 0.6729369985913887,-0.6193062871756463, 0.4044941514923666, + -0.31758898858193396,-0.7357676057205693, -0.5981498680963873 + ).finished(); + const Point3 r2_vec(0.06321286832132045, -0.9214393132931736, -0.12472480681013542); + const Velocity3 v2_vec(0.4770686298036335, 0.2799576331302327, -0.29190264050471715); + const double t2_val = 0.3757227805330059; + const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); + + const Vector10 expected_log_tau2 = (Vector10() << + -0.5687147057967125, -0.3805406510759017, -1.063343079044753, // rho + 0.5179342682694047, 0.3616924279678234, -0.0984011207117694, // nu + -2.215366977027571, -0.72705858167113, 0.7749725693135466, // theta + 0.3757227805330059 // t_tan + ).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 = + [](const Gal3& g_in) { return Gal3::Logmap(g_in); }; + Matrix H_numerical2 = numericalDerivative11(logmap_func2, g2, jac_tol); + EXPECT(assert_equal(H_numerical2, Hg2_gtsam, jac_tol)); +} + +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Expmap) { + // Hardcoded data from Python test test_expmap_jacobian + 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 + ).finished(); + + Matrix10 expected_H = (Matrix10() << + 0.9844524218735122, -0.1490063714302354, 0.02908427677999133, -0.24094497482047258, 0.04906744369191897, -0.008766606502586993, 0.0010755746403492512, 0.020896120374367614, 0.09422195803906698, -0.032194210151797825, + 0.13700585416694203, 0.9624511801109918, 0.18991633864490795, -0.04463269623239319, -0.23281449603592955, -0.06241249224896669, -0.05013517822202011, -0.011608679508659724, 0.08214064896800377, 0.05141115662809599, + -0.06540787266535304, -0.1806541475679301, 0.9749387339968734, 0.0221898591040537, 0.05898968326106501, -0.23742922610598202, -0.09935687017740953, -0.06570748233856251, -0.025136525844073204, 0.1253312320983055, + 0.0, 0.0, 0.0, 0.9844524218735122, -0.1490063714302354, 0.02908427677999133, -0.02734050131352483, -0.1309992318097018, 0.01786013841817754, 0.0, + 0.0, 0.0, 0.0, 0.13700585416694203, 0.9624511801109918, 0.18991633864490795, 0.1195266279722227, -0.032519018758919625, 0.035417068913109986, 0.0, + 0.0, 0.0, 0.0, -0.06540787266535304, -0.1806541475679301, 0.9749387339968734, -0.0560073503522914, -0.019830198590275447, -0.010039835763395311, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9844524218735122, -0.1490063714302354, 0.02908427677999133, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13700585416694203, 0.9624511801109918, 0.18991633864490795, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.06540787266535304, -0.1806541475679301, 0.9749387339968734, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 + ).finished(); + + // 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(Gal3, Jacobian_Between) { + // Define test points g1, g2 (using the same data as before for context) + 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); + + // --- Verify H1 --- + Matrix10 H1_analytical, H2_analytical; // Placeholders + // Calculate analytical Jacobians using your Gal3::between implementation + 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 + 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 + 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) + Matrix3 g1_R_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + Point3 g1_r_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + Velocity3 g1_v_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + double g1_t_val = -0.6866418214918308; + Gal3 g = Gal3(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + // 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 + Matrix10 Ad_analytical = g.AdjointMap(); + + // Define the Adjoint action function for numerical differentiation + // Calculates Ad_g(xi) + 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 + }; + + // 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); + + // 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 + Matrix10 H_g_analytical; + g.Adjoint(xi, H_g_analytical); // Call Adjoint to get analytical H_g + + // Compare analytical H_g with numerical H_g + 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) + Matrix3 g1_R_mat = (Matrix3() << + 0.1981112115076329, -0.12884463237051902, 0.9716743325745948, + 0.9776658305457298, -0.04497580389201028, -0.20529661674659028, + 0.0701532013404006, 0.9906443548385938, 0.11705678352030657 + ).finished(); + Point3 g1_r_vec(0.9044594503494257, 0.8323901360074013, 0.2714234559198019); + Velocity3 g1_v_vec(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808); + double g1_t_val = -0.6866418214918308; + Gal3 g = Gal3(Rot3(g1_R_mat), g1_r_vec, g1_v_vec, g1_t_val); + + // Calculate analytical Jacobian H_inv = d(g.inverse()) / d(g) + Matrix10 H_inv_analytical; + g.inverse(H_inv_analytical); // Assuming inverse calculates its H + + // Define inverse function wrapper + 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)); +} + +/* ************************************************************************* */ +// Test compose Jacobians against numerical derivatives +/* ************************************************************************* */ +TEST(Gal3, Jacobian_Compose2) { + // Use g1 and g2 from the between test + 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); + + // Calculate analytical Jacobians H1=d(g1*g2)/dg1, H2=d(g1*g2)/dg2 + Matrix10 H1_analytical, H2_analytical; + g1.compose(g2, H1_analytical, H2_analytical); + + // Define compose function wrapper + 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)); +} + + +/* ************************************************************************* */ +// Test act method Value and Jacobians (vs numerical) using captured data +/* ************************************************************************* */ +TEST(Gal3, Act) { + 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); + + 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 + + // Verify value + EXPECT(assert_equal(expected_p_out1, p_out1_gtsam, 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)); + + + // --- Test Case 2 --- + 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); + + 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 + + // Verify value + EXPECT(assert_equal(expected_p_out2, p_out2_gtsam, 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)); +} + + + +/* ************************************************************************* */ +// 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 --- + const Matrix3 R1_mat = (Matrix3() << + -0.5427153955878299, 0.8391431164114453, 0.03603927817303032, + 0.8040805715986894, 0.5314810596281534, -0.2664250694549225, + -0.24272295682417533,-0.11561450357036887,-0.963181630220753 + ).finished(); + const Point3 r1_vec(0.9978490360071179, -0.5634861893781862, 0.025864788808796835); + const Velocity3 v1_vec(0.04857438013356852, -0.012834026018545996, 0.945550047767139); + const double t1_val = -0.41496643117394594; + const Gal3 g1(Rot3(R1_mat), r1_vec, v1_vec, t1_val); + + const Matrix3 R2_mat = (Matrix3() << + -0.3264538540162394, 0.24276278793202133, -0.9135064914894779, + 0.9430076454867458, 0.1496317101600385, -0.2972321178273404, + 0.06453264097716288,-0.9584761760784951, -0.27777501352435885 + ).finished(); + const Point3 r2_vec(-0.1995427558196442, 0.7830589040103644, -0.433370507989717); + const Velocity3 v2_vec(0.8986541507293722, 0.051990700444202176, -0.8278883042875157); + const double t2_val = -0.6155723080111539; + const Gal3 g2(Rot3(R2_mat), r2_vec, v2_vec, t2_val); + + // Expected results for different alphas + const Gal3 expected_alpha0_00 = g1; // Should be identical to g1 + const Gal3 expected_alpha0_25(Rot3(-0.5305293740379828, 0.8049951577356123, -0.26555861745588893, + 0.8357380608208966, 0.444360967494815, -0.32262241748272774, + -0.14170559967126867, -0.39309811318459514,-0.9085116380281089), + Point3(0.7319725410358775, -0.24509083842031143, -0.20526665070473993), + Velocity3(0.447233815335834, 0.08156238532481315, 0.6212732810121263), + -0.46511790038324796); + const Gal3 expected_alpha0_50(Rot3(-0.4871812472793253, 0.6852678695634308, -0.5413523614461815, + 0.8717937932763143, 0.34522257149684665,-0.34755856791913387, + -0.051283665082120816,-0.6412716453057169, -0.7655982383878919), + Point3(0.4275876127256323, 0.0806397132393819, -0.3622648631703127), + Velocity3(0.7397753319939113, 0.12027591889377548, 0.19009330402180313), + -0.5152693695925499); + const Gal3 expected_alpha0_75(Rot3(-0.416880477991759, 0.49158776471130083,-0.7645600935541361, + 0.9087464582621175, 0.24369303223618222,-0.338812013712018, + 0.019762127046961175,-0.8360353913714909, -0.5483195078682666), + Point3(0.10860773480095642, 0.42140693978775756, -0.4380637787537704), + Velocity3(0.895925123398753, 0.10738792759782673, -0.3083508669060544), + -0.5654208388018519); + const Gal3 expected_alpha1_00 = g2; // Should be identical to g2 + + // Compare values + // Requires Gal3::interpolate to be implemented + EXPECT(assert_equal(expected_alpha0_00, g1.interpolate(g2, 0.0), interp_tol)); + EXPECT(assert_equal(expected_alpha0_25, g1.interpolate(g2, 0.25), interp_tol)); + EXPECT(assert_equal(expected_alpha0_50, g1.interpolate(g2, 0.5), interp_tol)); + EXPECT(assert_equal(expected_alpha0_75, g1.interpolate(g2, 0.75), interp_tol)); + EXPECT(assert_equal(expected_alpha1_00, g1.interpolate(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. + 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 + 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; + 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 + 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. + const double jac_tol = 1e-7; + + // --- Test rotation() / attitude() Jacobian --- + gtsam::Matrix Hrot_ana; // Use dynamic Matrix type + 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 + 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 + 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 + kTestGal3.time(Htime_ana); + std::function time_func = + [](const Gal3& g) { return g.time(); }; + Matrix Htime_num = numericalDerivative11(time_func, kTestGal3, jac_tol); + EXPECT(assert_equal(Htime_num, Htime_ana, jac_tol)); +} + +/* ************************************************************************* */ +// 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. + const double jac_tol = 1e-7; + const Gal3 g1 = kTestGal3_Lie1; // Use pre-defined states + const Gal3 g2 = kTestGal3_Lie2; + const double alpha = 0.3; // Example interpolation factor + + gtsam::Matrix H1_ana, H2_ana, Ha_ana; // Use dynamic Matrix type + g1.interpolate(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 g1_in.interpolate(g2_in, a); + }; + + // Pass alpha by const reference to match lambda + const double& alpha_ref = alpha; + 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. + 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 --- + 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)); + + + // Verify Jacobian of adjoint(xi,y) w.r.t y equals adjointMap(xi) + 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); + }; + 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 + 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)); + + // 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 + Matrix Hxi_num = numericalDerivative21(static_adjoint_func, xi, y, jac_tol); + EXPECT(assert_equal(Hxi_num, Hxi_ana, jac_tol)); + + // Verify Jacobi Identity: [x, [y, z]] + [y, [z, x]] + [z, [x, y]] = 0 + Vector10 z = (Vector10() << 0.7, 0.8, 0.9, -0.1, -0.2, -0.3, 0.5, 0.6, 0.7, 0.1).finished(); + Vector10 term1 = Gal3::adjoint(xi, Gal3::adjoint(y, z)); + Vector10 term2 = Gal3::adjoint(y, Gal3::adjoint(z, xi)); + Vector10 term3 = Gal3::adjoint(z, Gal3::adjoint(xi, y)); + // 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)); +} + + /* ************************************************************************* */ int main() { TestResult tr;