diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index f04f2ddd4..2e5c46025 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -29,12 +29,11 @@ * 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: ./CombinedImuFactorsExample [data_csv_path] [-c] - * optional arguments: - * data_csv_path path to the CSV file with the IMU data. - * -c use CombinedImuFactor + * See usage: ./CombinedImuFactorsExample --help */ +#include + // GTSAM related includes. #include #include @@ -56,6 +55,29 @@ 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) +namespace po = boost::program_options; + +po::variables_map parseOptions(int argc, char* argv[]) { + po::options_description desc; + desc.add_options()("help,h", "produce help message")( + "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data")( + "output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use")("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (vm.count("help")) { + cout << desc << "\n"; + exit(1); + } + + return vm; +} + Vector10 readInitialState(ifstream& file) { string value; // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) @@ -106,17 +128,10 @@ boost::shared_ptr imuParams() { int main(int argc, char* argv[]) { string data_filename, output_filename; - 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 CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - output_filename = "imuFactorExampleResults.csv"; - } + po::variables_map var_map = parseOptions(argc, argv); + + data_filename = findExampleDataFile(var_map["data_csv_path"].as()); + output_filename = var_map["output_filename"].as(); // Set up output file for plotting errors FILE* fp_out = fopen(output_filename.c_str(), "w+"); @@ -265,7 +280,8 @@ int main(int argc, char* argv[]) { // display statistics cout << "Position error:" << current_position_error << "\t " - << "Angular error:" << current_orientation_error << "\n" << endl; + << "Angular error:" << current_orientation_error << "\n" + << endl; fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", output_time, result_position(0), result_position(1), diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index fe1a99ed0..20dc2c261 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -31,14 +31,11 @@ * 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 - * Note: Define USE_LM to use Levenberg Marquardt Optimizer - * By default ISAM2 is used + * See usage: ./ImuFactorsExample --help */ +#include + // GTSAM related includes. #include #include @@ -54,9 +51,6 @@ #include #include -// Uncomment the following to use Levenberg Marquardt Optimizer -// #define USE_LM - using namespace gtsam; using namespace std; @@ -64,6 +58,29 @@ 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) +namespace po = boost::program_options; + +po::variables_map parseOptions(int argc, char* argv[]) { + po::options_description desc; + desc.add_options()("help,h", "produce help message")( + "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data")( + "output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use")("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (vm.count("help")) { + cout << desc << "\n"; + exit(1); + } + + return vm; +} + boost::shared_ptr imuParams() { // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; @@ -102,22 +119,11 @@ int main(int argc, char* argv[]) { bool use_isam = false; - if (argc == 4) { - data_filename = argv[1]; - output_filename = argv[2]; - use_isam = atoi(argv[3]); + po::variables_map var_map = parseOptions(argc, argv); - } 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"; - } + data_filename = findExampleDataFile(var_map["data_csv_path"].as()); + output_filename = var_map["output_filename"].as(); + use_isam = var_map["use_isam"].as(); ISAM2* isam2; if (use_isam) {