Frank Dellaert
GitHub
commit
d6b4383438
|
|
@ -17,7 +17,7 @@
|
|||
* @date December 2014
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
#include <gtsam/geometry/Event.h>
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -20,7 +20,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam_unstable/dllexport.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <iosfwd>
|
||||
|
|
@ -34,7 +34,7 @@ namespace gtsam {
|
|||
* SLAM, where we have "time of arrival" measurements at a set of sensors. The
|
||||
* TOA functor below provides a measurement function for those applications.
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT Event {
|
||||
class GTSAM_EXPORT Event {
|
||||
double time_; ///< Time event was generated
|
||||
Point3 location_; ///< Location at time event was generated
|
||||
|
||||
|
|
@ -84,7 +84,7 @@ template <>
|
|||
struct traits<Event> : internal::Manifold<Event> {};
|
||||
|
||||
/// Time of arrival to given sensor
|
||||
class TimeOfArrival {
|
||||
class GTSAM_EXPORT TimeOfArrival {
|
||||
const double speed_; ///< signal speed
|
||||
|
||||
public:
|
||||
|
|
@ -0,0 +1,493 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* See LICENSE for the license information
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Gal3.cpp
|
||||
* @brief Implementation of 3D Galilean Group SGal(3) state
|
||||
* @authors Matt Kielo, Scott Baker, Frank Dellaert
|
||||
* @date April 30, 2025
|
||||
*
|
||||
* This implementation is based on the paper:
|
||||
* Kelly, J. (2023). "All About the Galilean Group SGal(3)"
|
||||
* arXiv:2312.07555
|
||||
*
|
||||
* All section, equation, and page references in comments throughout this file
|
||||
* refer to the aforementioned paper.
|
||||
*/
|
||||
|
||||
|
||||
#include <gtsam/geometry/Gal3.h>
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
#include <gtsam/geometry/Event.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Constants and Helper function for Expmap/Logmap
|
||||
//------------------------------------------------------------------------------
|
||||
namespace { // Anonymous namespace for internal linkage
|
||||
constexpr double kSmallAngleThreshold = 1e-10;
|
||||
|
||||
// The type of the Lie algebra (matrix representation)
|
||||
using LieAlgebra = Matrix5;
|
||||
|
||||
// Helper functions for accessing tangent vector components
|
||||
Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
|
||||
Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
|
||||
Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
|
||||
Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
|
||||
// Const versions
|
||||
Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
|
||||
Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
|
||||
Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
|
||||
Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
|
||||
|
||||
} // end anonymous namespace
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Static Constructor/Create functions
|
||||
//------------------------------------------------------------------------------
|
||||
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) {
|
||||
if (H1) {
|
||||
H1->setZero();
|
||||
H1->block<3, 3>(6, 0) = Matrix3::Identity();
|
||||
}
|
||||
if (H2) {
|
||||
H2->setZero();
|
||||
H2->block<3, 3>(0, 0) = R.transpose();
|
||||
}
|
||||
if (H3) {
|
||||
H3->setZero();
|
||||
H3->block<3, 3>(3, 0) = R.transpose();
|
||||
}
|
||||
if (H4) {
|
||||
H4->setZero();
|
||||
Vector3 drho_dt = -R.transpose() * v;
|
||||
H4->block<3, 1>(0, 0) = drho_dt;
|
||||
(*H4)(9, 0) = 1.0;
|
||||
}
|
||||
return Gal3(R, r, v, t);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t,
|
||||
OptionalJacobian<10, 6> H1, OptionalJacobian<10, 3> H2,
|
||||
OptionalJacobian<10, 1> H3) {
|
||||
const Rot3& R = pose.rotation();
|
||||
const Point3& r = pose.translation();
|
||||
if (H1) {
|
||||
H1->setZero();
|
||||
H1->block<3, 3>(6, 0) = Matrix3::Identity();
|
||||
H1->block<3, 3>(0, 3) = Matrix3::Identity();
|
||||
}
|
||||
if (H2) {
|
||||
H2->setZero();
|
||||
H2->block<3, 3>(3, 0) = R.transpose();
|
||||
}
|
||||
if (H3) {
|
||||
H3->setZero();
|
||||
Vector3 drho_dt = -R.transpose() * v;
|
||||
H3->block<3, 1>(0, 0) = drho_dt;
|
||||
(*H3)(9, 0) = 1.0;
|
||||
}
|
||||
return Gal3(R, r, v, t);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Constructors
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3::Gal3(const Matrix5& M) {
|
||||
// Constructor from 5x5 matrix representation (Equation 9, Page 5)
|
||||
if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 ||
|
||||
M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) {
|
||||
throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones.");
|
||||
}
|
||||
R_ = Rot3(M.block<3, 3>(0, 0));
|
||||
v_ = M.block<3, 1>(0, 3);
|
||||
r_ = Point3(M.block<3, 1>(0, 4));
|
||||
t_ = M(3, 4);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Component Access
|
||||
//------------------------------------------------------------------------------
|
||||
const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
H->block<3, 3>(0, 6) = Matrix3::Identity();
|
||||
}
|
||||
return R_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
H->block<3,3>(0, 0) = R_.matrix();
|
||||
H->block<3,1>(0, 9) = v_;
|
||||
}
|
||||
return r_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
H->block<3, 3>(0, 3) = R_.matrix();
|
||||
}
|
||||
return v_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
const double& Gal3::time(OptionalJacobian<1, 10> H) const {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
(*H)(0, 9) = 1.0;
|
||||
}
|
||||
return t_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Matrix Representation
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix5 Gal3::matrix() const {
|
||||
// Returns 5x5 matrix representation as in Equation 9, Page 5
|
||||
Matrix5 M = Matrix5::Identity();
|
||||
M.block<3, 3>(0, 0) = R_.matrix();
|
||||
M.block<3, 1>(0, 3) = v_;
|
||||
M.block<3, 1>(0, 4) = Vector3(r_);
|
||||
M(3, 4) = t_;
|
||||
M.block<1,3>(3,0).setZero();
|
||||
M.block<1,4>(4,0).setZero();
|
||||
return M;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Stream operator
|
||||
//------------------------------------------------------------------------------
|
||||
std::ostream& operator<<(std::ostream& os, const Gal3& state) {
|
||||
os << "R: " << state.R_ << "\n";
|
||||
os << "r: " << state.r_.transpose() << "\n";
|
||||
os << "v: " << state.v_.transpose() << "\n";
|
||||
os << "t: " << state.t_;
|
||||
return os;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Testable Requirements
|
||||
//------------------------------------------------------------------------------
|
||||
void Gal3::print(const std::string& s) const {
|
||||
std::cout << (s.empty() ? "" : s + " ");
|
||||
std::cout << *this << std::endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool Gal3::equals(const Gal3& other, double tol) const {
|
||||
return R_.equals(other.R_, tol) &&
|
||||
traits<Point3>::Equals(r_, other.r_, tol) &&
|
||||
traits<Velocity3>::Equals(v_, other.v_, tol) &&
|
||||
std::abs(t_ - other.t_) < tol;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Group Operations
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::inverse() const {
|
||||
// Implements inverse formula from Equation 10, Page 5
|
||||
const Rot3 Rinv = R_.inverse();
|
||||
const Velocity3 v_inv = -(Rinv.rotate(v_));
|
||||
const Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_));
|
||||
const double t_inv = -t_;
|
||||
return Gal3(Rinv, r_inv, v_inv, t_inv);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::operator*(const Gal3& other) const {
|
||||
// Implements group composition through matrix multiplication
|
||||
const Gal3& g1 = *this;
|
||||
const Gal3& g2 = other;
|
||||
|
||||
const Rot3 R_comp = g1.R_.compose(g2.R_);
|
||||
const Vector3 r1_vec(g1.r_);
|
||||
const Vector3 r2_vec(g2.r_);
|
||||
const Vector3 r_comp_vec = g1.R_.rotate(r2_vec) + g2.t_ * g1.v_ + r1_vec;
|
||||
const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_;
|
||||
const double t_comp = g1.t_ + g2.t_;
|
||||
|
||||
return Gal3(R_comp, Point3(r_comp_vec), v_comp, t_comp);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Lie Group Static Functions
|
||||
//------------------------------------------------------------------------------
|
||||
gtsam::Gal3 gtsam::Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) {
|
||||
// Implements exponential map from Equations 16-19, Pages 7-8
|
||||
const Vector3 rho_tan = rho(xi);
|
||||
const Vector3 nu_tan = nu(xi);
|
||||
const Vector3 theta_tan = theta(xi);
|
||||
const double t_tan_val = t_tan(xi)(0);
|
||||
|
||||
const gtsam::so3::DexpFunctor dexp_functor(theta_tan);
|
||||
const Rot3 R = Rot3::Expmap(theta_tan);
|
||||
const Matrix3 Jl_theta = dexp_functor.leftJacobian();
|
||||
|
||||
Matrix3 E;
|
||||
if (dexp_functor.nearZero) {
|
||||
// Small angle approximation for E matrix (from Equation 19, Page 8)
|
||||
E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor.W + (1.0 / 24.0) * dexp_functor.WW;
|
||||
} else {
|
||||
// Closed form for E matrix (from Equation 19, Page 8)
|
||||
const double B_E = (1.0 - 2.0 * dexp_functor.B) / (2.0 * dexp_functor.theta2);
|
||||
E = 0.5 * Matrix3::Identity() + dexp_functor.C * dexp_functor.W + B_E * dexp_functor.WW;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
Gal3 result(R, r_final, v_final, t_final);
|
||||
|
||||
if (Hxi) {
|
||||
*Hxi = Gal3::ExpmapDerivative(xi);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) {
|
||||
// Implements logarithmic map from Equations 20-23, Page 8
|
||||
const Vector3 theta_vec = Rot3::Logmap(g.R_);
|
||||
const gtsam::so3::DexpFunctor dexp_functor_log(theta_vec);
|
||||
const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse();
|
||||
|
||||
Matrix3 E;
|
||||
if (dexp_functor_log.nearZero) {
|
||||
// Small angle approximation for E matrix
|
||||
E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor_log.W + (1.0 / 24.0) * dexp_functor_log.WW;
|
||||
} else {
|
||||
// Closed form for E matrix (from Equation 19, Page 8)
|
||||
const double B_E = (1.0 - 2.0 * dexp_functor_log.B) / (2.0 * dexp_functor_log.theta2);
|
||||
E = 0.5 * Matrix3::Identity() + dexp_functor_log.C * dexp_functor_log.W + B_E * dexp_functor_log.WW;
|
||||
}
|
||||
|
||||
const Vector3 r_vec = Vector3(g.r_);
|
||||
const Velocity3& v_vec = g.v_;
|
||||
const double& t_val = g.t_;
|
||||
|
||||
// Implementation of Equation 23, Page 8
|
||||
const Vector3 nu_tan = Jl_theta_inv * v_vec;
|
||||
const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan));
|
||||
const double t_tan_val = t_val;
|
||||
|
||||
Vector10 xi;
|
||||
rho(xi) = rho_tan;
|
||||
nu(xi) = nu_tan;
|
||||
theta(xi) = theta_vec;
|
||||
t_tan(xi)(0) = t_tan_val;
|
||||
|
||||
if (Hg) {
|
||||
*Hg = Gal3::LogmapDerivative(g);
|
||||
}
|
||||
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix10 Gal3::AdjointMap() const {
|
||||
// Implements adjoint map as in Equation 26, Page 9
|
||||
const Matrix3 Rmat = R_.matrix();
|
||||
const Vector3 v_vec = v_;
|
||||
const Vector3 r_minus_tv = Vector3(r_) - t_ * v_;
|
||||
|
||||
Matrix10 Ad = Matrix10::Zero();
|
||||
|
||||
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;
|
||||
Ad.block<3,1>(0,9) = v_vec;
|
||||
|
||||
Ad.block<3,3>(3,3) = Rmat;
|
||||
Ad.block<3,3>(3,6) = skewSymmetric(v_vec) * Rmat;
|
||||
|
||||
Ad.block<3,3>(6,6) = Rmat;
|
||||
|
||||
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();
|
||||
Vector10 y = Ad * xi;
|
||||
|
||||
if (H_xi) {
|
||||
*H_xi = Ad;
|
||||
}
|
||||
|
||||
if (H_g) {
|
||||
// NOTE: Using numerical derivative for the Jacobian with respect to
|
||||
// the group element instead of deriving the analytical expression.
|
||||
// Future work to use analytical instead.
|
||||
std::function<Vector10(const Gal3&, const Vector10&)> adjoint_action_wrt_g =
|
||||
[&](const Gal3& g_in, const Vector10& xi_in) {
|
||||
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) {
|
||||
// Implements adjoint representation as in Equation 28, Page 10
|
||||
const Matrix3 Theta_hat = skewSymmetric(theta(xi));
|
||||
const Matrix3 Nu_hat = skewSymmetric(nu(xi));
|
||||
const Matrix3 Rho_hat = skewSymmetric(rho(xi));
|
||||
const double t_val = t_tan(xi)(0);
|
||||
const Vector3 nu_vec = nu(xi);
|
||||
|
||||
Matrix10 ad = Matrix10::Zero();
|
||||
|
||||
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;
|
||||
|
||||
ad.block<3,3>(3,3) = Theta_hat;
|
||||
ad.block<3,3>(3,6) = Nu_hat;
|
||||
|
||||
ad.block<3,3>(6,6) = Theta_hat;
|
||||
|
||||
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;
|
||||
if (Hxi) {
|
||||
*Hxi = -adjointMap(y);
|
||||
}
|
||||
return ad_xi * y;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) {
|
||||
// Related to the left Jacobian in Equations 31-36, Pages 10-11
|
||||
// NOTE: Using numerical approximation instead of implementing the analytical
|
||||
// expression for the Jacobian. Future work to replace this
|
||||
// with analytical derivative.
|
||||
if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity();
|
||||
std::function<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) {
|
||||
// Related to the inverse of left Jacobian in Equations 31-36, Pages 10-11
|
||||
// NOTE: Using numerical approximation instead of implementing the analytical
|
||||
// expression for the inverse Jacobian. Future work to replace this
|
||||
// with analytical derivative.
|
||||
Vector10 xi = Gal3::Logmap(g);
|
||||
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)
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix5 Gal3::Hat(const Vector10& xi) {
|
||||
// Implements hat operator as in Equation 13, Page 6
|
||||
const Vector3 rho_tan = rho(xi);
|
||||
const Vector3 nu_tan = nu(xi);
|
||||
const Vector3 theta_tan = theta(xi);
|
||||
const double t_tan_val = t_tan(xi)(0);
|
||||
|
||||
Matrix5 X = Matrix5::Zero();
|
||||
X.block<3, 3>(0, 0) = skewSymmetric(theta_tan);
|
||||
X.block<3, 1>(0, 3) = nu_tan;
|
||||
X.block<3, 1>(0, 4) = rho_tan;
|
||||
X(3, 4) = t_tan_val;
|
||||
return X;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::Vee(const Matrix5& X) {
|
||||
// Implements vee operator (inverse of hat operator in Equation 13, Page 6)
|
||||
if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9 || std::abs(X(3,3)) > 1e-9) {
|
||||
throw std::invalid_argument("Matrix is not in sgal(3)");
|
||||
}
|
||||
|
||||
Vector10 xi;
|
||||
rho(xi) = X.block<3, 1>(0, 4);
|
||||
nu(xi) = X.block<3, 1>(0, 3);
|
||||
const Matrix3& S = X.block<3, 3>(0, 0);
|
||||
theta(xi) << S(2, 1), S(0, 2), S(1, 0);
|
||||
t_tan(xi)(0) = X(3, 4);
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// ChartAtOrigin
|
||||
//------------------------------------------------------------------------------
|
||||
Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) {
|
||||
return Gal3::Expmap(xi, Hxi);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) {
|
||||
return Gal3::Logmap(g, Hg);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Event Gal3::act(const Event& e, OptionalJacobian<4, 10> Hself,
|
||||
OptionalJacobian<4, 4> He) const {
|
||||
// Implements group action on events (spacetime points) as described in Section 4.1, Page 3-4
|
||||
const double& t_in = e.time();
|
||||
const Point3& p_in = e.location();
|
||||
|
||||
const double t_out = t_in + t_;
|
||||
const Point3 p_out = R_.rotate(p_in) + v_ * t_in + r_;
|
||||
|
||||
if (He) {
|
||||
He->setZero();
|
||||
(*He)(0, 0) = 1.0;
|
||||
He->block<3, 1>(1, 0) = v_;
|
||||
He->block<3, 3>(1, 1) = R_.matrix();
|
||||
}
|
||||
|
||||
if (Hself) {
|
||||
Hself->setZero();
|
||||
const Matrix3 Rmat = R_.matrix();
|
||||
|
||||
(*Hself)(0, 9) = 1.0;
|
||||
Hself->block<3, 3>(1, 0) = Rmat;
|
||||
Hself->block<3, 3>(1, 3) = Rmat * t_in;
|
||||
Hself->block<3, 3>(1, 6) = -Rmat * skewSymmetric(p_in);
|
||||
Hself->block<3, 1>(1, 9) = v_;
|
||||
}
|
||||
|
||||
return Event(t_out, p_out);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,235 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* See LICENSE for the license information
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Gal3.h
|
||||
* @brief 3D Galilean Group SGal(3) state (attitude, position, velocity, time)
|
||||
* @authors Matt Kielo, Scott Baker, Frank Dellaert
|
||||
* @date April 30, 2025
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h> // Includes Rot3, Point3
|
||||
#include <gtsam/geometry/Event.h>
|
||||
#include <gtsam/base/Lie.h> // For LieGroup base class and traits
|
||||
#include <gtsam/base/Manifold.h> // For Manifold traits
|
||||
|
||||
#include <cmath> // For std::sqrt, std::cos, std::sin
|
||||
#include <functional> // For std::function used in numerical derivatives
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration
|
||||
class Gal3;
|
||||
|
||||
// Use Vector3 for velocity for consistency with NavState
|
||||
using Velocity3 = Vector3;
|
||||
// Define Vector10 for tangent space
|
||||
using Vector10 = Eigen::Matrix<double, 10, 1>;
|
||||
// Define Matrix5 for Lie Algebra matrix representation
|
||||
using Matrix5 = Eigen::Matrix<double, 5, 5>;
|
||||
// Define Matrix10 for Jacobians
|
||||
using Matrix10 = Eigen::Matrix<double, 10, 10>;
|
||||
|
||||
/**
|
||||
* Represents an element of the 3D Galilean group SGal(3).
|
||||
* Internal state: rotation, translation, velocity, time.
|
||||
*/
|
||||
class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
|
||||
private:
|
||||
Rot3 R_; ///< Rotation world R body
|
||||
Point3 r_; ///< Position in world frame, n_r_b
|
||||
Velocity3 v_; ///< Velocity in world frame, n_v_b
|
||||
double t_; ///< Time component
|
||||
|
||||
public:
|
||||
|
||||
/// The dimension of the tangent space
|
||||
inline static constexpr size_t dimension = 10;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor: Identity element
|
||||
Gal3() : R_(Rot3::Identity()), r_(Point3::Zero()), v_(Velocity3::Zero()), t_(0.0) {}
|
||||
|
||||
/// Construct from attitude, position, velocity, time
|
||||
Gal3(const Rot3& R, const Point3& r, const Velocity3& v, double t) :
|
||||
R_(R), r_(r), v_(v), t_(t) {}
|
||||
|
||||
/// Construct from a 5x5 matrix representation
|
||||
explicit Gal3(const Matrix5& M);
|
||||
|
||||
/// Named constructor from components with derivatives
|
||||
static Gal3 Create(const Rot3& R, const Point3& r, const Velocity3& v, double t,
|
||||
OptionalJacobian<10, 3> H1 = {},
|
||||
OptionalJacobian<10, 3> H2 = {},
|
||||
OptionalJacobian<10, 3> H3 = {},
|
||||
OptionalJacobian<10, 1> H4 = {});
|
||||
|
||||
/// Named constructor from Pose3, velocity, and time with derivatives
|
||||
static Gal3 FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t,
|
||||
OptionalJacobian<10, 6> H1 = {},
|
||||
OptionalJacobian<10, 3> H2 = {},
|
||||
OptionalJacobian<10, 1> H3 = {});
|
||||
|
||||
/// @}
|
||||
/// @name Component Access
|
||||
/// @{
|
||||
|
||||
/// Access rotation component (Rot3)
|
||||
const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;
|
||||
|
||||
/// Access translation component (Point3)
|
||||
const Point3& translation(OptionalJacobian<3, 10> H = {}) const;
|
||||
|
||||
/// Access velocity component (Vector3)
|
||||
const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;
|
||||
|
||||
/// Access time component (double)
|
||||
const double& time(OptionalJacobian<1, 10> H = {}) const;
|
||||
|
||||
// Convenience accessors matching NavState names
|
||||
const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const { return rotation(H); }
|
||||
const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); }
|
||||
|
||||
/// @}
|
||||
/// @name Derived quantities
|
||||
/// @{
|
||||
|
||||
/// Return rotation matrix (Matrix3)
|
||||
Matrix3 R() const { return R_.matrix(); }
|
||||
|
||||
/// Return position as Vector3
|
||||
Vector3 r() const { return Vector3(r_); } // Conversion from Point3
|
||||
|
||||
/// Return velocity as Vector3
|
||||
const Vector3& v() const { return v_; }
|
||||
|
||||
/// Return time scalar
|
||||
const double& t() const { return t_; }
|
||||
|
||||
/// Return 5x5 homogeneous matrix representation
|
||||
Matrix5 matrix() const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT
|
||||
friend std::ostream &operator<<(std::ostream &os, const Gal3& state);
|
||||
|
||||
/// Print with optional string prefix
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// Check equality within tolerance
|
||||
bool equals(const Gal3& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// Return the identity element
|
||||
static Gal3 Identity() { return Gal3(); }
|
||||
|
||||
/// Return the inverse of this element
|
||||
Gal3 inverse() const;
|
||||
|
||||
// Bring LieGroup::inverse() into scope (version with derivative)
|
||||
using LieGroup<Gal3, 10>::inverse;
|
||||
|
||||
/// Group composition operator
|
||||
Gal3 operator*(const Gal3& other) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group Action
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Apply Galilean transform to a spacetime Event
|
||||
* @param e Input event (location, time)
|
||||
* @param Hself Optional Jacobian wrt this Gal3 element's tangent space
|
||||
* @param He Optional Jacobian wrt the input event's tangent space
|
||||
* @return Transformed event
|
||||
*/
|
||||
Event act(const Event& e, OptionalJacobian<4, 10> Hself = {},
|
||||
OptionalJacobian<4, 4> He = {}) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group Static Functions
|
||||
/// @{
|
||||
|
||||
/// Exponential map at identity: tangent vector xi -> manifold element g
|
||||
static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {});
|
||||
|
||||
/// Logarithmic map at identity: manifold element g -> tangent vector xi
|
||||
static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
|
||||
|
||||
/// Calculate Adjoint map Ad_g
|
||||
Matrix10 AdjointMap() const;
|
||||
|
||||
/// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at identity
|
||||
Vector10 Adjoint(const Vector10& xi_base, OptionalJacobian<10, 10> H_g = {},
|
||||
OptionalJacobian<10, 10> H_xi = {}) const;
|
||||
|
||||
/// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y`
|
||||
static Vector10 adjoint(const Vector10& xi, const Vector10& y,
|
||||
OptionalJacobian<10, 10> Hxi = {},
|
||||
OptionalJacobian<10, 10> Hy = {});
|
||||
|
||||
/// Compute the adjoint map `ad(xi)` associated with tangent vector xi
|
||||
static Matrix10 adjointMap(const Vector10& xi);
|
||||
|
||||
/// Derivative of Expmap(xi) w.r.t. xi evaluated at xi
|
||||
static Matrix10 ExpmapDerivative(const Vector10& xi);
|
||||
|
||||
/// Derivative of Logmap(g) w.r.t. g
|
||||
static Matrix10 LogmapDerivative(const Gal3& g);
|
||||
|
||||
/// Chart at origin, uses Expmap/Logmap for Retract/Local
|
||||
struct ChartAtOrigin {
|
||||
static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {});
|
||||
static Vector10 Local(const Gal3& g, ChartJacobian Hg = {});
|
||||
};
|
||||
|
||||
/// Hat operator: maps tangent vector xi to Lie algebra matrix
|
||||
static Matrix5 Hat(const Vector10& xi);
|
||||
|
||||
/// Vee operator: maps Lie algebra matrix to tangent vector xi
|
||||
static Vector10 Vee(const Matrix5& X);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/// @name Serialization
|
||||
/// @{
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(r_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
}
|
||||
#endif
|
||||
/// @}
|
||||
|
||||
}; // class Gal3
|
||||
|
||||
/// Traits specialization for Gal3
|
||||
template <>
|
||||
struct traits<Gal3> : public internal::LieGroup<Gal3> {};
|
||||
|
||||
template <>
|
||||
struct traits<const Gal3> : public internal::LieGroup<Gal3> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1515,6 +1515,17 @@ gtsam::TriangulationResult triangulateSafe(
|
|||
const gtsam::TriangulationParameters& params);
|
||||
|
||||
|
||||
#include <gtsam/geometry/Event.h>
|
||||
class Event {
|
||||
Event();
|
||||
Event(double t, const gtsam::Point3& p);
|
||||
Event(double t, double x, double y, double z);
|
||||
double time() const;
|
||||
gtsam::Point3 location() const;
|
||||
double height() const;
|
||||
void print(string s) const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam/geometry/BearingRange.h>
|
||||
template <POSE, POINT, BEARING, RANGE>
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/nonlinear/Expression.h>
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
#include <gtsam/geometry/Event.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -21,7 +21,7 @@
|
|||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
#include <gtsam/geometry/Event.h>
|
||||
#include <gtsam_unstable/slam/TOAFactor.h>
|
||||
|
||||
#include <vector>
|
||||
|
|
|
|||
|
|
@ -379,17 +379,6 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
|
|||
|
||||
typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
|
||||
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
class Event {
|
||||
Event();
|
||||
Event(double t, const gtsam::Point3& p);
|
||||
Event(double t, double x, double y, double z);
|
||||
double time() const;
|
||||
gtsam::Point3 location() const;
|
||||
double height() const;
|
||||
void print(string s) const;
|
||||
};
|
||||
|
||||
class TimeOfArrival {
|
||||
TimeOfArrival();
|
||||
TimeOfArrival(double speed);
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
#include <gtsam/geometry/Event.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@
|
|||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
#include <gtsam/geometry/Event.h>
|
||||
#include <gtsam_unstable/slam/TOAFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
|
|||
|
|
@ -12,8 +12,9 @@ Author: Frank Dellaert
|
|||
# pylint: disable=invalid-name, no-name-in-module
|
||||
|
||||
from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
|
||||
NonlinearFactorGraph, Point3, Values, noiseModel)
|
||||
from gtsam_unstable import Event, TimeOfArrival, TOAFactor
|
||||
NonlinearFactorGraph, Point3, Values, noiseModel, Event,
|
||||
TimeOfArrival)
|
||||
from gtsam_unstable import TOAFactor
|
||||
|
||||
# units
|
||||
MS = 1e-3
|
||||
|
|
|
|||
Loading…
Reference in New Issue