diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 315f7b8e1..fe1a99ed0 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -40,15 +40,15 @@ */ // GTSAM related includes. +#include #include #include #include -#include -#include +#include #include #include -#include -#include +#include +#include #include #include @@ -64,45 +64,75 @@ using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -static const char output_filename[] = "imuFactorExampleResults.csv"; -static const char use_combined_imu_flag[3] = "-c"; +boost::shared_ptr imuParams() { + // We use the sensor specs to build the noise model for the IMU factor. + double accel_noise_sigma = 0.0003924; + double gyro_noise_sigma = 0.000205689024915; + double accel_bias_rw_sigma = 0.004905; + double gyro_bias_rw_sigma = 0.000001454441043; + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration + + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + // PreintegrationBase params: + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + // should be using 2nd order integration + // PreintegratedRotation params: + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + // PreintegrationCombinedMeasurements params: + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccOmegaInt = bias_acc_omega_int; + + return p; +} int main(int argc, char* argv[]) { - string data_filename; - bool use_combined_imu = false; + string data_filename, output_filename; -#ifndef USE_LM - printf("Using ISAM2\n"); - ISAM2Params parameters; - parameters.relinearizeThreshold = 0.01; - parameters.relinearizeSkip = 1; - ISAM2 isam2(parameters); -#else - printf("Using Levenberg Marquardt Optimizer\n"); -#endif + bool use_isam = false; - if (argc < 2) { - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else if (argc < 3) { - if (strcmp(argv[1], use_combined_imu_flag) == 0) { - printf("using CombinedImuFactor\n"); - use_combined_imu = true; - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else { - data_filename = argv[1]; - } - } else { + if (argc == 4) { data_filename = argv[1]; - if (strcmp(argv[2], use_combined_imu_flag) == 0) { - printf("using CombinedImuFactor\n"); - use_combined_imu = true; - } + output_filename = argv[2]; + use_isam = atoi(argv[3]); + + } else if (argc == 3) { + data_filename = argv[1]; + output_filename = argv[2]; + } else if (argc == 2) { + data_filename = argv[1]; + output_filename = "imuFactorExampleResults.csv"; + } else { + printf("using default files\n"); + data_filename = findExampleDataFile("imuAndGPSdata.csv"); + output_filename = "imuFactorExampleResults.csv"; + } + + ISAM2* isam2; + if (use_isam) { + printf("Using ISAM2\n"); + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + isam2 = new ISAM2(parameters); + + } else { + printf("Using Levenberg Marquardt Optimizer\n"); } // Set up output file for plotting errors - FILE* fp_out = fopen(output_filename, "w+"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx," "gt_qy,gt_qz,gt_qw\n"); @@ -152,43 +182,11 @@ int main(int argc, char* argv[]) { graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model); graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model); - // We use the sensor specs to build the noise model for the IMU factor. - double accel_noise_sigma = 0.0003924; - double gyro_noise_sigma = 0.000205689024915; - double accel_bias_rw_sigma = 0.004905; - double gyro_bias_rw_sigma = 0.000001454441043; - Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); - Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); - Matrix33 integration_error_cov = - I_3x3 * 1e-8; // error committed in integrating position from velocities - Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); - Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = - I_6x6 * 1e-5; // error in the bias used for preintegration + auto p = imuParams(); - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); - // PreintegrationBase params: - p->accelerometerCovariance = - measured_acc_cov; // acc white noise in continuous - p->integrationCovariance = - integration_error_cov; // integration uncertainty continuous - // should be using 2nd order integration - // PreintegratedRotation params: - p->gyroscopeCovariance = - measured_omega_cov; // gyro white noise in continuous - // PreintegrationCombinedMeasurements params: - p->biasAccCovariance = bias_acc_cov; // acc bias in continuous - p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + std::shared_ptr preintegrated = + std::make_shared(p, prior_imu_bias); - std::shared_ptr preintegrated = nullptr; - if (use_combined_imu) { - preintegrated = - std::make_shared(p, prior_imu_bias); - } else { - preintegrated = - std::make_shared(p, prior_imu_bias); - } assert(preintegrated); // Store previous state for imu integration and latest predicted outcome. @@ -233,27 +231,16 @@ int main(int argc, char* argv[]) { correction_count++; // Adding IMU factor and GPS factor and optimizing. - if (use_combined_imu) { - auto preint_imu_combined = - dynamic_cast( - *preintegrated); - CombinedImuFactor imu_factor( - X(correction_count - 1), V(correction_count - 1), - X(correction_count), V(correction_count), B(correction_count - 1), - B(correction_count), preint_imu_combined); - graph->add(imu_factor); - } else { - auto preint_imu = - dynamic_cast(*preintegrated); - ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), - X(correction_count), V(correction_count), - B(correction_count - 1), preint_imu); - graph->add(imu_factor); - imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor( - B(correction_count - 1), B(correction_count), zero_bias, - bias_noise_model)); - } + auto preint_imu = + dynamic_cast(*preintegrated); + ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), + B(correction_count - 1), preint_imu); + graph->add(imu_factor); + imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + graph->add(BetweenFactor( + B(correction_count - 1), B(correction_count), zero_bias, + bias_noise_model)); auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); GPSFactor gps_factor(X(correction_count), @@ -270,18 +257,21 @@ int main(int argc, char* argv[]) { initial_values.insert(B(correction_count), prev_bias); Values result; -#ifdef USE_LM - LevenbergMarquardtOptimizer optimizer(*graph, initial_values); - result = optimizer.optimize(); -#else - isam2.update(*graph, initial_values); - isam2.update(); - result = isam2.calculateEstimate(); - // reset the graph - graph->resize(0); - initial_values.clear(); -#endif + if (use_isam) { + isam2->update(*graph, initial_values); + isam2->update(); + result = isam2->calculateEstimate(); + + // reset the graph + graph->resize(0); + initial_values.clear(); + + } else { + LevenbergMarquardtOptimizer optimizer(*graph, initial_values); + result = optimizer.optimize(); + } + // Overwrite the beginning of the preintegration for the next step. prev_state = NavState(result.at(X(correction_count)), result.at(V(correction_count)));