IMUKittiExampleGPS.cpp
Go to the documentation of this file.
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 
20 // GTSAM related includes.
21 #include <gtsam/inference/Symbol.h>
25 #include <gtsam/nonlinear/ISAM2.h>
29 #include <gtsam/slam/PriorFactor.h>
30 #include <gtsam/slam/dataset.h>
31 
32 #include <cstring>
33 #include <fstream>
34 #include <iostream>
35 
36 using namespace std;
37 using namespace gtsam;
38 
39 using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
40 using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
41 using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
42 
44  double body_ptx;
45  double body_pty;
46  double body_ptz;
47  double body_prx;
48  double body_pry;
49  double body_prz;
56 };
57 
59  double time;
60  double dt;
62  Vector3 gyroscope; // omega
63 };
64 
66  double time;
67  Vector3 position; // x,y,z
68 };
69 
70 const string output_filename = "IMUKittiExampleGPSResults.csv";
71 
72 void 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("KittiEquivBiasedImu_metadata.txt");
83  ifstream imu_metadata(imu_metadata_file.c_str());
84 
85  printf("-- Reading sensor metadata\n");
86 
87  getline(imu_metadata, line, '\n'); // ignore the first line
88 
89  // Load Kitti calibration
90  getline(imu_metadata, line, '\n');
91  sscanf(line.c_str(), "%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("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("KittiEquivBiasedImu.txt");
114  printf("-- Reading IMU measurements from file\n");
115  {
116  ifstream imu_data(imu_data_file.c_str());
117  getline(imu_data, line, '\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(imu_data, line, '\n');
123  sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
124  &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
125 
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(measurement);
132  }
133  }
134 
135  // Read GPS data
136  // Time,X,Y,Z
137  string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
138  printf("-- Reading GPS measurements from file\n");
139  {
140  ifstream gps_data(gps_data_file.c_str());
141  getline(gps_data, line, '\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(gps_data, line, '\n');
146  sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
147 
149  measurement.time = time;
150  measurement.position = Vector3(gps_x, gps_y, gps_z);
151  gps_measurements.push_back(measurement);
152  }
153  }
154 }
155 
156 int 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(BodyP);
168  if (!body_T_imu.equals(Pose3(), 1e-5)) {
169  printf(
170  "Currently only support IMUinBody is identity, i.e. IMU and body frame "
171  "are the same");
172  exit(-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  (Vector6() << Vector3::Constant(0), Vector3::Constant(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  (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
197  auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
198  auto sigma_init_b = noiseModel::Diagonal::Sigmas(
199  (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
200  .finished());
201 
202  // Set IMU preintegration parameters
203  Matrix33 measured_acc_cov =
204  I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
205  Matrix33 measured_omega_cov =
206  I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
207  // error committed in integrating position from velocities
208  Matrix33 integration_error_cov =
209  I_3x3 * pow(kitti_calibration.integration_sigma, 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 
240  printf(
241  "-- 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(i);
248  auto current_vel_key = V(i);
249  auto current_bias_key = B(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(current_pose_key, current_pose_global);
256  new_values.insert(current_vel_key, current_velocity_global);
257  new_values.insert(current_bias_key, current_bias);
258  new_factors.emplace_shared<PriorFactor<Pose3>>(
259  current_pose_key, current_pose_global, sigma_init_x);
261  current_vel_key, current_velocity_global, sigma_init_v);
263  current_bias_key, current_bias, 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>(imu_params,
270  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  imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
276  imu_measurements[j].dt);
277  included_imu_measurement_count++;
278  }
279  j++;
280  }
281 
282  // Create IMU factor
283  auto previous_pose_key = X(i - 1);
284  auto previous_vel_key = V(i - 1);
285  auto previous_bias_key = B(i - 1);
286 
287  new_factors.emplace_shared<ImuFactor>(
288  previous_pose_key, previous_vel_key, current_pose_key,
289  current_vel_key, previous_bias_key, *current_summarized_measurement);
290 
291  // Bias evolution as given in the IMU metadata
292  auto sigma_between_b = noiseModel::Diagonal::Sigmas(
293  (Vector6() << Vector3::Constant(
294  sqrt(included_imu_measurement_count) *
295  kitti_calibration.accelerometer_bias_sigma),
296  Vector3::Constant(sqrt(included_imu_measurement_count) *
297  kitti_calibration.gyroscope_bias_sigma))
298  .finished());
300  previous_bias_key, current_bias_key, imuBias::ConstantBias(),
301  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  current_pose_key, gps_pose, noise_model_gps);
309  new_values.insert(current_pose_key, gps_pose);
310 
311  printf("############ POSE INCLUDED AT TIME %.6lf ############\n",
312  t);
313  cout << gps_pose.translation();
314  printf("\n\n");
315  } else {
316  new_values.insert(current_pose_key, current_pose_global);
317  }
318 
319  // Add initial values for velocity and bias based on the previous
320  // estimates
321  new_values.insert(current_vel_key, current_velocity_global);
322  new_values.insert(current_bias_key, 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("############ NEW FACTORS AT TIME %.6lf ############\n",
330  t);
331  new_factors.print();
332 
333  isam.update(new_factors, new_values);
334 
335  // Reset the newFactors and newValues list
336  new_factors.resize(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>(current_pose_key);
343  current_velocity_global = result.at<Vector3>(current_vel_key);
344  current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
345 
346  printf("\n############ POSE AT TIME %lf ############\n", t);
347  current_pose_global.print();
348  printf("\n\n");
349  }
350  }
351  }
352 
353  // Save results to file
354  printf("\nWriting results to file...\n");
355  FILE* fp_out = fopen(output_filename.c_str(), "w+");
356  fprintf(fp_out,
357  "#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(i);
362  auto vel_key = V(i);
363  auto bias_key = B(i);
364 
365  auto pose = result.at<Pose3>(pose_key);
366  auto velocity = result.at<Vector3>(vel_key);
367  auto bias = result.at<imuBias::ConstantBias>(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(fp_out, "%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(fp_out);
384 }
loadKittiData
void loadKittiData(KittiCalibration &kitti_calibration, vector< ImuMeasurement > &imu_measurements, vector< GpsMeasurement > &gps_measurements)
Definition: IMUKittiExampleGPS.cpp:72
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
KittiCalibration::accelerometer_sigma
double accelerometer_sigma
Definition: IMUKittiExampleGPS.cpp:50
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::ISAM2
Definition: ISAM2.h:45
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
KittiCalibration::accelerometer_bias_sigma
double accelerometer_bias_sigma
Definition: IMUKittiExampleGPS.cpp:53
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ISAM2Params.h
Parameters for iSAM 2.
KittiCalibration::integration_sigma
double integration_sigma
Definition: IMUKittiExampleGPS.cpp:52
dt
const double dt
Definition: testVelocityConstraint.cpp:15
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
X
#define X
Definition: icosphere.cpp:20
ImuMeasurement::accelerometer
Vector3 accelerometer
Definition: IMUKittiExampleGPS.cpp:61
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
KittiCalibration::body_pry
double body_pry
Definition: IMUKittiExampleGPS.cpp:48
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
KittiCalibration
Definition: IMUKittiExampleGPS.cpp:43
dataset.h
utility functions for loading datasets
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
BetweenFactor.h
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:232
gtsam::internal::velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:28
GpsMeasurement::position
Vector3 position
Definition: IMUKittiExampleGPS.cpp:67
gtsam::Pose3::y
double y() const
get y
Definition: Pose3.h:297
gtsam::ISAM2Params::factorization
Factorization factorization
Definition: ISAM2Params.h:194
main
int main(int argc, char *argv[])
Definition: IMUKittiExampleGPS.cpp:156
gtsam::ImuMeasurement
Contains data from the IMU mesaurements.
Definition: ImuMeasurement.h:10
ImuMeasurement::dt
double dt
Definition: IMUKittiExampleGPS.cpp:60
Symbol.h
gtsam::ImuFactor
Definition: ImuFactor.h:169
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
KittiCalibration::gyroscope_sigma
double gyroscope_sigma
Definition: IMUKittiExampleGPS.cpp:51
time
#define time
Definition: timeAdaptAutoDiff.cpp:31
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
KittiCalibration::body_prz
double body_prz
Definition: IMUKittiExampleGPS.cpp:49
ImuMeasurement::gyroscope
Vector3 gyroscope
Definition: IMUKittiExampleGPS.cpp:62
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
KittiCalibration::body_ptz
double body_ptz
Definition: IMUKittiExampleGPS.cpp:46
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
KittiCalibration::gyroscope_bias_sigma
double gyroscope_bias_sigma
Definition: IMUKittiExampleGPS.cpp:54
KittiCalibration::body_pty
double body_pty
Definition: IMUKittiExampleGPS.cpp:45
gtsam
traits
Definition: SFMdata.h:40
PriorFactor.h
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
GpsMeasurement::time
double time
Definition: IMUKittiExampleGPS.cpp:66
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::Pose3::x
double x() const
get x
Definition: Pose3.h:292
gtsam::internal::position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:25
ImuMeasurement::time
double time
Definition: IMUKittiExampleGPS.cpp:59
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
KittiCalibration::body_prx
double body_prx
Definition: IMUKittiExampleGPS.cpp:47
CombinedImuFactor.h
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
GpsMeasurement
Definition: IMUKittiExampleGPS.cpp:65
output_filename
const string output_filename
Definition: IMUKittiExampleGPS.cpp:70
align_3::t
Point2 t(10, 10)
gtsam::ISAM2Params::relinearizeSkip
int relinearizeSkip
Definition: ISAM2Params.h:171
GPSFactor.h
Header file for GPS factor.
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
measurement
static Point2 measurement(323.0, 240.0)
KittiCalibration::average_delta_t
double average_delta_t
Definition: IMUKittiExampleGPS.cpp:55
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
ImuFactor.h
gtsam::Pose3::z
double z() const
get z
Definition: Pose3.h:302
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
KittiCalibration::body_ptx
double body_ptx
Definition: IMUKittiExampleGPS.cpp:44
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactorGraph.cpp:55


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:27