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

Now testing bias-correction Jacobians in right place

release/4.3a0
dellaert 2016-06-04 16:53:20 -07:00
parent 105a8183fb
commit facab116ce
3 changed files with 69 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -444,28 +444,6 @@ TEST(ImuFactor, fistOrderExponential) {
EXPECT(assert_equal(expectedRot, actualRot));
}
/* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
// Actual pre-integrated values
PreintegratedImuMeasurements pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
pim.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
pim.preintegrated_H_biasOmega(), 1e-3));
}
#endif
/* ************************************************************************* */
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
const Vector3& measuredAcc, const Vector3& measuredOmega) {

83
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -26,17 +26,6 @@
#include "imuFactorTesting.h"
static const double kDt = 0.1;
Vector9 f(ManifoldPreintegration pim, const Vector3& a, const Vector3& w) {
NavState state = pim.deltaXij();
Matrix9 aH1;
Matrix93 aH2, aH3;
pim.update(a, w, kDt, &aH1, &aH2, &aH3);
NavState newNavState = pim.deltaXij();
return state.localCoordinates(newNavState);
}
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> Params() {
@ -49,34 +38,54 @@ static boost::shared_ptr<PreintegrationParams> Params() {
}
/* ************************************************************************* */
TEST(ManifoldPreintegration, UpdateEstimate1) {
ManifoldPreintegration pim(testing::Params());
ManifoldPreintegration pimActual(pim);
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Vector9 zeta;
zeta.setZero();
Matrix9 aH1;
Matrix93 aH2, aH3;
pim.update(acc, omega, kDt, &aH1, &aH2, &aH3);
EXPECT(assert_equal(numericalDerivative31(f, pimActual, acc, omega), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, pimActual, acc, omega), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, pimActual, acc, omega), aH3, 1e-9));
}
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
/* ************************************************************************* */
TEST(ManifoldPreintegration, UpdateEstimate2) {
ManifoldPreintegration pim(testing::Params());
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Vector9 zeta;
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
Matrix9 aH1;
Matrix93 aH2, aH3;
pim.update(acc, omega, kDt, &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));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaVij();
};
// Actual pre-integrated values
ManifoldPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(
assert_equal(numericalDerivative21(deltaRij, kZero, kZero),
Matrix3(Z_3x3)));
EXPECT(
assert_equal(numericalDerivative22(deltaRij, kZero, kZero),
pim.delRdelBiasOmega(), 1e-3));
EXPECT(
assert_equal(numericalDerivative21(deltaPij, kZero, kZero),
pim.delPdelBiasAcc()));
EXPECT(
assert_equal(numericalDerivative22(deltaPij, kZero, kZero),
pim.delPdelBiasOmega(), 1e-3));
EXPECT(
assert_equal(numericalDerivative21(deltaVij, kZero, kZero),
pim.delVdelBiasAcc()));
EXPECT(
assert_equal(numericalDerivative22(deltaVij, kZero, kZero),
pim.delVdelBiasOmega(), 1e-3));
}
/* ************************************************************************* */

23
gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -72,6 +72,29 @@ TEST(TangentPreintegration, UpdateEstimate2) {
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
}
/* ************************************************************************* */
TEST(ImuFactor, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
TangentPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
// Actual pre-integrated values
TangentPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(
assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
pim.preintegrated_H_biasAcc()));
EXPECT(
assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
pim.preintegrated_H_biasOmega(), 1e-3));
}
/* ************************************************************************* */
TEST(TangentPreintegration, computeError) {
TangentPreintegration pim(testing::Params());