From 5e735e10f7c2b731b831c5f1ffec7631f0fc90e6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 16 Sep 2021 13:03:11 -0400 Subject: [PATCH] minor improvements to CombinedImuFactor --- examples/CombinedImuFactorsExample.cpp | 15 ++++++++------- gtsam/navigation/CombinedImuFactor.cpp | 5 +++++ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 9211a4d5f..49cdb6835 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -60,13 +60,14 @@ 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"); + desc.add_options()("help,h", "produce help message") // help message + ("data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data") // path to the data file + ("output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use") // filename to save results to + ("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); // flag for ISAM optimizer po::variables_map vm; po::store(po::parse_command_line(argc, argv, desc), vm); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ca1c5b93a..12e8ccea8 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -93,6 +93,11 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + if (dt <= 0) { + throw std::runtime_error( + "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0"); + } + // Update preintegrated measurements. Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C;