39 static std::shared_ptr<PreintegrationParams>
Params() {
40 auto p = PreintegrationParams::MakeSharedD(
kGravity);
43 p->integrationCovariance = 0.0001 * I_3x3;
98 const double a = 0.2,
v = 50;
103 const Point3 initial_position(10, 20, 0);
104 const Vector3 initial_velocity(
v, 0, 0);
109 const double T = 3.0;
122 const double a = 0.1,
w =
M_PI / 100.0;
130 Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
149 std::bind(&PreintegrationBase::computeError, actual,
150 std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
151 nullptr,
nullptr,
nullptr);
158 Vector3 expectedDeltaR2(2.0 * 0.5 *
M_PI / 100.0, 0.0, 0.0);
159 Vector3 expectedDeltaP2(0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0);
182 -kGravityAlongNavZDown);
195 p->omegaCoriolis =
Vector3(0.02, 0.03, 0.04);
196 p->use2ndOrderCoriolis =
true;
205 Matrix expectedH = numericalDerivative11<Vector9, Bias>(
206 std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
207 std::placeholders::_1,
nullptr), kZeroBias);
213 Matrix eH1 = numericalDerivative11<NavState, NavState>(
214 std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
215 kZeroBias,
nullptr,
nullptr),
state1);
217 Matrix eH2 = numericalDerivative11<NavState, Bias>(
218 std::bind(&PreintegrationBase::predict, pim,
state1,
219 std::placeholders::_1,
nullptr,
nullptr), kZeroBias);
255 expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
269 double diffDelta = 1
e-7;
273 Matrix H1a, H2a, H3a, H4a, H5a;
279 Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
280 std::bind(&evaluateRotationError, factor, std::placeholders::_1,
v1,
x2,
v2, kZeroBias),
284 Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
285 std::bind(&evaluateRotationError, factor,
x1,
v1, std::placeholders::_1,
v2, kZeroBias),
292 expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901;
301 Vector whitened =
R * expectedError;
325 p->omegaCoriolis = kNonZeroOmegaCoriolis;
334 Matrix expectedH = numericalDerivative11<Vector9, Bias>(
335 std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
336 std::placeholders::_1,
nullptr),
bias);
350 double diffDelta = 1
e-7;
371 p->omegaCoriolis = kNonZeroOmegaCoriolis;
372 p->use2ndOrderCoriolis =
true;
389 double diffDelta = 1
e-7;
402 auto evaluateRotation = [=](
const Vector3 biasOmega) {
407 Matrix expectedDelRdelBiasOmega =
408 numericalDerivative11<Rot3, Vector3>(evaluateRotation, biasOmega);
413 Matrix3 actualDelRdelBiasOmega = -Jr *
deltaT;
433 Matrix expectedDelFdelTheta =
434 numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
436 Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
456 const Matrix3 Jr = Rot3::ExpmapDerivative(
459 Matrix3 delRdelBiasOmega = -Jr *
deltaT;
464 const Matrix3 hatRot =
466 const Matrix3 actualRot = hatRot
467 *
Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
493 p->omegaCoriolis = kNonZeroOmegaCoriolis;
497 const double T = 3.0;
514 const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
515 const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
519 Matrix3 D_correctedAcc_measuredOmega = Z_3x3;
521 nullptr, D_correctedAcc_measuredOmega,
nullptr);
522 Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
570 double diffDelta = 1
e-8;
576 gttic(PredictPositionAndVelocity);
589 for (
int i = 0;
i < 1000; ++
i)
605 gttic(PredictRotation);
618 for (
int i = 0;
i < 1000; ++
i)
632 gttic(PredictArbitrary);
639 const double T = 3.0;
656 p->integrationCovariance = Z_3x3;
663 for (
int i = 0;
i < 1000; ++
i)
674 +0.903715275, -0.250741668, 0.347026393,
675 +0.347026393, 0.903715275, -0.250741668,
676 -0.250741668, 0.347026393, 0.903715275);
678 Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571);
685 gttic(bodyPSensorNoBias);
703 for (
int i = 0;
i < 1000; ++
i)
715 Vector3 expectedVelocity(0, 0, 0);
726 gttic(bodyPSensorWithBias);
731 Vector6 noiseBetweenBiasSigma;
734 SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
747 p->accelerometerCovariance = 1
e-7 * I_3x3;
748 p->gyroscopeCovariance = 1
e-8 * I_3x3;
749 p->integrationCovariance = 1
e-9 * I_3x3;
753 Vector6 priorNoisePoseSigmas(
754 (
Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished());
755 Vector3 priorNoiseVelSigmas((
Vector(3) << 0.1, 0.1, 0.1).finished());
756 Vector6 priorNoiseBiasSigmas(
757 (
Vector(6) << 0.1, 0.1, 0.1, 0.5
e-1, 0.5
e-1, 0.5
e-1).finished());
758 SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
759 SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas);
760 SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
782 for (
int i = 1;
i < numPoses;
i++) {
784 for (
int j = 0;
j < 200; ++
j)
806 #ifdef GTSAM_TANGENT_PREINTEGRATION
807 static const double kVelocity = 2.0, kAngularVelocity =
M_PI / 6;
809 struct ImuFactorMergeTest {
810 std::shared_ptr<PreintegrationParams> p_;
818 p_->gyroscopeCovariance = I_3x3 * 0.01;
819 p_->accelerometerCovariance = I_3x3 * 0.03;
822 int TestScenario(
TestResult& result_,
const std::string& name_,
825 const Bias& bias12,
double tol) {
835 for (
int i = 0;
i < 100;
i++) {
838 Vector3 accel_meas = runner.actualSpecificForce(
t);
839 Vector3 omega_meas = runner.actualAngularVelocity(
t);
840 pim02_expected.integrateMeasurement(accel_meas, omega_meas,
deltaT);
842 pim01.integrateMeasurement(accel_meas, omega_meas,
deltaT);
844 pim12.integrateMeasurement(accel_meas, omega_meas,
deltaT);
847 auto actual_pim02 = ImuFactor::Merge(pim01, pim12);
849 actual_pim02.preintegrated(),
tol));
853 std::make_shared<ImuFactor>(
X(0),
V(0),
X(1),
V(1),
B(0), pim01);
855 std::make_shared<ImuFactor>(
X(1),
V(1),
X(2),
V(2),
B(0), pim12);
856 auto factor02_expected = std::make_shared<ImuFactor>(
857 X(0),
V(0),
X(2),
V(2),
B(0), pim02_expected);
864 void TestScenarios(
TestResult& result_,
const std::string& name_,
866 const Bias& bias12,
double tol) {
867 for (
auto scenario : {forward_, loop_})
868 TestScenario(result_, name_,
scenario, bias01, bias12,
tol);
875 ImuFactorMergeTest mergeTest;
876 mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1
e-4);
881 ImuFactorMergeTest mergeTest;
883 mergeTest.TestScenarios(result_, name_,
bias,
bias, 1
e-4);
888 ImuFactorMergeTest mergeTest;
889 mergeTest.TestScenarios(result_, name_,
896 ImuFactorMergeTest mergeTest;
897 mergeTest.p_->omegaCoriolis =
Vector3(0.1, 0.2, -0.1);
898 mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1
e-4);
905 gttic(CheckCovariance);
914 expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0,
915 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0,
916 0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0,
917 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0,
918 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0,
919 0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07,
920 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0,
921 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0,
922 0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06;