Add flag for CombinedImu
parent
e0fb001702
commit
a607bf6526
|
@ -17,8 +17,8 @@
|
|||
|
||||
/**
|
||||
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
|
||||
* - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
|
||||
* the line #define USE_COMBINED (few lines below)
|
||||
* - imuFactor is used by default. You can test combinedImuFactor by
|
||||
* 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:
|
||||
* A row starting with "i" is the first initial position formatted with
|
||||
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
|
||||
|
@ -28,6 +28,11 @@
|
|||
* N, E, D, qX, qY, qZ, qW
|
||||
* 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.
|
||||
*
|
||||
* 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.
|
||||
|
@ -40,6 +45,7 @@
|
|||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
|
@ -51,6 +57,7 @@ using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
|
|||
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
|
||||
|
||||
const string output_filename = "imuFactorExampleResults.csv";
|
||||
const string use_combined_imu_flag = "-c";
|
||||
|
||||
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
|
||||
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
|
||||
|
@ -59,11 +66,25 @@ PreintegrationType *imu_preintegrated_;
|
|||
int main(int argc, char* argv[])
|
||||
{
|
||||
string data_filename;
|
||||
bool use_combined_imu = false;
|
||||
if (argc < 2) {
|
||||
printf("using default CSV file\n");
|
||||
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 {
|
||||
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
|
||||
|
@ -136,9 +157,8 @@ int main(int argc, char* argv[])
|
|||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
|
||||
static constexpr bool use_combined_preint = false;
|
||||
std::shared_ptr<PreintegrationType> imu_preintegrated_ = nullptr;
|
||||
if (use_combined_preint) {
|
||||
if (use_combined_imu) {
|
||||
imu_preintegrated_ =
|
||||
std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
|
||||
} else {
|
||||
|
@ -190,7 +210,7 @@ int main(int argc, char* argv[])
|
|||
correction_count++;
|
||||
|
||||
// Adding IMU factor and GPS factor and optimizing.
|
||||
if (use_combined_preint) {
|
||||
if (use_combined_imu) {
|
||||
const PreintegratedCombinedMeasurements& preint_imu_combined =
|
||||
dynamic_cast<const PreintegratedCombinedMeasurements&>(
|
||||
*imu_preintegrated_);
|
||||
|
|
Loading…
Reference in New Issue