Merge pull request #1930 from borglab/feature/NavStateGroup
Endow NavState with group operationsrelease/4.3a0
Frank Dellaert
GitHub
commit
bb7b6b39c7
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue