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
* @param bias Current estimate of acceleration and rotation rate biases
*/
PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat)
PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero();
}

4
gtsam/navigation/ImuFactor.cpp
View File

@ -69,9 +69,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
const Matrix3& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance;
// NOTE(luca): (1/dt) allows to pass from continuous time noise to discrete
// time noise
// measurementCovariance_discrete = measurementCovariance_contTime/dt
// (1/dt) allows to pass from continuous time noise to discrete time noise
preintMeasCov_ = A * preintMeasCov_ * A.transpose();
preintMeasCov_.noalias() += B * (aCov / dt) * B.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()); }
NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); }
const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; }
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; }
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; }
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; }
const Matrix93 zeta_H_biasAcc() {
return (Matrix93() << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_).finished();
}
const Matrix93 zeta_H_biasOmega() {
return (Matrix93() << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_).finished();
}
// Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); }

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

@ -34,30 +34,6 @@
#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 ) {
// Linearization point
@ -146,38 +122,24 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
/* ************************************************************************* */
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
// Actual preintegrated values
PreintegratedCombinedMeasurements pim =
evaluatePreintegratedMeasurements(kZeroBiasHat);
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
testing::SomeMeasurements measurements;
// Check derivative of rotation
boost::function<Vector3(const Vector3&, const Vector3&)> theta =
boost::function<Vector9(const Vector3&, const Vector3&)> zeta =
[=](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
Matrix expectedDelPdelBias =
numericalDerivative11<Vector, imuBias::ConstantBias>(
evaluatePreintegratedMeasurementsPosition, kZeroBiasHat);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
// Actual pre-integrated values
PreintegratedCombinedMeasurements pim(p);
testing::integrateMeasurements(measurements, &pim);
Matrix expectedDelVdelBias =
numericalDerivative11<Vector, imuBias::ConstantBias>(
&evaluatePreintegratedMeasurementsVelocity, kZeroBiasHat);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
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));
EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasOmega(), 1e-7));
}
/* ************************************************************************* */

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

@ -63,24 +63,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
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,
const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
@ -454,37 +437,23 @@ TEST(ImuFactor, fistOrderExponential) {
/* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
// Actual pre-integrated values
PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(kZeroBias);
testing::SomeMeasurements measurements;
// Check derivative of rotation
boost::function<Vector3(const Vector3&, const Vector3&)> theta =
boost::function<Vector9(const Vector3&, const Vector3&)> zeta =
[=](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
Matrix expectedDelPdelBias = numericalDerivative11<Vector, Bias>(
&evaluatePreintegratedMeasurementsPosition, kZeroBias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega,
preintegrated.delPdelBiasOmega(), 1e-8));
// Actual pre-integrated values
PreintegratedImuMeasurements pim(defaultParams());
testing::integrateMeasurements(measurements, &pim);
// Check derivative of velocity
Matrix expectedDelVdelBias = numericalDerivative11<Vector, Bias>(
&evaluatePreintegratedMeasurementsVelocity, kZeroBias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega,
preintegrated.delVdelBiasOmega(), 1e-8));
EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasOmega(), 1e-7));
}
/* ************************************************************************* */