From a607bf65264255faffbfe06a02ebcf1e82f07f84 Mon Sep 17 00:00:00 2001
From: Toni Rosinol <tonirosinol@hotmail.com>
Date: Fri, 20 Dec 2019 02:01:37 -0500
Subject: [PATCH] Add flag for CombinedImu

---
 examples/ImuFactorsExample.cpp | 30 +++++++++++++++++++++++++-----
 1 file changed, 25 insertions(+), 5 deletions(-)

diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index 1e8704d1b..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,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,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
@@ -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_);