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

Rename key method

release/4.3a0
Frank Dellaert 2016-01-30 18:29:04 -08:00
parent 3819b292ec
commit ebba015227
5 changed files with 124 additions and 26 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;
updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C);
PreintegrationBase::integrateMeasurement(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;
updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C);
PreintegrationBase::integrateMeasurement(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

82
gtsam/navigation/PreintegrationBase.cpp
View File

@ -178,7 +178,7 @@ Vector9 PreintegrationBase::UpdatePreintegrated(const Vector3& a_body,
}
//------------------------------------------------------------------------------
void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc,
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt, Matrix9* A,
Matrix93* B, Matrix93* C) {
@ -211,6 +211,15 @@ void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc,
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt) {
// NOTE(frank): integrateMeasuremtn always needs to compute the derivatives,
// even when not of interest to the caller. Provide scratch space here.
Matrix9 A;
Matrix93 B, C;
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
@ -320,6 +329,53 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
const Vector9& zeta12, double deltaT12,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const auto t01 = zeta01.segment<3>(0);
const auto p01 = zeta01.segment<3>(3);
const auto v01 = zeta01.segment<3>(6);
const auto t12 = zeta12.segment<3>(0);
const auto p12 = zeta12.segment<3>(3);
const auto v12 = zeta12.segment<3>(6);
Matrix3 R01_H_t01, R12_H_t12;
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
Matrix3 R02_H_R01, R02_H_R12;
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
Matrix3 t02_H_R02;
Vector9 zeta02;
const Matrix3 R = R01.matrix();
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
if (H1) {
H1->setZero();
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
D_t_t(H1) = I_3x3;
D_t_v(H1) = I_3x3 * deltaT12;
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
D_v_v(H1) = I_3x3;
}
if (H2) {
H2->setZero();
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
D_t_t(H2) = R;
D_v_v(H2) = R;
}
return zeta02;
}
//------------------------------------------------------------------------------
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Matrix9* H2) {
@ -335,28 +391,8 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
const double& t12 = pim12.deltaTij();
deltaTij_ = t01 + t12;
Matrix3 R01_H_theta01, R12_H_theta12;
const Rot3 R01 = Rot3::Expmap(theta(), R01_H_theta01);
const Rot3 R12 = Rot3::Expmap(pim12.theta(), R12_H_theta12);
Matrix3 R02_H_R01, R02_H_R12;
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
Matrix3 theta02_H_R02;
preintegrated_ << Rot3::Logmap(R02, theta02_H_R02),
deltaPij() + deltaVij() * t12 + R01 * pim12.deltaPij(),
deltaVij() + R01 * pim12.deltaVij();
H1->setZero();
D_R_R(H1) = theta02_H_R02 * R02_H_R01 * R01_H_theta01;
D_t_t(H1) = I_3x3;
D_t_v(H1) = I_3x3 * t12;
D_v_v(H1) = I_3x3;
H2->setZero();
D_R_R(H2) = theta02_H_R02 * R02_H_R12 * R12_H_theta12; // I_3x3 ??
D_t_t(H2) = R01.matrix(); // + rotated_H_theta1 ??
D_v_v(H2) = R01.matrix(); // + rotated_H_theta1 ??
preintegrated_ << PreintegrationBase::Compose(
preintegrated(), pim12.preintegrated(), t12, H1, H2);
preintegrated_H_biasAcc_ =
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;

12
gtsam/navigation/PreintegrationBase.h
View File

@ -183,10 +183,14 @@ public:
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
void updatedPreintegrated(const Vector3& measuredAcc,
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT,
Matrix9* A, Matrix93* B, Matrix93* C);
// Version without derivatives
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT);
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
@ -211,6 +215,12 @@ public:
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
// Compose the two preintegrated vectors
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
/// Merge in a different set of measurements and update bias derivatives accordingly
/// The derivatives apply to the preintegrated Vector9
void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);

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

@ -87,6 +87,58 @@ TEST(PreintegrationBase, computeError) {
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}
/* ************************************************************************* */
TEST(PreintegrationBase, Compose) {
testing::SomeMeasurements measurements;
PreintegrationBase pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
boost::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) {
return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij());
};
// Expected merge result
PreintegrationBase expected_pim02(testing::Params());
testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02);
// Actual result
Matrix9 H1, H2;
PreintegrationBase actual_pim02 = pim;
actual_pim02.mergeWith(pim, &H1, &H2);
const Vector9 zeta = pim.preintegrated();
const Vector9 actual_zeta =
PreintegrationBase::Compose(zeta, zeta, pim.deltaTij());
EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7));
EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7));
}
/* ************************************************************************* */
TEST(PreintegrationBase, MergedBiasDerivatives) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> f =
[=](const Vector3& a, const Vector3& w) {
PreintegrationBase pim02(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim02);
testing::integrateMeasurements(measurements, &pim02);
return pim02.preintegrated();
};
// Expected merge result
PreintegrationBase expected_pim02(testing::Params());
testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02);
EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1),
expected_pim02.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1),
expected_pim02.preintegrated_H_biasOmega(), 1e-3));
}
/* ************************************************************************* */
int main() {
TestResult tr;