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

A custom retract does the trick

release/4.3a0
dellaert 2015-07-19 03:31:11 -07:00
parent f32a7cbd00
commit d4d99c390d
2 changed files with 45 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/navigation/PreintegrationBase.cpp
View File

@ -129,10 +129,6 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
}
}
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); }
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
Vector9 result = Vector9::Zero();
@ -143,14 +139,14 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
const double dt = deltaTij(), dt2 = dt * dt;
const Vector3& omegaCoriolis = *p().omegaCoriolis;
dR(result) -= Ri.transpose() * omegaCoriolis * dt;
dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt;
NavState::dR(result) -= Ri.transpose() * omegaCoriolis * dt;
NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
NavState::dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt;
if (p().use2ndOrderCoriolis) {
Vector3 temp = omegaCoriolis.cross(
omegaCoriolis.cross(pose_i.translation().vector()));
dP(result) -= 0.5 * temp * dt2;
dV(result) -= temp * dt;
NavState::dP(result) -= 0.5 * temp * dt2;
NavState::dV(result) -= temp * dt;
}
}
return result;
@ -169,9 +165,9 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
// Rotation, translation, and velocity:
Vector9 delta;
dR(delta) = Rot3::Logmap(deltaRij_biascorrected);
dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2;
dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt;
NavState::dR(delta) = Rot3::Logmap(deltaRij_biascorrected);
NavState::dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt;
if (p().omegaCoriolis) delta += integrateCoriolis(state_i);
return delta;
@ -186,11 +182,7 @@ NavState PreintegrationBase::predict(const NavState& state_i,
Vector9 delta = recombinedPrediction(state_i, deltaRij_biascorrected,
deltaPij_biascorrected, deltaVij_biascorrected);
// TODO(frank): pose update below is separate expmap for R,t. Is that kosher?
const Pose3& pose_i = state_i.pose();
const Vector3& vel_i = state_i.velocity();
const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR(delta)), pose_i.translation() + Point3(dP(delta)));
return NavState(pose_j, vel_i + dV(delta));
return state_i.retract(delta);
}
//------------------------------------------------------------------------------
@ -309,6 +301,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
-delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias
-delVdelBiasAcc(), -delVdelBiasOmega(); // dfV/dBias
}
// TODO(frank): Vector9 r = state_i.localCoordinates(predictedState_j); does not work ???
Vector9 r;
r << fR, fp, fv;
return r;

36
gtsam/navigation/PreintegrationBase.h
View File

@ -36,23 +36,57 @@ typedef Vector3 Velocity3;
/**
* Navigation state: Pose (rotation, translation) + velocity
*/
class NavState: private ProductLieGroup<Pose3, Velocity3> {
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); }
// Specialize Retract/Local that agrees with IMUFactors
// TODO(frank): This is a very specific retract. Talk to Luca about implications.
NavState retract(Vector9& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
if (H1||H2) throw std::runtime_error("NavState::retract derivatives not implemented yet");
return NavState(rotation().expmap(dR(v)), translation() + Point3(dP(v)), velocity() + dV(v));
}
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");
Vector9 v;
dR(v) = rotation().logmap(g.rotation());
dP(v) = (g.translation() - translation()).vector();
dV(v) = g.velocity() - velocity();
return v;
}
/// @}
};
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<>
struct traits<NavState> : internal::LieGroupTraits<NavState> {};
/// @deprecated
struct PoseVelocityBias {
Pose3 pose;