12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor improvements to CombinedImuFactor

release/4.3a0
Varun Agrawal 2021-09-16 13:03:11 -04:00
parent 7076244b60
commit 5e735e10f7
2 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/CombinedImuFactorsExample.cpp
View File

@ -60,13 +60,14 @@ namespace po = boost::program_options;
po::variables_map parseOptions(int argc, char* argv[]) { po::variables_map parseOptions(int argc, char* argv[]) {
po::options_description desc; po::options_description desc;
desc.add_options()("help,h", "produce help message")( desc.add_options()("help,h", "produce help message") // help message
"data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"), ("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data")( "path to the CSV file with the IMU data") // path to the data file
"output_filename", ("output_filename",
po::value<string>()->default_value("imuFactorExampleResults.csv"), po::value<string>()->default_value("imuFactorExampleResults.csv"),
"path to the result file to use")("use_isam", po::bool_switch(), "path to the result file to use") // filename to save results to
"use ISAM as the optimizer"); ("use_isam", po::bool_switch(),
"use ISAM as the optimizer"); // flag for ISAM optimizer
po::variables_map vm; po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm); po::store(po::parse_command_line(argc, argv, desc), vm);

5
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -93,6 +93,11 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement( void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
if (dt <= 0) {
throw std::runtime_error(
"PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
}
// Update preintegrated measurements. // Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C; Matrix93 B, C;