Merge pull request #1930 from borglab/feature/NavStateGroup

Endow NavState with group operations
release/4.3a0
Frank Dellaert 2024-12-15 13:24:33 -05:00 committed by GitHub
commit bb7b6b39c7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 780 additions and 101 deletions

View File

@ -21,13 +21,10 @@
#include <cmath>
#include <iostream>
#include <limits>
#include <string>
namespace gtsam {
using std::vector;
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3)
@ -167,21 +164,23 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
if (Hxi) *Hxi = ExpmapDerivative(xi);
// Get angular velocity omega and translational velocity v from twist xi
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
// get angular velocity omega and translational velocity v from twist xi
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
// Compute rotation using Expmap
Rot3 R = Rot3::Expmap(w);
Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Vector3 omega_cross_v = omega.cross(v); // points towards axis
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);
// Compute translation and optionally its Jacobian in w
Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
if (Hxi) {
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
*Hxi << Jw, Z_3x3,
Q, Jw;
}
return Pose3(R, t);
}
/* ************************************************************************* */
@ -240,55 +239,68 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
}
/* ************************************************************************* */
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
Matrix3 Q;
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w);
Matrix3 Q;
#ifdef NUMERICAL_EXPMAP_DERIV
Matrix3 Qj = Z_3x3;
double invFac = 1.0;
Q = Z_3x3;
Matrix3 Wj = I_3x3;
for (size_t j=1; j<10; ++j) {
Qj = Qj*W + Wj*V;
invFac = -invFac/(j+1);
Q = Q + invFac*Qj;
Wj = Wj*W;
}
#else
// The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm();
const Matrix3 WVW = W * V * W;
if (std::abs(phi) > nearZeroThreshold) {
const double s = sin(phi), c = cos(phi);
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
phi5 = phi4 * phi;
// Invert the sign of odd-order terms to have the right Jacobian
Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
(1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
(WVW * W + W * WVW);
} else {
Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
1. / 120. * (WVW * W + W * WVW);
}
#endif
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
}
/* ************************************************************************* */
Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q,
const std::optional<Rot3>& R,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);
if (Q) {
const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w);
const Matrix3 WVW = W * V * W;
const double theta = w.norm();
if (nearZero) {
static constexpr double one_sixth = 1. / 6.;
static constexpr double one_twenty_fourth = 1. / 24.;
static constexpr double one_one_hundred_twentieth = 1. / 120.;
*Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
one_one_hundred_twentieth * (WVW * W + W * WVW);
} else {
const double s = sin(theta), c = cos(theta);
const double theta3 = theta2 * theta, theta4 = theta3 * theta,
theta5 = theta4 * theta;
// Invert the sign of odd-order terms to have the right Jacobian
*Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
(1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
0.5 *
((1 - theta2 / 2 - c) / theta4 -
3 * (theta - s - theta3 / 6.) / theta5) *
(WVW * W + W * WVW);
}
}
// TODO(Frank): this threshold is *different*. Why?
if (nearZero) {
return v + 0.5 * w.cross(v);
} else {
Vector3 t_parallel = w * w.dot(v); // translation parallel to axis
Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis
Rot3 rotation = R.value_or(Rot3::Expmap(w));
Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2;
return t;
}
}
/* ************************************************************************* */
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
Matrix6 J;
J << Jw, Z_3x3, Q, Jw;
Expmap(xi, J);
return J;
}

View File

@ -217,10 +217,25 @@ public:
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
* This is the Jacobian of ExpmapTranslation and computed there.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
/**
* Compute the translation part of the exponential map, with derivative.
* @param w 3D angular velocity
* @param v 3D velocity
* @param Q Optionally, compute 3x3 Jacobian wrpt w
* @param R Optionally, precomputed as Rot3::Expmap(w)
* @param nearZeroThreshold threshold for small values
* Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix
*/
static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q = {},
const std::optional<Rot3>& R = {},
double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

View File

@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
}
//------------------------------------------------------------------------------
Matrix7 NavState::matrix() const {
Matrix5 NavState::matrix() const {
Matrix3 R = this->R();
Matrix7 T;
T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
Matrix5 T = Matrix5::Identity();
T.block<3, 3>(0, 0) = R;
T.block<3, 1>(0, 3) = t_;
T.block<3, 1>(0, 4) = v_;
return T;
}
@ -103,6 +106,161 @@ bool NavState::equals(const NavState& other, double tol) const {
&& equal_with_abs_tol(v_, other.v_, tol);
}
//------------------------------------------------------------------------------
NavState NavState::inverse() const {
Rot3 Rt = R_.inverse();
return NavState(Rt, Rt * (-t_), Rt * -(v_));
}
//------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
// Get angular velocity w, translational velocity v, and acceleration a
Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
// Compute rotation using Expmap
Rot3 R = Rot3::Expmap(w);
// Compute translations and optionally their Jacobians
Matrix3 Qt, Qv;
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R);
if (Hxi) {
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
*Hxi << Jw, Z_3x3, Z_3x3,
Qt, Jw, Z_3x3,
Qv, Z_3x3, Jw;
}
return NavState(R, t, v);
}
//------------------------------------------------------------------------------
Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) {
if (Hstate) *Hstate = LogmapDerivative(state);
const Vector3 phi = Rot3::Logmap(state.rotation());
const Vector3& p = state.position();
const Vector3& v = state.velocity();
const double t = phi.norm();
if (t < 1e-8) {
Vector9 log;
log << phi, p, v;
return log;
} else {
const Matrix3 W = skewSymmetric(phi / t);
const double Tan = tan(0.5 * t);
const Vector3 Wp = W * p;
const Vector3 Wv = W * v;
const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp);
const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv);
Vector9 log;
// Order is ω, p, v
log << phi, rho, nu;
return log;
}
}
//------------------------------------------------------------------------------
Matrix9 NavState::AdjointMap() const {
const Matrix3 R = R_.matrix();
Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R;
// Eqn 2 in Barrau20icra
Matrix9 adj;
adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R;
return adj;
}
//------------------------------------------------------------------------------
Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
OptionalJacobian<9, 9> H_xib) const {
const Matrix9 Ad = AdjointMap();
// Jacobians
if (H_state) *H_state = -Ad * adjointMap(xi_b);
if (H_xib) *H_xib = Ad;
return Ad * xi_b;
}
//------------------------------------------------------------------------------
Matrix9 NavState::adjointMap(const Vector9& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8));
Matrix9 adj;
adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat;
return adj;
}
//------------------------------------------------------------------------------
Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
OptionalJacobian<9, 9> Hxi,
OptionalJacobian<9, 9> H_y) {
if (Hxi) {
Hxi->setZero();
for (int i = 0; i < 9; ++i) {
Vector9 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix9 Gi = adjointMap(dxi);
Hxi->col(i) = Gi * y;
}
}
const Matrix9& ad_xi = adjointMap(xi);
if (H_y) *H_y = ad_xi;
return ad_xi * y;
}
//------------------------------------------------------------------------------
Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
Matrix9 J;
Expmap(xi, J);
return J;
}
//------------------------------------------------------------------------------
Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector9 xi = Logmap(state);
const Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt, R);
Pose3::ExpmapTranslation(w, nu, Qv, R);
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw;
Matrix9 J;
J << Jw, Z_3x3, Z_3x3,
Qt2, Jw, Z_3x3,
Qv2, Z_3x3, Jw;
return J;
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
ChartJacobian Hxi) {
return Expmap(xi, Hxi);
}
//------------------------------------------------------------------------------
Vector9 NavState::ChartAtOrigin::Local(const NavState& state,
ChartJacobian Hstate) {
return Logmap(state, Hstate);
}
//------------------------------------------------------------------------------
NavState NavState::retract(const Vector9& xi, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
@ -142,15 +300,16 @@ Vector9 NavState::localCoordinates(const NavState& g, //
Matrix3 D_xi_R;
xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
if (H1) {
*H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
D_dt_R, -I_3x3, Z_3x3, //
D_dv_R, Z_3x3, -I_3x3;
*H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
D_dt_R, -I_3x3, Z_3x3, //
D_dv_R, Z_3x3, -I_3x3;
}
if (H2) {
*H2 << D_xi_R, Z_3x3, Z_3x3, //
Z_3x3, dR.matrix(), Z_3x3, //
Z_3x3, Z_3x3, dR.matrix();
*H2 << D_xi_R, Z_3x3, Z_3x3, //
Z_3x3, dR.matrix(), Z_3x3, //
Z_3x3, Z_3x3, dR.matrix();
}
return xi;
}
@ -213,7 +372,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
//------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
OptionalJacobian<9, 9> H) const {
auto [nRb, n_t, n_v] = (*this);
Rot3 nRb = R_;
Point3 n_t = t_, n_v = v_;
const double dt2 = dt * dt;
const Vector3 omega_cross_vel = omega.cross(n_v);

View File

@ -12,7 +12,7 @@
/**
* @file NavState.h
* @brief Navigation state composing of attitude, position, and velocity
* @author Frank Dellaert
* @authors Frank Dellaert, Varun Agrawal, Fan Jiang
* @date July 2015
**/
@ -25,14 +25,18 @@
namespace gtsam {
/// Velocity is currently typedef'd to Vector3
typedef Vector3 Velocity3;
using Velocity3 = Vector3;
/**
* Navigation state: Pose (rotation, translation) + velocity
* NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
* Following Barrau20icra, this class belongs to the Lie group SE_2(3).
* This group is also called "double direct isometries”.
*
* NOTE: While Barrau20icra follow a R,v,t order,
* we use a R,t,v order to maintain backwards compatibility.
*/
class GTSAM_EXPORT NavState {
private:
class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
private:
// TODO(frank):
// - should we rename t_ to p_? if not, we should rename dP do dT
@ -44,7 +48,6 @@ public:
inline constexpr static auto dimension = 9;
typedef std::pair<Point3, Velocity3> PositionAndVelocity;
/// @name Constructors
/// @{
@ -67,11 +70,14 @@ public:
}
/// Named constructor with derivatives
static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 3> H3);
OptionalJacobian<9, 3> H1 = {},
OptionalJacobian<9, 3> H2 = {},
OptionalJacobian<9, 3> H3 = {});
/// Named constructor with derivatives
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
OptionalJacobian<9, 6> H1 = {},
OptionalJacobian<9, 3> H2 = {});
/// @}
/// @name Component Access
@ -109,9 +115,8 @@ public:
Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;
/// Return matrix group representation, in MATLAB notation:
/// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
/// With this embedding in GL(3), matrix product agrees with compose
Matrix7 matrix() const;
/// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1]
Matrix5 matrix() const;
/// @}
/// @name Testable
@ -128,7 +133,29 @@ public:
bool equals(const NavState& other, double tol = 1e-8) const;
/// @}
/// @name Manifold
/// @name Group
/// @{
/// identity for group operation
static NavState Identity() {
return NavState();
}
/// inverse transformation with derivatives
NavState inverse() const;
using LieGroup<NavState, 9>::inverse; // version with derivative
/// compose syntactic sugar
NavState operator*(const NavState& T) const {
return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_);
}
/// Syntactic sugar
const Rot3& rotation() const { return attitude(); };
/// @}
/// @name Lie Group
/// @{
// Tangent space sugar.
@ -162,6 +189,59 @@ public:
OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
{}) const;
/**
* Exponential map at identity - create a NavState from canonical coordinates
* \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$
*/
static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {});
/**
* Log map at identity - return the canonical coordinates \f$
* [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState
*/
static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {});
/**
* Calculate Adjoint map, transforming a twist in this pose's (i.e, body)
* frame to the world spatial frame.
*/
Matrix9 AdjointMap() const;
/**
* Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
* body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
* Note that H_xib = AdjointMap()
*/
Vector9 Adjoint(const Vector9& xi_b,
OptionalJacobian<9, 9> H_this = {},
OptionalJacobian<9, 9> H_xib = {}) const;
/**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
* but for the NavState [ad(w,v)] = [w^, zero3; v^, w^]
*/
static Matrix9 adjointMap(const Vector9& xi);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector9 adjoint(const Vector9& xi, const Vector9& y,
OptionalJacobian<9, 9> Hxi = {},
OptionalJacobian<9, 9> H_y = {});
/// Derivative of Expmap
static Matrix9 ExpmapDerivative(const Vector9& xi);
/// Derivative of Logmap
static Matrix9 LogmapDerivative(const NavState& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct GTSAM_EXPORT ChartAtOrigin {
static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {});
static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
};
/// @}
/// @name Dynamics
/// @{
@ -169,8 +249,9 @@ public:
/// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const;
const double dt, OptionalJacobian<9, 9> F = {},
OptionalJacobian<9, 3> G1 = {},
OptionalJacobian<9, 3> G2 = {}) const;
/// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
@ -201,8 +282,10 @@ private:
};
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<>
struct traits<NavState> : internal::Manifold<NavState> {
};
template <>
struct traits<NavState> : public internal::LieGroup<NavState> {};
template <>
struct traits<const NavState> : public internal::LieGroup<NavState> {};
} // namespace gtsam

View File

@ -12,13 +12,16 @@
/**
* @file testNavState.cpp
* @brief Unit tests for NavState
* @author Frank Dellaert
* @authors Frank Dellaert, Varun Agrawal, Fan Jiang
* @date July 2015
*/
#include <gtsam/navigation/NavState.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
@ -26,6 +29,9 @@ using namespace std::placeholders;
using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(NavState)
GTSAM_CONCEPT_LIE_INST(NavState)
static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
static const Point3 kPosition(1.0, 2.0, 3.0);
static const Pose3 kPose(kAttitude, kPosition);
@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
static const Vector3 kGravity(0, 0, 9.81);
static const Vector9 kZeroXi = Vector9::Zero();
static const Point3 V(3, 0.4, -2.2);
static const Point3 P(0.2, 0.7, -2);
static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
static const Point3 V2(-6.5, 3.5, 6.2);
static const Point3 P2(3.5, -8.2, 4.2);
static const NavState T(R, P2, V2);
static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2);
static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7),
Point3(1, 2, 3));
/* ************************************************************************* */
TEST(NavState, Constructor) {
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
@ -46,14 +62,14 @@ TEST(NavState, Constructor) {
assert_equal(kState1,
NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
EXPECT(
assert_equal(
numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
assert_equal(
numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
EXPECT(
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
EXPECT(
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
}
/* ************************************************************************* */
@ -64,7 +80,7 @@ TEST(NavState, Constructor2) {
Matrix aH1, aH2;
EXPECT(
assert_equal(kState1,
NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
}
@ -127,8 +143,8 @@ TEST( NavState, Manifold ) {
Point3 dt = Point3(xi.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
NavState state2 = NavState(kState1.attitude() * drot,
kState1.position() + kState1.attitude() * dt,
kState1.velocity() + kState1.attitude() * dvel);
kState1.position() + kState1.attitude() * dt,
kState1.velocity() + kState1.attitude() * dvel);
EXPECT(assert_equal(state2, kState1.retract(xi)));
EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
@ -169,6 +185,143 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
}
/* ************************************************************************* */
TEST(NavState, Equals) {
NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3));
NavState pose2 = T3;
EXPECT(T3.equals(pose2));
NavState origin;
EXPECT(!T3.equals(origin));
}
/* ************************************************************************* */
TEST(NavState, Compose) {
NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
{3.0, -1.0, 1.0});
auto ab_c = (nav_state_a * nav_state_b) * nav_state_c;
auto a_bc = nav_state_a * (nav_state_b * nav_state_c);
CHECK(assert_equal(ab_c, a_bc));
Matrix actual = (T2 * T2).matrix();
Matrix expected = T2.matrix() * T2.matrix();
EXPECT(assert_equal(actual, expected, 1e-8));
Matrix actualDcompose1, actualDcompose2;
T2.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 =
numericalDerivative21(testing::compose<NavState>, T2, T2);
EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
Matrix numericalH2 =
numericalDerivative22(testing::compose<NavState>, T2, T2);
EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4));
}
/* ************************************************************************* */
// Check compose and its push-forward, another case
TEST(NavState, Compose2) {
const NavState& T1 = T;
Matrix actual = (T1 * T2).matrix();
Matrix expected = T1.matrix() * T2.matrix();
EXPECT(assert_equal(actual, expected, 1e-8));
Matrix actualDcompose1, actualDcompose2;
T1.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 =
numericalDerivative21(testing::compose<NavState>, T1, T2);
EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
Matrix numericalH2 =
numericalDerivative22(testing::compose<NavState>, T1, T2);
EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5));
}
/* ************************************************************************* */
TEST(NavState, Inverse) {
NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
{3.0, -1.0, 1.0});
auto a_inv = nav_state_a.inverse();
auto a_a_inv = nav_state_a * a_inv;
CHECK(assert_equal(a_a_inv, NavState()));
auto b_inv = nav_state_b.inverse();
auto b_b_inv = nav_state_b * b_inv;
CHECK(assert_equal(b_b_inv, NavState()));
Matrix actualDinverse;
Matrix actual = T.inverse(actualDinverse).matrix();
Matrix expected = T.matrix().inverse();
EXPECT(assert_equal(actual, expected, 1e-8));
Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
}
/* ************************************************************************* */
TEST(NavState, InverseDerivatives) {
Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5);
Vector3 v(3.5, -8.2, 4.2);
Point3 p(3.5, -8.2, 4.2);
NavState T(R, p, v);
Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
Matrix actualDinverse;
T.inverse(actualDinverse);
EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
}
/* ************************************************************************* */
TEST(NavState, Compose_Inverse) {
NavState actual = T * T.inverse();
NavState expected;
EXPECT(assert_equal(actual, expected, 1e-8));
}
/* ************************************************************************* */
TEST(NavState, Between) {
NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0));
NavState actual = s1.compose(s2);
EXPECT(assert_equal(s2, actual));
NavState between = s2.between(s1);
NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0));
EXPECT(assert_equal(expected_between, between));
NavState expected = T2.inverse() * T3;
Matrix actualDBetween1, actualDBetween2;
actual = T2.between(T3, actualDBetween1, actualDBetween2);
EXPECT(assert_equal(expected, actual));
Matrix numericalH1 =
numericalDerivative21(testing::between<NavState>, T2, T3);
EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3));
Matrix numericalH2 =
numericalDerivative22(testing::between<NavState>, T2, T3);
EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5));
}
/* ************************************************************************* */
TEST(NavState, interpolate) {
EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0)));
EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0)));
}
/* ************************************************************************* */
static const double dt = 2.0;
std::function<Vector9(const NavState&, const bool&)> coriolis =
@ -189,7 +342,7 @@ TEST(NavState, Coriolis) {
TEST(NavState, Coriolis2) {
Matrix9 aH;
NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
// first-order
state2.coriolis(dt, kOmegaCoriolis, false, aH);
@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) {
}
TEST(NavState, Coriolis3) {
/** Consider a massless planet with an attached nav frame at
* n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with
/** Consider a massless planet with an attached nav frame at
* n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with
* velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
* aligned with the nav frame (i.e., nRb != I_3x3). Test that first and
* aligned with the nav frame (i.e., nRb != I_3x3). Test that first and
* second order Coriolis corrections are as expected.
*/
@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) {
bRn = nRb.inverse();
// Get expected first and second order corrections in the nav frame
Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
n_dV1e = dt * n_aCorr1,
n_dV1e = dt * n_aCorr1,
n_dV2e = dt * (n_aCorr1 + n_aCorr2);
// Get expected first and second order corrections in the body frame
@ -271,6 +424,262 @@ TEST(NavState, Stream)
EXPECT(os.str() == expected);
}
/* ************************************************************************* */
TEST(NavState, Print) {
NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3));
// Generate the expected output
std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
std::string p = "p: 1 2 3\n";
std::string v = "v: 1 2 3\n";
std::string expected = R + p + v;
EXPECT(assert_print_equal(expected, state));
}
/* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST(NavState, Retract_first_order) {
NavState id;
Vector v = Z_9x1;
v(0) = 0.3;
EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)),
id.retract(v), 1e-2));
v(3) = 0.2;
v(4) = 0.7;
v(5) = -2;
v(6) = 3;
v(7) = 0.4;
v(8) = -2.2;
EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2));
}
#endif
/* ************************************************************************* */
TEST(NavState, RetractExpmap) {
Vector xi = Z_9x1;
xi(0) = 0.3;
NavState pose = NavState::Expmap(xi),
expected(R, Point3(0, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expected, pose, 1e-2));
EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2));
}
/* ************************************************************************* */
TEST(NavState, Expmap_A_Full) {
NavState id;
Vector xi = Z_9x1;
xi(0) = 0.3;
EXPECT(assert_equal(expmap_default<NavState>(id, xi),
NavState(R, Point3(0, 0, 0), Point3(0, 0, 0))));
xi(3) = -0.2;
xi(4) = -0.394742;
xi(5) = 2.08998;
xi(6) = 0.2;
xi(7) = 0.394742;
xi(8) = -2.08998;
NavState expected(R, -P, P);
EXPECT(assert_equal(expected, expmap_default<NavState>(id, xi), 1e-5));
}
/* ************************************************************************* */
TEST(NavState, Expmap_b) {
NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0));
NavState p2 = p1.retract(
(Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0),
Point3(100.0, 0.0, 0.0));
EXPECT(assert_equal(expected, p2));
}
/* ************************************************************************* */
// test case for screw motion in the plane
namespace screwNavState {
double a = 0.3, c = cos(a), s = sin(a), w = 0.3;
Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished();
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
Point3 expectedV(0.29552, 0.0446635, 1);
Point3 expectedP(0.29552, 0.0446635, 1);
NavState expected(expectedR, expectedV, expectedP);
} // namespace screwNavState
/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(NavState, Adjoint_full) {
NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse();
Vector xiPrime = T.Adjoint(screwNavState::xi);
EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6));
NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse();
Vector xiPrime2 = T2.Adjoint(screwNavState::xi);
EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6));
NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse();
Vector xiPrime3 = T3.Adjoint(screwNavState::xi);
EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6));
}
/* ************************************************************************* */
TEST(NavState, Adjoint_compose_full) {
// To debug derivatives of compose, assert that
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const NavState& T1 = T;
Vector x =
(Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished();
NavState expected = T1 * NavState::Expmap(x) * T2;
Vector y = T2.inverse().Adjoint(x);
NavState actual = T1 * T2 * NavState::Expmap(y);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* ************************************************************************* */
TEST(NavState, Retract_LocalCoordinates) {
Vector9 d;
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
d /= 10;
const Rot3 R = Rot3::Retract(d.head<3>());
NavState t = NavState::Retract(d);
EXPECT(assert_equal(d, NavState::LocalCoordinates(t)));
}
/* ************************************************************************* */
TEST(NavState, retract_localCoordinates) {
Vector9 d12;
d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
d12 /= 10;
NavState t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
/* ************************************************************************* */
TEST(NavState, expmap_logmap) {
Vector d12 = Vector9::Constant(0.1);
NavState t1 = T, t2 = t1.expmap(d12);
EXPECT(assert_equal(d12, t1.logmap(t2)));
}
/* ************************************************************************* */
TEST(NavState, retract_localCoordinates2) {
NavState t1 = T;
NavState t2 = T3;
NavState origin;
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
// NOTE(FRANK): d12 !== -d21 for arbitrary retractions.
}
/* ************************************************************************* */
TEST(NavState, manifold_expmap) {
NavState t1 = T;
NavState t2 = T3;
NavState origin;
Vector d12 = t1.logmap(t2);
EXPECT(assert_equal(t2, t1.expmap(d12)));
Vector d21 = t2.logmap(t1);
EXPECT(assert_equal(t1, t2.expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
EXPECT(assert_equal(d12, -d21));
}
/* ************************************************************************* */
TEST(NavState, subgroups) {
// Frank - Below only works for correct "Agrawal06iros style expmap
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d =
(Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
// exp(-d)=inverse(exp(d))
EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
NavState T2 = NavState::Expmap(2 * d);
NavState T3 = NavState::Expmap(3 * d);
NavState T5 = NavState::Expmap(5 * d);
EXPECT(assert_equal(T5, T2 * T3));
EXPECT(assert_equal(T5, T3 * T2));
}
/* ************************************************************************* */
TEST(NavState, adjointMap) {
Matrix res = NavState::adjointMap(screwNavState::xi);
Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1),
screwNavState::xi(2));
Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4),
screwNavState::xi(5));
Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7),
screwNavState::xi(8));
Matrix9 expected;
expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh;
EXPECT(assert_equal(expected, res, 1e-5));
}
/* ************************************************************************* */
TEST(NavState, ExpmapDerivative1) {
Matrix9 actualH;
Vector9 w;
w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
NavState::Expmap(w, actualH);
std::function<NavState(const Vector9&)> f = [](const Vector9& w) {
return NavState::Expmap(w);
};
Matrix expectedH =
numericalDerivative21<NavState, Vector9, OptionalJacobian<9, 9> >(
&NavState::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST(NavState, LogmapDerivative) {
Matrix9 actualH;
Vector9 w;
w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
NavState p = NavState::Expmap(w);
EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5));
std::function<Vector9(const NavState&)> f = [](const NavState& p) {
return NavState::Logmap(p);
};
Matrix expectedH =
numericalDerivative21<Vector9, NavState, OptionalJacobian<9, 9> >(
&NavState::Logmap, p, {});
EXPECT(assert_equal(expectedH, actualH));
}
//******************************************************************************
TEST(NavState, Invariants) {
NavState id;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T3));
EXPECT(check_group_invariants(T2, id));
EXPECT(check_group_invariants(T2, T3));
EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id, T3));
EXPECT(check_manifold_invariants(T2, id));
EXPECT(check_manifold_invariants(T2, T3));
}
//******************************************************************************
TEST(NavState, LieGroupDerivatives) {
NavState id;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
}
//******************************************************************************
TEST(NavState, ChartDerivatives) {
NavState id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3);
}
}
/* ************************************************************************* */
int main() {