39 static std::shared_ptr<PreintegrationParams>
Params() {
40 auto p = PreintegrationParams::MakeSharedD(
kGravity);
43 p->integrationCovariance = 0.0001 * I_3x3;
56 factor.
evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
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;
427 auto evaluateLogRotation = [=](
const Vector3 deltatheta) {
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();
494 p->omegaCoriolis = kNonZeroOmegaCoriolis;
498 const double T = 3.0;
515 const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
516 const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
520 Matrix3 D_correctedAcc_measuredOmega = Z_3x3;
522 nullptr, D_correctedAcc_measuredOmega,
nullptr);
523 Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
571 double diffDelta = 1
e-8;
577 gttic(PredictPositionAndVelocity);
590 for (
int i = 0;
i < 1000; ++
i)
606 gttic(PredictRotation);
619 for (
int i = 0;
i < 1000; ++
i)
633 gttic(PredictArbitrary);
640 const double T = 3.0;
657 p->integrationCovariance = Z_3x3;
664 for (
int i = 0;
i < 1000; ++
i)
675 +0.903715275, -0.250741668, 0.347026393,
676 +0.347026393, 0.903715275, -0.250741668,
677 -0.250741668, 0.347026393, 0.903715275);
679 Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571);
686 gttic(bodyPSensorNoBias);
704 for (
int i = 0;
i < 1000; ++
i)
716 Vector3 expectedVelocity(0, 0, 0);
727 gttic(bodyPSensorWithBias);
732 Vector6 noiseBetweenBiasSigma;
735 SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
748 p->accelerometerCovariance = 1
e-7 * I_3x3;
749 p->gyroscopeCovariance = 1
e-8 * I_3x3;
750 p->integrationCovariance = 1
e-9 * I_3x3;
754 Vector6 priorNoisePoseSigmas(
755 (
Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished());
756 Vector3 priorNoiseVelSigmas((
Vector(3) << 0.1, 0.1, 0.1).finished());
757 Vector6 priorNoiseBiasSigmas(
758 (
Vector(6) << 0.1, 0.1, 0.1, 0.5
e-1, 0.5
e-1, 0.5
e-1).finished());
759 SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
760 SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas);
761 SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
783 for (
int i = 1;
i < numFactors;
i++) {
786 for (
int j = 0;
j < 200; ++
j)
808 #ifdef GTSAM_TANGENT_PREINTEGRATION
809 static const double kVelocity = 2.0, kAngularVelocity =
M_PI / 6;
811 struct ImuFactorMergeTest {
812 std::shared_ptr<PreintegrationParams> p_;
820 p_->gyroscopeCovariance = I_3x3 * 0.01;
821 p_->accelerometerCovariance = I_3x3 * 0.03;
824 int TestScenario(
TestResult& result_,
const std::string& name_,
827 const Bias& bias12,
double tol) {
837 for (
int i = 0;
i < 100;
i++) {
840 Vector3 accel_meas = runner.actualSpecificForce(
t);
841 Vector3 omega_meas = runner.actualAngularVelocity(
t);
842 pim02_expected.integrateMeasurement(accel_meas, omega_meas,
deltaT);
844 pim01.integrateMeasurement(accel_meas, omega_meas,
deltaT);
846 pim12.integrateMeasurement(accel_meas, omega_meas,
deltaT);
849 auto actual_pim02 = ImuFactor::Merge(pim01, pim12);
851 actual_pim02.preintegrated(),
tol));
855 std::make_shared<ImuFactor>(
X(0),
V(0),
X(1),
V(1),
B(0), pim01);
857 std::make_shared<ImuFactor>(
X(1),
V(1),
X(2),
V(2),
B(0), pim12);
859 X(0),
V(0),
X(2),
V(2),
B(0), pim02_expected);
866 void TestScenarios(
TestResult& result_,
const std::string& name_,
868 const Bias& bias12,
double tol) {
869 for (
auto scenario : {forward_, loop_})
870 TestScenario(result_, name_,
scenario, bias01, bias12,
tol);
877 ImuFactorMergeTest mergeTest;
878 mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1
e-4);
883 ImuFactorMergeTest mergeTest;
885 mergeTest.TestScenarios(result_, name_, bias, bias, 1
e-4);
890 ImuFactorMergeTest mergeTest;
891 mergeTest.TestScenarios(result_, name_,
898 ImuFactorMergeTest mergeTest;
899 mergeTest.p_->omegaCoriolis =
Vector3(0.1, 0.2, -0.1);
900 mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1
e-4);
907 gttic(CheckCovariance);
916 expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0,
917 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0,
918 0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0,
919 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0,
920 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0,
921 0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07,
922 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0,
923 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0,
924 0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06;