Add a strong end-to-end test
Frank Dellaert
parent
ca50c82bad
commit
e47b4e5b2c
|
|
@ -29,9 +29,12 @@
|
|||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <list>
|
||||
#include <memory>
|
||||
#include "gtsam/nonlinear/LevenbergMarquardtParams.h"
|
||||
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
|
|
@ -39,7 +42,7 @@ using namespace gtsam;
|
|||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::B;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::R;
|
||||
|
||||
Vector3 kZeroOmegaCoriolis(0, 0, 0);
|
||||
|
||||
|
|
@ -136,7 +139,7 @@ TEST(AHRSFactor, Error) {
|
|||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {});
|
||||
|
||||
// Check value
|
||||
Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
|
||||
|
|
@ -145,8 +148,8 @@ TEST(AHRSFactor, Error) {
|
|||
|
||||
// Check Derivatives
|
||||
Values values;
|
||||
values.insert(X(1), Ri);
|
||||
values.insert(X(2), Rj);
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
|
||||
}
|
||||
|
|
@ -165,7 +168,7 @@ TEST(AHRSFactor, ErrorWithBiases) {
|
|||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
|
||||
// Check value
|
||||
Vector3 errorExpected(0, 0, 0);
|
||||
|
|
@ -174,8 +177,8 @@ TEST(AHRSFactor, ErrorWithBiases) {
|
|||
|
||||
// Check Derivatives
|
||||
Values values;
|
||||
values.insert(X(1), Ri);
|
||||
values.insert(X(2), Rj);
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
|
||||
}
|
||||
|
|
@ -324,12 +327,12 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov()));
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis);
|
||||
|
||||
// Check Derivatives
|
||||
Values values;
|
||||
values.insert(X(1), Ri);
|
||||
values.insert(X(2), Rj);
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
|
||||
}
|
||||
|
|
@ -350,7 +353,7 @@ TEST(AHRSFactor, predictTest) {
|
|||
expectedMeasCov = 200 * kMeasuredOmegaCovariance;
|
||||
EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
|
||||
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
|
||||
// Predict
|
||||
Rot3 x;
|
||||
|
|
@ -368,7 +371,6 @@ TEST(AHRSFactor, predictTest) {
|
|||
EXPECT(assert_equal(expectedH, H, 1e-8));
|
||||
}
|
||||
//******************************************************************************
|
||||
|
||||
TEST(AHRSFactor, graphTest) {
|
||||
// linearization point
|
||||
Rot3 Ri(Rot3::RzRyRx(0, 0, 0));
|
||||
|
|
@ -393,15 +395,87 @@ TEST(AHRSFactor, graphTest) {
|
|||
}
|
||||
|
||||
// pim.print("Pre integrated measurements");
|
||||
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
values.insert(X(1), Ri);
|
||||
values.insert(X(2), Rj);
|
||||
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
|
||||
values.insert(R(1), Ri);
|
||||
values.insert(R(2), Rj);
|
||||
values.insert(B(1), bias);
|
||||
graph.push_back(factor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values);
|
||||
Values result = optimizer.optimize();
|
||||
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0));
|
||||
EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
|
||||
EXPECT(assert_equal(expectedRot, result.at<Rot3>(R(2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(AHRSFactor, bodyPSensorWithBias) {
|
||||
using noiseModel::Diagonal;
|
||||
|
||||
int numRotations = 10;
|
||||
const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6);
|
||||
SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
|
||||
|
||||
// Measurements in the sensor frame:
|
||||
const double omega = 0.1;
|
||||
const Vector3 realOmega(omega, 0, 0);
|
||||
const Vector3 realBias(1, 2, 3); // large !
|
||||
const Vector3 measuredOmega = realOmega + realBias;
|
||||
|
||||
auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
|
||||
p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0));
|
||||
p->gyroscopeCovariance = 1e-8 * I_3x3;
|
||||
double deltaT = 0.005;
|
||||
|
||||
// Specify noise values on priors
|
||||
const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001);
|
||||
const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1);
|
||||
SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
|
||||
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
|
||||
|
||||
// Create a factor graph with priors on initial pose, velocity and bias
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
|
||||
graph.addPrior(R(0), Rot3(), priorNoisePose);
|
||||
values.insert(R(0), Rot3());
|
||||
|
||||
// The key to this test is that we specify the bias, in the sensor frame, as
|
||||
// known a priori. We also create factors below that encode our assumption
|
||||
// that this bias is constant over time In theory, after optimization, we
|
||||
// should recover that same bias estimate
|
||||
graph.addPrior(B(0), realBias, priorNoiseBias);
|
||||
values.insert(B(0), realBias);
|
||||
|
||||
// Now add IMU factors and bias noise models
|
||||
const Vector3 zeroBias(0, 0, 0);
|
||||
for (int i = 1; i < numRotations; i++) {
|
||||
PreintegratedAhrsMeasurements pim(p, realBias);
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredOmega, deltaT);
|
||||
|
||||
// Create factors
|
||||
graph.emplace_shared<AHRSFactor>(R(i - 1), R(i), B(i - 1), pim);
|
||||
graph.emplace_shared<BetweenFactor<Vector3> >(B(i - 1), B(i), zeroBias,
|
||||
biasNoiseModel);
|
||||
|
||||
values.insert(R(i), Rot3());
|
||||
values.insert(B(i), realBias);
|
||||
}
|
||||
|
||||
// Finally, optimize, and get bias at last time step
|
||||
LevenbergMarquardtParams params;
|
||||
// params.setVerbosityLM("SUMMARY");
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
|
||||
const Vector3 biasActual = result.at<Vector3>(B(numRotations - 1));
|
||||
|
||||
// Bias should be a self-fulfilling prophesy:
|
||||
EXPECT(assert_equal(realBias, biasActual, 1e-3));
|
||||
|
||||
// Check that the successive rotations are all `omega` apart:
|
||||
for (int i = 0; i < numRotations; i++) {
|
||||
Rot3 expectedRot = Rot3::Pitch(omega * i);
|
||||
Rot3 actualRot = result.at<Rot3>(R(i));
|
||||
EXPECT(assert_equal(expectedRot, actualRot, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
|
|||
const Matrix3 Jr =
|
||||
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PartialDerivativeLogmap) {
|
||||
// Linearization point
|
||||
Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
|
||||
Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 deltatheta(0, 0, 0);
|
||||
Vector3 deltaTheta(0, 0, 0);
|
||||
|
||||
auto evaluateLogRotation = [=](const Vector3 deltatheta) {
|
||||
auto evaluateLogRotation = [=](const Vector3 delta) {
|
||||
return Rot3::Logmap(
|
||||
Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
|
||||
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
|
||||
};
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelFdeltheta =
|
||||
numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltatheta);
|
||||
Matrix expectedDelFdelTheta =
|
||||
numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
|
||||
|
||||
Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
|
||||
Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
|
||||
EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
|
||||
// change w.r.t. linearization point
|
||||
double alpha = 0.0;
|
||||
Vector3 deltabiasOmega;
|
||||
deltabiasOmega << alpha, alpha, alpha;
|
||||
Vector3 deltaBiasOmega;
|
||||
deltaBiasOmega << alpha, alpha, alpha;
|
||||
|
||||
const Matrix3 Jr = Rot3::ExpmapDerivative(
|
||||
(measuredOmega - biasOmega) * deltaT);
|
||||
|
|
@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
const Matrix expectedRot = Rot3::Expmap(
|
||||
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
|
||||
(measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix();
|
||||
|
||||
const Matrix3 hatRot =
|
||||
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
|
||||
const Matrix3 actualRot = hatRot
|
||||
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
|
||||
// hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
|
||||
* Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
|
||||
|
||||
// This is a first order expansion so the equality is only an approximation
|
||||
EXPECT(assert_equal(expectedRot, actualRot));
|
||||
|
|
@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
using noiseModel::Diagonal;
|
||||
typedef Bias Bias;
|
||||
|
||||
int numFactors = 10;
|
||||
int numPoses = 10;
|
||||
Vector6 noiseBetweenBiasSigma;
|
||||
noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
|
||||
3.0e-6, 3.0e-6);
|
||||
|
|
@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
|
||||
Vector3 zeroVel(0, 0, 0);
|
||||
|
||||
// Create a factor graph with priors on initial pose, vlocity and bias
|
||||
// Create a factor graph with priors on initial pose, velocity and bias
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
|
||||
|
|
@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
|
||||
// Now add IMU factors and bias noise models
|
||||
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
for (int i = 1; i < numFactors; i++) {
|
||||
PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
|
||||
priorBias);
|
||||
for (int i = 1; i < numPoses; i++) {
|
||||
PreintegratedImuMeasurements pim(p, priorBias);
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
|
|
@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
}
|
||||
|
||||
// Finally, optimize, and get bias at last time step
|
||||
Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
|
||||
Bias biasActual = results.at<Bias>(B(numFactors - 1));
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
|
||||
Bias biasActual = result.at<Bias>(B(numPoses - 1));
|
||||
|
||||
// And compare it with expected value (our prior)
|
||||
Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
|
||||
|
|
@ -851,11 +849,11 @@ struct ImuFactorMergeTest {
|
|||
actual_pim02.preintegrated(), tol));
|
||||
EXPECT(assert_equal(pim02_expected, actual_pim02, tol));
|
||||
|
||||
ImuFactor::shared_ptr factor01 =
|
||||
auto factor01 =
|
||||
std::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
|
||||
ImuFactor::shared_ptr factor12 =
|
||||
auto factor12 =
|
||||
std::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
|
||||
ImuFactor::shared_ptr factor02_expected = std::make_shared<ImuFactor>(
|
||||
auto factor02_expected = std::make_shared<ImuFactor>(
|
||||
X(0), V(0), X(2), V(2), B(0), pim02_expected);
|
||||
|
||||
ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);
|
||||
|
|
|
|||
Loading…
Reference in New Issue