30 using namespace std::placeholders;
32 using namespace gtsam;
58 const Vector3& bias,
const list<Vector3>& measuredOmegas,
59 const list<double>& deltaTs,
60 const Vector3& initialRotationRate = Vector3::Zero()) {
63 list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
64 list<double>::const_iterator itDeltaT = deltaTs.begin();
65 for (; itOmega != measuredOmegas.end(); ++itOmega, ++itDeltaT) {
66 result.integrateMeasurement(*itOmega, *itDeltaT);
72 Rot3 evaluatePreintegratedMeasurementsRotation(
73 const Vector3& bias,
const list<Vector3>& measuredOmegas,
74 const list<double>& deltaTs,
75 const Vector3& initialRotationRate = Vector3::Zero()) {
77 evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
78 initialRotationRate).deltaRij());
101 Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 *
M_PI / 100.0, 0.0, 0.0);
102 double expectedDeltaT1(0.5);
112 Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 *
M_PI / 100.0, 0.0, 0.0);
113 double expectedDeltaT2(1);
125 Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4;
126 Vector3 omegaCoriolis(0.1, 0.5, 0.9);
130 double deltaTij = 0.02;
131 Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
132 Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
134 std::make_shared<PreintegratedRotationParams>(
params),
172 errorExpected << 0, 0, 0;
176 Matrix H1e = numericalDerivative11<Vector3, Rot3>(
178 Matrix H2e = numericalDerivative11<Vector3, Rot3>(
180 Matrix H3e = numericalDerivative11<Vector3, Vector3>(
184 Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
185 std::bind(&evaluateRotationError, factor, std::placeholders::_1,
x2, bias),
x1);
186 Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
187 std::bind(&evaluateRotationError, factor,
x1, std::placeholders::_1, bias),
x2);
191 (void) factor.evaluateError(
x1,
x2, bias, H1a, H2a, H3a);
231 errorExpected << 0, 0, 0;
235 Matrix H1e = numericalDerivative11<Vector, Rot3>(
237 Matrix H2e = numericalDerivative11<Vector, Rot3>(
239 Matrix H3e = numericalDerivative11<Vector, Vector3>(
243 Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
244 std::bind(&evaluateRotationError, factor, std::placeholders::_1,
x2, bias),
x1);
245 Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
246 std::bind(&evaluateRotationError, factor,
x1, std::placeholders::_1, bias),
x2);
247 Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
248 std::bind(&evaluateRotationError, factor,
x1,
x2, std::placeholders::_1), bias);
270 Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
273 const Matrix3 Jr = Rot3::ExpmapDerivative(
276 Matrix3 actualdelRdelBiasOmega = -Jr *
deltaT;
288 thetahat << 0.1, 0.1, 0;
292 deltatheta << 0, 0, 0;
295 Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
296 std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
300 double normx =
x.norm();
301 const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 *
X
302 + (1 / (normx * normx) - (1 +
cos(normx)) / (2 * normx *
sin(normx))) *
X
325 const Matrix3 Jr = Rot3::ExpmapDerivative(
328 Matrix3 delRdelBiasOmega = -Jr *
deltaT;
333 const Matrix3 hatRot =
335 const Matrix3 actualRot = hatRot
336 *
Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
345 Vector3 bias = Vector3::Zero();
350 list<Vector3> measuredOmegas;
351 list<double> deltaTs;
352 measuredOmegas.push_back(
Vector3(
M_PI / 100.0, 0.0, 0.0));
353 deltaTs.push_back(0.01);
354 measuredOmegas.push_back(
Vector3(
M_PI / 100.0, 0.0, 0.0));
355 deltaTs.push_back(0.01);
356 for (
int i = 1;
i < 100;
i++) {
357 measuredOmegas.push_back(
359 deltaTs.push_back(0.01);
364 evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
368 Matrix expectedDelRdelBias =
369 numericalDerivative11<Rot3, Vector3>(
370 std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
371 measuredOmegas, deltaTs,
Vector3(
M_PI / 100.0, 0.0, 0.0)), bias);
372 Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
392 omegaCoriolis << 0, 0.1, 0.1;
411 Matrix H1e = numericalDerivative11<Vector, Rot3>(
413 Matrix H2e = numericalDerivative11<Vector, Rot3>(
415 Matrix H3e = numericalDerivative11<Vector, Vector3>(
419 Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
420 std::bind(&evaluateRotationError, factor, std::placeholders::_1,
x2, bias),
x1);
421 Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
422 std::bind(&evaluateRotationError, factor,
x1, std::placeholders::_1, bias),
x2);
423 Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
424 std::bind(&evaluateRotationError, factor,
x1,
x2, std::placeholders::_1), bias);
443 for (
int i = 0;
i < 1000; ++
i) {
447 Matrix expectedMeasCov(3,3);
455 Rot3 expectedRot = Rot3::Ypr(20*
M_PI, 0, 0);
460 Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
474 Rot3 x1(Rot3::RzRyRx(0, 0, 0));
491 for (
size_t i = 0;
i < 5; ++
i) {
503 Rot3 expectedRot(Rot3::RzRyRx(0,
M_PI / 4, 0));