1/**
2 * @file ISAM2_SmartFactorStereo_IMU.cpp
3 * @brief test of iSAM2 with smart stereo factors and IMU preintegration,
4 * originally used to debug valgrind invalid reads with Eigen
5 * @author Nghia Ho
6 *
7 * Setup is a stationary stereo camera with an IMU attached.
8 * The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt
9 * It contains 5 frames of stereo matches and IMU data.
10 */
11#include <gtsam/navigation/CombinedImuFactor.h>
12#include <gtsam/nonlinear/ISAM2.h>
13#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
14
15#include <fstream>
16#include <iostream>
17#include <sstream>
18#include <string>
19#include <vector>
20
21using namespace std;
22using namespace gtsam;
23using symbol_shorthand::B;
24using symbol_shorthand::V;
25using symbol_shorthand::X;
26
27struct IMUHelper {
28 IMUHelper() {
29 {
30 auto gaussian = noiseModel::Diagonal::Sigmas(
31 sigmas: (Vector(6) << Vector3::Constant(value: 5.0e-2), Vector3::Constant(value: 5.0e-3))
32 .finished());
33 auto huber = noiseModel::Robust::Create(
34 robust: noiseModel::mEstimator::Huber::Create(k: 1.345), noise: gaussian);
35
36 biasNoiseModel = huber;
37 }
38
39 {
40 auto gaussian = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.01);
41 auto huber = noiseModel::Robust::Create(
42 robust: noiseModel::mEstimator::Huber::Create(k: 1.345), noise: gaussian);
43
44 velocityNoiseModel = huber;
45 }
46
47 // expect IMU to be rotated in image space co-ords
48 auto p = std::make_shared<PreintegratedCombinedMeasurements::Params>(
49 args: Vector3(0.0, 9.8, 0.0));
50
51 p->accelerometerCovariance =
52 I_3x3 * pow(x: 0.0565, y: 2.0); // acc white noise in continuous
53 p->integrationCovariance =
54 I_3x3 * 1e-9; // integration uncertainty continuous
55 p->gyroscopeCovariance =
56 I_3x3 * pow(x: 4.0e-5, y: 2.0); // gyro white noise in continuous
57 p->biasAccCovariance = I_3x3 * pow(x: 0.00002, y: 2.0); // acc bias in continuous
58 p->biasOmegaCovariance =
59 I_3x3 * pow(x: 0.001, y: 2.0); // gyro bias in continuous
60
61#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
62 p->biasAccOmegaInt = Matrix::Identity(rows: 6, cols: 6) * 1e-5;
63#endif
64
65 // body to IMU rotation
66 Rot3 iRb(0.036129, -0.998727, 0.035207, 0.045417, -0.033553, -0.998404,
67 0.998315, 0.037670, 0.044147);
68
69 // body to IMU translation (meters)
70 Point3 iTb(0.03, -0.025, -0.06);
71
72 // body in this example is the left camera
73 p->body_P_sensor = Pose3(iRb, iTb);
74
75 Rot3 prior_rotation = Rot3(I_3x3);
76 Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));
77
78 Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame
79 Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
80
81 priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);
82
83 prevState = NavState(prior_pose, Vector3(0, 0, 0));
84 propState = prevState;
85 prevBias = priorImuBias;
86
87 preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
88 }
89
90 imuBias::ConstantBias priorImuBias; // assume zero initial bias
91 noiseModel::Robust::shared_ptr velocityNoiseModel;
92 noiseModel::Robust::shared_ptr biasNoiseModel;
93 NavState prevState;
94 NavState propState;
95 imuBias::ConstantBias prevBias;
96 PreintegratedCombinedMeasurements* preintegrated;
97};
98
99int main(int argc, char* argv[]) {
100 if (argc != 2) {
101 cout << "./ISAM2_SmartFactorStereo_IMU [data.txt]\n";
102 return 0;
103 }
104
105 ifstream in(argv[1]);
106
107 if (!in) {
108 cerr << "error opening: " << argv[1] << "\n";
109 return 1;
110 }
111
112 // Camera parameters
113 double fx = 822.37;
114 double fy = 822.37;
115 double cx = 538.73;
116 double cy = 579.10;
117 double baseline = 0.372; // meters
118
119 Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
120
121 ISAM2Params parameters;
122 parameters.relinearizeThreshold = 0.1;
123 ISAM2 isam(parameters);
124
125 // Create a factor graph
126 std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
127 NonlinearFactorGraph graph;
128 Values initialEstimate;
129 IMUHelper imu;
130
131 // Pose prior - at identity
132 auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
133 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.1)).finished());
134 graph.addPrior(key: X(j: 1), prior: Pose3::Identity(), model: priorPoseNoise);
135 initialEstimate.insert(j: X(j: 0), val: Pose3::Identity());
136
137 // Bias prior
138 graph.addPrior(key: B(j: 1), prior: imu.priorImuBias, model: imu.biasNoiseModel);
139 initialEstimate.insert(j: B(j: 0), val: imu.priorImuBias);
140
141 // Velocity prior - assume stationary
142 graph.addPrior(key: V(j: 1), prior: Vector3(0, 0, 0), model: imu.velocityNoiseModel);
143 initialEstimate.insert(j: V(j: 0), val: Vector3(0, 0, 0));
144
145 int lastFrame = 1;
146 int frame;
147
148 while (true) {
149 char line[1024];
150
151 in.getline(s: line, n: sizeof(line));
152 stringstream ss(line);
153 char type;
154
155 ss >> type;
156 ss >> frame;
157
158 if (frame != lastFrame || in.eof()) {
159 cout << "Running iSAM for frame: " << lastFrame << "\n";
160
161 initialEstimate.insert(j: X(j: lastFrame), val: Pose3::Identity());
162 initialEstimate.insert(j: V(j: lastFrame), val: Vector3(0, 0, 0));
163 initialEstimate.insert(j: B(j: lastFrame), val: imu.prevBias);
164
165 CombinedImuFactor imuFactor(X(j: lastFrame - 1), V(j: lastFrame - 1),
166 X(j: lastFrame), V(j: lastFrame), B(j: lastFrame - 1),
167 B(j: lastFrame), *imu.preintegrated);
168
169 graph.add(factorOrContainer: imuFactor);
170
171 isam.update(newFactors: graph, newTheta: initialEstimate);
172
173 Values currentEstimate = isam.calculateEstimate();
174
175 imu.propState = imu.preintegrated->predict(state_i: imu.prevState, bias_i: imu.prevBias);
176 imu.prevState = NavState(currentEstimate.at<Pose3>(j: X(j: lastFrame)),
177 currentEstimate.at<Vector3>(j: V(j: lastFrame)));
178 imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(j: B(j: lastFrame));
179 imu.preintegrated->resetIntegrationAndSetBias(biasHat: imu.prevBias);
180
181 graph.resize(size: 0);
182 initialEstimate.clear();
183
184 if (in.eof()) {
185 break;
186 }
187 }
188
189 if (type == 'i') { // Process IMU measurement
190 double ax, ay, az;
191 double gx, gy, gz;
192 double dt = 1 / 800.0; // IMU at ~800Hz
193
194 ss >> ax;
195 ss >> ay;
196 ss >> az;
197
198 ss >> gx;
199 ss >> gy;
200 ss >> gz;
201
202 Vector3 acc(ax, ay, az);
203 Vector3 gyr(gx, gy, gz);
204
205 imu.preintegrated->integrateMeasurement(measuredAcc: acc, measuredOmega: gyr, dt);
206 } else if (type == 's') { // Process stereo measurement
207 int landmark;
208 double xl, xr, y;
209
210 ss >> landmark;
211 ss >> xl;
212 ss >> xr;
213 ss >> y;
214
215 if (smartFactors.count(x: landmark) == 0) {
216 auto gaussian = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1.0);
217
218 SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
219
220 smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(
221 new SmartStereoProjectionPoseFactor(gaussian, params));
222 graph.push_back(factor: smartFactors[landmark]);
223 }
224
225 smartFactors[landmark]->add(measured: StereoPoint2(xl, xr, y), poseKey: X(j: frame), K);
226 } else {
227 throw runtime_error("unexpected data type: " + string(1, type));
228 }
229
230 lastFrame = frame;
231 }
232
233 return 0;
234}
235