gtsam/examples/ImuFactorsExample2.cpp at develop · openspacelabs/gtsam · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file ImuFactorExample2
 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor with ISAM2.
 * @author Robert Truax
*/

#include<gtsam/geometry/PinholeCamera.h>
#include<gtsam/geometry/Cal3_S2.h>
#include<gtsam/inference/Symbol.h>
#include<gtsam/navigation/ImuBias.h>
#include<gtsam/navigation/ImuFactor.h>
#include<gtsam/navigation/Scenario.h>
#include<gtsam/nonlinear/ISAM2.h>
#include<gtsam/slam/BetweenFactor.h>

#include<vector>

usingnamespacestd;
usingnamespacegtsam;

// Shorthand for velocity and pose variables
using symbol_shorthand::V;
using symbol_shorthand::X;

constdoublekGravity = 9.81;

/* ************************************************************************* */
intmain(int argc, char* argv[]) {
auto params = PreintegrationParams::MakeSharedU(kGravity);
 params->setAccelerometerCovariance(I_3x3 * 0.1);
 params->setGyroscopeCovariance(I_3x3 * 0.1);
 params->setIntegrationCovariance(I_3x3 * 0.1);
 params->setUse2ndOrderCoriolis(false);
 params->setOmegaCoriolis(Vector3(0, 0, 0));

 Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));

// Start with a camera on x-axis looking at origin
double radius = 30;
const Point3 up(0, 0, 1), target(0, 0, 0);
const Point3 position(radius, 0, 0);
constauto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
constauto pose_0 = camera.pose();

// Now, create a constant-twist scenario that makes the camera orbit the
// origin
double angular_velocity = M_PI, // rad/sec
delta_t = 1.0 / 18; // makes for 10 degrees per step
 Vector3 angular_velocity_vector(0, -angular_velocity, 0);
 Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
auto scenario = ConstantTwistScenario(angular_velocity_vector,
 linear_velocity_vector, pose_0);

// Create a factor graph
 NonlinearFactorGraph newgraph;

// Create (incremental) ISAM2 solver
ISAM2 isam;

// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
 Values initialEstimate, totalEstimate, result;

// Add a prior on pose x0. This indirectly specifies where the origin is.
// 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
auto noise = noiseModel::Diagonal::Sigmas(
 (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
 newgraph.addPrior(X(0), pose_0, noise);

// Add imu priors
 Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
 newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
 initialEstimate.insert(biasKey, imuBias::ConstantBias());
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

 Vector n_velocity(3);
 n_velocity << 0, angular_velocity * radius, 0;
 newgraph.addPrior(V(0), n_velocity, velnoise);

 initialEstimate.insert(V(0), n_velocity);

// IMU preintegrator
 PreintegratedImuMeasurements accum(params);

// Simulate poses and imu measurements, adding them to the factor graph
for (size_t i = 0; i < 36; ++i) {
double t = i * delta_t;
if (i == 0) { // First time add two poses
auto pose_1 = scenario.pose(delta_t);
 initialEstimate.insert(X(0), pose_0.compose(delta));
 initialEstimate.insert(X(1), pose_1.compose(delta));
 } elseif (i >= 2) { // Add more poses as necessary
auto pose_i = scenario.pose(t);
 initialEstimate.insert(X(i), pose_i.compose(delta));
 }

if (i > 0) {
// Add Bias variables periodically
if (i % 5 == 0) {
 biasKey++;
 Symbol b1 = biasKey - 1;
 Symbol b2 = biasKey;
 Vector6 covvec;
 covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
auto cov = noiseModel::Diagonal::Variances(covvec);
auto f = std::make_shared<BetweenFactor<imuBias::ConstantBias> >(
 b1, b2, imuBias::ConstantBias(), cov);
 newgraph.add(f);
 initialEstimate.insert(biasKey, imuBias::ConstantBias());
 }
// Predict acceleration and gyro measurements in (actual) body frame
 Vector3 measuredAcc = scenario.acceleration_b(t) -
 scenario.rotation(t).transpose() * params->n_gravity;
 Vector3 measuredOmega = scenario.omega_b(t);
 accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);

// Add Imu Factor
 ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
 newgraph.add(imufac);

// insert new velocity, which is wrong
 initialEstimate.insert(V(i), n_velocity);
 accum.resetIntegration();
 }

// Incremental solution
 isam.update(newgraph, initialEstimate);
 result = isam.calculateEstimate();
 newgraph = NonlinearFactorGraph();
 initialEstimate.clear();
 }
GTSAM_PRINT(result);
return0;
}
/* ************************************************************************* */