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

Combined two methods and renamed static method

release/4.3a0
Frank Dellaert 2016-01-30 13:43:46 -08:00
parent 77d4e4c33e
commit 5cf98313bd
5 changed files with 38 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -70,7 +70,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF

2
gtsam/navigation/ImuFactor.cpp
View File

@ -55,7 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// Update preintegrated measurements (also get Jacobian)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a

50
gtsam/navigation/PreintegrationBase.cpp
View File

@ -116,7 +116,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
Vector9 PreintegrationBase::UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A,
@ -178,43 +178,31 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc,
void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt, Matrix9* A,
Matrix93* B, Matrix93* C) const {
Matrix93* B, Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega);
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
if (!p().body_P_sensor) {
return UpdateEstimate(unbiasedAcc, unbiasedOmega, dt, preintegrated_, A, B,
C);
} else {
// More complicated derivatives in case of sensor displacement
Matrix3 D_correctedAcc_unbiasedAcc, D_correctedAcc_unbiasedOmega,
D_correctedOmega_unbiasedOmega;
auto corrected = correctMeasurementsBySensorPose(
unbiasedAcc, unbiasedOmega, D_correctedAcc_unbiasedAcc,
D_correctedAcc_unbiasedOmega, D_correctedOmega_unbiasedOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
const Vector9 updated = UpdateEstimate(corrected.first, corrected.second, dt,
preintegrated_, A, B, C);
*C *= D_correctedOmega_unbiasedOmega;
if (!p().body_P_sensor->translation().vector().isZero())
*C += *B* D_correctedAcc_unbiasedOmega;
*B *= D_correctedAcc_unbiasedAcc; // NOTE(frank): needs to be last
return updated;
}
}
//------------------------------------------------------------------------------
void PreintegrationBase::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt,
Matrix9* A, Matrix93* B, Matrix93* C) {
// Do update
deltaTij_ += dt;
preintegrated_ = updatedPreintegrated(measuredAcc, measuredOmega, dt, A, B, C);
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().vector().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);

19
gtsam/navigation/PreintegrationBase.h
View File

@ -172,23 +172,20 @@ public:
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const;
// Update integrated vector on tangent manifold preintegrated with acceleration
// readings a_body and gyro readings w_body, bias-corrected in body frame.
static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
double dt, const Vector9& preintegrated,
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Calculate the updated preintegrated measurement, does not modify
/// It takes measured quantities in the j frame
Vector9 updatedPreintegrated(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt,
Matrix9* A, Matrix93* B, Matrix93* C) const;
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double deltaT, Matrix9* A, Matrix93* B, Matrix93* C);
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
void updatedPreintegrated(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT,
Matrix9* A, Matrix93* B, Matrix93* C);
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far

6
gtsam/navigation/tests/testPreintegrationBase.cpp
View File

@ -39,7 +39,7 @@ static boost::shared_ptr<PreintegrationBase::Params> defaultParams() {
}
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
return PreintegrationBase::UpdateEstimate(a, w, kDt, zeta);
return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
}
/* ************************************************************************* */
@ -50,7 +50,7 @@ TEST(PreintegrationBase, UpdateEstimate1) {
zeta.setZero();
Matrix9 aH1;
Matrix93 aH2, aH3;
pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
@ -64,7 +64,7 @@ TEST(PreintegrationBase, UpdateEstimate2) {
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
Matrix9 aH1;
Matrix93 aH2, aH3;
pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));