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

FromPoseVelocity

release/4.3a0
dellaert 2015-07-29 11:20:35 -07:00
parent 96c139ac87
commit 5ca67d0a3a
3 changed files with 35 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/NavState.cpp
View File

@ -24,6 +24,16 @@ 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::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
if (H1)
*H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
if (H2)
*H2 << Z_3x3, Z_3x3, pose.rotation().transpose();
return NavState(pose, vel);
}
//------------------------------------------------------------------------------
const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
if (H)
@ -252,9 +262,9 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
//------------------------------------------------------------------------------
Vector9 NavState::correctPIM(const Vector9& pim, double dt,
const Vector3& n_gravity,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
const Vector3& n_gravity, const boost::optional<Vector3>& omegaCoriolis,
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) const {
const Rot3& nRb = R_;
const Velocity3& n_v = v_; // derivative is Ri !
const double dt2 = dt * dt;

3
gtsam/navigation/NavState.h
View File

@ -67,6 +67,9 @@ public:
NavState(const Matrix3& R, const Vector9 tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Named constructor with derivatives
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
/// @}
/// @name Component Access

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

@ -26,6 +26,7 @@ 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 Pose3 kPose(kRotation, kPosition);
static const Velocity3 kVelocity(0.4, 0.5, 0.6);
static const NavState kIdentity;
static const NavState kState1(kRotation, kPosition, kVelocity);
@ -33,6 +34,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();
/* ************************************************************************* */
TEST(NavState, Constructor) {
boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
Matrix aH1, aH2;
EXPECT(assert_equal(kState1, NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
}
/* ************************************************************************* */
TEST( NavState, Attitude) {
Matrix39 aH, eH;
@ -127,9 +138,14 @@ TEST( NavState, Manifold ) {
// Check localCoordinates derivatives
kState1.localCoordinates(state2, aH1, aH2);
boost::function<Vector9(const NavState&, const NavState&)> localCoordinates =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21(localCoordinates, kState1, state2), aH1));
EXPECT(assert_equal(numericalDerivative22(localCoordinates, kState1, state2), aH2));
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
boost::none);
EXPECT(
assert_equal(numericalDerivative21(localCoordinates, kState1, state2),
aH1));
EXPECT(
assert_equal(numericalDerivative22(localCoordinates, kState1, state2),
aH2));
}
/* ************************************************************************* */