diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index f04b73f6b..e038f5117 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -17,8 +17,8 @@ /** * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS - * - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting) - * the line #define USE_COMBINED (few lines below) + * - imuFactor is used by default. You can test combinedImuFactor by + * appending a `-c` flag at the end (see below for example command). * - we read IMU and GPS data from a CSV file, with the following format: * A row starting with "i" is the first initial position formatted with * N, E, D, qx, qY, qZ, qW, velN, velE, velD @@ -28,6 +28,11 @@ * N, E, D, qX, qY, qZ, qW * Note that for GPS correction, we're only using the position not the rotation. The * rotation is provided in the file for ground truth comparison. + * + * Usage: ./ImuFactorsExample [data_csv_path] [-c] + * optional arguments: + * data_csv_path path to the CSV file with the IMU data. + * -c use CombinedImuFactor */ // GTSAM related includes. @@ -40,12 +45,10 @@ #include #include #include +#include #include #include -// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor. -// #define USE_COMBINED - using namespace gtsam; using namespace std; @@ -54,6 +57,7 @@ using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) const string output_filename = "imuFactorExampleResults.csv"; +const string use_combined_imu_flag = "-c"; // This will either be PreintegratedImuMeasurements (for ImuFactor) or // PreintegratedCombinedMeasurements (for CombinedImuFactor). @@ -62,11 +66,25 @@ PreintegrationType *imu_preintegrated_; int main(int argc, char* argv[]) { string data_filename; + bool use_combined_imu = 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.c_str()) == 0) { // strcmp returns 0 for a match + 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 { data_filename = argv[1]; + if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match + printf("using CombinedImuFactor\n"); + use_combined_imu = true; + } } // Set up output file for plotting errors @@ -139,11 +157,15 @@ int main(int argc, char* argv[]) p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous p->biasAccOmegaInt = bias_acc_omega_int; -#ifdef USE_COMBINED - imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias); -#else - imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias); -#endif + std::shared_ptr imu_preintegrated_ = nullptr; + if (use_combined_imu) { + imu_preintegrated_ = + std::make_shared(p, prior_imu_bias); + } else { + imu_preintegrated_ = + std::make_shared(p, prior_imu_bias); + } + assert(imu_preintegrated_); // Store previous state for the imu integration and the latest predicted outcome. NavState prev_state(prior_pose, prior_velocity); @@ -188,25 +210,29 @@ int main(int argc, char* argv[]) correction_count++; // Adding IMU factor and GPS factor and optimizing. -#ifdef USE_COMBINED - PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast(imu_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 - PreintegratedImuMeasurements *preint_imu = dynamic_cast(imu_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)); -#endif + if (use_combined_imu) { + const PreintegratedCombinedMeasurements& preint_imu_combined = + dynamic_cast( + *imu_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 { + const PreintegratedImuMeasurements& preint_imu = + dynamic_cast( + *imu_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)); + } noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0); GPSFactor gps_factor(X(correction_count),