1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file testInertialNavFactor_GlobalVelocity.cpp
14 * @brief Unit test for the InertialNavFactor_GlobalVelocity
15 * @author Vadim Indelman, Stephen Williams
16 */
17
18#include <iostream>
19#include <CppUnitLite/TestHarness.h>
20#include <gtsam/navigation/ImuBias.h>
21#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
22#include <gtsam/geometry/Pose3.h>
23#include <gtsam/nonlinear/Values.h>
24#include <gtsam/inference/Key.h>
25#include <gtsam/base/numericalDerivative.h>
26#include <gtsam/base/TestableAssertions.h>
27
28using namespace std::placeholders;
29using namespace std;
30using namespace gtsam;
31
32Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733,
33 0.67835, -0.60471);
34
35static const Vector3 world_g(0.0, 0.0, 9.81);
36static const Vector3 world_rho(0.0, -1.5724e-05, 0.0); // NED system
37static const Vector3 ECEF_omega_earth(0.0, 0.0, 7.292115e-5);
38static const Vector3 world_omega_earth = world_R_ECEF.matrix()
39 * ECEF_omega_earth;
40
41/* ************************************************************************* */
42Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1,
43 const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2,
44 const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
45 return Pose3::Expmap(xi: factor.evaluateError(x: p1, x: v1, x: b1, x: p2, x: v2).head(n: 6));
46}
47
48Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
49 const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2,
50 const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
51 return factor.evaluateError(x: p1, x: v1, x: b1, x: p2, x: v2).tail(n: 3);
52}
53
54/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Constructor) {
55 Key Pose1(11);
56 Key Pose2(12);
57 Key Vel1(21);
58 Key Vel2(22);
59 Key Bias1(31);
60
61 Vector3 measurement_acc(0.1, 0.2, 0.4);
62 Vector3 measurement_gyro(-0.2, 0.5, 0.03);
63
64 double measurement_dt(0.1);
65
66 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
67
68 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
69 Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
70 measurement_dt, world_g, world_rho, world_omega_earth, model);
71}
72
73/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Equals) {
74 Key Pose1(11);
75 Key Pose2(12);
76 Key Vel1(21);
77 Key Vel2(22);
78 Key Bias1(31);
79
80 Vector3 measurement_acc(0.1, 0.2, 0.4);
81 Vector3 measurement_gyro(-0.2, 0.5, 0.03);
82
83 double measurement_dt(0.1);
84
85 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
86
87 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
88 Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
89 measurement_dt, world_g, world_rho, world_omega_earth, model);
90 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> g(
91 Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
92 measurement_dt, world_g, world_rho, world_omega_earth, model);
93 CHECK(assert_equal(f, g, 1e-5));
94}
95
96/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Predict) {
97 Key PoseKey1(11);
98 Key PoseKey2(12);
99 Key VelKey1(21);
100 Key VelKey2(22);
101 Key BiasKey1(31);
102
103 double measurement_dt(0.1);
104
105 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
106
107 // First test: zero angular motion, some acceleration
108 Vector measurement_acc(Vector3(0.1, 0.2, 0.3 - 9.81));
109 Vector measurement_gyro(Vector3(0.0, 0.0, 0.0));
110
111 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
112 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
113 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
114 model);
115
116 Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
117 Vector3 Vel1(Vector3(0.50, -0.50, 0.40));
118 imuBias::ConstantBias Bias1;
119 Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
120 Vector3 expectedVel2(Vector3(0.51, -0.48, 0.43));
121 Pose3 actualPose2;
122 Vector3 actualVel2;
123 f.predict(Pose1, Vel1, Bias1, Pose2&: actualPose2, Vel2&: actualVel2);
124
125 CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
126 CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5));
127}
128
129/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) {
130 Key PoseKey1(11);
131 Key PoseKey2(12);
132 Key VelKey1(21);
133 Key VelKey2(22);
134 Key BiasKey1(31);
135
136 double measurement_dt(0.1);
137
138 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
139
140 // First test: zero angular motion, some acceleration
141 Vector measurement_acc(Vector3(0.1, 0.2, 0.3 - 9.81));
142 Vector measurement_gyro(Vector3(0.0, 0.0, 0.0));
143
144 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
145 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
146 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
147 model);
148
149 Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
150 Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
151 Vector3 Vel1(Vector3(0.50, -0.50, 0.40));
152 Vector3 Vel2(Vector3(0.51, -0.48, 0.43));
153 imuBias::ConstantBias Bias1;
154
155 Vector ActualErr(f.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2));
156 Vector ExpectedErr(Z_9x1);
157
158 CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
159}
160
161/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRot) {
162 Key PoseKey1(11);
163 Key PoseKey2(12);
164 Key VelKey1(21);
165 Key VelKey2(22);
166 Key BiasKey1(31);
167
168 double measurement_dt(0.1);
169
170 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
171
172 // Second test: zero angular motion, some acceleration
173 Vector measurement_acc(Vector3(0.0, 0.0, 0.0 - 9.81));
174 Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
175
176 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
177 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
178 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
179 model);
180
181 Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0));
182 Pose3 Pose2(Rot3::Expmap(v: measurement_gyro * measurement_dt),
183 Point3(2.0, 1.0, 3.0));
184 Vector3 Vel1(Vector3(0.0, 0.0, 0.0));
185 Vector3 Vel2(Vector3(0.0, 0.0, 0.0));
186 imuBias::ConstantBias Bias1;
187
188 Vector ActualErr(f.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2));
189 Vector ExpectedErr(Z_9x1);
190
191 CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
192}
193
194/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) {
195 Key PoseKey1(11);
196 Key PoseKey2(12);
197 Key VelKey1(21);
198 Key VelKey2(22);
199 Key BiasKey1(31);
200
201 double measurement_dt(0.1);
202
203 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
204
205 // Second test: zero angular motion, some acceleration - generated in matlab
206 Vector measurement_acc(
207 Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
208 Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
209
210 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
211 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
212 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
213 model);
214
215 Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
216 -0.427669306, -0.652537293, 0.709880342, 0.265075427);
217 Point3 t1(2.0, 1.0, 3.0);
218 Pose3 Pose1(R1, t1);
219 Vector3 Vel1(Vector3(0.5, -0.5, 0.4));
220 Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
221 -0.422594037, -0.636011287, 0.731761397, 0.244979388);
222 Point3 t2 = t1 + Point3(Vel1 * measurement_dt);
223 Pose3 Pose2(R2, t2);
224 Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
225 Vector3 Vel2 = Vel1 + dv;
226 imuBias::ConstantBias Bias1;
227
228 Vector ActualErr(f.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2));
229 Vector ExpectedErr(Z_9x1);
230
231 // TODO: Expected values need to be updated for global velocity version
232 CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
233}
234
235///* VADIM - START ************************************************************************* */
236//Vector3 predictionRq(const Vector3 angles, const Vector3 q) {
237// return (Rot3().RzRyRx(angles) * q);
238//}
239//
240//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
241// Vector3 angles(Vector3(3.001, -1.0004, 2.0005));
242// Rot3 R1(Rot3().RzRyRx(angles));
243// Vector3 q(Vector3(5.8, -2.2, 4.105));
244// Rot3 qx(0.0, -q[2], q[1],
245// q[2], 0.0, -q[0],
246// -q[1], q[0],0.0);
247// Matrix J_hyp( -(R1*qx).matrix() );
248//
249// Matrix J_expected;
250//
251// Vector3 v(predictionRq(angles, q));
252//
253// J_expected = numericalDerivative11<Vector3, Vector3>(std::bind(&predictionRq, std::placeholders::_1, q), angles);
254//
255// cout<<"J_hyp"<<J_hyp<<endl;
256// cout<<"J_expected"<<J_expected<<endl;
257//
258// CHECK( assert_equal(J_expected, J_hyp, 1e-6));
259//}
260///* VADIM - END ************************************************************************* */
261
262/* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
263
264 Key PoseKey1(11);
265 Key PoseKey2(12);
266 Key VelKey1(21);
267 Key VelKey2(22);
268 Key BiasKey1(31);
269
270 double measurement_dt(0.01);
271
272 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
273
274 Vector measurement_acc(
275 Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
276 Vector measurement_gyro((Vector(3) << 3.14, 3.14 / 2, -3.14).finished());
277
278 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
279 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
280 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
281 model);
282
283 Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
284 -0.427669306, -0.652537293, 0.709880342, 0.265075427);
285 Point3 t1(2.0, 1.0, 3.0);
286 Pose3 Pose1(R1, t1);
287 Vector3 Vel1(Vector3(0.5, -0.5, 0.4));
288 Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
289 -0.422594037, -0.636011287, 0.731761397, 0.244979388);
290 Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
291 Pose3 Pose2(R2, t2);
292 Vector3 Vel2(
293 Vector3(0.510000000000000, -0.480000000000000, 0.430000000000000));
294 imuBias::ConstantBias Bias1;
295
296 Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
297
298 Vector ActualErr(
299 factor.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2, H&: H1_actual,
300 H&: H2_actual, H&: H3_actual, H&: H4_actual, H&: H5_actual));
301
302 // Checking for Pose part in the jacobians
303 // ******
304 Matrix H1_actualPose(H1_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H1_actual.cols()));
305 Matrix H2_actualPose(H2_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H2_actual.cols()));
306 Matrix H3_actualPose(H3_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H3_actual.cols()));
307 Matrix H4_actualPose(H4_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H4_actual.cols()));
308 Matrix H5_actualPose(H5_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H5_actual.cols()));
309
310 // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
311 Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
312 H5_expectedPose;
313 H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
314 h: std::bind(f: &predictionErrorPose, args: std::placeholders::_1, args&: Vel1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
315 x: Pose1);
316 H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
317 h: std::bind(f: &predictionErrorPose, args&: Pose1, args: std::placeholders::_1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
318 x: Vel1);
319 H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
320 h: std::bind(f: &predictionErrorPose, args&: Pose1, args&: Vel1, args: std::placeholders::_1, args&: Pose2, args&: Vel2, args&: factor),
321 x: Bias1);
322 H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
323 h: std::bind(f: &predictionErrorPose, args&: Pose1, args&: Vel1, args&: Bias1, args: std::placeholders::_1, args&: Vel2, args&: factor),
324 x: Pose2);
325 H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
326 h: std::bind(f: &predictionErrorPose, args&: Pose1, args&: Vel1, args&: Bias1, args&: Pose2, args: std::placeholders::_1, args&: factor),
327 x: Vel2);
328
329 // Verify they are equal for this choice of state
330 CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
331 CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
332 CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
333 CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
334 CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
335
336 // Checking for Vel part in the jacobians
337 // ******
338 Matrix H1_actualVel(H1_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H1_actual.cols()));
339 Matrix H2_actualVel(H2_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H2_actual.cols()));
340 Matrix H3_actualVel(H3_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H3_actual.cols()));
341 Matrix H4_actualVel(H4_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H4_actual.cols()));
342 Matrix H5_actualVel(H5_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H5_actual.cols()));
343
344 // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
345 Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
346 H5_expectedVel;
347 H1_expectedVel = numericalDerivative11<Vector, Pose3>(
348 h: std::bind(f: &predictionErrorVel, args: std::placeholders::_1, args&: Vel1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
349 x: Pose1);
350 H2_expectedVel = numericalDerivative11<Vector, Vector3>(
351 h: std::bind(f: &predictionErrorVel, args&: Pose1, args: std::placeholders::_1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
352 x: Vel1);
353 H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
354 h: std::bind(f: &predictionErrorVel, args&: Pose1, args&: Vel1, args: std::placeholders::_1, args&: Pose2, args&: Vel2, args&: factor),
355 x: Bias1);
356 H4_expectedVel = numericalDerivative11<Vector, Pose3>(
357 h: std::bind(f: &predictionErrorVel, args&: Pose1, args&: Vel1, args&: Bias1, args: std::placeholders::_1, args&: Vel2, args&: factor),
358 x: Pose2);
359 H5_expectedVel = numericalDerivative11<Vector, Vector3>(
360 h: std::bind(f: &predictionErrorVel, args&: Pose1, args&: Vel1, args&: Bias1, args&: Pose2, args: std::placeholders::_1, args&: factor),
361 x: Vel2);
362
363 // Verify they are equal for this choice of state
364 CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
365 CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
366 CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
367 CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
368 CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
369}
370
371/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) {
372 Key Pose1(11);
373 Key Pose2(12);
374 Key Vel1(21);
375 Key Vel2(22);
376 Key Bias1(31);
377
378 Vector measurement_acc(Vector3(0.1, 0.2, 0.4));
379 Vector measurement_gyro(Vector3(-0.2, 0.5, 0.03));
380
381 double measurement_dt(0.1);
382
383 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
384
385 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
386
387 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
388 Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
389 measurement_dt, world_g, world_rho, world_omega_earth, model,
390 body_P_sensor);
391}
392
393/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) {
394 Key Pose1(11);
395 Key Pose2(12);
396 Key Vel1(21);
397 Key Vel2(22);
398 Key Bias1(31);
399
400 Vector measurement_acc(Vector3(0.1, 0.2, 0.4));
401 Vector measurement_gyro(Vector3(-0.2, 0.5, 0.03));
402
403 double measurement_dt(0.1);
404
405 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
406
407 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
408
409 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
410 Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
411 measurement_dt, world_g, world_rho, world_omega_earth, model,
412 body_P_sensor);
413 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> g(
414 Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
415 measurement_dt, world_g, world_rho, world_omega_earth, model,
416 body_P_sensor);
417 CHECK(assert_equal(f, g, 1e-5));
418}
419
420/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) {
421 Key PoseKey1(11);
422 Key PoseKey2(12);
423 Key VelKey1(21);
424 Key VelKey2(22);
425 Key BiasKey1(31);
426
427 double measurement_dt(0.1);
428
429 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
430
431 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
432
433 // First test: zero angular motion, some acceleration
434 Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); // Measured in ENU orientation
435 Matrix omega__cross = skewSymmetric(w: measurement_gyro);
436 Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81)
437 + omega__cross * omega__cross
438 * body_P_sensor.rotation().inverse().matrix()
439 * body_P_sensor.translation(); // Measured in ENU orientation
440
441 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
442 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
443 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
444 model, body_P_sensor);
445
446 Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
447 Vector3 Vel1(Vector3(0.50, -0.50, 0.40));
448 imuBias::ConstantBias Bias1;
449 Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
450 Vector3 expectedVel2(Vector3(0.51, -0.48, 0.43));
451 Pose3 actualPose2;
452 Vector3 actualVel2;
453 f.predict(Pose1, Vel1, Bias1, Pose2&: actualPose2, Vel2&: actualVel2);
454
455 CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
456 CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5));
457}
458
459/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) {
460 Key PoseKey1(11);
461 Key PoseKey2(12);
462 Key VelKey1(21);
463 Key VelKey2(22);
464 Key BiasKey1(31);
465
466 double measurement_dt(0.1);
467
468 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
469
470 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
471
472 // First test: zero angular motion, some acceleration
473 Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); // Measured in ENU orientation
474 Matrix omega__cross = skewSymmetric(w: measurement_gyro);
475 Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81)
476 + omega__cross * omega__cross
477 * body_P_sensor.rotation().inverse().matrix()
478 * body_P_sensor.translation(); // Measured in ENU orientation
479
480 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
481 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
482 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
483 model, body_P_sensor);
484
485 Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
486 Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
487 Vector3 Vel1(Vector3(0.50, -0.50, 0.40));
488 Vector3 Vel2(Vector3(0.51, -0.48, 0.43));
489 imuBias::ConstantBias Bias1;
490
491 Vector ActualErr(f.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2));
492 Vector ExpectedErr(Z_9x1);
493
494 CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
495}
496
497/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) {
498 Key PoseKey1(11);
499 Key PoseKey2(12);
500 Key VelKey1(21);
501 Key VelKey2(22);
502 Key BiasKey1(31);
503
504 double measurement_dt(0.1);
505
506 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
507
508 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
509
510 // Second test: zero angular motion, some acceleration
511 Vector measurement_gyro(Vector3(0.2, 0.1, -0.3)); // Measured in ENU orientation
512 Matrix omega__cross = skewSymmetric(w: measurement_gyro);
513 Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81)
514 + omega__cross * omega__cross
515 * body_P_sensor.rotation().inverse().matrix()
516 * body_P_sensor.translation(); // Measured in ENU orientation
517
518 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
519 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
520 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
521 model, body_P_sensor);
522
523 Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0));
524 Pose3 Pose2(
525 Rot3::Expmap(
526 v: body_P_sensor.rotation().matrix() * measurement_gyro
527 * measurement_dt), Point3(2.0, 1.0, 3.0));
528 Vector3 Vel1(Vector3(0.0, 0.0, 0.0));
529 Vector3 Vel2(Vector3(0.0, 0.0, 0.0));
530 imuBias::ConstantBias Bias1;
531
532 Vector ActualErr(f.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2));
533 Vector ExpectedErr(Z_9x1);
534
535 CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
536}
537
538/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) {
539 Key PoseKey1(11);
540 Key PoseKey2(12);
541 Key VelKey1(21);
542 Key VelKey2(22);
543 Key BiasKey1(31);
544
545 double measurement_dt(0.1);
546
547 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
548
549 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
550
551 // Second test: zero angular motion, some acceleration - generated in matlab
552 Vector measurement_gyro(Vector3(0.2, 0.1, -0.3)); // Measured in ENU orientation
553 Matrix omega__cross = skewSymmetric(w: measurement_gyro);
554 Vector measurement_acc =
555 Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
556 + omega__cross * omega__cross
557 * body_P_sensor.rotation().inverse().matrix()
558 * body_P_sensor.translation(); // Measured in ENU orientation
559
560 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
561 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
562 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
563 model, body_P_sensor);
564
565 Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
566 -0.427669306, -0.652537293, 0.709880342, 0.265075427);
567 Point3 t1(2.0, 1.0, 3.0);
568 Pose3 Pose1(R1, t1);
569 Vector3 Vel1(Vector3(0.5, -0.5, 0.4));
570 Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
571 -0.422594037, -0.636011287, 0.731761397, 0.244979388);
572 Point3 t2 = t1+ Point3(Vel1 * measurement_dt);
573 Pose3 Pose2(R2, t2);
574 Vector dv =
575 measurement_dt
576 * (R1.matrix() * body_P_sensor.rotation().matrix()
577 * Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
578 + world_g);
579 Vector3 Vel2 = Vel1 + dv;
580 imuBias::ConstantBias Bias1;
581
582 Vector ActualErr(f.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2));
583 Vector ExpectedErr(Z_9x1);
584
585 // TODO: Expected values need to be updated for global velocity version
586 CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
587}
588
589/* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
590
591 Key PoseKey1(11);
592 Key PoseKey2(12);
593 Key VelKey1(21);
594 Key VelKey2(22);
595 Key BiasKey1(31);
596
597 double measurement_dt(0.01);
598
599 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
600
601 Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
602
603 Vector measurement_gyro((Vector(3) << 3.14 / 2, 3.14, +3.14).finished()); // Measured in ENU orientation
604 Matrix omega__cross = skewSymmetric(w: measurement_gyro);
605 Vector measurement_acc =
606 Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
607 + omega__cross * omega__cross
608 * body_P_sensor.rotation().inverse().matrix()
609 * body_P_sensor.translation(); // Measured in ENU orientation
610
611 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
612 PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
613 measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
614 model, body_P_sensor);
615
616 Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
617 -0.427669306, -0.652537293, 0.709880342, 0.265075427);
618 Point3 t1(2.0, 1.0, 3.0);
619 Pose3 Pose1(R1, t1);
620 Vector3 Vel1(0.5, -0.5, 0.4);
621 Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
622 -0.422594037, -0.636011287, 0.731761397, 0.244979388);
623 Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
624 Pose3 Pose2(R2, t2);
625 Vector3 Vel2(0.510000000000000, -0.480000000000000, 0.430000000000000);
626 imuBias::ConstantBias Bias1;
627
628 Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
629
630 Vector ActualErr(
631 factor.evaluateError(x: Pose1, x: Vel1, x: Bias1, x: Pose2, x: Vel2, H&: H1_actual,
632 H&: H2_actual, H&: H3_actual, H&: H4_actual, H&: H5_actual));
633
634 // Checking for Pose part in the jacobians
635 // ******
636 Matrix H1_actualPose(H1_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H1_actual.cols()));
637 Matrix H2_actualPose(H2_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H2_actual.cols()));
638 Matrix H3_actualPose(H3_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H3_actual.cols()));
639 Matrix H4_actualPose(H4_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H4_actual.cols()));
640 Matrix H5_actualPose(H5_actual.block(startRow: 0, startCol: 0, blockRows: 6, blockCols: H5_actual.cols()));
641
642 // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
643 Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
644 H5_expectedPose;
645 H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
646 h: std::bind(f: &predictionErrorPose, args: std::placeholders::_1, args&: Vel1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
647 x: Pose1);
648 H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
649 h: std::bind(f: &predictionErrorPose, args&: Pose1, args: std::placeholders::_1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
650 x: Vel1);
651 H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
652 h: std::bind(f: &predictionErrorPose, args&: Pose1, args&: Vel1, args: std::placeholders::_1, args&: Pose2, args&: Vel2, args&: factor),
653 x: Bias1);
654 H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
655 h: std::bind(f: &predictionErrorPose, args&: Pose1, args&: Vel1, args&: Bias1, args: std::placeholders::_1, args&: Vel2, args&: factor),
656 x: Pose2);
657 H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
658 h: std::bind(f: &predictionErrorPose, args&: Pose1, args&: Vel1, args&: Bias1, args&: Pose2, args: std::placeholders::_1, args&: factor),
659 x: Vel2);
660
661 // Verify they are equal for this choice of state
662 CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
663 CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
664 CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
665 CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
666 CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
667
668 // Checking for Vel part in the jacobians
669 // ******
670 Matrix H1_actualVel(H1_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H1_actual.cols()));
671 Matrix H2_actualVel(H2_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H2_actual.cols()));
672 Matrix H3_actualVel(H3_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H3_actual.cols()));
673 Matrix H4_actualVel(H4_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H4_actual.cols()));
674 Matrix H5_actualVel(H5_actual.block(startRow: 6, startCol: 0, blockRows: 3, blockCols: H5_actual.cols()));
675
676 // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
677 Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
678 H5_expectedVel;
679 H1_expectedVel = numericalDerivative11<Vector, Pose3>(
680 h: std::bind(f: &predictionErrorVel, args: std::placeholders::_1, args&: Vel1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
681 x: Pose1);
682 H2_expectedVel = numericalDerivative11<Vector, Vector3>(
683 h: std::bind(f: &predictionErrorVel, args&: Pose1, args: std::placeholders::_1, args&: Bias1, args&: Pose2, args&: Vel2, args&: factor),
684 x: Vel1);
685 H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
686 h: std::bind(f: &predictionErrorVel, args&: Pose1, args&: Vel1, args: std::placeholders::_1, args&: Pose2, args&: Vel2, args&: factor),
687 x: Bias1);
688 H4_expectedVel = numericalDerivative11<Vector, Pose3>(
689 h: std::bind(f: &predictionErrorVel, args&: Pose1, args&: Vel1, args&: Bias1, args: std::placeholders::_1, args&: Vel2, args&: factor),
690 x: Pose2);
691 H5_expectedVel = numericalDerivative11<Vector, Vector3>(
692 h: std::bind(f: &predictionErrorVel, args&: Pose1, args&: Vel1, args&: Bias1, args&: Pose2, args: std::placeholders::_1, args&: factor),
693 x: Vel2);
694
695 // Verify they are equal for this choice of state
696 CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
697 CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
698 CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
699 CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
700 CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
701}
702
703/* ************************************************************************* */
704int main() {
705 TestResult tr;
706 return TestRegistry::runAllTests(result&: tr);
707}
708/* ************************************************************************* */
709