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 ImuFactorExample2
14 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor with ISAM2.
15 * @author Robert Truax
16 */
17
18#include <gtsam/geometry/PinholeCamera.h>
19#include <gtsam/geometry/Cal3_S2.h>
20#include <gtsam/inference/Symbol.h>
21#include <gtsam/navigation/ImuBias.h>
22#include <gtsam/navigation/ImuFactor.h>
23#include <gtsam/navigation/Scenario.h>
24#include <gtsam/nonlinear/ISAM2.h>
25#include <gtsam/slam/BetweenFactor.h>
26
27#include <vector>
28
29using namespace std;
30using namespace gtsam;
31
32// Shorthand for velocity and pose variables
33using symbol_shorthand::V;
34using symbol_shorthand::X;
35
36const double kGravity = 9.81;
37
38/* ************************************************************************* */
39int main(int argc, char* argv[]) {
40 auto params = PreintegrationParams::MakeSharedU(g: kGravity);
41 params->setAccelerometerCovariance(I_3x3 * 0.1);
42 params->setGyroscopeCovariance(I_3x3 * 0.1);
43 params->setIntegrationCovariance(I_3x3 * 0.1);
44 params->setUse2ndOrderCoriolis(false);
45 params->setOmegaCoriolis(Vector3(0, 0, 0));
46
47 Pose3 delta(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25), Point3(0.05, -0.10, 0.20));
48
49 // Start with a camera on x-axis looking at origin
50 double radius = 30;
51 const Point3 up(0, 0, 1), target(0, 0, 0);
52 const Point3 position(radius, 0, 0);
53 const auto camera = PinholeCamera<Cal3_S2>::Lookat(eye: position, target, upVector: up);
54 const auto pose_0 = camera.pose();
55
56 // Now, create a constant-twist scenario that makes the camera orbit the
57 // origin
58 double angular_velocity = M_PI, // rad/sec
59 delta_t = 1.0 / 18; // makes for 10 degrees per step
60 Vector3 angular_velocity_vector(0, -angular_velocity, 0);
61 Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
62 auto scenario = ConstantTwistScenario(angular_velocity_vector,
63 linear_velocity_vector, pose_0);
64
65 // Create a factor graph
66 NonlinearFactorGraph newgraph;
67
68 // Create (incremental) ISAM2 solver
69 ISAM2 isam;
70
71 // Create the initial estimate to the solution
72 // Intentionally initialize the variables off from the ground truth
73 Values initialEstimate, totalEstimate, result;
74
75 // Add a prior on pose x0. This indirectly specifies where the origin is.
76 // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
77 auto noise = noiseModel::Diagonal::Sigmas(
78 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3)).finished());
79 newgraph.addPrior(key: X(j: 0), prior: pose_0, model: noise);
80
81 // Add imu priors
82 Key biasKey = Symbol('b', 0);
83 auto biasnoise = noiseModel::Diagonal::Sigmas(sigmas: Vector6::Constant(value: 0.1));
84 newgraph.addPrior(key: biasKey, prior: imuBias::ConstantBias(), model: biasnoise);
85 initialEstimate.insert(j: biasKey, val: imuBias::ConstantBias());
86 auto velnoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.1, 0.1, 0.1));
87
88 Vector n_velocity(3);
89 n_velocity << 0, angular_velocity * radius, 0;
90 newgraph.addPrior(key: V(j: 0), prior: n_velocity, model: velnoise);
91
92 initialEstimate.insert(j: V(j: 0), val: n_velocity);
93
94 // IMU preintegrator
95 PreintegratedImuMeasurements accum(params);
96
97 // Simulate poses and imu measurements, adding them to the factor graph
98 for (size_t i = 0; i < 36; ++i) {
99 double t = i * delta_t;
100 if (i == 0) { // First time add two poses
101 auto pose_1 = scenario.pose(t: delta_t);
102 initialEstimate.insert(j: X(j: 0), val: pose_0.compose(g: delta));
103 initialEstimate.insert(j: X(j: 1), val: pose_1.compose(g: delta));
104 } else if (i >= 2) { // Add more poses as necessary
105 auto pose_i = scenario.pose(t);
106 initialEstimate.insert(j: X(j: i), val: pose_i.compose(g: delta));
107 }
108
109 if (i > 0) {
110 // Add Bias variables periodically
111 if (i % 5 == 0) {
112 biasKey++;
113 Symbol b1 = biasKey - 1;
114 Symbol b2 = biasKey;
115 Vector6 covvec;
116 covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
117 auto cov = noiseModel::Diagonal::Variances(variances: covvec);
118 auto f = std::make_shared<BetweenFactor<imuBias::ConstantBias> >(
119 args&: b1, args&: b2, args: imuBias::ConstantBias(), args&: cov);
120 newgraph.add(factor: f);
121 initialEstimate.insert(j: biasKey, val: imuBias::ConstantBias());
122 }
123 // Predict acceleration and gyro measurements in (actual) body frame
124 Vector3 measuredAcc = scenario.acceleration_b(t) -
125 scenario.rotation(t).transpose() * params->n_gravity;
126 Vector3 measuredOmega = scenario.omega_b(t);
127 accum.integrateMeasurement(measuredAcc, measuredOmega, dt: delta_t);
128
129 // Add Imu Factor
130 ImuFactor imufac(X(j: i - 1), V(j: i - 1), X(j: i), V(j: i), biasKey, accum);
131 newgraph.add(factorOrContainer: imufac);
132
133 // insert new velocity, which is wrong
134 initialEstimate.insert(j: V(j: i), val: n_velocity);
135 accum.resetIntegration();
136 }
137
138 // Incremental solution
139 isam.update(newFactors: newgraph, newTheta: initialEstimate);
140 result = isam.calculateEstimate();
141 newgraph = NonlinearFactorGraph();
142 initialEstimate.clear();
143 }
144 GTSAM_PRINT(result);
145 return 0;
146}
147/* ************************************************************************* */
148