Abandoned silly coercion of NavState into Lie group. It's just a manifold: it does not make sense to compose two NavStates, or take the inverse of a NavState.
dellaert
parent
6302a79533
commit
6bed20b28a
|
|
@ -24,6 +24,18 @@ namespace gtsam {
|
|||
|
||||
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v,
|
||||
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
|
||||
OptionalJacobian<9, 3> H3) {
|
||||
if (H1)
|
||||
*H1 << I_3x3, Z_3x3, Z_3x3;
|
||||
if (H2)
|
||||
*H2 << Z_3x3, R.transpose(), Z_3x3;
|
||||
if (H3)
|
||||
*H3 << Z_3x3, Z_3x3, R.transpose();
|
||||
return NavState(R, t, v);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
|
||||
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
|
||||
|
|
@ -94,145 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const {
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::inverse() const {
|
||||
NavState NavState::retract(const Vector9& xi, //
|
||||
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
|
||||
TIE(nRb, n_t, n_v, *this);
|
||||
const Rot3 bRn = nRb.inverse();
|
||||
return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::operator*(const NavState& bTc) const {
|
||||
TIE(nRb, n_t, n_v, *this);
|
||||
TIE(bRc, b_t, b_v, bTc);
|
||||
return NavState(nRb * bRc, n_t + nRb * b_t, n_v + nRb * b_v);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
//NavState NavState::retract(const Vector9& xi) const {
|
||||
// TIE(nRb, n_t, n_v, *this);
|
||||
// const Rot3 bRc = Rot3::Expmap(dR(xi));
|
||||
// return NavState(nRb * bRc, n_t + nRb * dP(xi), n_v + nRb * dV(xi));
|
||||
//}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState::PositionAndVelocity //
|
||||
NavState::operator*(const PositionAndVelocity& b_tv) const {
|
||||
TIE(nRb, n_t, n_v, *this);
|
||||
const Point3& b_t = b_tv.first;
|
||||
const Velocity3& b_v = b_tv.second;
|
||||
return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Point3 NavState::operator*(const Point3& b_t) const {
|
||||
return Point3(R_ * b_t + t_);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
|
||||
OptionalJacobian<9, 9> H) {
|
||||
Matrix3 D_R_xi;
|
||||
const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0);
|
||||
const Point3 p = Point3(dP(xi));
|
||||
const Vector v = dV(xi);
|
||||
const NavState result(R, p, v);
|
||||
if (H) {
|
||||
*H << D_R_xi, Z_3x3, Z_3x3, //
|
||||
Z_3x3, R.transpose(), Z_3x3, //
|
||||
Z_3x3, Z_3x3, R.transpose();
|
||||
Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
|
||||
const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
|
||||
const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
|
||||
const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
|
||||
const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
|
||||
if (H1) {
|
||||
*H1 << D_R_nRb, Z_3x3, Z_3x3, //
|
||||
// Note(frank): the derivative of n_t with respect to xi is nRb
|
||||
// We pre-multiply with nRc' to account for NavState::Create
|
||||
// Then we make use of the identity nRc' * nRb = bRc'
|
||||
nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3,
|
||||
// Similar reasoning for v:
|
||||
nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
|
||||
}
|
||||
return result;
|
||||
if (H2) {
|
||||
*H2 << D_bRc_xi, Z_3x3, Z_3x3, //
|
||||
Z_3x3, bRc.transpose(), Z_3x3, //
|
||||
Z_3x3, Z_3x3, bRc.transpose();
|
||||
}
|
||||
return NavState(nRc, t, v);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
|
||||
OptionalJacobian<9, 9> H) {
|
||||
Vector9 NavState::localCoordinates(const NavState& g, //
|
||||
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
|
||||
Matrix3 D_dR_R, D_dt_R, D_dv_R;
|
||||
const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
|
||||
const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
|
||||
const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_xi_R;
|
||||
xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v();
|
||||
if (H) {
|
||||
*H << D_xi_R, Z_3x3, Z_3x3, //
|
||||
Z_3x3, x.R(), Z_3x3, //
|
||||
Z_3x3, Z_3x3, x.R();
|
||||
xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, 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;
|
||||
}
|
||||
if (H2) {
|
||||
*H2 << D_xi_R, Z_3x3, Z_3x3, //
|
||||
Z_3x3, dR.matrix(), Z_3x3, //
|
||||
Z_3x3, Z_3x3, dR.matrix();
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
|
||||
if (H)
|
||||
throw runtime_error("NavState::Expmap derivative not implemented yet");
|
||||
|
||||
Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
|
||||
Eigen::Block<const Vector9, 3, 1> v = dP(xi);
|
||||
Eigen::Block<const Vector9, 3, 1> a = dV(xi);
|
||||
|
||||
// NOTE(frank): See Pose3::Expmap
|
||||
Rot3 nRb = Rot3::Expmap(n_omega_nb);
|
||||
double theta2 = n_omega_nb.dot(n_omega_nb);
|
||||
if (theta2 > numeric_limits<double>::epsilon()) {
|
||||
// Expmap implements a "screw" motion in the direction of n_omega_nb
|
||||
Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
|
||||
Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
|
||||
Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel)
|
||||
/ theta2;
|
||||
Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis
|
||||
Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis
|
||||
Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2;
|
||||
return NavState(nRb, n_t, n_v);
|
||||
} else {
|
||||
return NavState(nRb, Point3(v), a);
|
||||
}
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
|
||||
if (H)
|
||||
throw runtime_error("NavState::Logmap derivative not implemented yet");
|
||||
|
||||
TIE(nRb, n_p, n_v, nTb);
|
||||
Vector3 n_t = n_p;
|
||||
|
||||
// NOTE(frank): See Pose3::Logmap
|
||||
Vector9 xi;
|
||||
Vector3 n_omega_nb = Rot3::Logmap(nRb);
|
||||
double theta = n_omega_nb.norm();
|
||||
if (theta * theta <= numeric_limits<double>::epsilon()) {
|
||||
xi << n_omega_nb, n_t, n_v;
|
||||
} else {
|
||||
Matrix3 W = skewSymmetric(n_omega_nb / theta);
|
||||
// Formula from Agrawal06iros, equation (14)
|
||||
// simplified with Mathematica, and multiplying in n_t to avoid matrix math
|
||||
double factor = (1 - theta / (2. * tan(0.5 * theta)));
|
||||
Vector3 n_x = W * n_t;
|
||||
Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x);
|
||||
Vector3 n_y = W * n_v;
|
||||
Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y);
|
||||
xi << n_omega_nb, v, a;
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix9 NavState::AdjointMap() const {
|
||||
// NOTE(frank): See Pose3::AdjointMap
|
||||
const Matrix3 nRb = R();
|
||||
Matrix3 pAr = skewSymmetric(t()) * nRb;
|
||||
Matrix3 vAr = skewSymmetric(v()) * nRb;
|
||||
Matrix9 adj;
|
||||
// nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
|
||||
adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
|
||||
return adj;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix7 NavState::wedge(const Vector9& xi) {
|
||||
const Matrix3 Omega = skewSymmetric(dR(xi));
|
||||
Matrix7 T;
|
||||
T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0;
|
||||
return T;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// sugar for derivative blocks
|
||||
#define D_R_R(H) (H)->block<3,3>(0,0)
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/ProductLieGroup.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -29,9 +29,9 @@ typedef Vector3 Velocity3;
|
|||
|
||||
/**
|
||||
* Navigation state: Pose (rotation, translation) + velocity
|
||||
* Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity
|
||||
* NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
|
||||
*/
|
||||
class NavState: public LieGroup<NavState, 9> {
|
||||
class NavState {
|
||||
private:
|
||||
|
||||
// TODO(frank):
|
||||
|
|
@ -42,6 +42,10 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum {
|
||||
dimension = 9
|
||||
};
|
||||
|
||||
typedef std::pair<Point3, Velocity3> PositionAndVelocity;
|
||||
|
||||
/// @name Constructors
|
||||
|
|
@ -49,7 +53,7 @@ public:
|
|||
|
||||
/// Default constructor
|
||||
NavState() :
|
||||
t_(0,0,0), v_(Vector3::Zero()) {
|
||||
t_(0, 0, 0), v_(Vector3::Zero()) {
|
||||
}
|
||||
/// Construct from attitude, position, velocity
|
||||
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
|
||||
|
|
@ -59,15 +63,15 @@ public:
|
|||
NavState(const Pose3& pose, const Velocity3& v) :
|
||||
R_(pose.rotation()), t_(pose.translation()), v_(v) {
|
||||
}
|
||||
/// Construct from Matrix group representation (no checking)
|
||||
NavState(const Matrix7& T) :
|
||||
R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
|
||||
}
|
||||
/// Construct from SO(3) and R^6
|
||||
NavState(const Matrix3& R, const Vector9 tv) :
|
||||
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
|
||||
}
|
||||
/// 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);
|
||||
/// Named constructor with derivatives
|
||||
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
|
||||
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
|
||||
|
||||
|
|
@ -116,7 +120,8 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state);
|
||||
GTSAM_EXPORT
|
||||
friend std::ostream &operator<<(std::ostream &os, const NavState& state);
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
|
@ -124,29 +129,6 @@ public:
|
|||
/// equals
|
||||
bool equals(const NavState& other, double tol = 1e-8) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static NavState identity() {
|
||||
return NavState();
|
||||
}
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
NavState inverse() const;
|
||||
|
||||
/// Group compose is the semi-direct product as specified below:
|
||||
/// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v)
|
||||
NavState operator*(const NavState& bTc) const;
|
||||
|
||||
/// Native group action is on position/velocity pairs *in the body frame* as follows:
|
||||
/// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v)
|
||||
PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const;
|
||||
|
||||
/// Act on position alone, n_t = nRb * b_t + n_t
|
||||
Point3 operator*(const Point3& b_t) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
|
@ -172,32 +154,15 @@ public:
|
|||
return v.segment<3>(6);
|
||||
}
|
||||
|
||||
// Chart at origin, constructs components separately (as opposed to Expmap)
|
||||
struct ChartAtOrigin {
|
||||
static NavState Retract(const Vector9& xi, //
|
||||
OptionalJacobian<9, 9> H = boost::none);
|
||||
static Vector9 Local(const NavState& x, //
|
||||
OptionalJacobian<9, 9> H = boost::none);
|
||||
};
|
||||
/// retract with optional derivatives
|
||||
NavState retract(const Vector9& v, //
|
||||
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
|
||||
boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/// Exponential map at identity - create a NavState from canonical coordinates
|
||||
static NavState Expmap(const Vector9& xi, //
|
||||
OptionalJacobian<9, 9> H = boost::none);
|
||||
|
||||
/// Log map at identity - return the canonical coordinates for this NavState
|
||||
static Vector9 Logmap(const NavState& p, //
|
||||
OptionalJacobian<9, 9> H = boost::none);
|
||||
|
||||
/// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
|
||||
/// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
|
||||
Matrix9 AdjointMap() const;
|
||||
|
||||
/// wedge creates Lie algebra element from tangent vector
|
||||
static Matrix7 wedge(const Vector9& xi);
|
||||
/// localCoordinates with optional derivatives
|
||||
Vector9 localCoordinates(const NavState& g, //
|
||||
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
|
||||
boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Dynamics
|
||||
|
|
@ -237,14 +202,7 @@ private:
|
|||
|
||||
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
|
||||
template<>
|
||||
struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> {
|
||||
struct traits<NavState> : internal::Manifold<NavState> {
|
||||
};
|
||||
|
||||
// Partial specialization of wedge
|
||||
// TODO: deprecate, make part of traits
|
||||
template<>
|
||||
inline Matrix wedge<NavState>(const Vector& xi) {
|
||||
return NavState::wedge(xi);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ public:
|
|||
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
|
||||
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
|
||||
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
|
||||
NavState deltaXij() const override { return NavState::Retract(preintegrated_); }
|
||||
NavState deltaXij() const override { return NavState().retract(preintegrated_); }
|
||||
|
||||
const Vector9& preintegrated() const { return preintegrated_; }
|
||||
Vector3 theta() const { return preintegrated_.head<3>(); }
|
||||
|
|
|
|||
|
|
@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero();
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(NavState, Constructor) {
|
||||
boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
|
||||
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
|
||||
boost::none);
|
||||
Matrix aH1, aH2, aH3;
|
||||
EXPECT(
|
||||
assert_equal(kState1,
|
||||
NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
|
||||
EXPECT(
|
||||
assert_equal(
|
||||
numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
|
||||
EXPECT(
|
||||
assert_equal(
|
||||
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
|
||||
EXPECT(
|
||||
assert_equal(
|
||||
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(NavState, Constructor2) {
|
||||
boost::function<NavState(const Pose3&, const Vector3&)> construct =
|
||||
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
|
||||
boost::none);
|
||||
|
|
@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) {
|
|||
EXPECT(assert_equal((Matrix )eH, aH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NavState, MatrixGroup ) {
|
||||
// check roundtrip conversion to 7*7 matrix representation
|
||||
Matrix7 T = kState1.matrix();
|
||||
EXPECT(assert_equal(kState1, NavState(T)));
|
||||
|
||||
// check group product agrees with matrix product
|
||||
NavState state2 = kState1 * kState1;
|
||||
Matrix T2 = T * T;
|
||||
EXPECT(assert_equal(state2, NavState(T2)));
|
||||
EXPECT(assert_equal(T2, state2.matrix()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NavState, Manifold ) {
|
||||
// Check zero xi
|
||||
|
|
@ -114,7 +121,9 @@ TEST( NavState, Manifold ) {
|
|||
Rot3 drot = Rot3::Expmap(xi.head<3>());
|
||||
Point3 dt = Point3(xi.segment<3>(3));
|
||||
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
|
||||
NavState state2 = kState1 * NavState(drot, dt, dvel);
|
||||
NavState state2 = NavState(kState1.attitude() * drot,
|
||||
kState1.position() + kState1.attitude() * dt,
|
||||
kState1.velocity() + kState1.attitude() * dvel);
|
||||
EXPECT(assert_equal(state2, kState1.retract(xi)));
|
||||
EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
|
||||
|
||||
|
|
@ -122,27 +131,6 @@ TEST( NavState, Manifold ) {
|
|||
NavState state3 = state2.retract(xi);
|
||||
EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
|
||||
|
||||
// Check derivatives for ChartAtOrigin::Retract
|
||||
Matrix9 aH;
|
||||
// For zero xi
|
||||
boost::function<NavState(const Vector9&)> Retract = boost::bind(
|
||||
NavState::ChartAtOrigin::Retract, _1, boost::none);
|
||||
NavState::ChartAtOrigin::Retract(kZeroXi, aH);
|
||||
EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH));
|
||||
// For non-zero xi
|
||||
NavState::ChartAtOrigin::Retract(xi, aH);
|
||||
EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH));
|
||||
|
||||
// Check derivatives for ChartAtOrigin::Local
|
||||
// For zero xi
|
||||
boost::function<Vector9(const NavState&)> Local = boost::bind(
|
||||
NavState::ChartAtOrigin::Local, _1, boost::none);
|
||||
NavState::ChartAtOrigin::Local(kIdentity, aH);
|
||||
EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH));
|
||||
// For non-zero xi
|
||||
NavState::ChartAtOrigin::Local(kState1, aH);
|
||||
EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH));
|
||||
|
||||
// Check retract derivatives
|
||||
Matrix9 aH1, aH2;
|
||||
kState1.retract(xi, aH1, aH2);
|
||||
|
|
@ -151,6 +139,12 @@ TEST( NavState, Manifold ) {
|
|||
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
|
||||
|
||||
// Check retract derivatives on state 2
|
||||
const Vector9 xi2 = -3.0*xi;
|
||||
state2.retract(xi2, aH1, aH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
|
||||
|
||||
// Check localCoordinates derivatives
|
||||
boost::function<Vector9(const NavState&, const NavState&)> local =
|
||||
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
|
||||
|
|
@ -169,29 +163,6 @@ TEST( NavState, Manifold ) {
|
|||
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NavState, Lie ) {
|
||||
// Check zero xi
|
||||
EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi)));
|
||||
EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity)));
|
||||
EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi)));
|
||||
EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1)));
|
||||
|
||||
// Expmap/Logmap roundtrip
|
||||
Vector xi(9);
|
||||
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
|
||||
NavState state2 = NavState::Expmap(xi);
|
||||
EXPECT(assert_equal(xi, NavState::Logmap(state2)));
|
||||
|
||||
// roundtrip from state2 to state3 and back
|
||||
NavState state3 = state2.expmap(xi);
|
||||
EXPECT(assert_equal(xi, state2.logmap(state3)));
|
||||
|
||||
// For the expmap/logmap (not necessarily expmap/local) -xi goes other way
|
||||
EXPECT(assert_equal(state2, state3.expmap(-xi)));
|
||||
EXPECT(assert_equal(xi, -state3.logmap(state2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
TEST(NavState, Update) {
|
||||
|
|
@ -201,8 +172,8 @@ TEST(NavState, Update) {
|
|||
Matrix9 aF;
|
||||
Matrix93 aG1, aG2;
|
||||
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
|
||||
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
|
||||
boost::none, boost::none);
|
||||
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
|
||||
boost::none, boost::none);
|
||||
Vector3 b_acc = kAttitude * acc;
|
||||
NavState expected(kAttitude.expmap(dt * omega),
|
||||
kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),
|
||||
|
|
|
|||
Loading…
Reference in New Issue