37 using namespace gtsam;
73 vector<ImuMeasurement>& imu_measurements,
74 vector<GpsMeasurement>& gps_measurements) {
81 string imu_metadata_file =
83 ifstream imu_metadata(imu_metadata_file.c_str());
85 printf(
"-- Reading sensor metadata\n");
87 getline(imu_metadata, line,
'\n');
90 getline(imu_metadata, line,
'\n');
91 sscanf(line.c_str(),
"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
101 printf(
"IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
114 printf(
"-- Reading IMU measurements from file\n");
116 ifstream imu_data(imu_data_file.c_str());
117 getline(imu_data, line,
'\n');
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);
138 printf(
"-- Reading GPS measurements from file\n");
140 ifstream gps_data(gps_data_file.c_str());
141 getline(gps_data, line,
'\n');
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);
156 int main(
int argc,
char* argv[]) {
158 vector<ImuMeasurement> imu_measurements;
159 vector<GpsMeasurement> gps_measurements;
160 loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
168 if (!body_T_imu.equals(
Pose3(), 1
e-5)) {
170 "Currently only support IMUinBody is identity, i.e. IMU and body frame "
177 size_t first_gps_pose = 1;
178 size_t gps_skip = 10;
180 auto w_coriolis = Vector3::Zero();
183 auto noise_model_gps = noiseModel::Diagonal::Precisions(
184 (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
189 auto current_pose_global =
192 Vector3 current_velocity_global = Vector3::Zero();
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.00
e-05))
203 Matrix33 measured_acc_cov =
205 Matrix33 measured_omega_cov =
208 Matrix33 integration_error_cov =
211 auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(
g);
212 imu_params->accelerometerCovariance =
214 imu_params->integrationCovariance =
215 integration_error_cov;
216 imu_params->gyroscopeCovariance =
218 imu_params->omegaCoriolis = w_coriolis;
220 std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement =
241 "-- Starting main loop: inference is performed at each time step, but we "
242 "plot trajectory every 10 steps\n");
245 for (
size_t i = first_gps_pose;
i < gps_measurements.size() - 1;
i++) {
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;
253 if (
i == first_gps_pose) {
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);
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);
265 double t_previous = gps_measurements[
i - 1].time;
268 current_summarized_measurement =
269 std::make_shared<PreintegratedImuMeasurements>(imu_params,
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++;
283 auto previous_pose_key =
X(
i - 1);
284 auto previous_vel_key =
V(
i - 1);
285 auto previous_bias_key =
B(
i - 1);
288 previous_pose_key, previous_vel_key, current_pose_key,
289 current_vel_key, previous_bias_key, *current_summarized_measurement);
292 auto sigma_between_b = noiseModel::Diagonal::Sigmas(
293 (Vector6() << Vector3::Constant(
294 sqrt(included_imu_measurement_count) *
296 Vector3::Constant(
sqrt(included_imu_measurement_count) *
305 Pose3(current_pose_global.rotation(), gps_measurements[
i].position);
306 if ((
i % gps_skip) == 0) {
308 current_pose_key, gps_pose, noise_model_gps);
309 new_values.
insert(current_pose_key, gps_pose);
311 printf(
"############ POSE INCLUDED AT TIME %.6lf ############\n",
313 cout << gps_pose.translation();
316 new_values.
insert(current_pose_key, current_pose_global);
321 new_values.
insert(current_vel_key, current_velocity_global);
322 new_values.
insert(current_bias_key, current_bias);
328 if (
i > (first_gps_pose + 2 * gps_skip)) {
329 printf(
"############ NEW FACTORS AT TIME %.6lf ############\n",
346 printf(
"\n############ POSE AT TIME %lf ############\n",
t);
347 current_pose_global.print();
354 printf(
"\nWriting results to file...\n");
357 "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
360 for (
size_t i = first_gps_pose;
i < gps_measurements.size() - 1;
i++) {
361 auto pose_key =
X(
i);
363 auto bias_key =
B(
i);
370 auto gps = gps_measurements[
i].position;
372 cout <<
"State at #" <<
i << endl;
373 cout <<
"Pose:" << endl <<
pose << endl;
374 cout <<
"Velocity:" << endl <<
velocity << endl;
375 cout <<
"Bias:" << endl <<
bias << endl;
377 fprintf(fp_out,
"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
379 pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),