reworked basic ImuFactorsExample
Varun Agrawal
parent
311491abfc
commit
4949f8bb9d
|
|
@ -40,15 +40,15 @@
|
|||
*/
|
||||
|
||||
// GTSAM related includes.
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/GPSFactor.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
|
|
@ -64,45 +64,75 @@ using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
|
|||
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
|
||||
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
|
||||
|
||||
static const char output_filename[] = "imuFactorExampleResults.csv";
|
||||
static const char use_combined_imu_flag[3] = "-c";
|
||||
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
||||
// We use the sensor specs to build the noise model for the IMU factor.
|
||||
double accel_noise_sigma = 0.0003924;
|
||||
double gyro_noise_sigma = 0.000205689024915;
|
||||
double accel_bias_rw_sigma = 0.004905;
|
||||
double gyro_bias_rw_sigma = 0.000001454441043;
|
||||
Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
|
||||
Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
|
||||
Matrix33 integration_error_cov =
|
||||
I_3x3 * 1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
|
||||
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
|
||||
Matrix66 bias_acc_omega_int =
|
||||
I_6x6 * 1e-5; // error in the bias used for preintegration
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
// PreintegrationBase params:
|
||||
p->accelerometerCovariance =
|
||||
measured_acc_cov; // acc white noise in continuous
|
||||
p->integrationCovariance =
|
||||
integration_error_cov; // integration uncertainty continuous
|
||||
// should be using 2nd order integration
|
||||
// PreintegratedRotation params:
|
||||
p->gyroscopeCovariance =
|
||||
measured_omega_cov; // gyro white noise in continuous
|
||||
// PreintegrationCombinedMeasurements params:
|
||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
string data_filename;
|
||||
bool use_combined_imu = false;
|
||||
string data_filename, output_filename;
|
||||
|
||||
#ifndef USE_LM
|
||||
printf("Using ISAM2\n");
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.01;
|
||||
parameters.relinearizeSkip = 1;
|
||||
ISAM2 isam2(parameters);
|
||||
#else
|
||||
printf("Using Levenberg Marquardt Optimizer\n");
|
||||
#endif
|
||||
bool use_isam = 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) == 0) {
|
||||
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 {
|
||||
if (argc == 4) {
|
||||
data_filename = argv[1];
|
||||
if (strcmp(argv[2], use_combined_imu_flag) == 0) {
|
||||
printf("using CombinedImuFactor\n");
|
||||
use_combined_imu = true;
|
||||
}
|
||||
output_filename = argv[2];
|
||||
use_isam = atoi(argv[3]);
|
||||
|
||||
} else if (argc == 3) {
|
||||
data_filename = argv[1];
|
||||
output_filename = argv[2];
|
||||
} else if (argc == 2) {
|
||||
data_filename = argv[1];
|
||||
output_filename = "imuFactorExampleResults.csv";
|
||||
} else {
|
||||
printf("using default files\n");
|
||||
data_filename = findExampleDataFile("imuAndGPSdata.csv");
|
||||
output_filename = "imuFactorExampleResults.csv";
|
||||
}
|
||||
|
||||
ISAM2* isam2;
|
||||
if (use_isam) {
|
||||
printf("Using ISAM2\n");
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.01;
|
||||
parameters.relinearizeSkip = 1;
|
||||
isam2 = new ISAM2(parameters);
|
||||
|
||||
} else {
|
||||
printf("Using Levenberg Marquardt Optimizer\n");
|
||||
}
|
||||
|
||||
// Set up output file for plotting errors
|
||||
FILE* fp_out = fopen(output_filename, "w+");
|
||||
FILE* fp_out = fopen(output_filename.c_str(), "w+");
|
||||
fprintf(fp_out,
|
||||
"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
|
||||
"gt_qy,gt_qz,gt_qw\n");
|
||||
|
|
@ -152,43 +182,11 @@ int main(int argc, char* argv[]) {
|
|||
graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
|
||||
graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);
|
||||
|
||||
// We use the sensor specs to build the noise model for the IMU factor.
|
||||
double accel_noise_sigma = 0.0003924;
|
||||
double gyro_noise_sigma = 0.000205689024915;
|
||||
double accel_bias_rw_sigma = 0.004905;
|
||||
double gyro_bias_rw_sigma = 0.000001454441043;
|
||||
Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
|
||||
Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
|
||||
Matrix33 integration_error_cov =
|
||||
I_3x3 * 1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
|
||||
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
|
||||
Matrix66 bias_acc_omega_int =
|
||||
I_6x6 * 1e-5; // error in the bias used for preintegration
|
||||
auto p = imuParams();
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
// PreintegrationBase params:
|
||||
p->accelerometerCovariance =
|
||||
measured_acc_cov; // acc white noise in continuous
|
||||
p->integrationCovariance =
|
||||
integration_error_cov; // integration uncertainty continuous
|
||||
// should be using 2nd order integration
|
||||
// PreintegratedRotation params:
|
||||
p->gyroscopeCovariance =
|
||||
measured_omega_cov; // gyro white noise in continuous
|
||||
// PreintegrationCombinedMeasurements params:
|
||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
std::shared_ptr<PreintegrationType> preintegrated =
|
||||
std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
|
||||
|
||||
std::shared_ptr<PreintegrationType> preintegrated = nullptr;
|
||||
if (use_combined_imu) {
|
||||
preintegrated =
|
||||
std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
|
||||
} else {
|
||||
preintegrated =
|
||||
std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
|
||||
}
|
||||
assert(preintegrated);
|
||||
|
||||
// Store previous state for imu integration and latest predicted outcome.
|
||||
|
|
@ -233,27 +231,16 @@ int main(int argc, char* argv[]) {
|
|||
correction_count++;
|
||||
|
||||
// Adding IMU factor and GPS factor and optimizing.
|
||||
if (use_combined_imu) {
|
||||
auto preint_imu_combined =
|
||||
dynamic_cast<const PreintegratedCombinedMeasurements&>(
|
||||
*preintegrated);
|
||||
CombinedImuFactor imu_factor(
|
||||
X(correction_count - 1), V(correction_count - 1),
|
||||
X(correction_count), V(correction_count), B(correction_count - 1),
|
||||
B(correction_count), preint_imu_combined);
|
||||
graph->add(imu_factor);
|
||||
} else {
|
||||
auto preint_imu =
|
||||
dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
|
||||
ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
|
||||
X(correction_count), V(correction_count),
|
||||
B(correction_count - 1), preint_imu);
|
||||
graph->add(imu_factor);
|
||||
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));
|
||||
}
|
||||
auto preint_imu =
|
||||
dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
|
||||
ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
|
||||
X(correction_count), V(correction_count),
|
||||
B(correction_count - 1), preint_imu);
|
||||
graph->add(imu_factor);
|
||||
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));
|
||||
|
||||
auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
GPSFactor gps_factor(X(correction_count),
|
||||
|
|
@ -270,18 +257,21 @@ int main(int argc, char* argv[]) {
|
|||
initial_values.insert(B(correction_count), prev_bias);
|
||||
|
||||
Values result;
|
||||
#ifdef USE_LM
|
||||
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
|
||||
result = optimizer.optimize();
|
||||
#else
|
||||
isam2.update(*graph, initial_values);
|
||||
isam2.update();
|
||||
result = isam2.calculateEstimate();
|
||||
|
||||
// reset the graph
|
||||
graph->resize(0);
|
||||
initial_values.clear();
|
||||
#endif
|
||||
if (use_isam) {
|
||||
isam2->update(*graph, initial_values);
|
||||
isam2->update();
|
||||
result = isam2->calculateEstimate();
|
||||
|
||||
// reset the graph
|
||||
graph->resize(0);
|
||||
initial_values.clear();
|
||||
|
||||
} else {
|
||||
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
|
||||
result = optimizer.optimize();
|
||||
}
|
||||
|
||||
// Overwrite the beginning of the preintegration for the next step.
|
||||
prev_state = NavState(result.at<Pose3>(X(correction_count)),
|
||||
result.at<Vector3>(V(correction_count)));
|
||||
|
|
|
|||
Loading…
Reference in New Issue