Implement working Gal3 class with passing tests
mkielo3
parent
42cc0ac922
commit
07c9bc8ffa
|
|
@ -0,0 +1,546 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* See LICENSE for the license information
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Gal3.cpp
|
||||
* @brief Implementation of 3D Galilean Group SGal(3) state
|
||||
* @author Based on Python implementation by User
|
||||
* @date April 29, 2025
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/Gal3.h> // Note: Adjust path if needed
|
||||
#include <gtsam/geometry/SO3.h> // For so3::DexpFunctor if needed, and Rot3 Log/Exp
|
||||
#include <gtsam/base/numericalDerivative.h> // For derivative checking if desired
|
||||
#include <gtsam/base/Matrix.h> // For Z_3x3 etc.
|
||||
#include <gtsam/nonlinear/expressions.h> // For constructing expressions
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <functional> // For std::function
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Static Helper Functions Implementation
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
Matrix3 Gal3::SO3_LeftJacobian(const Vector3& theta) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
using std::sqrt;
|
||||
|
||||
const double angle_sq = theta.squaredNorm();
|
||||
|
||||
// Use Taylor series expansion for small angles
|
||||
if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) {
|
||||
// Jl(w) approx = I + 0.5*[w]x + (1/6)*[w]x^2
|
||||
const Matrix3 W = skewSymmetric(theta);
|
||||
return Matrix3::Identity() + 0.5 * W + (1.0/6.0) * W * W;
|
||||
}
|
||||
|
||||
const double angle = sqrt(angle_sq);
|
||||
const Matrix3 W = skewSymmetric(theta);
|
||||
const Matrix3 W2 = W * W;
|
||||
|
||||
const double inv_angle_sq = 1.0 / angle_sq;
|
||||
const double sin_angle = sin(angle);
|
||||
const double cos_angle = cos(angle);
|
||||
|
||||
// coeff1 = (1 - cos(theta)) / theta^2
|
||||
const double coeff1 = (1.0 - cos_angle) * inv_angle_sq;
|
||||
// coeff2 = (theta - sin(theta)) / theta^3
|
||||
const double coeff2 = (angle - sin_angle) / (angle_sq * angle);
|
||||
|
||||
return Matrix3::Identity() + coeff1 * W + coeff2 * W2;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix3 Gal3::SO3_LeftJacobianInverse(const Vector3& theta) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
using std::sqrt;
|
||||
using std::tan;
|
||||
|
||||
const double angle_sq = theta.squaredNorm();
|
||||
const Matrix3 W = skewSymmetric(theta);
|
||||
|
||||
// Use Taylor series expansion for small angles
|
||||
if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) {
|
||||
// Jl(w)^-1 approx = I - 0.5*W + (1/12)*W^2
|
||||
return Matrix3::Identity() - 0.5 * W + (1.0/12.0) * W * W;
|
||||
}
|
||||
|
||||
const double angle = sqrt(angle_sq);
|
||||
const double half_angle = 0.5 * angle;
|
||||
|
||||
// Formula: I - 0.5*W + (1/theta^2 - cot(theta/2)/(2*theta)) * W^2
|
||||
// which is I - 0.5*W + (1 - 0.5*theta*cot(0.5*theta))/theta^2 * W^2
|
||||
double cot_half_angle;
|
||||
// Avoid division by zero/very small numbers for tan(half_angle)
|
||||
if (std::abs(sin(half_angle)) < kSmallAngleThreshold) {
|
||||
// If sin(half_angle) is near zero, theta is near multiples of 2*pi.
|
||||
// cot(x) ~ 1/x for small x. Let x = half_angle.
|
||||
// If half_angle is very small, use Taylor expansion of the coefficient:
|
||||
// (1 - 0.5*theta*cot(0.5*theta))/theta^2 -> 1/12 + O(theta^2)
|
||||
if (std::abs(half_angle) < kSmallAngleThreshold) {
|
||||
// Use Taylor approx for the coefficient directly
|
||||
const double coeff = 1.0 / 12.0; // + angle_sq / 720.0 + ...; // Higher order if needed
|
||||
return Matrix3::Identity() - 0.5 * W + coeff * (W * W);
|
||||
} else {
|
||||
// theta is near non-zero multiples of 2*pi. tan(half_angle) is near zero.
|
||||
// cot becomes large. Use limit or high-precision calculation if needed.
|
||||
// For now, use standard tan. May need robustness improvements.
|
||||
cot_half_angle = cos(half_angle) / sin(half_angle); // Avoid 1.0/tan()
|
||||
}
|
||||
} else {
|
||||
cot_half_angle = cos(half_angle) / sin(half_angle); // Avoid 1.0/tan()
|
||||
}
|
||||
|
||||
const double coeff = (1.0 - 0.5 * angle * cot_half_angle) / angle_sq;
|
||||
return Matrix3::Identity() - 0.5 * W + coeff * (W * W);
|
||||
|
||||
// Alternative using GTSAM SO3 properties (verify this matches J_l^-1):
|
||||
// J_l(theta)^-1 = J_r(-theta).
|
||||
// return gtsam::so3::DexpFunctor(-theta).rightJacobian();
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix3 Gal3::Calculate_E(const Vector3& theta) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
using std::sqrt;
|
||||
|
||||
const double angle_sq = theta.squaredNorm();
|
||||
const Matrix3 W = skewSymmetric(theta);
|
||||
const Matrix3 W2 = W * W;
|
||||
|
||||
Matrix3 E = 0.5 * Matrix3::Identity();
|
||||
|
||||
// Small angle approximation (from manif SGal3Tangent_base.h / Expmap formula)
|
||||
// E approx = 0.5*I + (1/6)*W + (1/24)*W^2
|
||||
if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) {
|
||||
E += (1.0 / 6.0) * W + (1.0 / 24.0) * W2;
|
||||
return E;
|
||||
}
|
||||
|
||||
const double angle = sqrt(angle_sq);
|
||||
const double sin_angle = sin(angle);
|
||||
const double cos_angle = cos(angle);
|
||||
|
||||
// Coefficients A and B from manif::SGal3TangentBase::exp()
|
||||
// A = (theta - sin(theta)) / theta^3
|
||||
const double A = (angle - sin_angle) / (angle_sq * angle);
|
||||
// B = (theta^2 + 2*cos(theta) - 2) / (2 * theta^4)
|
||||
const double B = (angle_sq + 2.0 * cos_angle - 2.0) / (2.0 * angle_sq * angle_sq);
|
||||
|
||||
E += A * W + B * W2;
|
||||
return E;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Constructors
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3::Gal3(const Matrix5& M) {
|
||||
// CORRECTED: Use head(N) with correct sizes for checks
|
||||
if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 ||
|
||||
M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) { // Row 4 first 4 = 0; Row 3 first 3 = 0
|
||||
throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones.");
|
||||
}
|
||||
R_ = Rot3(M.block<3, 3>(0, 0));
|
||||
v_ = M.block<3, 1>(0, 3); // Manif: v is in column 3 (0-indexed)
|
||||
r_ = Point3(M.block<3, 1>(0, 4)); // Manif: r is in column 4
|
||||
t_ = M(3, 4); // Manif: t is at (3, 4)
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Component Access
|
||||
//------------------------------------------------------------------------------
|
||||
const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const {
|
||||
if (H) {
|
||||
*H = Matrix::Zero(3, 10);
|
||||
H->block<3, 3>(0, 6) = Matrix3::Identity(); // Derivative w.r.t theta
|
||||
}
|
||||
return R_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const {
|
||||
if (H) {
|
||||
*H = Matrix::Zero(3, 10);
|
||||
H->block<3, 3>(0, 0) = R_.matrix(); // Derivative w.r.t rho (local tangent space)
|
||||
}
|
||||
return r_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const {
|
||||
if (H) {
|
||||
*H = Matrix::Zero(3, 10);
|
||||
H->block<3, 3>(0, 3) = R_.matrix(); // d(v)/d(nu)
|
||||
}
|
||||
return v_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
const double& Gal3::time(OptionalJacobian<1, 10> H) const {
|
||||
if (H) {
|
||||
*H = Matrix::Zero(1, 10);
|
||||
(*H)(0, 9) = 1.0; // Derivative w.r.t t_tan
|
||||
}
|
||||
return t_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Matrix Representation
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix5 Gal3::matrix() const {
|
||||
Matrix5 M = Matrix5::Identity();
|
||||
M.block<3, 3>(0, 0) = R_.matrix();
|
||||
M.block<3, 1>(0, 3) = v_; // Manif: v in col 3
|
||||
M.block<3, 1>(0, 4) = Vector3(r_); // Manif: r in col 4 // CORRECTED: Assign Vector3 from Point3
|
||||
M(3, 4) = t_; // Manif: t at (3, 4)
|
||||
M.block<1,3>(3,0).setZero(); // Zero out row 3, cols 0-2
|
||||
M.block<1,4>(4,0).setZero(); // Zero out row 4, cols 0-3
|
||||
return M;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Testable Requirements
|
||||
//------------------------------------------------------------------------------
|
||||
void Gal3::print(const std::string& s) const {
|
||||
std::cout << (s.empty() ? "" : s + " ");
|
||||
std::cout << *this << std::endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool Gal3::equals(const Gal3& other, double tol) const {
|
||||
return R_.equals(other.R_, tol) &&
|
||||
traits<Point3>::Equals(r_, other.r_, tol) &&
|
||||
traits<Velocity3>::Equals(v_, other.v_, tol) &&
|
||||
std::abs(t_ - other.t_) < tol;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Group Operations
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::inverse() const {
|
||||
// Formula: R_inv = R', v_inv = -R'*v, r_inv = -R'*(r - t*v), t_inv = -t
|
||||
const Rot3 Rinv = R_.inverse();
|
||||
const Velocity3 v_inv = -(Rinv.rotate(v_));
|
||||
// CORRECTED: Cast r_ to Vector3 for subtraction
|
||||
const Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_));
|
||||
const double t_inv = -t_;
|
||||
return Gal3(Rinv, r_inv, v_inv, t_inv);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::compose(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const {
|
||||
// Formula: R = R1*R2, v = R1*v2 + v1, r = R1*r2 + t2*v1 + r1, t = t1+t2
|
||||
// where this = g1, other = g2
|
||||
const Gal3& g1 = *this;
|
||||
const Gal3& g2 = other;
|
||||
|
||||
const Rot3 R_comp = g1.R_.compose(g2.R_);
|
||||
// CORRECTED: Cast g1.r_ to Vector3 for addition
|
||||
const Vector3 r_comp_vec = g1.R_.rotate(g2.r_) + g2.t_ * g1.v_ + Vector3(g1.r_);
|
||||
const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_;
|
||||
const double t_comp = g1.t_ + g2.t_;
|
||||
|
||||
Gal3 result(R_comp, Point3(r_comp_vec), v_comp, t_comp);
|
||||
|
||||
// Jacobians require correct AdjointMap implementation
|
||||
if (H1) {
|
||||
*H1 = g2.inverse().AdjointMap();
|
||||
}
|
||||
if (H2) {
|
||||
*H2 = Matrix10::Identity();
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::between(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const {
|
||||
const Gal3& g1 = *this;
|
||||
const Gal3& g2 = other;
|
||||
Gal3 g1_inv = g1.inverse();
|
||||
Gal3 result = g1_inv.compose(g2); // g1_inv * g2
|
||||
|
||||
// Jacobians require correct AdjointMap implementation
|
||||
if (H1) {
|
||||
*H1 = -result.AdjointMap();
|
||||
}
|
||||
if (H2) {
|
||||
*H2 = g1_inv.AdjointMap();
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Manifold Operations (Retract/Local)
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::retract(const Vector10& xi, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const {
|
||||
Gal3 g_exp = Gal3::Expmap(xi);
|
||||
Matrix10 H_exp_xi = Matrix10::Zero(); // Placeholder
|
||||
if (H2) {
|
||||
H_exp_xi = Gal3::ExpmapDerivative(xi); // Needs ExpmapDerivative implemented
|
||||
}
|
||||
|
||||
Matrix10 H_comp_this, H_comp_gexp;
|
||||
Gal3 result = this->compose(g_exp, H_comp_this, H_comp_gexp);
|
||||
|
||||
if (H1) {
|
||||
*H1 = H_comp_this; // Requires AdjointMap
|
||||
}
|
||||
if (H2) {
|
||||
// Needs ExpmapDerivative and potentially AdjointMap if H_comp_gexp is not Identity
|
||||
*H2 = H_comp_gexp * H_exp_xi; // H_comp_gexp is Identity here
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const {
|
||||
Matrix10 H_between_this, H_between_other;
|
||||
Gal3 result_g = this->between(other, H_between_this, H_between_other);
|
||||
|
||||
Matrix10 H_log_g = Matrix10::Zero(); // Placeholder
|
||||
if (H1 || H2) {
|
||||
H_log_g = Gal3::LogmapDerivative(result_g); // Needs LogmapDerivative implemented
|
||||
}
|
||||
Vector10 result_xi = Gal3::Logmap(result_g);
|
||||
|
||||
// Jacobians require LogmapDerivative and AdjointMap (via between Jacobians)
|
||||
if (H1) {
|
||||
*H1 = H_log_g * H_between_this;
|
||||
}
|
||||
if (H2) {
|
||||
*H2 = H_log_g * H_between_other;
|
||||
}
|
||||
return result_xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Lie Group Static Functions
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
Matrix5 Gal3::Hat(const Vector10& xi) {
|
||||
const Vector3 rho_tan = rho(xi);
|
||||
const Vector3 nu_tan = nu(xi);
|
||||
const Vector3 theta_tan = theta(xi);
|
||||
const double t_tan_val = t_tan(xi)(0);
|
||||
|
||||
Matrix5 X = Matrix5::Zero();
|
||||
X.block<3, 3>(0, 0) = skewSymmetric(theta_tan);
|
||||
X.block<3, 1>(0, 3) = nu_tan; // nu in column 3
|
||||
X.block<3, 1>(0, 4) = rho_tan; // rho in column 4
|
||||
X(3, 4) = t_tan_val; // t in element (3, 4)
|
||||
return X;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::Vee(const Matrix5& X) {
|
||||
// CORRECTED: Check X.row(3).head(N) for Lie algebra structure
|
||||
// Lie algebra row 3 is [0 0 0 0 t], so first 3 elements should be 0.
|
||||
// Lie algebra row 4 is [0 0 0 0 0], so all 5 elements should be 0.
|
||||
if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9) { // Row 4 all=0; Row 3 first 3=0
|
||||
throw std::invalid_argument("Matrix is not in sgal(3)");
|
||||
}
|
||||
|
||||
Vector10 xi;
|
||||
rho(xi) = X.block<3, 1>(0, 4); // rho from column 4
|
||||
nu(xi) = X.block<3, 1>(0, 3); // nu from column 3
|
||||
const Matrix3& S = X.block<3, 3>(0, 0);
|
||||
theta(xi) << S(2, 1), S(0, 2), S(1, 0);
|
||||
t_tan(xi)(0) = X(3, 4); // t from element (3, 4)
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) {
|
||||
const Vector3 rho_tan = rho(xi);
|
||||
const Vector3 nu_tan = nu(xi);
|
||||
const Vector3 theta_tan = theta(xi);
|
||||
const double t_tan_val = t_tan(xi)(0);
|
||||
|
||||
const Rot3 R = Rot3::Expmap(theta_tan);
|
||||
const Matrix3 Jl_theta = SO3_LeftJacobian(theta_tan);
|
||||
const Matrix3 E = Calculate_E(theta_tan);
|
||||
|
||||
const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan));
|
||||
const Velocity3 v_final = Jl_theta * nu_tan;
|
||||
const double t_final = t_tan_val;
|
||||
|
||||
if (Hxi) {
|
||||
// Jacobian placeholder - requires derivation
|
||||
*Hxi = ExpmapDerivative(xi);
|
||||
}
|
||||
|
||||
return Gal3(R, r_final, v_final, t_final);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) {
|
||||
const Vector3 theta_vec = Rot3::Logmap(g.R_);
|
||||
const Matrix3 Jl_theta_inv = SO3_LeftJacobianInverse(theta_vec);
|
||||
const Matrix3 E = Calculate_E(theta_vec);
|
||||
|
||||
// CORRECTED: Cast r_ to Vector3 for subtraction
|
||||
const Vector3 r_vec = Vector3(g.r_);
|
||||
const Velocity3& v_vec = g.v_;
|
||||
const double& t_val = g.t_;
|
||||
|
||||
const Vector3 nu_tan = Jl_theta_inv * v_vec;
|
||||
const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan));
|
||||
const double t_tan_val = t_val;
|
||||
|
||||
// Jacobian placeholder - requires derivation
|
||||
if (Hg) {
|
||||
*Hg = LogmapDerivative(g);
|
||||
}
|
||||
|
||||
Vector10 xi;
|
||||
rho(xi) = rho_tan;
|
||||
nu(xi) = nu_tan;
|
||||
theta(xi) = theta_vec;
|
||||
t_tan(xi)(0) = t_tan_val;
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix10 Gal3::AdjointMap() const {
|
||||
const Matrix3 Rmat = R_.matrix();
|
||||
const Matrix3 Vhat = skewSymmetric(v_);
|
||||
// CORRECTED: Cast r_ to Vector3 for subtraction
|
||||
const Vector3 r_minus_tv = Vector3(r_) - t_ * v_;
|
||||
const Matrix3 S_r_minus_tv_hat = skewSymmetric(r_minus_tv);
|
||||
|
||||
Matrix10 Ad = Matrix10::Zero();
|
||||
|
||||
// Row block 1: drho' output (maps input drho, dnu, dtheta, dt)
|
||||
Ad.block<3,3>(0,0) = Rmat;
|
||||
Ad.block<3,3>(0,3) = t_ * Rmat;
|
||||
Ad.block<3,3>(0,6) = -Rmat * S_r_minus_tv_hat;
|
||||
Ad.block<3,1>(0,9) = Rmat * v_;
|
||||
|
||||
// Row block 2: dnu' output
|
||||
Ad.block<3,3>(3,3) = Rmat;
|
||||
Ad.block<3,3>(3,6) = -Rmat * Vhat;
|
||||
|
||||
// Row block 3: dtheta' output
|
||||
Ad.block<3,3>(6,6) = Rmat;
|
||||
|
||||
// Row block 4: dt' output
|
||||
Ad(9,9) = 1.0;
|
||||
|
||||
return Ad;
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const {
|
||||
Matrix10 Ad = AdjointMap();
|
||||
if (H_xi) {
|
||||
*H_xi = Ad;
|
||||
}
|
||||
// Jacobian H_g requires adjointMap
|
||||
if (H_g) {
|
||||
*H_g = -adjointMap(Ad * xi);
|
||||
}
|
||||
return Ad * xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix10 Gal3::adjointMap(const Vector10& xi) {
|
||||
// Based on Manif SGal3Tangent::adj() / Lie bracket [xi, y]
|
||||
const Matrix3 Theta_hat = skewSymmetric(theta(xi));
|
||||
const Matrix3 Nu_hat = skewSymmetric(nu(xi));
|
||||
const Matrix3 Rho_hat = skewSymmetric(rho(xi));
|
||||
const double t_val = t_tan(xi)(0);
|
||||
|
||||
Matrix10 ad = Matrix10::Zero();
|
||||
|
||||
// Row block 1: rho_out = Theta_hat*m + Rho_hat*eta + t_val*p
|
||||
ad.block<3,3>(0,0) = Theta_hat; // d/dm
|
||||
ad.block<3,3>(0,3) = t_val * Matrix3::Identity(); // d/dp
|
||||
ad.block<3,3>(0,6) = Rho_hat; // d/deta
|
||||
|
||||
// Row block 2: nu_out = Theta_hat*p + Nu_hat*eta
|
||||
ad.block<3,3>(3,3) = Theta_hat; // d/dp
|
||||
ad.block<3,3>(3,6) = Nu_hat; // d/deta
|
||||
|
||||
// Row block 3: theta_out = Theta_hat*eta
|
||||
ad.block<3,3>(6,6) = Theta_hat; // d/deta
|
||||
|
||||
// Row block 4: t_out = 0 (already zero)
|
||||
|
||||
return ad;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) {
|
||||
Matrix10 ad_xi = adjointMap(xi);
|
||||
if (Hy) *Hy = ad_xi;
|
||||
// Jacobian Hxi requires adjointMap
|
||||
if (Hxi) {
|
||||
*Hxi = -adjointMap(y);
|
||||
}
|
||||
return ad_xi * y;
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) {
|
||||
if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity();
|
||||
// CORRECTED: Explicitly create std::function to resolve ambiguity
|
||||
std::function<Gal3(const Vector10&)> fn =
|
||||
[](const Vector10& v) { return Gal3::Expmap(v); };
|
||||
// Note: Default delta for numericalDerivative might be too large or small
|
||||
return numericalDerivative11<Gal3, Vector10>(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<Vector10(const Gal3&)> fn =
|
||||
[](const Gal3& g_in) { return Gal3::Logmap(g_in); };
|
||||
// Note: Default delta for numericalDerivative might be too large or small
|
||||
return numericalDerivative11<Vector10, Gal3>(fn, g, 1e-5); // Pass function object
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// ChartAtOrigin
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) {
|
||||
return Gal3::Expmap(xi, Hxi);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) {
|
||||
return Gal3::Logmap(g, Hg);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Stream operator
|
||||
//------------------------------------------------------------------------------
|
||||
std::ostream& operator<<(std::ostream& os, const Gal3& state) {
|
||||
os << "R: " << state.R_ << "\n";
|
||||
os << "r: " << state.r_.transpose() << "\n";
|
||||
os << "v: " << state.v_.transpose() << "\n";
|
||||
os << "t: " << state.t_;
|
||||
return os;
|
||||
}
|
||||
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,282 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* See LICENSE for the license information
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Gal3.h
|
||||
* @brief 3D Galilean Group SGal(3) state (attitude, position, velocity, time)
|
||||
* Based on manif convention: [R, r, v, t], tangent [rho, nu, theta, t]
|
||||
* @author Based on Python implementation by User
|
||||
* @date April 29, 2025
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h> // Includes Rot3, Point3
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Lie.h> // For LieGroup base class
|
||||
|
||||
#include <cmath> // For std::sqrt, std::cos, std::sin, std::tan
|
||||
#include <limits> // For std::numeric_limits
|
||||
#include <functional> // For std::function
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration
|
||||
class Gal3;
|
||||
|
||||
// Use Vector3 for velocity for consistency with NavState
|
||||
using Velocity3 = Vector3;
|
||||
// Define Vector10 for tangent space
|
||||
using Vector10 = Eigen::Matrix<double, 10, 1>;
|
||||
// Define Matrix5 for Lie Algebra matrix representation
|
||||
using Matrix5 = Eigen::Matrix<double, 5, 5>;
|
||||
// Define Matrix10 for Jacobians
|
||||
using Matrix10 = Eigen::Matrix<double, 10, 10>;
|
||||
|
||||
|
||||
/**
|
||||
* Represents an element of the 3D Galilean group SGal(3).
|
||||
* Aligned with manif conventions: state (R, r, v, t), tangent (rho, nu, theta, t_tan).
|
||||
* Internal state:
|
||||
* R_: Rotation (Rot3) - attitude (world R body)
|
||||
* r_: Translation (Point3) - position in world frame
|
||||
* v_: Velocity Boost (Velocity3) - velocity in world frame
|
||||
* t_: Time (double)
|
||||
*
|
||||
* Homogeneous Matrix Representation (manif convention):
|
||||
* [ R v r ]
|
||||
* [ 0 1 t ]
|
||||
* [ 0 0 1 ] (Where R is 3x3, v,r are 3x1, t is scalar, 0 are row vectors)
|
||||
*
|
||||
* Lie Algebra Matrix Representation (manif convention):
|
||||
* [ skew(theta) nu rho ]
|
||||
* [ 0 0 t ]
|
||||
* [ 0 0 0 ] (Where skew(theta) is 3x3, nu,rho are 3x1, t is scalar)
|
||||
*
|
||||
* Tangent Vector xi (Vector10): [rho; nu; theta; t_tan]
|
||||
* rho (3x1): Position tangent component
|
||||
* nu (3x1): Velocity tangent component
|
||||
* theta(3x1): Rotation tangent component (so(3))
|
||||
* t_tan (1x1): Time tangent component
|
||||
*/
|
||||
class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
|
||||
private:
|
||||
Rot3 R_; ///< Rotation world R body
|
||||
Point3 r_; ///< Position in world frame, n_r_b
|
||||
Velocity3 v_; ///< Velocity in world frame, n_v_b
|
||||
double t_; ///< Time component
|
||||
|
||||
// Small number epsilon for checking small angles, etc.
|
||||
static constexpr double kSmallAngleThreshold = 1e-10;
|
||||
|
||||
public:
|
||||
|
||||
inline constexpr static size_t dimension = 10;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor: Identity element
|
||||
Gal3() : R_(Rot3::Identity()), r_(Point3::Zero()), v_(Velocity3::Zero()), t_(0.0) {}
|
||||
|
||||
/// Construct from attitude, position, velocity, time (manif order)
|
||||
Gal3(const Rot3& R, const Point3& r, const Velocity3& v, double t) :
|
||||
R_(R), r_(r), v_(v), t_(t) {}
|
||||
|
||||
/// Construct from a 5x5 matrix representation (manif convention)
|
||||
explicit Gal3(const Matrix5& M);
|
||||
|
||||
/// @}
|
||||
/// @name Component Access
|
||||
/// @{
|
||||
|
||||
const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;
|
||||
const Point3& translation(OptionalJacobian<3, 10> H = {}) const;
|
||||
const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;
|
||||
const double& time(OptionalJacobian<1, 10> H = {}) const;
|
||||
|
||||
// Convenience accessors matching NavState where names overlap
|
||||
const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const { return rotation(H); }
|
||||
const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); }
|
||||
|
||||
/// @}
|
||||
/// @name Derived quantities
|
||||
/// @{
|
||||
|
||||
/// Return rotation matrix.
|
||||
Matrix3 R() const { return R_.matrix(); }
|
||||
|
||||
/// Return position as Vector3
|
||||
Vector3 r() const { return Vector3(r_); }
|
||||
|
||||
/// Return velocity as Vector3. Computation-free.
|
||||
const Vector3& v() const { return v_; }
|
||||
|
||||
/// Return time scalar. Computation-free.
|
||||
const double& t() const { return t_; }
|
||||
|
||||
/// Return 5x5 homogeneous matrix representation (manif convention).
|
||||
Matrix5 matrix() const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT
|
||||
friend std::ostream &operator<<(std::ostream &os, const Gal3& state);
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// equals
|
||||
bool equals(const Gal3& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Gal3 Identity() { return Gal3(); }
|
||||
|
||||
/// inverse transformation (Manif formula from SGal3_base.h)
|
||||
Gal3 inverse() const;
|
||||
|
||||
// Bring LieGroup::inverse() into scope (version with derivative)
|
||||
using LieGroup<Gal3, 10>::inverse;
|
||||
|
||||
/// compose (Manif formula from SGal3_base.h)
|
||||
/// Returns this * other
|
||||
Gal3 compose(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
||||
OptionalJacobian<10, 10> H2 = {}) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
Gal3 operator*(const Gal3& other) const {
|
||||
return compose(other);
|
||||
}
|
||||
|
||||
/// Calculate the relative transformation: this->inverse() * other
|
||||
Gal3 between(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
||||
OptionalJacobian<10, 10> H2 = {}) const;
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Manifold Operations (Retract/Local)
|
||||
/// @{
|
||||
|
||||
/// Default retract uses Expmap
|
||||
Gal3 retract(const Vector10& xi, OptionalJacobian<10, 10> H1 = {},
|
||||
OptionalJacobian<10, 10> H2 = {}) const;
|
||||
|
||||
/// Default localCoordinates uses Logmap
|
||||
Vector10 localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
||||
OptionalJacobian<10, 10> H2 = {}) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
using LieAlgebra = Matrix5;
|
||||
/// @{
|
||||
|
||||
// Helper functions for accessing tangent vector components
|
||||
// Input xi is [rho(3), nu(3), theta(3), t_tan(1)]
|
||||
static Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.segment<3>(0); }
|
||||
static Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.segment<3>(3); }
|
||||
static Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.segment<3>(6); }
|
||||
static Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.segment<1>(9); }
|
||||
// Const versions too
|
||||
static const Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.segment<3>(0); }
|
||||
static const Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.segment<3>(3); }
|
||||
static const Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.segment<3>(6); }
|
||||
static const Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.segment<1>(9); }
|
||||
|
||||
/// Exponential map at identity - create a Gal3 element from Lie algebra tangent vector xi.
|
||||
/// xi = [rho, nu, theta, t_tan] (10x1)
|
||||
static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {});
|
||||
// REMOVED using LieGroup<Gal3, 10>::Expmap;
|
||||
|
||||
/// Logarithmic map at identity - return the Lie algebra tangent vector xi for this element.
|
||||
/// Returns xi = [rho, nu, theta, t_tan] (10x1)
|
||||
static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
|
||||
// REMOVED using LieGroup<Gal3, 10>::Logmap;
|
||||
|
||||
/// Calculate Adjoint map, transforming a twist in the identity frame to the frame of this element.
|
||||
Matrix10 AdjointMap() const;
|
||||
|
||||
/// Apply this element's AdjointMap Ad_g to a tangent vector xi, H_g = d(Ad_g * xi)/dg, H_xi = d(Ad_g * xi)/dxi = Ad_g
|
||||
Vector10 Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g = {}, OptionalJacobian<10, 10> H_xi = {}) const;
|
||||
|
||||
/// The adjoint action of xi on y `ad(xi, y)`
|
||||
static Vector10 adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi = {}, OptionalJacobian<10, 10> Hy = {});
|
||||
|
||||
/// Compute the 10x10 adjoint map `ad(xi)` that acts on a tangent vector y `ad(xi, y) = ad(xi) * y`
|
||||
static Matrix10 adjointMap(const Vector10& xi);
|
||||
|
||||
/// Derivative of Expmap(xi) w.r.t. xi
|
||||
static Matrix10 ExpmapDerivative(const Vector10& xi);
|
||||
|
||||
/// Derivative of Logmap(g) w.r.t. g (represented as tangent vector at identity)
|
||||
static Matrix10 LogmapDerivative(const Gal3& g);
|
||||
|
||||
|
||||
// Chart at origin, depends on compile-time flag GTSAM_USE_MANIFOLD_MARKERS
|
||||
struct ChartAtOrigin {
|
||||
static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {});
|
||||
static Vector10 Local(const Gal3& g, ChartJacobian Hg = {});
|
||||
};
|
||||
|
||||
|
||||
/// Hat operator: maps tangent vector xi to Lie algebra matrix (Manif convention).
|
||||
/// xi = [rho, nu, theta, t] -> [ skew(theta) nu rho ; 0 0 t ; 0 0 0 ]
|
||||
static Matrix5 Hat(const Vector10& xi);
|
||||
|
||||
/// Vee operator: maps Lie algebra matrix to tangent vector xi (Manif convention).
|
||||
/// [ S N R ; 0 0 T ; 0 0 0 ] -> [ R ; N ; vee(S) ; T ] = [rho; nu; theta; t]
|
||||
static Vector10 Vee(const Matrix5& X);
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Helper Functions (moved inside class for context, could be private or in detail namespace)
|
||||
/// @{
|
||||
|
||||
/// Calculate the SO(3) Left Jacobian J_l(theta)
|
||||
static Matrix3 SO3_LeftJacobian(const Vector3& theta);
|
||||
|
||||
/// Calculate the inverse SO(3) Left Jacobian J_l(theta)^-1
|
||||
static Matrix3 SO3_LeftJacobianInverse(const Vector3& theta);
|
||||
|
||||
/// Calculate the 3x3 E matrix used in SGal3 Expmap/Logmap
|
||||
static Matrix3 Calculate_E(const Vector3& theta);
|
||||
|
||||
/// @}
|
||||
|
||||
|
||||
private:
|
||||
/// @{
|
||||
/// serialization
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_NVP(R_);
|
||||
ar& BOOST_SERIALIZATION_NVP(r_);
|
||||
ar& BOOST_SERIALIZATION_NVP(v_);
|
||||
ar& BOOST_SERIALIZATION_NVP(t_);
|
||||
}
|
||||
#endif
|
||||
/// @}
|
||||
}; // class Gal3
|
||||
|
||||
// // Specialize Gal3 traits
|
||||
template <>
|
||||
struct traits<Gal3> : public internal::MatrixLieGroup<Gal3> {};
|
||||
|
||||
template <>
|
||||
struct traits<const Gal3> : public internal::MatrixLieGroup<Gal3> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,412 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* See LICENSE for the license information
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testGal3.cpp
|
||||
* @brief Unit tests for Gal3 class, mirroring custom_gal3_tests.py
|
||||
* @author Based on Python tests by User and NavState tests
|
||||
* @date April 29, 2025 // Updated Date
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/Gal3.h>
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/testLie.h> // Include for Lie group concepts if needed later
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h> // Point3 is already included via Gal3.h usually
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <vector>
|
||||
#include <functional> // For std::bind if using numerical derivatives
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Define tolerance matching Python TOL
|
||||
static const double kTol = 1e-8;
|
||||
GTSAM_CONCEPT_TESTABLE_INST(gtsam::Gal3)
|
||||
GTSAM_CONCEPT_LIE_INST(gtsam::Gal3)
|
||||
|
||||
// Instantiate Testable concept for Gal3 to allow assert_equal(Gal3, Gal3)
|
||||
// Instantiate Lie group concepts if you plan to use CHECK_LIE_GROUP_DERIVATIVES etc.
|
||||
// GTSAM_CONCEPT_LIE_INST(Gal3)
|
||||
|
||||
// ========================================================================
|
||||
// Test Cases Section (mirroring Python's TestGal3 class)
|
||||
// ========================================================================
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Identity) {
|
||||
// Hardcoded expected data
|
||||
const Matrix3 expected_R_mat = Matrix3::Identity();
|
||||
const Vector3 expected_r_vec = Vector3::Zero();
|
||||
const Vector3 expected_v_vec = Vector3::Zero();
|
||||
const double expected_t_val = 0.0;
|
||||
|
||||
Gal3 custom_ident = Gal3::Identity();
|
||||
|
||||
// Check components individually
|
||||
EXPECT(assert_equal(expected_R_mat, custom_ident.rotation().matrix(), kTol));
|
||||
// Ensure translation() returns Point3, then convert if needed or compare Point3
|
||||
EXPECT(assert_equal(Point3(expected_r_vec), custom_ident.translation(), kTol));
|
||||
EXPECT(assert_equal(expected_v_vec, custom_ident.velocity(), kTol));
|
||||
EXPECT_DOUBLES_EQUAL(expected_t_val, custom_ident.time(), kTol);
|
||||
|
||||
// Alternative: Check whole object if Testable concept works as expected
|
||||
EXPECT(assert_equal(Gal3(Rot3(expected_R_mat), Point3(expected_r_vec), expected_v_vec, expected_t_val), custom_ident, kTol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, HatVee) {
|
||||
// Hardcoded test cases for Hat/Vee (from Python script)
|
||||
// Case 1
|
||||
const Vector10 tau_vec_1 = (Vector10() <<
|
||||
-0.9919387548344049, 0.41796965721894275, -0.08567669832855362, // rho
|
||||
-0.24728318780816563, 0.42470896104182426, 0.37654216726012074, // nu
|
||||
-0.4665439537974297, -0.46391731412948783, -0.46638346398428376, // theta
|
||||
-0.2703091399101363 // t_tan
|
||||
).finished();
|
||||
const Matrix5 expected_xi_matrix_1 = (Matrix5() <<
|
||||
0.0, 0.46638346398428376, -0.46391731412948783, -0.24728318780816563, -0.9919387548344049,
|
||||
-0.46638346398428376, 0.0, 0.4665439537974297, 0.42470896104182426, 0.41796965721894275,
|
||||
0.46391731412948783, -0.4665439537974297, 0.0, 0.37654216726012074, -0.08567669832855362,
|
||||
0.0, 0.0, 0.0, 0.0, -0.2703091399101363,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0
|
||||
).finished();
|
||||
|
||||
// Test Hat operation
|
||||
Matrix5 custom_Xi_1 = Gal3::Hat(tau_vec_1);
|
||||
EXPECT(assert_equal(expected_xi_matrix_1, custom_Xi_1, kTol));
|
||||
|
||||
// Test Vee operation (using the expected Xi matrix as input)
|
||||
Vector10 custom_tau_rec_1 = Gal3::Vee(expected_xi_matrix_1);
|
||||
EXPECT(assert_equal(tau_vec_1, custom_tau_rec_1, kTol));
|
||||
|
||||
// Test Vee(Hat(tau)) round trip
|
||||
Vector10 custom_tau_rec_roundtrip_1 = Gal3::Vee(custom_Xi_1);
|
||||
EXPECT(assert_equal(tau_vec_1, custom_tau_rec_roundtrip_1, kTol));
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Expmap) {
|
||||
// Hardcoded test case for Expmap (from Python script)
|
||||
// Case 1
|
||||
const Vector10 tau_vec_1 = (Vector10() <<
|
||||
-0.6659680127970163, 0.0801034296770802, -0.005425197747099379, // rho
|
||||
-0.24823309993679712, -0.3282613511681929, -0.3614305580886979, // nu
|
||||
0.3267045270397072, -0.21318895514136532, -0.1810111529904679, // theta
|
||||
-0.11137002094819903 // t_tan
|
||||
).finished();
|
||||
const Matrix3 expected_R_mat_1 = (Matrix3() <<
|
||||
0.961491754653074, 0.14119138670272793, -0.23579354964696073,
|
||||
-0.20977429976081094, 0.9313179826319476, -0.297727322203087,
|
||||
0.17756223949368974, 0.33572579219851445, 0.925072885538575
|
||||
).finished();
|
||||
const Vector3 expected_r_vec_1 = (Vector3() << -0.637321673031978, 0.16116104619552254, -0.03248254605908951).finished();
|
||||
const Vector3 expected_v_vec_1 = (Vector3() << -0.22904001980446076, -0.23988916442951638, -0.4308710747620977).finished();
|
||||
const double expected_t_val_1 = -0.11137002094819903;
|
||||
|
||||
Gal3 custom_exp_1 = Gal3::Expmap(tau_vec_1);
|
||||
|
||||
// Check components individually
|
||||
EXPECT(assert_equal(expected_R_mat_1, custom_exp_1.rotation().matrix(), kTol));
|
||||
EXPECT(assert_equal(Point3(expected_r_vec_1), custom_exp_1.translation(), kTol));
|
||||
EXPECT(assert_equal(expected_v_vec_1, custom_exp_1.velocity(), kTol));
|
||||
EXPECT_DOUBLES_EQUAL(expected_t_val_1, custom_exp_1.time(), kTol);
|
||||
|
||||
// Check derivatives (optional, but good practice like in testNavState)
|
||||
// Matrix9 aH;
|
||||
// Gal3::Expmap(tau_vec_1, aH);
|
||||
// std::function<Gal3(const Vector9&)> expmap_func = [](const Vector9& v){ return Gal3::Expmap(v); };
|
||||
// Matrix expectedH = numericalDerivative11(expmap_func, tau_vec_1);
|
||||
// EXPECT(assert_equal(expectedH, aH, 1e-6)); // Use appropriate tolerance
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Logmap) {
|
||||
// --- Part 1: Test Logmap(GroupElement) ---
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Matrix3 input_R_mat_1 = (Matrix3() <<
|
||||
-0.8479395778141634, 0.26601628670932354, -0.4585126035145831,
|
||||
0.5159883846729487, 0.612401478276553, -0.5989327310201821,
|
||||
0.1214679351060988, -0.744445944720015, -0.6565407650184298
|
||||
).finished();
|
||||
// Use Point3/Velocity3 directly for construction
|
||||
const Point3 input_r_vec_1(0.6584021866593519, -0.3393257406257678, -0.5420636579124554);
|
||||
const Velocity3 input_v_vec_1(0.1772802663861217, 0.3146080790621266, 0.7173535084539808);
|
||||
const double input_t_val_1 = -0.12088016100268817;
|
||||
const Gal3 custom_g_1(Rot3(input_R_mat_1), input_r_vec_1, input_v_vec_1, input_t_val_1);
|
||||
|
||||
const Vector10 expected_log_tau_1 = (Vector10() <<
|
||||
-0.6366686897004876, -0.2565186503803428, -1.1108185946230884, // rho
|
||||
1.122213550821757, -0.21828427331226408, 0.03100839182220047, // nu
|
||||
-0.6312616056853186, -2.516056355068653, 1.0844223965979727, // theta
|
||||
-0.12088016100268817 // t_tan
|
||||
).finished();
|
||||
|
||||
Vector10 custom_log_tau_1 = Gal3::Logmap(custom_g_1);
|
||||
EXPECT(assert_equal(expected_log_tau_1, custom_log_tau_1, kTol*1e3));
|
||||
// Add more test cases here if they exist in the Python script
|
||||
|
||||
|
||||
// --- Part 2: Test Log(Exp(tau)) round trip ---
|
||||
// Using data from the Expmap test case
|
||||
const Vector10 tau_vec_orig_rt1 = (Vector10() <<
|
||||
-0.6659680127970163, 0.0801034296770802, -0.005425197747099379,
|
||||
-0.24823309993679712, -0.3282613511681929, -0.3614305580886979,
|
||||
0.3267045270397072, -0.21318895514136532, -0.1810111529904679,
|
||||
-0.11137002094819903
|
||||
).finished();
|
||||
Gal3 g_exp_rt1 = Gal3::Expmap(tau_vec_orig_rt1);
|
||||
Vector10 tau_log_exp_rt1 = Gal3::Logmap(g_exp_rt1);
|
||||
// Use slightly larger tolerance for round trip
|
||||
EXPECT(assert_equal(tau_vec_orig_rt1, tau_log_exp_rt1, kTol * 10));
|
||||
|
||||
|
||||
// --- Part 3: Test Exp(Log(g)) round trip ---
|
||||
// Using data from the first Logmap test case
|
||||
Gal3 custom_g_orig_rt2 = custom_g_1; // Reuse from Part 1
|
||||
Vector10 tau_log_rt2 = Gal3::Logmap(custom_g_orig_rt2);
|
||||
Gal3 g_exp_log_rt2 = Gal3::Expmap(tau_log_rt2);
|
||||
// Compare the reconstructed g against the original using assert_equal for Gal3
|
||||
EXPECT(assert_equal(custom_g_orig_rt2, g_exp_log_rt2, kTol * 10));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Compose) {
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Matrix3 g1_R_mat_1 = (Matrix3() <<
|
||||
-0.5427153955878299, 0.8391431164114453, 0.03603927817303032,
|
||||
0.8040805715986894, 0.5314810596281534, -0.2664250694549225,
|
||||
-0.24272295682417533, -0.11561450357036887, -0.963181630220753
|
||||
).finished();
|
||||
const Point3 g1_r_vec_1(0.9978490360071179, -0.5634861893781862, 0.025864788808796835);
|
||||
const Velocity3 g1_v_vec_1(0.04857438013356852, -0.012834026018545996, 0.945550047767139);
|
||||
const double g1_t_val_1 = -0.41496643117394594;
|
||||
const Gal3 c1_1(Rot3(g1_R_mat_1), g1_r_vec_1, g1_v_vec_1, g1_t_val_1);
|
||||
|
||||
const Matrix3 g2_R_mat_1 = (Matrix3() <<
|
||||
-0.3264538540162394, 0.24276278793202133, -0.9135064914894779,
|
||||
0.9430076454867458, 0.1496317101600385, -0.2972321178273404,
|
||||
0.06453264097716288, -0.9584761760784951, -0.27777501352435885
|
||||
).finished();
|
||||
const Point3 g2_r_vec_1(-0.1995427558196442, 0.7830589040103644, -0.433370507989717);
|
||||
const Velocity3 g2_v_vec_1(0.8986541507293722, 0.051990700444202176, -0.8278883042875157);
|
||||
const double g2_t_val_1 = -0.6155723080111539;
|
||||
const Gal3 c2_1(Rot3(g2_R_mat_1), g2_r_vec_1, g2_v_vec_1, g2_t_val_1);
|
||||
|
||||
// Expected result of composition
|
||||
const Matrix3 expected_R_mat_1 = (Matrix3() <<
|
||||
0.9708156167565788, -0.04073147244077803, 0.23634294026763253,
|
||||
0.22150238776832248, 0.5300893429357028, -0.8184998355032984,
|
||||
-0.09194417042137741, 0.8469629482207595, 0.5236411308011658
|
||||
).finished();
|
||||
const Point3 expected_r_vec_1(1.7177230471349891, -0.18439262777314758, -0.18087448280827323);
|
||||
const Velocity3 expected_v_vec_1(-0.4253479212754224, 0.9579585887030762, 1.5188219826819997);
|
||||
const double expected_t_val_1 = -1.0305387391850998;
|
||||
const Gal3 expected_comp_1(Rot3(expected_R_mat_1), expected_r_vec_1, expected_v_vec_1, expected_t_val_1);
|
||||
|
||||
Gal3 custom_comp_1 = c1_1 * c2_1; // Or c1_1.compose(c2_1)
|
||||
|
||||
// Compare the whole Gal3 object
|
||||
EXPECT(assert_equal(expected_comp_1, custom_comp_1, kTol));
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Inverse) {
|
||||
// Hardcoded identity for comparison
|
||||
Gal3 expected_identity = Gal3::Identity();
|
||||
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Matrix3 g_R_mat_1 = (Matrix3() <<
|
||||
0.6680516673568877, 0.2740542884848495, -0.6918101016209183,
|
||||
0.6729369985913887, -0.6193062871756463, 0.4044941514923666,
|
||||
-0.31758898858193396, -0.7357676057205693, -0.5981498680963873
|
||||
).finished();
|
||||
const Point3 g_r_vec_1(0.06321286832132045, -0.9214393132931736, -0.12472480681013542);
|
||||
const Velocity3 g_v_vec_1(0.4770686298036335, 0.2799576331302327, -0.29190264050471715);
|
||||
const double g_t_val_1 = 0.3757227805330059;
|
||||
const Gal3 custom_g_1(Rot3(g_R_mat_1), g_r_vec_1, g_v_vec_1, g_t_val_1);
|
||||
|
||||
// Expected inverse
|
||||
const Matrix3 expected_inv_R_mat_1 = (Matrix3() <<
|
||||
0.6680516673568877, 0.6729369985913887, -0.31758898858193396,
|
||||
0.2740542884848495, -0.6193062871756463, -0.7357676057205693,
|
||||
-0.6918101016209183, 0.4044941514923666, -0.5981498680963873
|
||||
).finished();
|
||||
const Point3 expected_inv_r_vec_1(0.7635904739613719, -0.6150700906051861, 0.32598918251792036);
|
||||
const Velocity3 expected_inv_v_vec_1(-0.5998054073176801, -0.17213568846657853, 0.042198146082895516);
|
||||
const double expected_inv_t_val_1 = -0.3757227805330059;
|
||||
const Gal3 expected_inv_1(Rot3(expected_inv_R_mat_1), expected_inv_r_vec_1, expected_inv_v_vec_1, expected_inv_t_val_1);
|
||||
|
||||
// Test inverse calculation
|
||||
Gal3 custom_inv_1 = custom_g_1.inverse();
|
||||
EXPECT(assert_equal(expected_inv_1, custom_inv_1, kTol));
|
||||
|
||||
// Check g * g.inverse() == Identity
|
||||
Gal3 ident_c_1 = custom_g_1 * custom_inv_1;
|
||||
EXPECT(assert_equal(expected_identity, ident_c_1, kTol));
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Between) {
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Matrix3 g1_R_mat_1 = (Matrix3() <<
|
||||
-0.2577418495238488, 0.7767168385303765, 0.5747000015202739,
|
||||
-0.6694062876067332, -0.572463082520484, 0.47347781496466923,
|
||||
0.6967527259484512, -0.26267274676776586, 0.6674868290752107
|
||||
).finished();
|
||||
const Point3 g1_r_vec_1(0.680375434309419, -0.21123414636181392, 0.5661984475172117);
|
||||
const Velocity3 g1_v_vec_1(0.536459189623808, -0.44445057839362445, 0.10793991159086103);
|
||||
const double g1_t_val_1 = -0.0452058962756795;
|
||||
const Gal3 c1_1(Rot3(g1_R_mat_1), g1_r_vec_1, g1_v_vec_1, g1_t_val_1);
|
||||
|
||||
const Matrix3 g2_R_mat_1 = (Matrix3() <<
|
||||
0.1981112115076329, -0.12884463237051902, 0.9716743325745948,
|
||||
0.9776658305457298, -0.04497580389201028, -0.20529661674659028,
|
||||
0.0701532013404006, 0.9906443548385938, 0.11705678352030657
|
||||
).finished();
|
||||
const Point3 g2_r_vec_1(0.9044594503494257, 0.8323901360074013, 0.2714234559198019);
|
||||
const Velocity3 g2_v_vec_1(-0.5142264587405261, -0.7255368464279626, 0.6083535084539808);
|
||||
const double g2_t_val_1 = -0.6866418214918308;
|
||||
const Gal3 c2_1(Rot3(g2_R_mat_1), g2_r_vec_1, g2_v_vec_1, g2_t_val_1);
|
||||
|
||||
// Expected result of c1.between(c2) which is c1.inverse() * c2
|
||||
const Matrix3 expected_R_mat_1 = (Matrix3() <<
|
||||
-0.6566377699430239, 0.753549894443112, -0.0314546605295386,
|
||||
-0.4242286152401611, -0.3345440819370167, 0.8414929228771536,
|
||||
0.6235839326792096, 0.5659000033801861, 0.5393517081447288
|
||||
).finished();
|
||||
const Point3 expected_r_vec_1(-0.8113603292280276, 0.06632931940009146, 0.535144598419476);
|
||||
const Velocity3 expected_v_vec_1(0.8076311151757332, -0.7866187376416414, -0.4028976707214998);
|
||||
const double expected_t_val_1 = -0.6414359252161513;
|
||||
const Gal3 expected_btw_1(Rot3(expected_R_mat_1), expected_r_vec_1, expected_v_vec_1, expected_t_val_1);
|
||||
|
||||
|
||||
Gal3 custom_btw_1 = c1_1.between(c2_1);
|
||||
EXPECT(assert_equal(expected_btw_1, custom_btw_1, kTol));
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, MatrixComponents) {
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Matrix3 source_R_mat_1 = (Matrix3() <<
|
||||
0.43788117516687186, -0.12083239518241493, -0.8908757538001356,
|
||||
0.4981128609611659, 0.8575347951217139, 0.12852102124027118,
|
||||
0.7484274541861499, -0.5000336063006573, 0.43568651389548174
|
||||
).finished();
|
||||
const Point3 source_r_vec_1(0.3684370505476542, 0.8219440615838134, -0.03501868668711683);
|
||||
const Velocity3 source_v_vec_1(0.7621243390078305, 0.282161192634218, -0.13609316346053646);
|
||||
const double source_t_val_1 = 0.23919296788014144;
|
||||
const Gal3 c1(Rot3(source_R_mat_1), source_r_vec_1, source_v_vec_1, source_t_val_1);
|
||||
|
||||
// Expected matrix representation
|
||||
Matrix5 expected_Mc;
|
||||
expected_Mc << source_R_mat_1, source_v_vec_1, source_r_vec_1,
|
||||
Vector3::Zero().transpose(), 1.0, source_t_val_1,
|
||||
Vector4::Zero().transpose(), 1.0;
|
||||
|
||||
Matrix5 Mc = c1.matrix();
|
||||
|
||||
// Compare the whole matrix
|
||||
EXPECT(assert_equal(expected_Mc, Mc, kTol));
|
||||
|
||||
// Optional: Individual component checks (redundant if the above passes)
|
||||
// EXPECT(assert_equal(source_R_mat_1, Mc.block<3,3>(0,0), kTol));
|
||||
// EXPECT(assert_equal(source_v_vec_1, Mc.block<3,1>(0,3), kTol));
|
||||
// EXPECT(assert_equal(source_r_vec_1.vector(), Mc.block<3,1>(0,4), kTol)); // .vector() for Point3
|
||||
// EXPECT_DOUBLES_EQUAL(source_t_val_1, Mc(3,4), kTol);
|
||||
// EXPECT_DOUBLES_EQUAL(1.0, Mc(3,3), kTol);
|
||||
// EXPECT_DOUBLES_EQUAL(1.0, Mc(4,4), kTol);
|
||||
// EXPECT(assert_equal(Vector3::Zero(), Mc.block<1,3>(3,0).transpose(), kTol));
|
||||
// EXPECT(assert_equal(Vector4::Zero(), Mc.block<1,4>(4,0).transpose(), kTol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, Associativity) {
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Vector10 tau1_1 = (Vector10() <<
|
||||
0.14491869060866264, -0.21431172810692092, -0.4956042914756127,
|
||||
-0.13411549788475238, 0.44534636811395767, -0.33281648500962074,
|
||||
-0.012095339359589743, -0.20104157502639808, 0.08197507996416392,
|
||||
-0.006285789459736368
|
||||
).finished();
|
||||
const Vector10 tau2_1 = (Vector10() <<
|
||||
0.29551108535517384, 0.2938672197508704, 0.0788811297200055,
|
||||
-0.07107463852205165, 0.22379088709954872, -0.26508231911443875,
|
||||
-0.11625916165500054, 0.04661407377867886, 0.1788274027858556,
|
||||
-0.015243024791797033
|
||||
).finished();
|
||||
const Vector10 tau3_1 = (Vector10() <<
|
||||
0.37646834040234084, 0.3782542871960659, -0.6525520272351378,
|
||||
0.005426723127791683, 0.09951505587485733, 0.3813252061414707,
|
||||
-0.09289643299325814, -0.0017149201262141199, -0.08507973168896384,
|
||||
-0.1109049754985476
|
||||
).finished();
|
||||
|
||||
Gal3 g1_1 = Gal3::Expmap(tau1_1);
|
||||
Gal3 g2_1 = Gal3::Expmap(tau2_1);
|
||||
Gal3 g3_1 = Gal3::Expmap(tau3_1);
|
||||
|
||||
Gal3 left_assoc_1 = (g1_1 * g2_1) * g3_1;
|
||||
Gal3 right_assoc_1 = g1_1 * (g2_1 * g3_1);
|
||||
|
||||
// Compare the results directly using assert_equal for Gal3
|
||||
EXPECT(assert_equal(left_assoc_1, right_assoc_1, kTol * 10)); // Slightly larger tol for composed operations
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Gal3, IdentityProperties) {
|
||||
Gal3 custom_identity = Gal3::Identity();
|
||||
|
||||
// Hardcoded test case 1 (from Python script)
|
||||
const Matrix3 g_R_mat_1 = (Matrix3() <<
|
||||
-0.5204974727334908, 0.7067813015326174, 0.4791060140322894,
|
||||
0.773189425449982, 0.15205374379417114, 0.6156766776243058,
|
||||
0.3622989004266723, 0.6908978584436601, -0.6256194178153267
|
||||
).finished();
|
||||
const Point3 g_r_vec_1(-0.8716573584227159, -0.9599539022706234, -0.08459652545144625);
|
||||
const Velocity3 g_v_vec_1(0.7018395735425127, -0.4666685012479632, 0.07952068144433233);
|
||||
const double g_t_val_1 = -0.24958604725524136;
|
||||
const Gal3 custom_g_1(Rot3(g_R_mat_1), g_r_vec_1, g_v_vec_1, g_t_val_1);
|
||||
|
||||
// Test g * g.inverse() == identity
|
||||
Gal3 g_inv_1 = custom_g_1.inverse();
|
||||
Gal3 g_times_inv_1 = custom_g_1 * g_inv_1;
|
||||
EXPECT(assert_equal(custom_identity, g_times_inv_1, kTol * 10)); // Slightly larger tol
|
||||
|
||||
// Test identity * g == g
|
||||
Gal3 id_times_g_1 = custom_identity * custom_g_1;
|
||||
EXPECT(assert_equal(custom_g_1, id_times_g_1, kTol));
|
||||
|
||||
// Test g * identity == g
|
||||
Gal3 g_times_id_1 = custom_g_1 * custom_identity;
|
||||
EXPECT(assert_equal(custom_g_1, g_times_id_1, kTol));
|
||||
|
||||
// Add more test cases here if they exist in the Python script
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue