Add flag for CombinedImu

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)
+ *  - imuFactor is used by default. You can test combinedImuFactor by
- *  the line #define USE_COMBINED (few lines below)
+ *  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,6 +45,7 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <cstring>
 #include <fstream>
 #include <iostream>
 
@@ -51,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).
@@ -59,12 +66,26 @@ 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
   FILE* fp_out = fopen(output_filename.c_str(), "w+");
@@ -136,9 +157,8 @@ int main(int argc, char* argv[])
   p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
   p->biasAccOmegaInt = bias_acc_omega_int;
 
-  static constexpr bool use_combined_preint = false;
   std::shared_ptr<PreintegrationType> imu_preintegrated_ = nullptr;
-  if (use_combined_preint) {
+  if (use_combined_imu) {
     imu_preintegrated_ =
         std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
   } else {
@@ -190,7 +210,7 @@ int main(int argc, char* argv[])
       correction_count++;
 
       // Adding IMU factor and GPS factor and optimizing.
-      if (use_combined_preint) {
+      if (use_combined_imu) {
         const PreintegratedCombinedMeasurements& preint_imu_combined =
             dynamic_cast<const PreintegratedCombinedMeasurements&>(
               *imu_preintegrated_);