ImuFactorsExample.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 
38 #include <boost/program_options.hpp>
39 
40 // GTSAM related includes.
41 #include <gtsam/inference/Symbol.h>
45 #include <gtsam/nonlinear/ISAM2.h>
49 #include <gtsam/slam/dataset.h>
50 
51 #include <cstring>
52 #include <fstream>
53 #include <iostream>
54 
55 using namespace gtsam;
56 using namespace std;
57 
58 using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
59 using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
60 using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
61 
62 namespace po = boost::program_options;
63 
64 po::variables_map parseOptions(int argc, char* argv[]) {
65  po::options_description desc;
66  desc.add_options()("help,h", "produce help message")(
67  "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
68  "path to the CSV file with the IMU data")(
69  "output_filename",
70  po::value<string>()->default_value("imuFactorExampleResults.csv"),
71  "path to the result file to use")("use_isam", po::bool_switch(),
72  "use ISAM as the optimizer");
73 
74  po::variables_map vm;
75  po::store(po::parse_command_line(argc, argv, desc), vm);
76 
77  if (vm.count("help")) {
78  cout << desc << "\n";
79  exit(1);
80  }
81 
82  return vm;
83 }
84 
85 std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
86  // We use the sensor specs to build the noise model for the IMU factor.
87  double accel_noise_sigma = 0.0003924;
88  double gyro_noise_sigma = 0.000205689024915;
89  double accel_bias_rw_sigma = 0.004905;
90  double gyro_bias_rw_sigma = 0.000001454441043;
91  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
92  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
93  Matrix33 integration_error_cov =
94  I_3x3 * 1e-8; // error committed in integrating position from velocities
95  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
96  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
97  Matrix66 bias_acc_omega_init =
98  I_6x6 * 1e-5; // error in the bias used for preintegration
99 
101  // PreintegrationBase params:
102  p->accelerometerCovariance =
103  measured_acc_cov; // acc white noise in continuous
104  p->integrationCovariance =
105  integration_error_cov; // integration uncertainty continuous
106  // should be using 2nd order integration
107  // PreintegratedRotation params:
108  p->gyroscopeCovariance =
109  measured_omega_cov; // gyro white noise in continuous
110  // PreintegrationCombinedMeasurements params:
111  p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
112  p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
113  p->biasAccOmegaInt = bias_acc_omega_init;
114 
115  return p;
116 }
117 
118 int main(int argc, char* argv[]) {
119  string data_filename, output_filename;
120 
121  bool use_isam = false;
122 
123  po::variables_map var_map = parseOptions(argc, argv);
124 
125  data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
126  output_filename = var_map["output_filename"].as<string>();
127  use_isam = var_map["use_isam"].as<bool>();
128 
129  ISAM2* isam2 = 0;
130  if (use_isam) {
131  printf("Using ISAM2\n");
133  parameters.relinearizeThreshold = 0.01;
134  parameters.relinearizeSkip = 1;
135  isam2 = new ISAM2(parameters);
136 
137  } else {
138  printf("Using Levenberg Marquardt Optimizer\n");
139  }
140 
141  // Set up output file for plotting errors
142  FILE* fp_out = fopen(output_filename.c_str(), "w+");
143  fprintf(fp_out,
144  "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
145  "gt_qy,gt_qz,gt_qw\n");
146 
147  // Begin parsing the CSV file. Input the first line for initialization.
148  // From there, we'll iterate through the file and we'll preintegrate the IMU
149  // or add in the GPS given the input.
150  ifstream file(data_filename.c_str());
151  string value;
152 
153  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
154  Vector10 initial_state;
155  getline(file, value, ','); // i
156  for (int i = 0; i < 9; i++) {
157  getline(file, value, ',');
158  initial_state(i) = stof(value.c_str());
159  }
160  getline(file, value, '\n');
161  initial_state(9) = stof(value.c_str());
162  cout << "initial state:\n" << initial_state.transpose() << "\n\n";
163 
164  // Assemble initial quaternion through GTSAM constructor
165  // ::quaternion(w,x,y,z);
166  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
167  initial_state(4), initial_state(5));
168  Point3 prior_point(initial_state.head<3>());
169  Pose3 prior_pose(prior_rotation, prior_point);
170  Vector3 prior_velocity(initial_state.tail<3>());
171  imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
172 
173  Values initial_values;
174  int correction_count = 0;
175  initial_values.insert(X(correction_count), prior_pose);
176  initial_values.insert(V(correction_count), prior_velocity);
177  initial_values.insert(B(correction_count), prior_imu_bias);
178 
179  // Assemble prior noise model and add it the graph.`
180  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
181  (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
182  .finished()); // rad,rad,rad,m, m, m
183  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s
184  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);
185 
186  // Add all prior factors (pose, velocity, bias) to the graph.
188  graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
189  graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
190  graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);
191 
192  auto p = imuParams();
193 
194  std::shared_ptr<PreintegrationType> preintegrated =
195  std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
196 
197  assert(preintegrated);
198 
199  // Store previous state for imu integration and latest predicted outcome.
200  NavState prev_state(prior_pose, prior_velocity);
201  NavState prop_state = prev_state;
202  imuBias::ConstantBias prev_bias = prior_imu_bias;
203 
204  // Keep track of total error over the entire run as simple performance metric.
205  double current_position_error = 0.0, current_orientation_error = 0.0;
206 
207  double output_time = 0.0;
208  double dt = 0.005; // The real system has noise, but here, results are nearly
209  // exactly the same, so keeping this for simplicity.
210 
211  // All priors have been set up, now iterate through the data file.
212  while (file.good()) {
213  // Parse out first value
214  getline(file, value, ',');
215  int type = stoi(value.c_str());
216 
217  if (type == 0) { // IMU measurement
218  Vector6 imu;
219  for (int i = 0; i < 5; ++i) {
220  getline(file, value, ',');
221  imu(i) = stof(value.c_str());
222  }
223  getline(file, value, '\n');
224  imu(5) = stof(value.c_str());
225 
226  // Adding the IMU preintegration.
227  preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
228 
229  } else if (type == 1) { // GPS measurement
230  Vector7 gps;
231  for (int i = 0; i < 6; ++i) {
232  getline(file, value, ',');
233  gps(i) = stof(value.c_str());
234  }
235  getline(file, value, '\n');
236  gps(6) = stof(value.c_str());
237 
238  correction_count++;
239 
240  // Adding IMU factor and GPS factor and optimizing.
241  auto preint_imu =
242  dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
243  ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
244  X(correction_count), V(correction_count),
245  B(correction_count - 1), preint_imu);
246  graph->add(imu_factor);
247  imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
249  B(correction_count - 1), B(correction_count), zero_bias,
250  bias_noise_model));
251 
252  auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
253  GPSFactor gps_factor(X(correction_count),
254  Point3(gps(0), // N,
255  gps(1), // E,
256  gps(2)), // D,
257  correction_noise);
258  graph->add(gps_factor);
259 
260  // Now optimize and compare results.
261  prop_state = preintegrated->predict(prev_state, prev_bias);
262  initial_values.insert(X(correction_count), prop_state.pose());
263  initial_values.insert(V(correction_count), prop_state.v());
264  initial_values.insert(B(correction_count), prev_bias);
265 
266  Values result;
267 
268  if (use_isam) {
269  isam2->update(*graph, initial_values);
270  result = isam2->calculateEstimate();
271 
272  // reset the graph
273  graph->resize(0);
274  initial_values.clear();
275 
276  } else {
277  LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
278  result = optimizer.optimize();
279  }
280 
281  // Overwrite the beginning of the preintegration for the next step.
282  prev_state = NavState(result.at<Pose3>(X(correction_count)),
283  result.at<Vector3>(V(correction_count)));
284  prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
285 
286  // Reset the preintegration object.
287  preintegrated->resetIntegrationAndSetBias(prev_bias);
288 
289  // Print out the position and orientation error for comparison.
290  Vector3 gtsam_position = prev_state.pose().translation();
291  Vector3 position_error = gtsam_position - gps.head<3>();
292  current_position_error = position_error.norm();
293 
294  Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
295  Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
296  Quaternion quat_error = gtsam_quat * gps_quat.inverse();
297  quat_error.normalize();
298  Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
299  quat_error.z() * 2);
300  current_orientation_error = euler_angle_error.norm();
301 
302  // display statistics
303  cout << "Position error:" << current_position_error << "\t "
304  << "Angular error:" << current_orientation_error << "\n";
305 
306  fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
307  output_time, gtsam_position(0), gtsam_position(1),
308  gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
309  gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
310  gps_quat.y(), gps_quat.z(), gps_quat.w());
311 
312  output_time += 1.0;
313 
314  } else {
315  cerr << "ERROR parsing file\n";
316  return 1;
317  }
318  }
319  fclose(fp_out);
320  cout << "Complete, results written to " << output_filename << "\n\n";
321 
322  return 0;
323 }
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Rot3::Quaternion
static Rot3 Quaternion(double w, double x, double y, double z)
Definition: Rot3.h:204
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
dt
const double dt
Definition: testVelocityConstraint.cpp:15
main
int main(int argc, char *argv[])
Definition: ImuFactorsExample.cpp:118
E1::B
@ B
gtsam::NavState
Definition: NavState.h:34
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition: NavState.h:107
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
make_changelog.desc
desc
Definition: make_changelog.py:71
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
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
dataset.h
utility functions for loading datasets
gtsam::PreintegrationCombinedParams::MakeSharedD
static std::shared_ptr< PreintegrationCombinedParams > MakeSharedD(double g=9.81)
Definition: PreintegrationCombinedParams.h:60
BetweenFactor.h
parseOptions
po::variables_map parseOptions(int argc, char *argv[])
Definition: ImuFactorsExample.cpp:64
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
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:232
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
gtsam::ImuFactor
Definition: ImuFactor.h:169
gtsam::symbol_shorthand::B
Key B(std::uint64_t j)
Definition: inference/Symbol.h:149
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
CombinedImuFactor.h
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
output_filename
const string output_filename
Definition: IMUKittiExampleGPS.cpp:70
imuParams
std::shared_ptr< PreintegratedCombinedMeasurements::Params > imuParams()
Definition: ImuFactorsExample.cpp:85
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:86
GPSFactor.h
Header file for GPS factor.
gtsam::symbol_shorthand::V
Key V(std::uint64_t j)
Definition: inference/Symbol.h:169
matlab_wrap.file
file
Definition: matlab_wrap.py:57
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::GPSFactor
Definition: GPSFactor.h:35
ImuFactor.h
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:45