12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.

release/4.3a0
dellaert 2016-06-05 00:58:38 -07:00
parent 6302a79533
commit 6bed20b28a
4 changed files with 105 additions and 254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

178
gtsam/navigation/NavState.cpp
View File

@ -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_; #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, NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { 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); TIE(nRb, n_t, n_v, *this);
const Rot3 bRn = nRb.inverse(); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); 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);
NavState NavState::operator*(const NavState& bTc) const { if (H1) {
TIE(nRb, n_t, n_v, *this); *H1 << D_R_nRb, Z_3x3, Z_3x3, //
TIE(bRc, b_t, b_v, bTc); // Note(frank): the derivative of n_t with respect to xi is nRb
return NavState(nRb * bRc, n_t + nRb * b_t, n_v + nRb * b_v); // 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:
//NavState NavState::retract(const Vector9& xi) const { nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
// 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();
} }
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, Vector9 NavState::localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H) { 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; Vector9 xi;
Matrix3 D_xi_R; Matrix3 D_xi_R;
xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
if (H) { if (H1) {
*H << D_xi_R, Z_3x3, Z_3x3, // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
Z_3x3, x.R(), Z_3x3, // D_dt_R, -I_3x3, Z_3x3, //
Z_3x3, Z_3x3, x.R(); 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; 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 // sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0) #define D_R_R(H) (H)->block<3,3>(0,0)

88
gtsam/navigation/NavState.h
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/ProductLieGroup.h> #include <gtsam/base/Manifold.h>
namespace gtsam { namespace gtsam {
@ -29,9 +29,9 @@ typedef Vector3 Velocity3;
/** /**
* Navigation state: Pose (rotation, translation) + velocity * 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: private:
// TODO(frank): // TODO(frank):
@ -42,6 +42,10 @@ private:
public: public:
enum {
dimension = 9
};
typedef std::pair<Point3, Velocity3> PositionAndVelocity; typedef std::pair<Point3, Velocity3> PositionAndVelocity;
/// @name Constructors /// @name Constructors
@ -49,7 +53,7 @@ public:
/// Default constructor /// Default constructor
NavState() : NavState() :
t_(0,0,0), v_(Vector3::Zero()) { t_(0, 0, 0), v_(Vector3::Zero()) {
} }
/// Construct from attitude, position, velocity /// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) : NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
@ -59,15 +63,15 @@ public:
NavState(const Pose3& pose, const Velocity3& v) : NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(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 /// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) : NavState(const Matrix3& R, const Vector9 tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
} }
/// Named constructor with derivatives /// 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, 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);
@ -116,7 +120,8 @@ public:
/// @{ /// @{
/// Output stream operator /// 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 /// print
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
@ -124,29 +129,6 @@ public:
/// equals /// equals
bool equals(const NavState& other, double tol = 1e-8) const; 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 /// @name Manifold
/// @{ /// @{
@ -172,32 +154,15 @@ public:
return v.segment<3>(6); return v.segment<3>(6);
} }
// Chart at origin, constructs components separately (as opposed to Expmap) /// retract with optional derivatives
struct ChartAtOrigin { NavState retract(const Vector9& v, //
static NavState Retract(const Vector9& xi, // OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
OptionalJacobian<9, 9> H = boost::none); boost::none) const;
static Vector9 Local(const NavState& x, //
OptionalJacobian<9, 9> H = boost::none);
};
/// @} /// localCoordinates with optional derivatives
/// @name Lie Group Vector9 localCoordinates(const NavState& g, //
/// @{ OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
/// 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);
/// @} /// @}
/// @name Dynamics /// @name Dynamics
@ -237,14 +202,7 @@ private:
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<> 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 } // namespace gtsam

2
gtsam/navigation/TangentPreintegration.h
View File

@ -67,7 +67,7 @@ public:
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } 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_; } const Vector9& preintegrated() const { return preintegrated_; }
Vector3 theta() const { return preintegrated_.head<3>(); } Vector3 theta() const { return preintegrated_.head<3>(); }

91
gtsam/navigation/tests/testNavState.cpp
View File

@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NavState, Constructor) { 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::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none); boost::none);
@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) {
EXPECT(assert_equal((Matrix )eH, aH)); 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 ) { TEST( NavState, Manifold ) {
// Check zero xi // Check zero xi
@ -114,7 +121,9 @@ TEST( NavState, Manifold ) {
Rot3 drot = Rot3::Expmap(xi.head<3>()); Rot3 drot = Rot3::Expmap(xi.head<3>());
Point3 dt = Point3(xi.segment<3>(3)); Point3 dt = Point3(xi.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.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(state2, kState1.retract(xi)));
EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
@ -122,27 +131,6 @@ TEST( NavState, Manifold ) {
NavState state3 = state2.retract(xi); NavState state3 = state2.retract(xi);
EXPECT(assert_equal(xi, state2.localCoordinates(state3))); 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 // Check retract derivatives
Matrix9 aH1, aH2; Matrix9 aH1, aH2;
kState1.retract(xi, 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(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); 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 // Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local = boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
@ -169,29 +163,6 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); 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 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
TEST(NavState, Update) { TEST(NavState, Update) {
@ -201,8 +172,8 @@ TEST(NavState, Update) {
Matrix9 aF; Matrix9 aF;
Matrix93 aG1, aG2; Matrix93 aG1, aG2;
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update = boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
boost::none, boost::none); boost::none, boost::none);
Vector3 b_acc = kAttitude * acc; Vector3 b_acc = kAttitude * acc;
NavState expected(kAttitude.expmap(dt * omega), NavState expected(kAttitude.expmap(dt * omega),
kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),