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;