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

Now uses NavState::predict !

release/4.3a0
dellaert 2015-07-22 22:24:19 +02:00
parent 7ccfb95339
commit a99911b997
3 changed files with 25 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
gtsam/navigation/PreintegrationBase.cpp
View File

@ -63,7 +63,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
const Vector3& correctedAcc, const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij().matrix(); // expensive
const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Vector3 temp = dRij * correctedAcc * deltaT;
if (!p().use2ndOrderIntegration) {
@ -98,9 +98,9 @@ void PreintegrationBase::updatePreintegratedMeasurements(
void PreintegrationBase::updatePreintegratedJacobians(
const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 dRij = deltaRij().matrix(); // expensive
const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
* delRdelBiasOmega();
* delRdelBiasOmega_;
if (!p().use2ndOrderIntegration) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
@ -157,48 +157,6 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
return xi;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) const {
const Rot3& rot_i = state_i.attitude();
const Vector3& vel_i = state_i.velocity();
const double dt = deltaTij(), dt2 = dt * dt;
// Rotation, position, and velocity:
Vector9 xi;
Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
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(xi) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri,
D_dV_bc) + p().gravity * dt;
Matrix9 Hcoriolis;
if (p().omegaCoriolis) {
state_i.addCoriolis(&xi, *(p().omegaCoriolis), deltaTij(),
p().use2ndOrderCoriolis, H1 ? &Hcoriolis : 0);
}
if (H1) {
H1->setZero();
H1->block<3, 3>(3, 0) = D_dP_Ri;
H1->block<3, 3>(3, 6) = I_3x3 * dt;
H1->block<3, 3>(6, 0) = D_dV_Ri;
if (p().omegaCoriolis)
*H1 += Hcoriolis;
}
if (H2) {
H2->setZero();
Matrix3 Ri = rot_i.matrix();
H2->block<3, 3>(0, 0) = I_3x3;
H2->block<3, 3>(3, 3) = Ri;
H2->block<3, 3>(6, 6) = Ri;
}
return xi;
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
@ -207,8 +165,9 @@ 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 xi = recombinedPrediction(state_i, biasCorrected,
H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0);
Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1)
@ -300,7 +259,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
Matrix3 D_fR_fRrot;
Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0);
const double dt = deltaTij(), dt2 = dt * dt;
const double dt = deltaTij_, dt2 = dt * dt;
Matrix3 RitOmegaCoriolisHat = Z_3x3;
if ((H1 || H2) && p().omegaCoriolis)
RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis);

5
gtsam/navigation/PreintegrationBase.h
View File

@ -152,11 +152,6 @@ class PreintegrationBase : public PreintegratedRotation {
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> 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,
OptionalJacobian<9, 9> H2 = boost::none) const;
/// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const;

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

@ -251,10 +251,12 @@ TEST( NavState, Lie ) {
Matrix9 aH1, aH2;
state1.retract(delta, aH1, aH2);
Matrix eH1 = numericalDerivative11<NavState, NavState>(
boost::bind(&NavState::retract, _1, delta, boost::none, boost::none),state1);
boost::bind(&NavState::retract, _1, delta, boost::none, boost::none),
state1);
EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<NavState, Vector9>(
boost::bind(&NavState::retract, state1, _1, boost::none, boost::none), delta);
boost::bind(&NavState::retract, state1, _1, boost::none, boost::none),
delta);
EXPECT(assert_equal(eH2, aH2));
}
@ -282,19 +284,6 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
boost::none), bias);
EXPECT(assert_equal(expectedH, actualH));
}
{
Matrix9 aH1, aH2;
Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);
pim.recombinedPrediction(state1, biasCorrectedDelta, aH1, aH2);
Matrix eH1 = numericalDerivative11<Vector9, NavState>(
boost::bind(&PreintegrationBase::recombinedPrediction, pim, _1,
biasCorrectedDelta, boost::none, boost::none), state1);
EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<Vector9, Vector9>(
boost::bind(&PreintegrationBase::recombinedPrediction, pim, state1, _1,
boost::none, boost::none), biasCorrectedDelta);
EXPECT(assert_equal(eH2, aH2));
}
{
Matrix9 aH1;
Matrix96 aH2;
@ -302,11 +291,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
Matrix eH1 = numericalDerivative11<NavState, NavState>(
boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none,
boost::none), state1);
EXPECT(assert_equal(eH1, aH1));
EXPECT(assert_equal(eH1, aH1, 1e-8));
Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
boost::none), bias);
EXPECT(assert_equal(eH2, aH2));
EXPECT(assert_equal(eH2, aH2, 1e-8));
}
}
@ -314,9 +303,9 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
TEST(ImuFactor, ErrorAndJacobians) {
using namespace common;
bool use2ndOrderIntegration = true;
PreintegratedImuMeasurements pim(bias,
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderIntegration);
PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
@ -933,7 +922,8 @@ TEST(ImuFactor, PredictRotation) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, kZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
kZeroOmegaCoriolis);
// Predict
Pose3 x1, x2;