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

Merge pull request #195 from ToniRV/fix/remove_preprocessor_directives_imu 

Remove ugly preprocessor directives
release/4.3a0
Frank Dellaert 2019-12-22 19:40:31 -05:00 committed by GitHub
parent 97297d874d a607bf6526
commit f2aaa2a770
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 55 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
examples/ImuFactorsExample.cpp
View File

@ -17,8 +17,8 @@
/** /**
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
* - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting) * - imuFactor is used by default. You can test combinedImuFactor by
* the line #define USE_COMBINED (few lines below) * 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: * - 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 * A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD * N, E, D, qx, qY, qZ, qW, velN, velE, velD
@ -28,6 +28,11 @@
* N, E, D, qX, qY, qZ, qW * N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the rotation. The * 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. * 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. // GTSAM related includes.
@ -40,12 +45,10 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <cstring>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
// #define USE_COMBINED
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
@ -54,6 +57,7 @@ using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
const string output_filename = "imuFactorExampleResults.csv"; const string output_filename = "imuFactorExampleResults.csv";
const string use_combined_imu_flag = "-c";
// This will either be PreintegratedImuMeasurements (for ImuFactor) or // This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor). // PreintegratedCombinedMeasurements (for CombinedImuFactor).
@ -62,11 +66,25 @@ PreintegrationType *imu_preintegrated_;
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
string data_filename; string data_filename;
bool use_combined_imu = false;
if (argc < 2) { if (argc < 2) {
printf("using default CSV file\n"); printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv"); 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 { } else {
data_filename = argv[1]; 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 // Set up output file for plotting errors
@ -139,11 +157,15 @@ int main(int argc, char* argv[])
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int; p->biasAccOmegaInt = bias_acc_omega_int;
#ifdef USE_COMBINED std::shared_ptr<PreintegrationType> imu_preintegrated_ = nullptr;
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias); if (use_combined_imu) {
#else imu_preintegrated_ =
imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias); std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
#endif } else {
imu_preintegrated_ =
std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
}
assert(imu_preintegrated_);
// Store previous state for the imu integration and the latest predicted outcome. // Store previous state for the imu integration and the latest predicted outcome.
NavState prev_state(prior_pose, prior_velocity); NavState prev_state(prior_pose, prior_velocity);
@ -188,25 +210,29 @@ int main(int argc, char* argv[])
correction_count++; correction_count++;
// Adding IMU factor and GPS factor and optimizing. // Adding IMU factor and GPS factor and optimizing.
#ifdef USE_COMBINED if (use_combined_imu) {
PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_); const PreintegratedCombinedMeasurements& preint_imu_combined =
CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1), dynamic_cast<const PreintegratedCombinedMeasurements&>(
X(correction_count ), V(correction_count ), *imu_preintegrated_);
B(correction_count-1), B(correction_count ), CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
*preint_imu_combined); X(correction_count ), V(correction_count ),
graph->add(imu_factor); B(correction_count-1), B(correction_count ),
#else preint_imu_combined);
PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_); graph->add(imu_factor);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1), } else {
X(correction_count ), V(correction_count ), const PreintegratedImuMeasurements& preint_imu =
B(correction_count-1), dynamic_cast<const PreintegratedImuMeasurements&>(
*preint_imu); *imu_preintegrated_);
graph->add(imu_factor); ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); X(correction_count ), V(correction_count ),
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1), B(correction_count-1),
B(correction_count ), preint_imu);
zero_bias, bias_noise_model)); graph->add(imu_factor);
#endif imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
}
noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0); noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
GPSFactor gps_factor(X(correction_count), GPSFactor gps_factor(X(correction_count),