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