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

New retract. Still some bug in derivative

release/4.3a0
dellaert 2015-07-20 21:40:58 -07:00
parent 814c170caa
commit 8a8503ee33
2 changed files with 23 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/navigation/PreintegrationBase.h
View File

@ -67,31 +67,33 @@ public:
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); }
// Specialized Retract/Local that agrees with IMUFactors
// NOTE(frank): This also agrees with Pose3.retract
// 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 H1R, H2R;
const Rot3 R = rotation().expmap(dR(xi), H1 ? &H1R : 0, H2 ? &H2R : 0);
const Point3 p = translation() + Point3(dP(xi));
const Vector v = velocity() + dV(xi);
if (H1) {
H1->setIdentity();
H1->topLeftCorner<3,3>() = H1R;
}
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->setIdentity();
H2->topLeftCorner<3,3>() = H2R;
*H2 << D_R_xi, Z_3x3, Z_3x3, //
Z_3x3, I_3x3, Z_3x3, //
Z_3x3, Z_3x3, I_3x3;
}
return NavState(R, p, v);
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) = rotation().logmap(g.rotation());
dP(v) = (g.translation() - translation()).vector();
dV(v) = g.velocity() - velocity();
dR(v) = Rot3::Logmap(delta.rotation());
dP(v) = delta.translation().vector();
dV(v) = delta.velocity();
return v;
}
/// @}

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

@ -227,10 +227,10 @@ TEST( NavState, Lie ) {
// Special retract
Vector delta(9);
delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
Rot3 rot2 = rot.expmap(delta.head<3>());
Point3 t2 = pt + Point3(delta.segment<3>(3));
Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3);
NavState state2(rot2, t2, vel2);
Rot3 drot = Rot3::Expmap(delta.head<3>());
Point3 dt = Point3(delta.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
NavState state2 = state1 * NavState(drot, dt, dvel);
EXPECT(assert_equal(state2, (NavState)state1.retract(delta), tol));
EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol));