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

Radically simplified bias derivative tests

release/4.3a0
Frank Dellaert 2016-01-30 11:19:49 -08:00
parent 5d95d66077
commit b7d54e60b6
5 changed files with 35 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/CombinedImuFactor.h
View File

@ -120,8 +120,9 @@ public:
* Default constructor, initializes the class with no measurements * Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases * @param bias Current estimate of acceleration and rotation rate biases
*/ */
PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p, PreintegratedCombinedMeasurements(
const imuBias::ConstantBias& biasHat) const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationBase(p, biasHat) { : PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }

4
gtsam/navigation/ImuFactor.cpp
View File

@ -69,9 +69,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
const Matrix3& wCov = p().gyroscopeCovariance; const Matrix3& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance; const Matrix3& iCov = p().integrationCovariance;
// NOTE(luca): (1/dt) allows to pass from continuous time noise to discrete // (1/dt) allows to pass from continuous time noise to discrete time noise
// time noise
// measurementCovariance_discrete = measurementCovariance_contTime/dt
preintMeasCov_ = A * preintMeasCov_ * A.transpose(); preintMeasCov_ = A * preintMeasCov_ * A.transpose();
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();

11
gtsam/navigation/PreintegrationBase.h
View File

@ -202,11 +202,12 @@ public:
Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); } Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); }
NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); } NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); }
const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } const Matrix93 zeta_H_biasAcc() {
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } return (Matrix93() << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_).finished();
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; } }
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; } const Matrix93 zeta_H_biasOmega() {
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } return (Matrix93() << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_).finished();
}
// Exposed for MATLAB // Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); } Vector6 biasHatVector() const { return biasHat_.vector(); }

64
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -34,30 +34,6 @@
#include "imuFactorTesting.h" #include "imuFactorTesting.h"
namespace {
// Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias) {
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
PreintegratedCombinedMeasurements pim(p, bias);
integrateMeasurements(testing::SomeMeasurements(), &pim);
return pim;
}
Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias) {
return evaluatePreintegratedMeasurements(bias).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias) {
return evaluatePreintegratedMeasurements(bias).deltaVij();
}
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements ) { TEST( CombinedImuFactor, PreintegratedMeasurements ) {
// Linearization point // Linearization point
@ -146,38 +122,24 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
// Actual preintegrated values auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
PreintegratedCombinedMeasurements pim = testing::SomeMeasurements measurements;
evaluatePreintegratedMeasurements(kZeroBiasHat);
// Check derivative of rotation boost::function<Vector9(const Vector3&, const Vector3&)> zeta =
boost::function<Vector3(const Vector3&, const Vector3&)> theta =
[=](const Vector3& a, const Vector3& w) { [=](const Vector3& a, const Vector3& w) {
return evaluatePreintegratedMeasurements(Bias(a, w)).theta(); PreintegratedImuMeasurements pim(p, Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.zeta();
}; };
EXPECT(
assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1),
pim.delRdelBiasOmega(), 1e-7));
// Compute numerical derivatives // Actual pre-integrated values
Matrix expectedDelPdelBias = PreintegratedCombinedMeasurements pim(p);
numericalDerivative11<Vector, imuBias::ConstantBias>( testing::integrateMeasurements(measurements, &pim);
evaluatePreintegratedMeasurementsPosition, kZeroBiasHat);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1),
numericalDerivative11<Vector, imuBias::ConstantBias>( pim.zeta_H_biasAcc()));
&evaluatePreintegratedMeasurementsVelocity, kZeroBiasHat); EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1),
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); pim.zeta_H_biasOmega(), 1e-7));
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(), 1e-8));
EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(), 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -63,24 +63,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
return p; return p;
} }
// Auxiliary functions to test pre-integrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */ /* ************************************************************************* */
PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
const Bias& bias) {
PreintegratedImuMeasurements pim(defaultParams(), bias);
integrateMeasurements(testing::SomeMeasurements(), &pim);
return pim;
}
Vector3 evaluatePreintegratedMeasurementsPosition(const Bias& bias) {
return evaluatePreintegratedMeasurements(bias).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(const Bias& bias) {
return evaluatePreintegratedMeasurements(bias).deltaVij();
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
const double deltaT) { const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
@ -454,37 +437,23 @@ TEST(ImuFactor, fistOrderExponential) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
// Actual pre-integrated values testing::SomeMeasurements measurements;
PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(kZeroBias);
// Check derivative of rotation boost::function<Vector9(const Vector3&, const Vector3&)> zeta =
boost::function<Vector3(const Vector3&, const Vector3&)> theta =
[=](const Vector3& a, const Vector3& w) { [=](const Vector3& a, const Vector3& w) {
return evaluatePreintegratedMeasurements(Bias(a, w)).theta(); PreintegratedImuMeasurements pim(defaultParams(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.zeta();
}; };
EXPECT(
assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1),
preintegrated.delRdelBiasOmega(), 1e-7));
// Check derivative of translation // Actual pre-integrated values
Matrix expectedDelPdelBias = numericalDerivative11<Vector, Bias>( PreintegratedImuMeasurements pim(defaultParams());
&evaluatePreintegratedMeasurementsPosition, kZeroBias); testing::integrateMeasurements(measurements, &pim);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega,
preintegrated.delPdelBiasOmega(), 1e-8));
// Check derivative of velocity EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1),
Matrix expectedDelVdelBias = numericalDerivative11<Vector, Bias>( pim.zeta_H_biasAcc()));
&evaluatePreintegratedMeasurementsVelocity, kZeroBias); EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1),
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); pim.zeta_H_biasOmega(), 1e-7));
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega,
preintegrated.delVdelBiasOmega(), 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */