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

Merge branch 'feature/cleanup_ImuFactor' into noiseModelMixup

release/4.3a0
dellaert 2015-07-29 11:54:16 -07:00
parent 5b9bf9affa 9d1111a767
commit 2a9e1db4cb
5 changed files with 45 additions and 9 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

2
gtsam/navigation/PreintegrationBase.cpp
View File

@ -180,6 +180,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
// Note that derivative of constructors below is not identity for velocity, but
// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
NavState state_i(pose_i, vel_i);
NavState state_j(pose_j, vel_j);

3
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -866,8 +866,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
values.insert(B(1), bias);
// Make sure linearization is correct
double diffDelta = 1e-5;
// This fails, except if tol = 1e-1: probably wrong!
double diffDelta = 1e-7;
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}

30
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,18 @@ 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;
@ -125,11 +138,20 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
// Check localCoordinates derivatives
kState1.localCoordinates(state2, aH1, aH2);
boost::function<Vector9(const NavState&, const NavState&)> localCoordinates =
boost::function<Vector9(const NavState&, const NavState&)> local =
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));
// from state1 to state2
kState1.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2));
// from identity to state2
kIdentity.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1));
EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2));
// from state2 to identity
state2.localCoordinates(kIdentity, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1));
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
}
/* ************************************************************************* */