Moved NavState to its own header and totally revamped as a semi-direct product
dellaert
parent
205d20d4dc
commit
4c8c669d71
|
|
@ -0,0 +1,120 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file NavState.h
|
||||
* @brief Navigation state composing of attitude, position, and velocity
|
||||
* @author Frank Dellaert
|
||||
* @date July 2015
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
|
||||
|
||||
Matrix7 NavState::matrix() const {
|
||||
Matrix3 R = this->R();
|
||||
Matrix7 T;
|
||||
T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
|
||||
return T;
|
||||
}
|
||||
|
||||
void NavState::print(const std::string& s) const {
|
||||
attitude().print(s + ".R");
|
||||
position().print(s + ".p");
|
||||
gtsam::print((Vector) v_, s + ".v");
|
||||
}
|
||||
|
||||
bool NavState::equals(const NavState& other, double tol) const {
|
||||
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
|
||||
&& equal_with_abs_tol(v_, other.v_, tol);
|
||||
}
|
||||
|
||||
NavState NavState::inverse() 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, nRb * b_t + n_t, nRb * b_v + n_v);
|
||||
}
|
||||
|
||||
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_);
|
||||
}
|
||||
|
||||
Velocity3 NavState::operator*(const Velocity3& b_v) const {
|
||||
return Velocity3(R_ * b_v + v_);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
|
||||
OptionalJacobian<9, 9> H) {
|
||||
if (H)
|
||||
throw std::runtime_error(
|
||||
"NavState::ChartOrigin::Local derivative not implemented yet");
|
||||
Vector9 xi;
|
||||
xi << Rot3::Logmap(x.R_), x.t(), x.v();
|
||||
return xi;
|
||||
}
|
||||
|
||||
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
|
||||
if (H)
|
||||
throw std::runtime_error("NavState::Expmap derivative not implemented yet");
|
||||
// use brute force matrix exponential
|
||||
return expm<NavState>(xi);
|
||||
}
|
||||
|
||||
Vector9 NavState::Logmap(const NavState& p, OptionalJacobian<9, 9> H) {
|
||||
if (H)
|
||||
throw std::runtime_error("NavState::Logmap derivative not implemented yet");
|
||||
return Vector9::Zero();
|
||||
}
|
||||
|
||||
Matrix9 NavState::AdjointMap() const {
|
||||
throw std::runtime_error("NavState::AdjointMap not implemented yet");
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,225 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file NavState.h
|
||||
* @brief Navigation state composing of attitude, position, and velocity
|
||||
* @author Frank Dellaert
|
||||
* @date July 2015
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/ProductLieGroup.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Velocity is currently typedef'd to Vector3
|
||||
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
|
||||
*/
|
||||
class NavState: public LieGroup<NavState, 9> {
|
||||
private:
|
||||
Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
|
||||
Point3 t_; ///< position n_t, in nav frame
|
||||
Velocity3 v_; ///< velocity n_v in nav frame
|
||||
|
||||
public:
|
||||
|
||||
typedef std::pair<Point3, Velocity3> PositionAndVelocity;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
NavState() :
|
||||
v_(Vector3::Zero()) {
|
||||
}
|
||||
/// Construct from attitude, position, velocity
|
||||
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
|
||||
R_(R), t_(t), v_(v) {
|
||||
}
|
||||
/// Construct from pose and velocity
|
||||
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>()) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Component Access
|
||||
/// @{
|
||||
|
||||
const Rot3& attitude() const {
|
||||
return R_;
|
||||
}
|
||||
const Point3& position() const {
|
||||
return t_;
|
||||
}
|
||||
const Velocity3& velocity() const {
|
||||
return v_;
|
||||
}
|
||||
const Pose3 pose() const {
|
||||
return Pose3(attitude(), position());
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Derived quantities
|
||||
/// @{
|
||||
|
||||
/// Return rotation matrix. Induces computation in quaternion mode
|
||||
Matrix3 R() const {
|
||||
return R_.matrix();
|
||||
}
|
||||
/// Return position as Vector3
|
||||
Vector3 t() const {
|
||||
return t_.vector();
|
||||
}
|
||||
/// Return velocity as Vector3. Computation-free.
|
||||
const Vector3& v() const {
|
||||
return v_;
|
||||
}
|
||||
/// Return quaternion. Induces computation in matrix mode
|
||||
Quaternion quaternion() const {
|
||||
return R_.toQuaternion();
|
||||
}
|
||||
/// 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;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Act on velocity alone, n_v = nRb * b_v + n_v
|
||||
Velocity3 operator*(const Velocity3& b_v) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
// Tangent space sugar.
|
||||
// TODO(frank): move to private navstate namespace in cpp
|
||||
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
|
||||
return v.segment<3>(0);
|
||||
}
|
||||
static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
|
||||
return v.segment<3>(3);
|
||||
}
|
||||
static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
|
||||
return v.segment<3>(6);
|
||||
}
|
||||
static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
|
||||
return v.segment<3>(0);
|
||||
}
|
||||
static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
|
||||
return v.segment<3>(3);
|
||||
}
|
||||
static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
|
||||
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);
|
||||
};
|
||||
|
||||
/// @}
|
||||
/// @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 idenity, 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);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/// @{
|
||||
/// serialization
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v_);
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
|
||||
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
|
||||
template<>
|
||||
struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<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
|
||||
|
|
@ -182,7 +182,7 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
|
|||
const Pose3& pose_i = state_i.pose();
|
||||
const Vector3& vel_i = state_i.velocity();
|
||||
const Matrix3 Ri = pose_i.rotation().matrix();
|
||||
const Vector3& t_i = state_i.translation().vector();
|
||||
const Vector3& t_i = state_i.position().vector();
|
||||
const double dt = deltaTij(), dt2 = dt * dt;
|
||||
|
||||
const Vector3& omegaCoriolis = *p().omegaCoriolis;
|
||||
|
|
@ -220,7 +220,7 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
|
|||
const Matrix3 Ri = pose_i.rotation().matrix();
|
||||
const double dt = deltaTij(), dt2 = dt * dt;
|
||||
|
||||
// Rotation, translation, and velocity:
|
||||
// Rotation, position, and velocity:
|
||||
Vector9 delta;
|
||||
Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
|
||||
NavState::dR(delta) = NavState::dR(biasCorrectedDelta);
|
||||
|
|
@ -274,18 +274,18 @@ NavState PreintegrationBase::predict(const NavState& state_i,
|
|||
static Vector9 computeError(const NavState& state_i, const NavState& state_j,
|
||||
const NavState& predictedState_j) {
|
||||
|
||||
const Rot3& rot_i = state_i.rotation();
|
||||
const Rot3& rot_i = state_i.attitude();
|
||||
const Matrix Ri = rot_i.matrix();
|
||||
|
||||
// Residual rotation error
|
||||
// TODO: this also seems to be flipped from localCoordinates
|
||||
const Rot3 fRrot = predictedState_j.rotation().between(state_j.rotation());
|
||||
const Rot3 fRrot = predictedState_j.attitude().between(state_j.attitude());
|
||||
const Vector3 fR = Rot3::Logmap(fRrot);
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
|
||||
const Vector3 fp = Ri.transpose()
|
||||
* (state_j.translation() - predictedState_j.translation()).vector();
|
||||
* (state_j.position() - predictedState_j.position()).vector();
|
||||
|
||||
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
|
||||
const Vector3 fv = Ri.transpose()
|
||||
|
|
|
|||
|
|
@ -22,97 +22,11 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/navigation/PreintegratedRotation.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/ProductLieGroup.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Velocity in 3D is just a Vector3
|
||||
typedef Vector3 Velocity3;
|
||||
|
||||
/**
|
||||
* Navigation state: Pose (rotation, translation) + velocity
|
||||
*/
|
||||
class NavState: public ProductLieGroup<Pose3, Velocity3> {
|
||||
protected:
|
||||
typedef ProductLieGroup<Pose3, Velocity3> Base;
|
||||
typedef OptionalJacobian<9, 9> ChartJacobian;
|
||||
using Base::first;
|
||||
using Base::second;
|
||||
|
||||
public:
|
||||
// constructors
|
||||
NavState() {}
|
||||
NavState(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {}
|
||||
NavState(const Rot3& rot, const Point3& t, const Velocity3& vel): Base(Pose3(rot, t), vel) {}
|
||||
NavState(const Base& product) : Base(product) {}
|
||||
|
||||
// access
|
||||
const Pose3& pose() const { return first; }
|
||||
const Point3& translation() const { return pose().translation(); }
|
||||
const Rot3& rotation() const { return pose().rotation(); }
|
||||
const Velocity3& velocity() const { return second; }
|
||||
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
// NavState tangent space sugar.
|
||||
static Eigen::Block<Vector9,3,1> dR(Vector9& v) { return v.segment<3>(0); }
|
||||
static Eigen::Block<Vector9,3,1> dP(Vector9& v) { return v.segment<3>(3); }
|
||||
static Eigen::Block<Vector9,3,1> dV(Vector9& v) { return v.segment<3>(6); }
|
||||
static Eigen::Block<const Vector9,3,1> dR(const Vector9& v) { return v.segment<3>(0); }
|
||||
static Eigen::Block<const Vector9,3,1> dP(const Vector9& v) { return v.segment<3>(3); }
|
||||
static Eigen::Block<const Vector9,3,1> dV(const Vector9& v) { return v.segment<3>(6); }
|
||||
|
||||
// Retract/Local that creates a de-novo Nav-state from xi, on all components
|
||||
// separately, but then composes with this, i.e., xi is in rotated frame!
|
||||
// NOTE(frank): This agrees with Pose3.retract, in non-EXPMAP mode, treating V like P
|
||||
NavState retract(const Vector9& xi, //
|
||||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
|
||||
Matrix3 D_R_xi;
|
||||
const Rot3 R = Rot3::Expmap(dR(xi), H2 ? &D_R_xi : 0);
|
||||
const Point3 p = Point3(dP(xi));
|
||||
const Vector v = dV(xi);
|
||||
const NavState delta(R, p, v);
|
||||
// retract only depends on this via compose, and second derivative in delta is I_9x9
|
||||
const NavState result = this->compose(delta, H1);
|
||||
if (H2) {
|
||||
*H2 << D_R_xi, Z_3x3, Z_3x3, //
|
||||
Z_3x3, I_3x3, Z_3x3, //
|
||||
Z_3x3, Z_3x3, I_3x3;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
Vector9 localCoordinates(const NavState& g, //
|
||||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
|
||||
if (H1||H2) throw std::runtime_error("NavState::localCoordinates derivatives not implemented yet");
|
||||
const NavState delta = this->between(g);
|
||||
Vector9 v;
|
||||
dR(v) = Rot3::Logmap(delta.rotation());
|
||||
dP(v) = delta.translation().vector();
|
||||
dV(v) = delta.velocity();
|
||||
return v;
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
|
||||
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
|
||||
template<>
|
||||
struct traits<NavState> : internal::LieGroupTraits<NavState> {
|
||||
static void Print(const NavState& m, const std::string& s = "") {
|
||||
m.rotation().print(s+".R");
|
||||
m.translation().print(s+".P");
|
||||
print((Vector)m.velocity(),s+".V");
|
||||
}
|
||||
static bool Equals(const NavState& m1, const NavState& m2, double tol = 1e-8) {
|
||||
return m1.pose().equals(m2.pose(), tol)
|
||||
&& equal_with_abs_tol(m1.velocity(), m2.velocity(), tol);
|
||||
}
|
||||
};
|
||||
|
||||
/// @deprecated
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,124 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testNavState.cpp
|
||||
* @brief Unit tests for NavState
|
||||
* @author Frank Dellaert
|
||||
* @date July 2015
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
static const Rot3 kRotation = Rot3::RzRyRx(0.1, 0.2, 0.3);
|
||||
static const Point3 kPosition(1.0, 2.0, 3.0);
|
||||
static const Velocity3 kVelocity(0.4, 0.5, 0.6);
|
||||
static const NavState kIdentity;
|
||||
static const NavState kState1(kRotation, kPosition, kVelocity);
|
||||
|
||||
const double tol = 1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NavState, MatrixGroup ) {
|
||||
// check roundtrip conversion to 7*7 matrix representation
|
||||
Matrix7 T = kState1.matrix();
|
||||
EXPECT(assert_equal(kState1, NavState(T), tol));
|
||||
|
||||
// check group product agrees with matrix product
|
||||
NavState state2 = kState1 * kState1;
|
||||
Matrix T2 = T * T;
|
||||
EXPECT(assert_equal(state2, NavState(T2), tol));
|
||||
EXPECT(assert_equal(T2, state2.matrix(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NavState, Manifold ) {
|
||||
// Check zero xi
|
||||
EXPECT(assert_equal(kIdentity, kIdentity.retract(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), kIdentity.localCoordinates(kIdentity), tol));
|
||||
EXPECT(assert_equal(kState1, kState1.retract(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), kState1.localCoordinates(kState1), tol));
|
||||
|
||||
// Check definition of retract as operating on components separately
|
||||
Vector xi(9);
|
||||
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
|
||||
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);
|
||||
EXPECT(assert_equal(state2, kState1.retract(xi), tol));
|
||||
EXPECT(assert_equal(xi, kState1.localCoordinates(state2), tol));
|
||||
|
||||
// roundtrip from state2 to state3 and back
|
||||
NavState state3 = state2.retract(xi);
|
||||
EXPECT(assert_equal(xi, state2.localCoordinates(state3), tol));
|
||||
|
||||
// Check derivatives for ChartAtOrigin::Retract
|
||||
Matrix9 aH, eH;
|
||||
// For zero xi
|
||||
boost::function<NavState(Vector9)> f = boost::bind(
|
||||
NavState::ChartAtOrigin::Retract, _1, boost::none);
|
||||
NavState::ChartAtOrigin::Retract(zero(9), aH);
|
||||
eH = numericalDerivative11<NavState, Vector9>(f, zero(9));
|
||||
EXPECT(assert_equal((Matrix )eH, aH));
|
||||
// For non-zero xi
|
||||
NavState::ChartAtOrigin::Retract(xi, aH);
|
||||
eH = numericalDerivative11<NavState, Vector9>(f, xi);
|
||||
EXPECT(assert_equal((Matrix )eH, aH));
|
||||
|
||||
// Check retract derivatives
|
||||
// Matrix9 aH1, aH2;
|
||||
// kState1.retract(xi, aH1, aH2);
|
||||
// Matrix eH1 = numericalDerivative11<NavState, NavState>(
|
||||
// boost::bind(&NavState::retract, _1, xi, boost::none, boost::none),
|
||||
// kState1);
|
||||
// EXPECT(assert_equal(eH1, aH1));
|
||||
// Matrix eH2 = numericalDerivative11<NavState, Vector9>(
|
||||
// boost::bind(&NavState::retract, kState1, _1, boost::none, boost::none),
|
||||
// xi);
|
||||
// EXPECT(assert_equal(eH2, aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NavState, Lie ) {
|
||||
// Check zero xi
|
||||
EXPECT(assert_equal(kIdentity, kIdentity.expmap(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), kIdentity.logmap(kIdentity), tol));
|
||||
EXPECT(assert_equal(kState1, kState1.expmap(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), kState1.logmap(kState1), tol));
|
||||
|
||||
// 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), tol));
|
||||
|
||||
// roundtrip from state2 to state3 and back
|
||||
NavState state3 = state2.expmap(xi);
|
||||
EXPECT(assert_equal(xi, state2.logmap(state3), tol));
|
||||
|
||||
// For the expmap/logmap (not necessarily expmap/local) -xi goes other way
|
||||
EXPECT(assert_equal(state2, state3.expmap(-xi), tol));
|
||||
EXPECT(assert_equal(xi, -state3.logmap(state2), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue