38 #include <boost/program_options.hpp>
55 using namespace gtsam;
62 namespace po = boost::program_options;
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")(
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");
75 po::store(po::parse_command_line(argc, argv,
desc), vm);
77 if (vm.count(
"help")) {
85 std::shared_ptr<PreintegratedCombinedMeasurements::Params>
imuParams() {
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 =
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 =
102 p->accelerometerCovariance =
104 p->integrationCovariance =
105 integration_error_cov;
108 p->gyroscopeCovariance =
111 p->biasAccCovariance = bias_acc_cov;
112 p->biasOmegaCovariance = bias_omega_cov;
113 p->biasAccOmegaInt = bias_acc_omega_init;
118 int main(
int argc,
char* argv[]) {
121 bool use_isam =
false;
127 use_isam = var_map[
"use_isam"].as<
bool>();
131 printf(
"Using ISAM2\n");
138 printf(
"Using Levenberg Marquardt Optimizer\n");
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");
150 ifstream
file(data_filename.c_str());
154 Vector10 initial_state;
156 for (
int i = 0;
i < 9;
i++) {
158 initial_state(
i) = stof(
value.c_str());
161 initial_state(9) = stof(
value.c_str());
162 cout <<
"initial state:\n" << initial_state.transpose() <<
"\n\n";
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>());
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);
181 (
Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
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);
194 std::shared_ptr<PreintegrationType> preintegrated =
195 std::make_shared<PreintegratedImuMeasurements>(
p, prior_imu_bias);
197 assert(preintegrated);
200 NavState prev_state(prior_pose, prior_velocity);
205 double current_position_error = 0.0, current_orientation_error = 0.0;
207 double output_time = 0.0;
212 while (
file.good()) {
219 for (
int i = 0;
i < 5; ++
i) {
221 imu(
i) = stof(
value.c_str());
224 imu(5) = stof(
value.c_str());
227 preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(),
dt);
229 }
else if (
type == 1) {
231 for (
int i = 0;
i < 6; ++
i) {
233 gps(
i) = stof(
value.c_str());
236 gps(6) = stof(
value.c_str());
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);
249 B(correction_count - 1),
B(correction_count), zero_bias,
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);
270 result = isam2->calculateEstimate();
274 initial_values.
clear();
287 preintegrated->resetIntegrationAndSetBias(prev_bias);
291 Vector3 position_error = gtsam_position - gps.head<3>();
292 current_position_error = position_error.norm();
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,
300 current_orientation_error = euler_angle_error.norm();
303 cout <<
"Position error:" << current_position_error <<
"\t "
304 <<
"Angular error:" << current_orientation_error <<
"\n";
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());
315 cerr <<
"ERROR parsing file\n";