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 ImuFactorsExample
14 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
15 * navigation code.
16 * @author Garrett (ghemann@gmail.com), Luca Carlone
17 */
18
19/**
20 * Example of use of the imuFactors (imuFactor and combinedImuFactor) in
21 * conjunction with GPS
22 * - imuFactor is used by default. You can test combinedImuFactor by
23 * appending a `-c` flag at the end (see below for example command).
24 * - we read IMU and GPS data from a CSV file, with the following format:
25 * A row starting with "i" is the first initial position formatted with
26 * N, E, D, qx, qY, qZ, qW, velN, velE, velD
27 * A row starting with "0" is an imu measurement
28 * (body frame - Forward, Right, Down)
29 * linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
30 * A row starting with "1" is a gps correction formatted with
31 * N, E, D, qX, qY, qZ, qW
32 * Note that for GPS correction, we're only using the position not the
33 * rotation. The rotation is provided in the file for ground truth comparison.
34 *
35 * See usage: ./ImuFactorsExample --help
36 */
37
38#include <boost/program_options.hpp>
39
40// GTSAM related includes.
41#include <gtsam/inference/Symbol.h>
42#include <gtsam/navigation/CombinedImuFactor.h>
43#include <gtsam/navigation/GPSFactor.h>
44#include <gtsam/navigation/ImuFactor.h>
45#include <gtsam/nonlinear/ISAM2.h>
46#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
47#include <gtsam/nonlinear/NonlinearFactorGraph.h>
48#include <gtsam/slam/BetweenFactor.h>
49#include <gtsam/slam/dataset.h>
50
51#include <cassert>
52#include <cstring>
53#include <fstream>
54#include <iostream>
55
56using namespace gtsam;
57using namespace std;
58
59using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
60using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
61using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
62
63namespace po = boost::program_options;
64
65po::variables_map parseOptions(int argc, char* argv[]) {
66 po::options_description desc;
67 desc.add_options()("help,h", "produce help message")(
68 "data_csv_path", po::value<string>()->default_value(v: "imuAndGPSdata.csv"),
69 "path to the CSV file with the IMU data")(
70 "output_filename",
71 po::value<string>()->default_value(v: "imuFactorExampleResults.csv"),
72 "path to the result file to use")("use_isam", po::bool_switch(),
73 "use ISAM as the optimizer");
74
75 po::variables_map vm;
76 po::store(options: po::parse_command_line(argc, argv, desc), m&: vm);
77
78 if (vm.count(x: "help")) {
79 cout << desc << "\n";
80 exit(status: 1);
81 }
82
83 return vm;
84}
85
86std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
87 // We use the sensor specs to build the noise model for the IMU factor.
88 double accel_noise_sigma = 0.0003924;
89 double gyro_noise_sigma = 0.000205689024915;
90 double accel_bias_rw_sigma = 0.004905;
91 double gyro_bias_rw_sigma = 0.000001454441043;
92 Matrix33 measured_acc_cov = I_3x3 * pow(x: accel_noise_sigma, y: 2);
93 Matrix33 measured_omega_cov = I_3x3 * pow(x: gyro_noise_sigma, y: 2);
94 Matrix33 integration_error_cov =
95 I_3x3 * 1e-8; // error committed in integrating position from velocities
96 Matrix33 bias_acc_cov = I_3x3 * pow(x: accel_bias_rw_sigma, y: 2);
97 Matrix33 bias_omega_cov = I_3x3 * pow(x: gyro_bias_rw_sigma, y: 2);
98
99 auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(g: 0.0);
100 // PreintegrationBase params:
101 p->accelerometerCovariance =
102 measured_acc_cov; // acc white noise in continuous
103 p->integrationCovariance =
104 integration_error_cov; // integration uncertainty continuous
105 // should be using 2nd order integration
106 // PreintegratedRotation params:
107 p->gyroscopeCovariance =
108 measured_omega_cov; // gyro white noise in continuous
109 // PreintegrationCombinedMeasurements params:
110 p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
111 p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
112#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
113 Matrix66 bias_acc_omega_init =
114 I_6x6 * 1e-5; // error in the bias used for preintegration
115 p->biasAccOmegaInt = bias_acc_omega_init;
116#endif
117
118 return p;
119}
120
121int main(int argc, char* argv[]) {
122 string data_filename, output_filename;
123
124 bool use_isam = false;
125
126 po::variables_map var_map = parseOptions(argc, argv);
127
128 data_filename = findExampleDataFile(name: var_map["data_csv_path"].as<string>());
129 output_filename = var_map["output_filename"].as<string>();
130 use_isam = var_map["use_isam"].as<bool>();
131
132 ISAM2* isam2 = 0;
133 if (use_isam) {
134 printf(format: "Using ISAM2\n");
135 ISAM2Params parameters;
136 parameters.relinearizeThreshold = 0.01;
137 parameters.relinearizeSkip = 1;
138 isam2 = new ISAM2(parameters);
139
140 } else {
141 printf(format: "Using Levenberg Marquardt Optimizer\n");
142 }
143
144 // Set up output file for plotting errors
145 FILE* fp_out = fopen(filename: output_filename.c_str(), modes: "w+");
146 fprintf(stream: fp_out,
147 format: "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
148 "gt_qy,gt_qz,gt_qw\n");
149
150 // Begin parsing the CSV file. Input the first line for initialization.
151 // From there, we'll iterate through the file and we'll preintegrate the IMU
152 // or add in the GPS given the input.
153 ifstream file(data_filename.c_str());
154 string value;
155
156 // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
157 Vector10 initial_state;
158 getline(in&: file, str&: value, delim: ','); // i
159 for (int i = 0; i < 9; i++) {
160 getline(in&: file, str&: value, delim: ',');
161 initial_state(i) = stof(str: value.c_str());
162 }
163 getline(in&: file, str&: value, delim: '\n');
164 initial_state(9) = stof(str: value.c_str());
165 cout << "initial state:\n" << initial_state.transpose() << "\n\n";
166
167 // Assemble initial quaternion through GTSAM constructor
168 // ::quaternion(w,x,y,z);
169 Rot3 prior_rotation = Rot3::Quaternion(w: initial_state(6), x: initial_state(3),
170 y: initial_state(4), z: initial_state(5));
171 Point3 prior_point(initial_state.head<3>());
172 Pose3 prior_pose(prior_rotation, prior_point);
173 Vector3 prior_velocity(initial_state.tail<3>());
174 imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
175
176 Values initial_values;
177 int correction_count = 0;
178 initial_values.insert(j: X(j: correction_count), val: prior_pose);
179 initial_values.insert(j: V(j: correction_count), val: prior_velocity);
180 initial_values.insert(j: B(j: correction_count), val: prior_imu_bias);
181
182 // Assemble prior noise model and add it the graph.`
183 auto pose_noise_model = noiseModel::Diagonal::Sigmas(
184 sigmas: (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
185 .finished()); // rad,rad,rad,m, m, m
186 auto velocity_noise_model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1); // m/s
187 auto bias_noise_model = noiseModel::Isotropic::Sigma(dim: 6, sigma: 1e-3);
188
189 // Add all prior factors (pose, velocity, bias) to the graph.
190 NonlinearFactorGraph* graph = new NonlinearFactorGraph();
191 graph->addPrior(key: X(j: correction_count), prior: prior_pose, model: pose_noise_model);
192 graph->addPrior(key: V(j: correction_count), prior: prior_velocity, model: velocity_noise_model);
193 graph->addPrior(key: B(j: correction_count), prior: prior_imu_bias, model: bias_noise_model);
194
195 auto p = imuParams();
196
197 std::shared_ptr<DefaultPreintegrationType> preintegrated =
198 std::make_shared<PreintegratedImuMeasurements>(args&: p, args&: prior_imu_bias);
199
200 assert(preintegrated);
201
202 // Store previous state for imu integration and latest predicted outcome.
203 NavState prev_state(prior_pose, prior_velocity);
204 NavState prop_state = prev_state;
205 imuBias::ConstantBias prev_bias = prior_imu_bias;
206
207 // Keep track of total error over the entire run as simple performance metric.
208 double current_position_error = 0.0, current_orientation_error = 0.0;
209
210 double output_time = 0.0;
211 double dt = 0.005; // The real system has noise, but here, results are nearly
212 // exactly the same, so keeping this for simplicity.
213
214 // All priors have been set up, now iterate through the data file.
215 std::string line;
216 int type{1000};
217 while (std::getline(is&: file, str&: line)) {
218 std::stringstream ss(line);
219 std::string value;
220
221 // Read value until comma. Skip to next line on failure to read.
222 if (!std::getline(in&: ss, str&: value, delim: ',')) continue;
223 try {
224 type = std::stoi(str: value);
225 } catch (const std::invalid_argument& e) {
226 std::cerr << "Invalid integer in input: \"" << value << "\"\n";
227 continue; // Or break, depending on desired behavior
228 }
229
230 if (type == 0) { // IMU measurement
231 Vector6 imu;
232 for (int i = 0; i < 5; ++i) {
233 getline(in&: file, str&: value, delim: ',');
234 imu(i) = stof(str: value.c_str());
235 }
236 getline(in&: file, str&: value, delim: '\n');
237 imu(5) = stof(str: value.c_str());
238
239 // Adding the IMU preintegration.
240 preintegrated->integrateMeasurement(measuredAcc: imu.head<3>(), measuredOmega: imu.tail<3>(), dt);
241
242 } else if (type == 1) { // GPS measurement
243 Vector7 gps;
244 for (int i = 0; i < 6; ++i) {
245 getline(in&: file, str&: value, delim: ',');
246 gps(i) = stof(str: value.c_str());
247 }
248 getline(in&: file, str&: value, delim: '\n');
249 gps(6) = stof(str: value.c_str());
250
251 correction_count++;
252
253 // Adding IMU factor and GPS factor and optimizing.
254 auto preint_imu =
255 dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
256 ImuFactor imu_factor(X(j: correction_count - 1), V(j: correction_count - 1),
257 X(j: correction_count), V(j: correction_count),
258 B(j: correction_count - 1), preint_imu);
259 graph->add(factorOrContainer: imu_factor);
260 imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
261 graph->add(factorOrContainer: BetweenFactor<imuBias::ConstantBias>(
262 B(j: correction_count - 1), B(j: correction_count), zero_bias,
263 bias_noise_model));
264
265 auto correction_noise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1.0);
266 GPSFactor gps_factor(X(j: correction_count),
267 Point3(gps(0), // N,
268 gps(1), // E,
269 gps(2)), // D,
270 correction_noise);
271 graph->add(factorOrContainer: gps_factor);
272
273 // Now optimize and compare results.
274 prop_state = preintegrated->predict(state_i: prev_state, bias_i: prev_bias);
275 initial_values.insert(j: X(j: correction_count), val: prop_state.pose());
276 initial_values.insert(j: V(j: correction_count), val: prop_state.v());
277 initial_values.insert(j: B(j: correction_count), val: prev_bias);
278
279 Values result;
280
281 if (use_isam) {
282 isam2->update(newFactors: *graph, newTheta: initial_values);
283 result = isam2->calculateEstimate();
284
285 // reset the graph
286 graph->resize(size: 0);
287 initial_values.clear();
288
289 } else {
290 LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
291 result = optimizer.optimize();
292 }
293
294 // Overwrite the beginning of the preintegration for the next step.
295 prev_state = NavState(result.at<Pose3>(j: X(j: correction_count)),
296 result.at<Vector3>(j: V(j: correction_count)));
297 prev_bias = result.at<imuBias::ConstantBias>(j: B(j: correction_count));
298
299 // Reset the preintegration object.
300 preintegrated->resetIntegrationAndSetBias(biasHat: prev_bias);
301
302 // Print out the position and orientation error for comparison.
303 Vector3 gtsam_position = prev_state.pose().translation();
304 Vector3 position_error = gtsam_position - gps.head<3>();
305 current_position_error = position_error.norm();
306
307 Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
308 Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
309 Quaternion quat_error = gtsam_quat * gps_quat.inverse();
310 quat_error.normalize();
311 Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
312 quat_error.z() * 2);
313 current_orientation_error = euler_angle_error.norm();
314
315 // display statistics
316 cout << "Position error:" << current_position_error << "\t "
317 << "Angular error:" << current_orientation_error << "\n";
318
319 fprintf(stream: fp_out, format: "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
320 output_time, gtsam_position(0), gtsam_position(1),
321 gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
322 gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
323 gps_quat.y(), gps_quat.z(), gps_quat.w());
324
325 output_time += 1.0;
326
327 } else {
328 cerr << "ERROR parsing file\n";
329 return 1;
330 }
331 }
332 fclose(stream: fp_out);
333 cout << "Complete, results written to " << output_filename << "\n\n";
334
335 return 0;
336}
337