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 <gtsam/navigation/ImuBias.h>
20#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
21#include <gtsam/geometry/Pose3.h>
22#include <gtsam/nonlinear/Values.h>
23#include <gtsam/base/numericalDerivative.h>
24#include <gtsam/inference/Key.h>
25
26using namespace std;
27using namespace gtsam;
28
29gtsam::Rot3 world_R_ECEF(
30 0.31686, 0.51505, 0.79645,
31 0.85173, -0.52399, 0,
32 0.41733, 0.67835, -0.60471);
33
34gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
35gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
36
37/* ************************************************************************* */
38gtsam::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) {
39 return Pose3::Expmap(xi: factor.evaluateError(x: p1, x: v1, x: b1, x: p2, x: v2).head(n: 6));
40}
41
42gtsam::Vector3 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) {
43 return factor.evaluateError(x: p1, x: v1, x: b1, x: p2, x: v2).tail(n: 3);
44}
45
46#include <gtsam/linear/GaussianFactorGraph.h>
47/* ************************************************************************* */
48int main() {
49 gtsam::Key PoseKey1(11);
50 gtsam::Key PoseKey2(12);
51 gtsam::Key VelKey1(21);
52 gtsam::Key VelKey2(22);
53 gtsam::Key BiasKey1(31);
54
55 double measurement_dt(0.1);
56 Vector world_g(Vector3(0.0, 0.0, 9.81));
57 Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system
58 gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
59 gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
60
61 SharedGaussian model(noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
62
63 // Second test: zero angular motion, some acceleration - generated in matlab
64 Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
65 Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
66
67 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
68
69 Rot3 R1(0.487316618, 0.125253866, 0.86419557,
70 0.580273724, 0.693095498, -0.427669306,
71 -0.652537293, 0.709880342, 0.265075427);
72 Point3 t1(2.0,1.0,3.0);
73 Pose3 Pose1(R1, t1);
74 Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4));
75 Rot3 R2(0.473618898, 0.119523052, 0.872582019,
76 0.609241153, 0.67099888, -0.422594037,
77 -0.636011287, 0.731761397, 0.244979388);
78 Point3 t2 = t1 + Point3(Vel1*measurement_dt);
79 Pose3 Pose2(R2, t2);
80 Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
81 Vector3 Vel2 = Vel1 + dv;
82 imuBias::ConstantBias Bias1;
83
84 Values values;
85 values.insert(j: PoseKey1, val: Pose1);
86 values.insert(j: PoseKey2, val: Pose2);
87 values.insert(j: VelKey1, val: Vel1);
88 values.insert(j: VelKey2, val: Vel2);
89 values.insert(j: BiasKey1, val: Bias1);
90
91 Ordering ordering;
92 ordering.push_back(x: PoseKey1);
93 ordering.push_back(x: VelKey1);
94 ordering.push_back(x: BiasKey1);
95 ordering.push_back(x: PoseKey2);
96 ordering.push_back(x: VelKey2);
97
98 GaussianFactorGraph graph;
99 gttic_(LinearizeTiming);
100 for(size_t i = 0; i < 100000; ++i) {
101 GaussianFactor::shared_ptr g = f.linearize(x: values);
102 graph.push_back(factor: g);
103 }
104 gttoc_(LinearizeTiming);
105 tictoc_finishedIteration_();
106 tictoc_print_();
107}
108
109/* ************************************************************************* */
110