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

9
gtsam/navigation/NavState.h
View File

@ -204,8 +204,13 @@ public:
/// @{
// Compute tangent space contribution due to coriolis forces
Vector9 coriolis(const Vector3& omega, double dt, bool secondOrder,
OptionalJacobian<9, 9> H) const;
Vector9 coriolis(const Vector3& omega, double dt, bool secondOrder = false,
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(
biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
Vector9 delta;
Vector9 xi;
Matrix3 D_dR_deltaRij;
NavState::dR(delta) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
NavState::dP(delta) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
NavState::dR(xi) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
NavState::dP(xi) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(delta) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
@ -154,20 +154,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
}
return delta;
}
//------------------------------------------------------------------------------
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();
}
return xi;
}
//------------------------------------------------------------------------------
@ -180,17 +167,18 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
const double dt = deltaTij(), dt2 = dt * dt;
// Rotation, position, and velocity:
Vector9 delta;
Vector9 xi;
Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
NavState::dR(delta) = NavState::dR(biasCorrectedDelta);
NavState::dP(delta) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri,
NavState::dR(xi) = NavState::dR(biasCorrectedDelta);
NavState::dP(xi) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri,
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;
Matrix9 Hcoriolis;
if (p().omegaCoriolis) {
delta += integrateCoriolis(state_i, H1 ? &Hcoriolis : 0);
state_i.addCoriolis(&xi, *(p().omegaCoriolis), deltaTij(),
p().use2ndOrderCoriolis, H1 ? &Hcoriolis : 0);
}
if (H1) {
H1->setZero();
@ -208,7 +196,7 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
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,
H2 ? &D_biasCorrected_bias : 0);
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);
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)
*H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)

4
gtsam/navigation/PreintegrationBase.h
View File

@ -152,10 +152,6 @@ class PreintegrationBase : public PreintegratedRotation {
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
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
Vector9 recombinedPrediction(const NavState& state_i,
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);
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;
Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);