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 IMUKittiExampleGPS
14 * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI
15 * VISION BENCHMARK SUITE
16 * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ
17 * Electronics
18 */
19
20// GTSAM related includes.
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/navigation/CombinedImuFactor.h>
23#include <gtsam/navigation/GPSFactor.h>
24#include <gtsam/navigation/ImuFactor.h>
25#include <gtsam/nonlinear/ISAM2.h>
26#include <gtsam/nonlinear/ISAM2Params.h>
27#include <gtsam/nonlinear/NonlinearFactorGraph.h>
28#include <gtsam/slam/BetweenFactor.h>
29#include <gtsam/slam/PriorFactor.h>
30#include <gtsam/slam/dataset.h>
31
32#include <cstring>
33#include <fstream>
34#include <iostream>
35
36using namespace std;
37using namespace gtsam;
38
39using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
40using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
41using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
42
43struct KittiCalibration {
44 double body_ptx;
45 double body_pty;
46 double body_ptz;
47 double body_prx;
48 double body_pry;
49 double body_prz;
50 double accelerometer_sigma;
51 double gyroscope_sigma;
52 double integration_sigma;
53 double accelerometer_bias_sigma;
54 double gyroscope_bias_sigma;
55 double average_delta_t;
56};
57
58struct ImuMeasurement {
59 double time;
60 double dt;
61 Vector3 accelerometer;
62 Vector3 gyroscope; // omega
63};
64
65struct GpsMeasurement {
66 double time;
67 Vector3 position; // x,y,z
68};
69
70const string output_filename = "IMUKittiExampleGPSResults.csv";
71
72void loadKittiData(KittiCalibration& kitti_calibration,
73 vector<ImuMeasurement>& imu_measurements,
74 vector<GpsMeasurement>& gps_measurements) {
75 string line;
76
77 // Read IMU metadata and compute relative sensor pose transforms
78 // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
79 // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
80 // AverageDeltaT
81 string imu_metadata_file =
82 findExampleDataFile(name: "KittiEquivBiasedImu_metadata.txt");
83 ifstream imu_metadata(imu_metadata_file.c_str());
84
85 printf(format: "-- Reading sensor metadata\n");
86
87 getline(in&: imu_metadata, str&: line, delim: '\n'); // ignore the first line
88
89 // Load Kitti calibration
90 getline(in&: imu_metadata, str&: line, delim: '\n');
91 sscanf(s: line.c_str(), format: "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
92 &kitti_calibration.body_ptx, &kitti_calibration.body_pty,
93 &kitti_calibration.body_ptz, &kitti_calibration.body_prx,
94 &kitti_calibration.body_pry, &kitti_calibration.body_prz,
95 &kitti_calibration.accelerometer_sigma,
96 &kitti_calibration.gyroscope_sigma,
97 &kitti_calibration.integration_sigma,
98 &kitti_calibration.accelerometer_bias_sigma,
99 &kitti_calibration.gyroscope_bias_sigma,
100 &kitti_calibration.average_delta_t);
101 printf(format: "IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
102 kitti_calibration.body_ptx, kitti_calibration.body_pty,
103 kitti_calibration.body_ptz, kitti_calibration.body_prx,
104 kitti_calibration.body_pry, kitti_calibration.body_prz,
105 kitti_calibration.accelerometer_sigma,
106 kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma,
107 kitti_calibration.accelerometer_bias_sigma,
108 kitti_calibration.gyroscope_bias_sigma,
109 kitti_calibration.average_delta_t);
110
111 // Read IMU data
112 // Time dt accelX accelY accelZ omegaX omegaY omegaZ
113 string imu_data_file = findExampleDataFile(name: "KittiEquivBiasedImu.txt");
114 printf(format: "-- Reading IMU measurements from file\n");
115 {
116 ifstream imu_data(imu_data_file.c_str());
117 getline(in&: imu_data, str&: line, delim: '\n'); // ignore the first line
118
119 double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0,
120 gyro_y = 0, gyro_z = 0;
121 while (!imu_data.eof()) {
122 getline(in&: imu_data, str&: line, delim: '\n');
123 sscanf(s: line.c_str(), format: "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
124 &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
125
126 ImuMeasurement measurement;
127 measurement.time = time;
128 measurement.dt = dt;
129 measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
130 measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
131 imu_measurements.push_back(x: measurement);
132 }
133 }
134
135 // Read GPS data
136 // Time,X,Y,Z
137 string gps_data_file = findExampleDataFile(name: "KittiGps_converted.txt");
138 printf(format: "-- Reading GPS measurements from file\n");
139 {
140 ifstream gps_data(gps_data_file.c_str());
141 getline(in&: gps_data, str&: line, delim: '\n'); // ignore the first line
142
143 double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
144 while (!gps_data.eof()) {
145 getline(in&: gps_data, str&: line, delim: '\n');
146 sscanf(s: line.c_str(), format: "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
147
148 GpsMeasurement measurement;
149 measurement.time = time;
150 measurement.position = Vector3(gps_x, gps_y, gps_z);
151 gps_measurements.push_back(x: measurement);
152 }
153 }
154}
155
156int main(int argc, char* argv[]) {
157 KittiCalibration kitti_calibration;
158 vector<ImuMeasurement> imu_measurements;
159 vector<GpsMeasurement> gps_measurements;
160 loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
161
162 Vector6 BodyP =
163 (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
164 kitti_calibration.body_ptz, kitti_calibration.body_prx,
165 kitti_calibration.body_pry, kitti_calibration.body_prz)
166 .finished();
167 auto body_T_imu = Pose3::Expmap(xi: BodyP);
168 if (!body_T_imu.equals(pose: Pose3(), tol: 1e-5)) {
169 printf(
170 format: "Currently only support IMUinBody is identity, i.e. IMU and body frame "
171 "are the same");
172 exit(status: -1);
173 }
174
175 // Configure different variables
176 // double t_offset = gps_measurements[0].time;
177 size_t first_gps_pose = 1;
178 size_t gps_skip = 10; // Skip this many GPS measurements each time
179 double g = 9.8;
180 auto w_coriolis = Vector3::Zero(); // zero vector
181
182 // Configure noise models
183 auto noise_model_gps = noiseModel::Diagonal::Precisions(
184 precisions: (Vector6() << Vector3::Constant(value: 0), Vector3::Constant(value: 1.0 / 0.07))
185 .finished());
186
187 // Set initial conditions for the estimated trajectory
188 // initial pose is the reference frame (navigation frame)
189 auto current_pose_global =
190 Pose3(Rot3(), gps_measurements[first_gps_pose].position);
191 // the vehicle is stationary at the beginning at position 0,0,0
192 Vector3 current_velocity_global = Vector3::Zero();
193 auto current_bias = imuBias::ConstantBias(); // init with zero bias
194
195 auto sigma_init_x = noiseModel::Diagonal::Precisions(
196 precisions: (Vector6() << Vector3::Constant(value: 0), Vector3::Constant(value: 1.0)).finished());
197 auto sigma_init_v = noiseModel::Diagonal::Sigmas(sigmas: Vector3::Constant(value: 1000.0));
198 auto sigma_init_b = noiseModel::Diagonal::Sigmas(
199 sigmas: (Vector6() << Vector3::Constant(value: 0.100), Vector3::Constant(value: 5.00e-05))
200 .finished());
201
202 // Set IMU preintegration parameters
203 Matrix33 measured_acc_cov =
204 I_3x3 * pow(x: kitti_calibration.accelerometer_sigma, y: 2);
205 Matrix33 measured_omega_cov =
206 I_3x3 * pow(x: kitti_calibration.gyroscope_sigma, y: 2);
207 // error committed in integrating position from velocities
208 Matrix33 integration_error_cov =
209 I_3x3 * pow(x: kitti_calibration.integration_sigma, y: 2);
210
211 auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
212 imu_params->accelerometerCovariance =
213 measured_acc_cov; // acc white noise in continuous
214 imu_params->integrationCovariance =
215 integration_error_cov; // integration uncertainty continuous
216 imu_params->gyroscopeCovariance =
217 measured_omega_cov; // gyro white noise in continuous
218 imu_params->omegaCoriolis = w_coriolis;
219
220 std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement =
221 nullptr;
222
223 // Set ISAM2 parameters and create ISAM2 solver object
224 ISAM2Params isam_params;
225 isam_params.factorization = ISAM2Params::CHOLESKY;
226 isam_params.relinearizeSkip = 10;
227
228 ISAM2 isam(isam_params);
229
230 // Create the factor graph and values object that will store new factors and
231 // values to add to the incremental graph
232 NonlinearFactorGraph new_factors;
233 Values new_values; // values storing the initial estimates of new nodes in
234 // the factor graph
235
236 /// Main loop:
237 /// (1) we read the measurements
238 /// (2) we create the corresponding factors in the graph
239 /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
240 printf(
241 format: "-- Starting main loop: inference is performed at each time step, but we "
242 "plot trajectory every 10 steps\n");
243 size_t j = 0;
244
245 for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
246 // At each non=IMU measurement we initialize a new node in the graph
247 auto current_pose_key = X(j: i);
248 auto current_vel_key = V(j: i);
249 auto current_bias_key = B(j: i);
250 double t = gps_measurements[i].time;
251 size_t included_imu_measurement_count = 0;
252
253 if (i == first_gps_pose) {
254 // Create initial estimate and prior on initial pose, velocity, and biases
255 new_values.insert(j: current_pose_key, val: current_pose_global);
256 new_values.insert(j: current_vel_key, val: current_velocity_global);
257 new_values.insert(j: current_bias_key, val: current_bias);
258 new_factors.emplace_shared<PriorFactor<Pose3>>(
259 args&: current_pose_key, args&: current_pose_global, args&: sigma_init_x);
260 new_factors.emplace_shared<PriorFactor<Vector3>>(
261 args&: current_vel_key, args&: current_velocity_global, args&: sigma_init_v);
262 new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
263 args&: current_bias_key, args&: current_bias, args&: sigma_init_b);
264 } else {
265 double t_previous = gps_measurements[i - 1].time;
266
267 // Summarize IMU data between the previous GPS measurement and now
268 current_summarized_measurement =
269 std::make_shared<PreintegratedImuMeasurements>(args&: imu_params,
270 args&: current_bias);
271
272 while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
273 if (imu_measurements[j].time >= t_previous) {
274 current_summarized_measurement->integrateMeasurement(
275 measuredAcc: imu_measurements[j].accelerometer, measuredOmega: imu_measurements[j].gyroscope,
276 dt: imu_measurements[j].dt);
277 included_imu_measurement_count++;
278 }
279 j++;
280 }
281
282 // Create IMU factor
283 auto previous_pose_key = X(j: i - 1);
284 auto previous_vel_key = V(j: i - 1);
285 auto previous_bias_key = B(j: i - 1);
286
287 new_factors.emplace_shared<ImuFactor>(
288 args&: previous_pose_key, args&: previous_vel_key, args&: current_pose_key,
289 args&: current_vel_key, args&: previous_bias_key, args&: *current_summarized_measurement);
290
291 // Bias evolution as given in the IMU metadata
292 auto sigma_between_b = noiseModel::Diagonal::Sigmas(
293 sigmas: (Vector6() << Vector3::Constant(
294 value: sqrt(x: included_imu_measurement_count) *
295 kitti_calibration.accelerometer_bias_sigma),
296 Vector3::Constant(value: sqrt(x: included_imu_measurement_count) *
297 kitti_calibration.gyroscope_bias_sigma))
298 .finished());
299 new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(
300 args&: previous_bias_key, args&: current_bias_key, args: imuBias::ConstantBias(),
301 args&: sigma_between_b);
302
303 // Create GPS factor
304 auto gps_pose =
305 Pose3(current_pose_global.rotation(), gps_measurements[i].position);
306 if ((i % gps_skip) == 0) {
307 new_factors.emplace_shared<PriorFactor<Pose3>>(
308 args&: current_pose_key, args&: gps_pose, args&: noise_model_gps);
309 new_values.insert(j: current_pose_key, val: gps_pose);
310
311 printf(format: "############ POSE INCLUDED AT TIME %.6lf ############\n",
312 t);
313 cout << gps_pose.translation();
314 printf(format: "\n\n");
315 } else {
316 new_values.insert(j: current_pose_key, val: current_pose_global);
317 }
318
319 // Add initial values for velocity and bias based on the previous
320 // estimates
321 new_values.insert(j: current_vel_key, val: current_velocity_global);
322 new_values.insert(j: current_bias_key, val: current_bias);
323
324 // Update solver
325 // =======================================================================
326 // We accumulate 2*GPSskip GPS measurements before updating the solver at
327 // first so that the heading becomes observable.
328 if (i > (first_gps_pose + 2 * gps_skip)) {
329 printf(format: "############ NEW FACTORS AT TIME %.6lf ############\n",
330 t);
331 new_factors.print();
332
333 isam.update(newFactors: new_factors, newTheta: new_values);
334
335 // Reset the newFactors and newValues list
336 new_factors.resize(size: 0);
337 new_values.clear();
338
339 // Extract the result/current estimates
340 Values result = isam.calculateEstimate();
341
342 current_pose_global = result.at<Pose3>(j: current_pose_key);
343 current_velocity_global = result.at<Vector3>(j: current_vel_key);
344 current_bias = result.at<imuBias::ConstantBias>(j: current_bias_key);
345
346 printf(format: "\n############ POSE AT TIME %lf ############\n", t);
347 current_pose_global.print();
348 printf(format: "\n\n");
349 }
350 }
351 }
352
353 // Save results to file
354 printf(format: "\nWriting results to file...\n");
355 FILE* fp_out = fopen(filename: output_filename.c_str(), modes: "w+");
356 fprintf(stream: fp_out,
357 format: "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
358
359 Values result = isam.calculateEstimate();
360 for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
361 auto pose_key = X(j: i);
362 auto vel_key = V(j: i);
363 auto bias_key = B(j: i);
364
365 auto pose = result.at<Pose3>(j: pose_key);
366 auto velocity = result.at<Vector3>(j: vel_key);
367 auto bias = result.at<imuBias::ConstantBias>(j: bias_key);
368
369 auto pose_quat = pose.rotation().toQuaternion();
370 auto gps = gps_measurements[i].position;
371
372 cout << "State at #" << i << endl;
373 cout << "Pose:" << endl << pose << endl;
374 cout << "Velocity:" << endl << velocity << endl;
375 cout << "Bias:" << endl << bias << endl;
376
377 fprintf(stream: fp_out, format: "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
378 gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
379 pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
380 gps(1), gps(2));
381 }
382
383 fclose(stream: fp_out);
384}
385