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

Additive version for coriolis forces

release/4.3a0
dellaert 2015-07-22 01:01:19 -04:00
parent eb42a57860
commit 2a38ed9603
5 changed files with 61 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam/navigation/NavState.cpp
View File

@ -24,24 +24,28 @@ namespace gtsam {
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
//------------------------------------------------------------------------------
const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
if (H) if (H)
*H << I_3x3, Z_3x3, Z_3x3; *H << I_3x3, Z_3x3, Z_3x3;
return R_; return R_;
} }
//------------------------------------------------------------------------------
const Point3& NavState::position(OptionalJacobian<3, 9> H) const { const Point3& NavState::position(OptionalJacobian<3, 9> H) const {
if (H) if (H)
*H << Z_3x3, R(), Z_3x3; *H << Z_3x3, R(), Z_3x3;
return t_; return t_;
} }
//------------------------------------------------------------------------------
const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const {
if (H) if (H)
*H << Z_3x3, Z_3x3, R(); *H << Z_3x3, Z_3x3, R();
return v_; return v_;
} }
//------------------------------------------------------------------------------
Matrix7 NavState::matrix() const { Matrix7 NavState::matrix() const {
Matrix3 R = this->R(); Matrix3 R = this->R();
Matrix7 T; Matrix7 T;
@ -49,29 +53,34 @@ Matrix7 NavState::matrix() const {
return T; return T;
} }
//------------------------------------------------------------------------------
void NavState::print(const string& s) const { void NavState::print(const string& s) const {
attitude().print(s + ".R"); attitude().print(s + ".R");
position().print(s + ".p"); position().print(s + ".p");
gtsam::print((Vector) v_, s + ".v"); gtsam::print((Vector) v_, s + ".v");
} }
//------------------------------------------------------------------------------
bool NavState::equals(const NavState& other, double tol) const { bool NavState::equals(const NavState& other, double tol) const {
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
&& equal_with_abs_tol(v_, other.v_, tol); && equal_with_abs_tol(v_, other.v_, tol);
} }
//------------------------------------------------------------------------------
NavState NavState::inverse() const { NavState NavState::inverse() const {
TIE(nRb, n_t, n_v, *this); TIE(nRb, n_t, n_v, *this);
const Rot3 bRn = nRb.inverse(); const Rot3 bRn = nRb.inverse();
return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
} }
//------------------------------------------------------------------------------
NavState NavState::operator*(const NavState& bTc) const { NavState NavState::operator*(const NavState& bTc) const {
TIE(nRb, n_t, n_v, *this); TIE(nRb, n_t, n_v, *this);
TIE(bRc, b_t, b_v, bTc); TIE(bRc, b_t, b_v, bTc);
return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v);
} }
//------------------------------------------------------------------------------
NavState::PositionAndVelocity // NavState::PositionAndVelocity //
NavState::operator*(const PositionAndVelocity& b_tv) const { NavState::operator*(const PositionAndVelocity& b_tv) const {
TIE(nRb, n_t, n_v, *this); TIE(nRb, n_t, n_v, *this);
@ -80,14 +89,17 @@ NavState::operator*(const PositionAndVelocity& b_tv) const {
return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v);
} }
//------------------------------------------------------------------------------
Point3 NavState::operator*(const Point3& b_t) const { Point3 NavState::operator*(const Point3& b_t) const {
return Point3(R_ * b_t + t_); return Point3(R_ * b_t + t_);
} }
//------------------------------------------------------------------------------
Velocity3 NavState::operator*(const Velocity3& b_v) const { Velocity3 NavState::operator*(const Velocity3& b_v) const {
return Velocity3(R_ * b_v + v_); return Velocity3(R_ * b_v + v_);
} }
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
OptionalJacobian<9, 9> H) { OptionalJacobian<9, 9> H) {
Matrix3 D_R_xi; Matrix3 D_R_xi;
@ -103,6 +115,7 @@ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
return result; return result;
} }
//------------------------------------------------------------------------------
Vector9 NavState::ChartAtOrigin::Local(const NavState& x, Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
OptionalJacobian<9, 9> H) { OptionalJacobian<9, 9> H) {
if (H) if (H)
@ -113,6 +126,7 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
return xi; return xi;
} }
//------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
if (H) if (H)
throw runtime_error("NavState::Expmap derivative not implemented yet"); throw runtime_error("NavState::Expmap derivative not implemented yet");
@ -139,6 +153,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
} }
} }
//------------------------------------------------------------------------------
Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
if (H) if (H)
throw runtime_error("NavState::Logmap derivative not implemented yet"); throw runtime_error("NavState::Logmap derivative not implemented yet");
@ -166,6 +181,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
return xi; return xi;
} }
//------------------------------------------------------------------------------
Matrix9 NavState::AdjointMap() const { Matrix9 NavState::AdjointMap() const {
// NOTE(frank): See Pose3::AdjointMap // NOTE(frank): See Pose3::AdjointMap
const Matrix3 nRb = R(); const Matrix3 nRb = R();
@ -177,6 +193,7 @@ Matrix9 NavState::AdjointMap() const {
return adj; return adj;
} }
//------------------------------------------------------------------------------
Matrix7 NavState::wedge(const Vector9& xi) { Matrix7 NavState::wedge(const Vector9& xi) {
const Matrix3 Omega = skewSymmetric(dR(xi)); const Matrix3 Omega = skewSymmetric(dR(xi));
Matrix7 T; Matrix7 T;
@ -184,6 +201,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
return T; return T;
} }
//------------------------------------------------------------------------------
// sugar for derivative blocks // sugar for derivative blocks
#define D_R_R(H) H->block<3,3>(0,0) #define D_R_R(H) H->block<3,3>(0,0)
#define D_t_t(H) H->block<3,3>(3,3) #define D_t_t(H) H->block<3,3>(3,3)
@ -191,9 +209,9 @@ Matrix7 NavState::wedge(const Vector9& xi) {
#define D_v_t(H) H->block<3,3>(6,3) #define D_v_t(H) H->block<3,3>(6,3)
#define D_v_v(H) H->block<3,3>(6,6) #define D_v_v(H) H->block<3,3>(6,6)
Vector9 NavState::coriolis(const Vector3& omega, double dt, bool secondOrder, //------------------------------------------------------------------------------
OptionalJacobian<9, 9> H) const { void NavState::addCoriolis(Vector9* xi, const Vector3& omega, double dt,
Vector9 result; bool secondOrder, OptionalJacobian<9, 9> H) const {
const Rot3& nRb = attitude(); const Rot3& nRb = attitude();
const Point3& n_t = position(); // derivative is R() ! const Point3& n_t = position(); // derivative is R() !
const Vector3& n_v = velocity(); // ditto const Vector3& n_v = velocity(); // ditto
@ -201,28 +219,38 @@ Vector9 NavState::coriolis(const Vector3& omega, double dt, bool secondOrder,
const Vector3 omega_cross_vel = omega.cross(n_v); const Vector3 omega_cross_vel = omega.cross(n_v);
Matrix3 D_dP_R; Matrix3 D_dP_R;
dR(result) << -nRb.unrotate(omega * dt, H ? &D_dP_R : 0); dR(*xi) -= nRb.unrotate(omega * dt, H ? &D_dP_R : 0);
dP(result) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper dP(*xi) -= (dt2 * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(result) << ((-2.0 * dt) * omega_cross_vel); dV(*xi) -= ((2.0 * dt) * omega_cross_vel);
if (secondOrder) { if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector()));
dP(result) -= (0.5 * dt2) * omega_cross2_t; dP(*xi) -= (0.5 * dt2) * omega_cross2_t;
dV(result) -= dt * omega_cross2_t; dV(*xi) -= dt * omega_cross2_t;
} }
if (H) { if (H) {
const Matrix3 Omega = skewSymmetric(omega); const Matrix3 Omega = skewSymmetric(omega);
const Matrix3 D_cross_state = Omega * R(); const Matrix3 D_cross_state = Omega * R();
H->setZero(); H->setZero();
D_R_R(H) << -D_dP_R; D_R_R(H) -= D_dP_R;
D_t_v(H) << (-dt2) * D_cross_state; D_t_v(H) -= dt2 * D_cross_state;
D_v_v(H) << (-2.0 * dt) * D_cross_state; D_v_v(H) -= (2.0 * dt) * D_cross_state;
if (secondOrder) { if (secondOrder) {
const Matrix3 D_cross2_state = Omega * D_cross_state; const Matrix3 D_cross2_state = Omega * D_cross_state;
D_t_t(H) -= (0.5 * dt2) * D_cross2_state; D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= dt * D_cross2_state; D_v_t(H) -= dt * D_cross2_state;
} }
} }
return result; }
//------------------------------------------------------------------------------
Vector9 NavState::coriolis(const Vector3& omega, double dt, bool secondOrder,
OptionalJacobian<9, 9> H) const {
Vector9 xi;
xi.setZero();
if (H)
H->setZero();
addCoriolis(&xi, omega, dt, secondOrder, H);
return xi;
} }
} /// namespace gtsam } /// namespace gtsam

9
gtsam/navigation/NavState.h
View File

@ -204,8 +204,13 @@ public:
/// @{ /// @{
// Compute tangent space contribution due to coriolis forces // Compute tangent space contribution due to coriolis forces
Vector9 coriolis(const Vector3& omega, double dt, bool secondOrder, Vector9 coriolis(const Vector3& omega, double dt, bool secondOrder = false,
OptionalJacobian<9, 9> H) const; OptionalJacobian<9, 9> H = boost::none) const;
// Add tangent space contribution due to coriolis forces
// Additively modifies xi and H in place (if given)
void addCoriolis(Vector9* xi, const Vector3& omega, double dt,
bool secondOrder = false, OptionalJacobian<9, 9> H = boost::none) const;
/// @} /// @}

40
gtsam/navigation/PreintegrationBase.cpp
View File

@ -140,12 +140,12 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij( Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij(
biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0); biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
Vector9 delta; Vector9 xi;
Matrix3 D_dR_deltaRij; Matrix3 D_dR_deltaRij;
NavState::dR(delta) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0); NavState::dR(xi) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
NavState::dP(delta) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer() NavState::dP(xi) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope(); + delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(delta) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer() NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope(); + delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) { if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
@ -154,20 +154,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias; (*H) << D_dR_bias, D_dP_bias, D_dV_bias;
} }
return delta; return xi;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
OptionalJacobian<9, 9> H) const {
if (p().omegaCoriolis) {
return state_i.coriolis(*(p().omegaCoriolis), deltaTij(),
p().use2ndOrderCoriolis, H);
} else {
if (H)
H->setZero();
return Vector9::Zero();
}
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -180,17 +167,18 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
const double dt = deltaTij(), dt2 = dt * dt; const double dt = deltaTij(), dt2 = dt * dt;
// Rotation, position, and velocity: // Rotation, position, and velocity:
Vector9 delta; Vector9 xi;
Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc; Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
NavState::dR(delta) = NavState::dR(biasCorrectedDelta); NavState::dR(xi) = NavState::dR(biasCorrectedDelta);
NavState::dP(delta) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri, NavState::dP(xi) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri,
D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2; D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri, NavState::dV(xi) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri,
D_dV_bc) + p().gravity * dt; D_dV_bc) + p().gravity * dt;
Matrix9 Hcoriolis; Matrix9 Hcoriolis;
if (p().omegaCoriolis) { if (p().omegaCoriolis) {
delta += integrateCoriolis(state_i, H1 ? &Hcoriolis : 0); state_i.addCoriolis(&xi, *(p().omegaCoriolis), deltaTij(),
p().use2ndOrderCoriolis, H1 ? &Hcoriolis : 0);
} }
if (H1) { if (H1) {
H1->setZero(); H1->setZero();
@ -208,7 +196,7 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
H2->block<3, 3>(6, 6) = Ri; H2->block<3, 3>(6, 6) = Ri;
} }
return delta; return xi;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -219,10 +207,10 @@ NavState PreintegrationBase::predict(const NavState& state_i,
Vector9 biasCorrected = biasCorrectedDelta(bias_i, Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0); H2 ? &D_biasCorrected_bias : 0);
Matrix9 D_delta_state, D_delta_biasCorrected; Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 delta = recombinedPrediction(state_i, biasCorrected, Vector9 xi = recombinedPrediction(state_i, biasCorrected,
H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0); H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0);
Matrix9 D_predict_state, D_predict_delta; Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(delta, D_predict_state, D_predict_delta); NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1) if (H1)
*H1 = D_predict_state + D_predict_delta * D_delta_state; *H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2) if (H2)

4
gtsam/navigation/PreintegrationBase.h
View File

@ -152,10 +152,6 @@ class PreintegrationBase : public PreintegratedRotation {
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const; OptionalJacobian<9, 6> H = boost::none) const;
/// Integrate coriolis correction in body frame state_i
Vector9 integrateCoriolis(const NavState& state_i,
OptionalJacobian<9, 9> H = boost::none) const;
/// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector /// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector
Vector9 recombinedPrediction(const NavState& state_i, Vector9 recombinedPrediction(const NavState& state_i,
const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none, const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none,

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

@ -282,14 +282,6 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
boost::none), bias); boost::none), bias);
EXPECT(assert_equal(expectedH, actualH)); EXPECT(assert_equal(expectedH, actualH));
} }
{
Matrix9 actualH;
pim.integrateCoriolis(state1, actualH);
Matrix expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&PreintegrationBase::integrateCoriolis, pim, _1,
boost::none), state1);
EXPECT(assert_equal(expectedH, actualH));
}
{ {
Matrix9 aH1, aH2; Matrix9 aH1, aH2;
Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias); Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);