Add Galilean 3 group
mkielo3
parent
07c9bc8ffa
commit
42985c6326
|
|
@ -23,3 +23,4 @@ xcode/
|
||||||
|
|
||||||
# MyST build outputs
|
# MyST build outputs
|
||||||
_build
|
_build
|
||||||
|
examples/evaluatePreintegrationNavState.cpp
|
||||||
|
|
|
||||||
|
|
@ -10,7 +10,7 @@
|
||||||
* @file Gal3.cpp
|
* @file Gal3.cpp
|
||||||
* @brief Implementation of 3D Galilean Group SGal(3) state
|
* @brief Implementation of 3D Galilean Group SGal(3) state
|
||||||
* @author Based on Python implementation by User
|
* @author Based on Python implementation by User
|
||||||
* @date April 29, 2025
|
* @date April 30, 2025 // Updated Date
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/navigation/Gal3.h> // Note: Adjust path if needed
|
#include <gtsam/navigation/Gal3.h> // Note: Adjust path if needed
|
||||||
|
|
@ -26,140 +26,125 @@
|
||||||
namespace gtsam {
|
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) {
|
// Define the threshold locally within the cpp file
|
||||||
using std::cos;
|
constexpr double kSmallAngleThreshold = 1e-10;
|
||||||
using std::sin;
|
|
||||||
using std::sqrt;
|
|
||||||
|
|
||||||
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
|
const double angle_sq = theta.squaredNorm();
|
||||||
if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) {
|
const Matrix3 W = skewSymmetric(theta);
|
||||||
// Jl(w) approx = I + 0.5*[w]x + (1/6)*[w]x^2
|
const Matrix3 W2 = W * W;
|
||||||
const Matrix3 W = skewSymmetric(theta);
|
|
||||||
return Matrix3::Identity() + 0.5 * W + (1.0/6.0) * W * W;
|
|
||||||
}
|
|
||||||
|
|
||||||
const double angle = sqrt(angle_sq);
|
Matrix3 E = 0.5 * Matrix3::Identity();
|
||||||
const Matrix3 W = skewSymmetric(theta);
|
|
||||||
const Matrix3 W2 = W * W;
|
|
||||||
|
|
||||||
const double inv_angle_sq = 1.0 / angle_sq;
|
// Small angle approximation - Use locally defined constant
|
||||||
const double sin_angle = sin(angle);
|
if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) {
|
||||||
const double cos_angle = cos(angle);
|
E += (1.0 / 6.0) * W + (1.0 / 24.0) * W2;
|
||||||
|
return E;
|
||||||
|
}
|
||||||
|
|
||||||
// coeff1 = (1 - cos(theta)) / theta^2
|
const double angle = sqrt(angle_sq);
|
||||||
const double coeff1 = (1.0 - cos_angle) * inv_angle_sq;
|
const double sin_angle = sin(angle);
|
||||||
// coeff2 = (theta - sin(theta)) / theta^3
|
const double cos_angle = cos(angle);
|
||||||
const double coeff2 = (angle - sin_angle) / (angle_sq * 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) {
|
Gal3 Gal3::FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t,
|
||||||
using std::cos;
|
OptionalJacobian<10, 6> H1, OptionalJacobian<10, 3> H2,
|
||||||
using std::sin;
|
OptionalJacobian<10, 1> H3) {
|
||||||
using std::sqrt;
|
const Rot3& R = pose.rotation();
|
||||||
using std::tan;
|
const Point3& r = pose.translation(); // Get r for H3 calculation if needed below
|
||||||
|
if (H1) { // d xi_out / d xi_pose (10x6), xi_pose = [omega_R; v_r]
|
||||||
const double angle_sq = theta.squaredNorm();
|
*H1 = Matrix::Zero(10, 6);
|
||||||
const Matrix3 W = skewSymmetric(theta);
|
// d theta / d omega_pose = I (rows 6-8, cols 0-2)
|
||||||
|
H1->block<3, 3>(6, 0) = Matrix3::Identity();
|
||||||
// Use Taylor series expansion for small angles
|
// d rho / d v_r = I (rows 0-2, cols 3-5) - Corrected
|
||||||
if (angle_sq < kSmallAngleThreshold * kSmallAngleThreshold) {
|
H1->block<3, 3>(0, 3) = Matrix3::Identity();
|
||||||
// Jl(w)^-1 approx = I - 0.5*W + (1/12)*W^2
|
// Other blocks (d_rho/d_omega, d_nu/d_omega, d_nu/d_vr, d_tt/d_omega, d_tt/d_vr) are zero.
|
||||||
return Matrix3::Identity() - 0.5 * W + (1.0/12.0) * W * W;
|
|
||||||
}
|
}
|
||||||
|
if (H2) { // d xi_out / d v_in (10x3)
|
||||||
const double angle = sqrt(angle_sq);
|
*H2 = Matrix::Zero(10, 3);
|
||||||
const double half_angle = 0.5 * angle;
|
H2->block<3, 3>(3, 0) = R.transpose(); // d(nu_out)/d(v_in) = R^T
|
||||||
|
|
||||||
// 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 (H3) { // d xi_out / d t_in (10x1)
|
||||||
const double coeff = (1.0 - 0.5 * angle * cot_half_angle) / angle_sq;
|
*H3 = Matrix::Zero(10, 1);
|
||||||
return Matrix3::Identity() - 0.5 * W + coeff * (W * W);
|
// As derived: d xi / dt = [-R^T v; 0; 0; 1]
|
||||||
|
Vector3 drho_dt = -R.transpose() * v; // Corrected derivative for rho
|
||||||
// Alternative using GTSAM SO3 properties (verify this matches J_l^-1):
|
H3->block<3, 1>(0, 0) = drho_dt; // Assign to rho rows (0-2)
|
||||||
// J_l(theta)^-1 = J_r(-theta).
|
// d nu / dt = 0
|
||||||
// return gtsam::so3::DexpFunctor(-theta).rightJacobian();
|
// d theta / dt = 0
|
||||||
}
|
(*H3)(9, 0) = 1.0; // d t_tan / dt = 1
|
||||||
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
// Pass r (translation from pose) to the Gal3 constructor
|
||||||
const double angle = sqrt(angle_sq);
|
return Gal3(R, r, v, t);
|
||||||
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
|
// Constructors (NavState order: 2 - Included in Constructor section)
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
Gal3::Gal3(const Matrix5& M) {
|
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 ||
|
if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 ||
|
||||||
M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) { // Row 4 first 4 = 0; Row 3 first 3 = 0
|
M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) { // Row 4 first 4 = 0; Row 3 first 3 = 0
|
||||||
throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones.");
|
throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones.");
|
||||||
}
|
}
|
||||||
R_ = Rot3(M.block<3, 3>(0, 0));
|
R_ = Rot3(M.block<3, 3>(0, 0));
|
||||||
v_ = M.block<3, 1>(0, 3); // Manif: v is in column 3 (0-indexed)
|
v_ = M.block<3, 1>(0, 3); // v is in column 3
|
||||||
r_ = Point3(M.block<3, 1>(0, 4)); // Manif: r is in column 4
|
r_ = Point3(M.block<3, 1>(0, 4)); // r is in column 4
|
||||||
t_ = M(3, 4); // Manif: t is at (3, 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 {
|
const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const {
|
||||||
if (H) {
|
if (H) {
|
||||||
|
|
@ -171,9 +156,17 @@ const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const {
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
const Point3& Gal3::translation(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) {
|
if (H) {
|
||||||
*H = Matrix::Zero(3, 10);
|
*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_;
|
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 Gal3::matrix() const {
|
||||||
Matrix5 M = Matrix5::Identity();
|
Matrix5 M = Matrix5::Identity();
|
||||||
M.block<3, 3>(0, 0) = R_.matrix();
|
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, 3) = v_;
|
||||||
M.block<3, 1>(0, 4) = Vector3(r_); // Manif: r in col 4 // CORRECTED: Assign Vector3 from Point3
|
M.block<3, 1>(0, 4) = Vector3(r_);
|
||||||
M(3, 4) = t_; // Manif: t at (3, 4)
|
M(3, 4) = t_;
|
||||||
M.block<1,3>(3,0).setZero(); // Zero out row 3, cols 0-2
|
M.block<1,3>(3,0).setZero(); // Zero out row 3, cols 0-2
|
||||||
M.block<1,4>(4,0).setZero(); // Zero out row 4, cols 0-3
|
M.block<1,4>(4,0).setZero(); // Zero out row 4, cols 0-3
|
||||||
return M;
|
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 {
|
void Gal3::print(const std::string& s) const {
|
||||||
std::cout << (s.empty() ? "" : s + " ");
|
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 {
|
Gal3 Gal3::inverse() const {
|
||||||
// Formula: R_inv = R', v_inv = -R'*v, r_inv = -R'*(r - t*v), t_inv = -t
|
// Formula: R_inv = R', v_inv = -R'*v, r_inv = -R'*(r - t*v), t_inv = -t
|
||||||
const Rot3 Rinv = R_.inverse();
|
const Rot3 Rinv = R_.inverse();
|
||||||
const Velocity3 v_inv = -(Rinv.rotate(v_));
|
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 Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_));
|
||||||
const double t_inv = -t_;
|
const double t_inv = -t_;
|
||||||
return Gal3(Rinv, r_inv, v_inv, t_inv);
|
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 {
|
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
|
// 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& g1 = *this;
|
||||||
const Gal3& g2 = other;
|
const Gal3& g2 = other;
|
||||||
|
|
||||||
const Rot3 R_comp = g1.R_.compose(g2.R_);
|
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 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 Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_;
|
||||||
const double t_comp = g1.t_ + g2.t_;
|
const double t_comp = g1.t_ + g2.t_;
|
||||||
|
|
||||||
Gal3 result(R_comp, Point3(r_comp_vec), v_comp, t_comp);
|
Gal3 result(R_comp, Point3(r_comp_vec), v_comp, t_comp);
|
||||||
|
|
||||||
// Jacobians require correct AdjointMap implementation
|
|
||||||
if (H1) {
|
if (H1) {
|
||||||
*H1 = g2.inverse().AdjointMap();
|
*H1 = g2.inverse().AdjointMap(); // Jacobian w.r.t this is Ad(g2.inverse())
|
||||||
}
|
}
|
||||||
if (H2) {
|
if (H2) {
|
||||||
*H2 = Matrix10::Identity();
|
*H2 = Matrix10::Identity(); // Jacobian w.r.t other is Identity
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
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 {
|
Gal3 Gal3::between(const Gal3& other, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const {
|
||||||
const Gal3& g1 = *this;
|
const Gal3& g1 = *this;
|
||||||
const Gal3& g2 = other;
|
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) {
|
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) {
|
if (H2) {
|
||||||
*H2 = g1_inv.AdjointMap();
|
// H2 = d(compose)/d(g2) = H_comp_g2 = Identity
|
||||||
|
*H2 = H_comp_g2; // Should be Identity()
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
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 Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) {
|
||||||
Gal3 g_exp = Gal3::Expmap(xi);
|
const Vector3 rho_tan = rho(xi);
|
||||||
Matrix10 H_exp_xi = Matrix10::Zero(); // Placeholder
|
const Vector3 nu_tan = nu(xi);
|
||||||
if (H2) {
|
const Vector3 theta_tan = theta(xi);
|
||||||
H_exp_xi = Gal3::ExpmapDerivative(xi); // Needs ExpmapDerivative implemented
|
const double t_tan_val = t_tan(xi)(0);
|
||||||
}
|
|
||||||
|
|
||||||
Matrix10 H_comp_this, H_comp_gexp;
|
const Rot3 R = Rot3::Expmap(theta_tan);
|
||||||
Gal3 result = this->compose(g_exp, H_comp_this, H_comp_gexp);
|
const gtsam::so3::DexpFunctor dexp_functor(theta_tan);
|
||||||
|
const Matrix3 Jl_theta = dexp_functor.leftJacobian(); // Left Jacobian of SO(3) Expmap
|
||||||
|
|
||||||
if (H1) {
|
const Matrix3 E = Calculate_E(theta_tan); // Use helper matrix E(theta)
|
||||||
*H1 = H_comp_this; // Requires AdjointMap
|
|
||||||
}
|
const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan));
|
||||||
if (H2) {
|
const Velocity3 v_final = Jl_theta * nu_tan;
|
||||||
// Needs ExpmapDerivative and potentially AdjointMap if H_comp_gexp is not Identity
|
const double t_final = t_tan_val;
|
||||||
*H2 = H_comp_gexp * H_exp_xi; // H_comp_gexp is Identity here
|
|
||||||
}
|
if (Hxi) {
|
||||||
return result;
|
*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 {
|
Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) {
|
||||||
Matrix10 H_between_this, H_between_other;
|
const Vector3 theta_vec = Rot3::Logmap(g.R_);
|
||||||
Gal3 result_g = this->between(other, H_between_this, H_between_other);
|
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
|
const Matrix3 E = Calculate_E(theta_vec); // Use helper matrix E(theta)
|
||||||
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)
|
const Vector3 r_vec = Vector3(g.r_);
|
||||||
if (H1) {
|
const Velocity3& v_vec = g.v_;
|
||||||
*H1 = H_log_g * H_between_this;
|
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;
|
Vector10 xi;
|
||||||
}
|
rho(xi) = rho_tan;
|
||||||
return result_xi;
|
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<Vector10(const Gal3&, const Vector10&)> 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<Gal3(const Vector10&)> fn =
|
||||||
|
[](const Vector10& v) { return Gal3::Expmap(v); };
|
||||||
|
return numericalDerivative11<Gal3, Vector10>(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<Vector10(const Gal3&)> fn =
|
||||||
|
[](const Gal3& g_in) { return Gal3::Logmap(g_in); };
|
||||||
|
return numericalDerivative11<Vector10, Gal3>(fn, g, 1e-5);
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
// Lie Algebra (Hat/Vee maps) (NavState order: 9)
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
Matrix5 Gal3::Hat(const Vector10& xi) {
|
Matrix5 Gal3::Hat(const Vector10& xi) {
|
||||||
const Vector3 rho_tan = rho(xi);
|
const Vector3 rho_tan = rho(xi);
|
||||||
const Vector3 nu_tan = nu(xi);
|
const Vector3 nu_tan = nu(xi);
|
||||||
|
|
@ -348,10 +482,8 @@ Matrix5 Gal3::Hat(const Vector10& xi) {
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
Vector10 Gal3::Vee(const Matrix5& X) {
|
Vector10 Gal3::Vee(const Matrix5& X) {
|
||||||
// CORRECTED: Check X.row(3).head(N) for Lie algebra structure
|
// Check Lie algebra structure: Row 4 all=0; Row 3 first 3=0, last = t.
|
||||||
// Lie algebra row 3 is [0 0 0 0 t], so first 3 elements should be 0.
|
if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9 || std::abs(X(3,3)) > 1e-9) {
|
||||||
// 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)");
|
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
|
rho(xi) = X.block<3, 1>(0, 4); // rho from column 4
|
||||||
nu(xi) = X.block<3, 1>(0, 3); // nu from column 3
|
nu(xi) = X.block<3, 1>(0, 3); // nu from column 3
|
||||||
const Matrix3& S = X.block<3, 3>(0, 0);
|
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)
|
t_tan(xi)(0) = X(3, 4); // t from element (3, 4)
|
||||||
return xi;
|
return xi;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
Gal3 Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) {
|
// ChartAtOrigin (NavState order: 10)
|
||||||
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) {
|
Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) {
|
||||||
|
// Retraction at origin is Expmap
|
||||||
return Gal3::Expmap(xi, Hxi);
|
return Gal3::Expmap(xi, Hxi);
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) {
|
Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) {
|
||||||
|
// Local coordinates at origin is Logmap
|
||||||
return Gal3::Logmap(g, Hg);
|
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) {
|
Gal3 Gal3::retract(const Vector10& xi, OptionalJacobian<10, 10> H1, OptionalJacobian<10, 10> H2) const {
|
||||||
os << "R: " << state.R_ << "\n";
|
// Default retract is Expmap applied via composition: this->compose(Expmap(xi))
|
||||||
os << "r: " << state.r_.transpose() << "\n";
|
Matrix10 H_exp_xi;
|
||||||
os << "v: " << state.v_.transpose() << "\n";
|
Gal3 g_exp = Gal3::Expmap(xi, H2 ? &H_exp_xi : nullptr); // Expmap(xi) and its Jacobian H2'
|
||||||
os << "t: " << state.t_;
|
|
||||||
return os;
|
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
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -9,21 +9,19 @@
|
||||||
/**
|
/**
|
||||||
* @file Gal3.h
|
* @file Gal3.h
|
||||||
* @brief 3D Galilean Group SGal(3) state (attitude, position, velocity, time)
|
* @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
|
* @author Based on Python implementation by User
|
||||||
* @date April 29, 2025
|
* @date April 30, 2025 // Updated Date
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/geometry/Pose3.h> // Includes Rot3, Point3
|
#include <gtsam/geometry/Pose3.h> // Includes Rot3, Point3
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Lie.h> // For LieGroup base class and traits
|
||||||
#include <gtsam/base/Manifold.h>
|
#include <gtsam/base/Manifold.h> // For Manifold traits
|
||||||
#include <gtsam/base/Lie.h> // For LieGroup base class
|
|
||||||
|
|
||||||
#include <cmath> // For std::sqrt, std::cos, std::sin, std::tan
|
#include <cmath> // For std::sqrt, std::cos, std::sin
|
||||||
#include <limits> // For std::numeric_limits
|
#include <functional> // For std::function used in numerical derivatives
|
||||||
#include <functional> // For std::function
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -34,9 +32,9 @@ class Gal3;
|
||||||
using Velocity3 = Vector3;
|
using Velocity3 = Vector3;
|
||||||
// Define Vector10 for tangent space
|
// Define Vector10 for tangent space
|
||||||
using Vector10 = Eigen::Matrix<double, 10, 1>;
|
using Vector10 = Eigen::Matrix<double, 10, 1>;
|
||||||
// Define Matrix5 for Lie Algebra matrix representation
|
// Define Matrix5 for Lie Algebra matrix representation (Hat map output)
|
||||||
using Matrix5 = Eigen::Matrix<double, 5, 5>;
|
using Matrix5 = Eigen::Matrix<double, 5, 5>;
|
||||||
// Define Matrix10 for Jacobians
|
// Define Matrix10 for Jacobians (AdjointMap, Expmap/Logmap derivatives)
|
||||||
using Matrix10 = Eigen::Matrix<double, 10, 10>;
|
using Matrix10 = Eigen::Matrix<double, 10, 10>;
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -72,12 +70,10 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
|
||||||
Velocity3 v_; ///< Velocity in world frame, n_v_b
|
Velocity3 v_; ///< Velocity in world frame, n_v_b
|
||||||
double t_; ///< Time component
|
double t_; ///< Time component
|
||||||
|
|
||||||
// Small number epsilon for checking small angles, etc.
|
|
||||||
static constexpr double kSmallAngleThreshold = 1e-10;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
inline constexpr static size_t dimension = 10;
|
/// The dimension of the tangent space
|
||||||
|
inline static constexpr size_t dimension = 10;
|
||||||
|
|
||||||
/// @name Constructors
|
/// @name Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -92,28 +88,49 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
|
||||||
/// Construct from a 5x5 matrix representation (manif convention)
|
/// Construct from a 5x5 matrix representation (manif convention)
|
||||||
explicit Gal3(const Matrix5& M);
|
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
|
/// @name Component Access
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
|
/// Access rotation component (Rot3)
|
||||||
const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;
|
const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;
|
||||||
|
|
||||||
|
/// Access translation component (Point3)
|
||||||
const Point3& translation(OptionalJacobian<3, 10> H = {}) const;
|
const Point3& translation(OptionalJacobian<3, 10> H = {}) const;
|
||||||
|
|
||||||
|
/// Access velocity component (Vector3)
|
||||||
const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;
|
const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;
|
||||||
|
|
||||||
|
/// Access time component (double)
|
||||||
const double& time(OptionalJacobian<1, 10> H = {}) const;
|
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 Rot3& attitude(OptionalJacobian<3, 10> H = {}) const { return rotation(H); }
|
||||||
const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); }
|
const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); }
|
||||||
|
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Derived quantities
|
/// @name Derived quantities
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// Return rotation matrix.
|
/// Return rotation matrix (Matrix3).
|
||||||
Matrix3 R() const { return R_.matrix(); }
|
Matrix3 R() const { return R_.matrix(); }
|
||||||
|
|
||||||
/// Return position as Vector3
|
/// Return position as Vector3.
|
||||||
Vector3 r() const { return Vector3(r_); }
|
Vector3 r() const { return Vector3(r_); } // Conversion from Point3
|
||||||
|
|
||||||
/// Return velocity as Vector3. Computation-free.
|
/// Return velocity as Vector3. Computation-free.
|
||||||
const Vector3& v() const { return v_; }
|
const Vector3& v() const { return v_; }
|
||||||
|
|
@ -132,137 +149,158 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
|
||||||
GTSAM_EXPORT
|
GTSAM_EXPORT
|
||||||
friend std::ostream &operator<<(std::ostream &os, const Gal3& state);
|
friend std::ostream &operator<<(std::ostream &os, const Gal3& state);
|
||||||
|
|
||||||
/// print
|
/// Print with optional string prefix
|
||||||
void print(const std::string& s = "") const;
|
void print(const std::string& s = "") const;
|
||||||
|
|
||||||
/// equals
|
/// Check equality within tolerance
|
||||||
bool equals(const Gal3& other, double tol = 1e-9) const;
|
bool equals(const Gal3& other, double tol = 1e-9) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Group
|
/// @name Group
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// identity for group operation
|
/// Return the identity element
|
||||||
static Gal3 Identity() { return Gal3(); }
|
static Gal3 Identity() { return Gal3(); }
|
||||||
|
|
||||||
/// inverse transformation (Manif formula from SGal3_base.h)
|
/// Return the inverse of this element
|
||||||
Gal3 inverse() const;
|
Gal3 inverse() const;
|
||||||
|
|
||||||
// Bring LieGroup::inverse() into scope (version with derivative)
|
// Bring LieGroup::inverse() into scope (version with derivative)
|
||||||
using LieGroup<Gal3, 10>::inverse;
|
using LieGroup<Gal3, 10>::inverse;
|
||||||
|
|
||||||
/// compose (Manif formula from SGal3_base.h)
|
/// Compose with another element: `this * other`
|
||||||
/// Returns this * other
|
|
||||||
Gal3 compose(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
Gal3 compose(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
||||||
OptionalJacobian<10, 10> H2 = {}) const;
|
OptionalJacobian<10, 10> H2 = {}) const;
|
||||||
|
|
||||||
/// compose syntactic sugar
|
/// Group composition operator: `this * other`
|
||||||
Gal3 operator*(const Gal3& other) const {
|
Gal3 operator*(const Gal3& other) const { return compose(other); }
|
||||||
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 = {},
|
Gal3 between(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
||||||
OptionalJacobian<10, 10> H2 = {}) const;
|
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 = {},
|
Gal3 retract(const Vector10& xi, OptionalJacobian<10, 10> H1 = {},
|
||||||
OptionalJacobian<10, 10> H2 = {}) const;
|
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 = {},
|
Vector10 localCoordinates(const Gal3& other, OptionalJacobian<10, 10> H1 = {},
|
||||||
OptionalJacobian<10, 10> H2 = {}) const;
|
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
|
/// @name Group Action
|
||||||
using LieAlgebra = Matrix5;
|
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
// Helper functions for accessing tangent vector components
|
/**
|
||||||
// Input xi is [rho(3), nu(3), theta(3), t_tan(1)]
|
* Action of the group element on a 3D point.
|
||||||
static Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.segment<3>(0); }
|
* Following GTSAM convention for Pose3::transformFrom, this applies
|
||||||
static Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.segment<3>(3); }
|
* the instantaneous rotation and translation: p' = R*p + r.
|
||||||
static Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.segment<3>(6); }
|
* If time evolution is needed, use compose or specific dynamics model.
|
||||||
static Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.segment<1>(9); }
|
*/
|
||||||
// Const versions too
|
Point3 act(const Point3& p,
|
||||||
static const Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.segment<3>(0); }
|
OptionalJacobian<3, 10> Hself = {},
|
||||||
static const Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.segment<3>(3); }
|
OptionalJacobian<3, 3> Hp = {}) const;
|
||||||
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.
|
// 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<rows, cols>(startRow, startCol) syntax ***
|
||||||
|
static Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
|
||||||
|
static Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
|
||||||
|
static Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
|
||||||
|
static Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
|
||||||
|
// Const versions
|
||||||
|
static Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
|
||||||
|
static Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
|
||||||
|
static Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
|
||||||
|
static Eigen::Block<const Vector10, 1, 1> 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)
|
/// xi = [rho, nu, theta, t_tan] (10x1)
|
||||||
static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {});
|
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.
|
/// Logarithmic map at identity: manifold element g -> tangent vector xi
|
||||||
/// Returns xi = [rho, nu, theta, t_tan] (10x1)
|
/// Returns xi = [rho, nu, theta, t_tan] (10x1)
|
||||||
static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
|
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.
|
/// 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;
|
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
|
/// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at identity.
|
||||||
Vector10 Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g = {}, OptionalJacobian<10, 10> H_xi = {}) const;
|
/// 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 = {});
|
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);
|
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);
|
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);
|
static Matrix10 LogmapDerivative(const Gal3& g);
|
||||||
|
|
||||||
|
/// Chart at origin, uses Expmap/Logmap for Retract/Local
|
||||||
// Chart at origin, depends on compile-time flag GTSAM_USE_MANIFOLD_MARKERS
|
|
||||||
struct ChartAtOrigin {
|
struct ChartAtOrigin {
|
||||||
static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {});
|
static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {});
|
||||||
static Vector10 Local(const Gal3& g, ChartJacobian Hg = {});
|
static Vector10 Local(const Gal3& g, ChartJacobian Hg = {});
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/// Hat operator: maps tangent vector xi to Lie algebra matrix (Manif convention).
|
/// 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);
|
static Matrix5 Hat(const Vector10& xi);
|
||||||
|
|
||||||
/// Vee operator: maps Lie algebra matrix to tangent vector xi (Manif convention).
|
/// 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);
|
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
|
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template <class ARCHIVE>
|
template <class ARCHIVE>
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
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(r_);
|
ar& BOOST_SERIALIZATION_NVP(r_);
|
||||||
ar& BOOST_SERIALIZATION_NVP(v_);
|
ar& BOOST_SERIALIZATION_NVP(v_);
|
||||||
|
|
@ -270,13 +308,14 @@ private:
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
}; // class Gal3
|
}; // class Gal3
|
||||||
|
|
||||||
// // Specialize Gal3 traits
|
/// Traits specialization for Gal3
|
||||||
template <>
|
template <>
|
||||||
struct traits<Gal3> : public internal::MatrixLieGroup<Gal3> {};
|
struct traits<Gal3> : public internal::LieGroup<Gal3> {};
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
struct traits<const Gal3> : public internal::MatrixLieGroup<Gal3> {};
|
struct traits<const Gal3> : public internal::LieGroup<Gal3> {};
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -36,6 +36,150 @@ GTSAM_CONCEPT_LIE_INST(gtsam::Gal3)
|
||||||
// Instantiate Testable concept for Gal3 to allow assert_equal(Gal3, 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.
|
// Instantiate Lie group concepts if you plan to use CHECK_LIE_GROUP_DERIVATIVES etc.
|
||||||
// GTSAM_CONCEPT_LIE_INST(Gal3)
|
// 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<Gal3>);
|
||||||
|
GTSAM_CONCEPT_ASSERT(IsManifold<Gal3>);
|
||||||
|
// Since its LieAlgebra is Matrix5, it also technically fits IsMatrixLieGroup,
|
||||||
|
// but IsLieGroup is sufficient here.
|
||||||
|
GTSAM_CONCEPT_ASSERT(IsLieGroup<Gal3>);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// 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)
|
// 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
|
// 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<Vector10(const Gal3&)> logmap_func1 =
|
||||||
|
[](const Gal3& g_in) { return Gal3::Logmap(g_in); };
|
||||||
|
Matrix H_numerical1 = numericalDerivative11<Vector10, Gal3>(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<Vector10(const Gal3&)> logmap_func2 =
|
||||||
|
[](const Gal3& g_in) { return Gal3::Logmap(g_in); };
|
||||||
|
Matrix H_numerical2 = numericalDerivative11<Vector10, Gal3>(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<Gal3(const Gal3&, const Gal3&)> 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<Vector10(const Gal3&, const Vector10&)> 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<Vector10(const Gal3&, const Vector10&)> 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<Gal3(const Gal3&)> 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<Gal3(const Gal3&, const Gal3&)> 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<Point3(const Gal3&, const Point3&)> 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<Point3(const Gal3&, const Point3&)> 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<Gal3(const Rot3&, const Point3&, const Velocity3&, const double&)> 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<Gal3(const Pose3&, const Velocity3&, const double&)> 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<Rot3(const Gal3&)> rot_func =
|
||||||
|
[](const Gal3& g) { return g.rotation(); };
|
||||||
|
Matrix Hrot_num = numericalDerivative11<Rot3, Gal3>(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<Point3(const Gal3&)> pos_func =
|
||||||
|
[](const Gal3& g) { return g.translation(); };
|
||||||
|
Matrix Hpos_num = numericalDerivative11<Point3, Gal3>(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<Velocity3(const Gal3&)> vel_func =
|
||||||
|
[](const Gal3& g) { return g.velocity(); };
|
||||||
|
Matrix Hvel_num = numericalDerivative11<Velocity3, Gal3>(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<double(const Gal3&)> time_func =
|
||||||
|
[](const Gal3& g) { return g.time(); };
|
||||||
|
Matrix Htime_num = numericalDerivative11<double, Gal3>(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<Gal3(const Gal3&, const Gal3&, const double&)> 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<Vector10(const Vector10&, const Vector10&)> 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() {
|
int main() {
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue