testInertialNavFactor_GlobalVelocity.cpp
Go to the documentation of this file.
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 
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 
28 using namespace std::placeholders;
29 using namespace std;
30 using namespace gtsam;
31 
32 Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733,
33  0.67835, -0.60471);
34 
35 static const Vector3 world_g(0.0, 0.0, 9.81);
36 static const Vector3 world_rho(0.0, -1.5724e-05, 0.0); // NED system
37 static const Vector3 ECEF_omega_earth(0.0, 0.0, 7.292115e-5);
38 static const Vector3 world_omega_earth = world_R_ECEF.matrix()
39  * ECEF_omega_earth;
40 
41 /* ************************************************************************* */
42 Pose3 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(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
46 }
47 
48 Vector 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(p1, v1, b1, p2, v2).tail(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(9, 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(9, 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(9, 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, actualPose2, 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(9, 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(Pose1, Vel1, Bias1, Pose2, 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(9, 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(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(Pose1, Vel1, Bias1, Pose2, 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(9, 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(Pose1, Vel1, Bias1, Pose2, 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 
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 //}
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(9, 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(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual,
300  H2_actual, H3_actual, H4_actual, H5_actual));
301 
302  // Checking for Pose part in the jacobians
303  // ******
304  Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols()));
305  Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols()));
306  Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols()));
307  Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols()));
308  Matrix H5_actualPose(H5_actual.block(0, 0, 6, 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  std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
315  Pose1);
316  H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
317  std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
318  Vel1);
319  H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
320  std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
321  Bias1);
322  H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
323  std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
324  Pose2);
325  H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
326  std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
327  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(6, 0, 3, H1_actual.cols()));
339  Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols()));
340  Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols()));
341  Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols()));
342  Matrix H5_actualVel(H5_actual.block(6, 0, 3, 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  std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
349  Pose1);
350  H2_expectedVel = numericalDerivative11<Vector, Vector3>(
351  std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
352  Vel1);
353  H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
354  std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
355  Bias1);
356  H4_expectedVel = numericalDerivative11<Vector, Pose3>(
357  std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
358  Pose2);
359  H5_expectedVel = numericalDerivative11<Vector, Vector3>(
360  std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
361  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(9, 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(9, 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(9, 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(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, actualPose2, 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(9, 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(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(Pose1, Vel1, Bias1, Pose2, 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(9, 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(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  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(Pose1, Vel1, Bias1, Pose2, 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(9, 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(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(Pose1, Vel1, Bias1, Pose2, 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(9, 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(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(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual,
632  H2_actual, H3_actual, H4_actual, H5_actual));
633 
634  // Checking for Pose part in the jacobians
635  // ******
636  Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols()));
637  Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols()));
638  Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols()));
639  Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols()));
640  Matrix H5_actualPose(H5_actual.block(0, 0, 6, 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  std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
647  Pose1);
648  H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
649  std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
650  Vel1);
651  H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
652  std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
653  Bias1);
654  H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
655  std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
656  Pose2);
657  H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
658  std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
659  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(6, 0, 3, H1_actual.cols()));
671  Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols()));
672  Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols()));
673  Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols()));
674  Matrix H5_actualVel(H5_actual.block(6, 0, 3, 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  std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
681  Pose1);
682  H2_expectedVel = numericalDerivative11<Vector, Vector3>(
683  std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
684  Vel1);
685  H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
686  std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
687  Bias1);
688  H4_expectedVel = numericalDerivative11<Vector, Pose3>(
689  std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
690  Pose2);
691  H5_expectedVel = numericalDerivative11<Vector, Vector3>(
692  std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
693  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 /* ************************************************************************* */
704 int main() {
705  TestResult tr;
706  return TestRegistry::runAllTests(tr);
707 }
708 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
simple_graph::b1
Vector2 b1(2, -1)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TestHarness.h
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
ECEF_omega_earth
static const Vector3 ECEF_omega_earth(0.0, 0.0, 7.292115e-5)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
world_g
static const Vector3 world_g(0.0, 0.0, 9.81)
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:381
TestableAssertions.h
Provides additional testing facilities for common data structures.
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Key.h
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::InertialNavFactor_GlobalVelocity
Definition: InertialNavFactor_GlobalVelocity.h:77
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:161
world_rho
static const Vector3 world_rho(0.0, -1.5724e-05, 0.0)
predictionErrorVel
Vector predictionErrorVel(const Pose3 &p1, const Vector3 &v1, const imuBias::ConstantBias &b1, const Pose3 &p2, const Vector3 &v2, const InertialNavFactor_GlobalVelocity< Pose3, Vector3, imuBias::ConstantBias > &factor)
Definition: testInertialNavFactor_GlobalVelocity.cpp:48
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:342
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
world_omega_earth
static const Vector3 world_omega_earth
Definition: testInertialNavFactor_GlobalVelocity.cpp:38
TEST
TEST(InertialNavFactor_GlobalVelocity, Constructor)
Definition: testInertialNavFactor_GlobalVelocity.cpp:54
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
world_R_ECEF
Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471)
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:312
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
ImuBias.h
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:348
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: ABC.h:17
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
predictionErrorPose
Pose3 predictionErrorPose(const Pose3 &p1, const Vector3 &v1, const imuBias::ConstantBias &b1, const Pose3 &p2, const Vector3 &v2, const InertialNavFactor_GlobalVelocity< Pose3, Vector3, imuBias::ConstantBias > &factor)
Definition: testInertialNavFactor_GlobalVelocity.cpp:42
v2
Vector v2
Definition: testSerializationBase.cpp:39
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:222
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
main
int main()
Definition: testInertialNavFactor_GlobalVelocity.cpp:704
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
Values.h
A non-templated config holding any types of Manifold-group elements.
InertialNavFactor_GlobalVelocity.h
Inertial navigation factor (velocity in the global frame)
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Pose2
Definition: Pose2.h:39

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:06:37