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

Remove ugly preprocessor directives, still hardcoded though

release/4.3a0
Toni 2019-12-18 21:48:38 -05:00
parent 0cb32d7c24
commit e0fb001702
1 changed files with 33 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
examples/ImuFactorsExample.cpp
View File

@ -43,9 +43,6 @@
#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;
@ -139,11 +136,16 @@ 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 static constexpr bool use_combined_preint = false;
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias); std::shared_ptr<PreintegrationType> imu_preintegrated_ = nullptr;
#else if (use_combined_preint) {
imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias); imu_preintegrated_ =
#endif std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
} 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 +190,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_preint) {
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),