Reformatted and refactored
Frank dellaert
parent
6a23c476a1
commit
ff5a14831b
|
|
@ -12,7 +12,7 @@
|
|||
/**
|
||||
* @file FisheyeExample.cpp
|
||||
* @brief A visualSLAM example for the structure-from-motion problem on a
|
||||
* simulated dataset. This version uses a fisheye camera model and a GaussNewton
|
||||
* simulated dataset. This version uses a fisheye camera model and a GaussNewton
|
||||
* solver to solve the graph in one batch
|
||||
* @author ghaggin
|
||||
* @Date Apr 9,2020
|
||||
|
|
@ -55,75 +55,76 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
using symbol_shorthand::L; // for landmarks
|
||||
using symbol_shorthand::X; // for poses
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
// Define the camera calibration parameters
|
||||
// Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||
boost::shared_ptr<Cal3Fisheye> K(new Cal3Fisheye(
|
||||
278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625));
|
||||
int main(int argc, char *argv[]) {
|
||||
// Define the camera calibration parameters
|
||||
auto K = boost::make_shared<Cal3Fisheye>(
|
||||
278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035,
|
||||
0.020727425669427896, -0.012786476702685545, 0.0025242267320687625);
|
||||
|
||||
// Define the camera observation noise model, 1 pixel stddev
|
||||
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
// Define the camera observation noise model, 1 pixel stddev
|
||||
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
// Create the set of ground-truth landmarks
|
||||
const vector<Point3> points = createPoints();
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
vector<Pose3> poses = createPoses();
|
||||
// Create the set of ground-truth poses
|
||||
const vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create a Factor Graph and Values to hold the new data
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
// Create a Factor Graph and Values to hold the new data
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
|
||||
// Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
|
||||
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0], kPosePrior);
|
||||
// Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
|
||||
auto posePrior = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], posePrior);
|
||||
|
||||
// Add a prior on landmark l0
|
||||
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0], kPointPrior);
|
||||
// Add a prior on landmark l0
|
||||
auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.emplace_shared<PriorFactor<Point3>>(L(0), points[0], pointPrior);
|
||||
|
||||
// Add initial guesses to all observed landmarks
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
|
||||
// Add initial guesses to all observed landmarks
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
static const Point3 kDeltaPoint(-0.25, 0.20, 0.15);
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert<Point3>(L(j), points[j] + kDeltaPoint);
|
||||
|
||||
// Loop over the poses, adding the observations to the graph
|
||||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
{
|
||||
// Add factors for each landmark observation
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
{
|
||||
PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
|
||||
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
|
||||
}
|
||||
|
||||
// Add an initial guess for the current pose
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
|
||||
// Loop over the poses, adding the observations to the graph
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
// Add factors for each landmark observation
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
|
||||
measurement, measurementNoise, X(i), L(j), K);
|
||||
}
|
||||
|
||||
GaussNewtonParams params;
|
||||
params.setVerbosity("TERMINATION");
|
||||
params.maxIterations = 10000;
|
||||
// Add an initial guess for the current pose
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
|
||||
Point3(0.05, -0.10, 0.20));
|
||||
initialEstimate.insert(X(i), poses[i] * kDeltaPose);
|
||||
}
|
||||
|
||||
std::cout << "Optimizing the factor graph" << std::endl;
|
||||
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
|
||||
Values result = optimizer.optimize();
|
||||
std::cout << "Optimization complete" << std::endl;
|
||||
GaussNewtonParams params;
|
||||
params.setVerbosity("TERMINATION");
|
||||
params.maxIterations = 10000;
|
||||
|
||||
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
|
||||
std::cout << "final error=" << graph.error(result) << std::endl;
|
||||
std::cout << "Optimizing the factor graph" << std::endl;
|
||||
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
|
||||
Values result = optimizer.optimize();
|
||||
std::cout << "Optimization complete" << std::endl;
|
||||
|
||||
std::ofstream os("examples/vio_batch.dot");
|
||||
graph.saveGraph(os, result);
|
||||
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
|
||||
std::cout << "final error=" << graph.error(result) << std::endl;
|
||||
|
||||
return 0;
|
||||
std::ofstream os("examples/vio_batch.dot");
|
||||
graph.saveGraph(os, result);
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -11,12 +11,14 @@
|
|||
|
||||
/**
|
||||
* @file imuFactorsExample
|
||||
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
|
||||
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
|
||||
* navigation code.
|
||||
* @author Garrett (ghemann@gmail.com), Luca Carlone
|
||||
*/
|
||||
|
||||
/**
|
||||
* 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
|
||||
* - 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:
|
||||
|
|
@ -26,8 +28,8 @@
|
|||
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
|
||||
* A row starting with "1" is a gps correction formatted with
|
||||
* 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.
|
||||
* 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:
|
||||
|
|
@ -36,14 +38,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/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
|
@ -51,26 +54,21 @@
|
|||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
|
||||
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)
|
||||
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
|
||||
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
|
||||
|
||||
const string output_filename = "imuFactorExampleResults.csv";
|
||||
const string use_combined_imu_flag = "-c";
|
||||
static const char output_filename[] = "imuFactorExampleResults.csv";
|
||||
static const char use_combined_imu_flag[3] = "-c";
|
||||
|
||||
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
|
||||
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
|
||||
PreintegrationType *imu_preintegrated_;
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
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
|
||||
} 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");
|
||||
|
|
@ -80,15 +78,17 @@ int main(int argc, char* argv[])
|
|||
}
|
||||
} else {
|
||||
data_filename = argv[1];
|
||||
if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match
|
||||
if (strcmp(argv[2], use_combined_imu_flag) == 0) {
|
||||
printf("using CombinedImuFactor\n");
|
||||
use_combined_imu = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Set up output file for plotting errors
|
||||
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");
|
||||
FILE* fp_out = fopen(output_filename, "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");
|
||||
|
||||
// Begin parsing the CSV file. Input the first line for initialization.
|
||||
// From there, we'll iterate through the file and we'll preintegrate the IMU
|
||||
|
|
@ -97,9 +97,9 @@ int main(int argc, char* argv[])
|
|||
string value;
|
||||
|
||||
// Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
|
||||
Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
|
||||
getline(file, value, ','); // i
|
||||
for (int i=0; i<9; i++) {
|
||||
Vector10 initial_state;
|
||||
getline(file, value, ','); // i
|
||||
for (int i = 0; i < 9; i++) {
|
||||
getline(file, value, ',');
|
||||
initial_state(i) = atof(value.c_str());
|
||||
}
|
||||
|
|
@ -107,13 +107,14 @@ int main(int argc, char* argv[])
|
|||
initial_state(9) = atof(value.c_str());
|
||||
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
|
||||
|
||||
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
|
||||
// Assemble initial quaternion through GTSAM constructor
|
||||
// ::quaternion(w,x,y,z);
|
||||
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
|
||||
initial_state(4), initial_state(5));
|
||||
Point3 prior_point(initial_state.head<3>());
|
||||
Pose3 prior_pose(prior_rotation, prior_point);
|
||||
Vector3 prior_velocity(initial_state.tail<3>());
|
||||
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
|
||||
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
|
||||
|
||||
Values initial_values;
|
||||
int correction_count = 0;
|
||||
|
|
@ -121,57 +122,64 @@ int main(int argc, char* argv[])
|
|||
initial_values.insert(V(correction_count), prior_velocity);
|
||||
initial_values.insert(B(correction_count), prior_imu_bias);
|
||||
|
||||
// Assemble prior noise model and add it the graph.
|
||||
noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
|
||||
noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
|
||||
noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
|
||||
// Assemble prior noise model and add it the graph.`
|
||||
auto pose_noise_model = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
|
||||
.finished()); // rad,rad,rad,m, m, m
|
||||
auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s
|
||||
auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);
|
||||
|
||||
// Add all prior factors (pose, velocity, bias) to the graph.
|
||||
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
|
||||
NonlinearFactorGraph* graph = new NonlinearFactorGraph();
|
||||
graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
|
||||
graph->addPrior(V(correction_count), prior_velocity,velocity_noise_model);
|
||||
graph->addPrior(B(correction_count), prior_imu_bias,bias_noise_model);
|
||||
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 = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
|
||||
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
|
||||
Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
|
||||
Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
|
||||
Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
|
||||
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
|
||||
|
||||
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
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
|
||||
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
|
||||
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->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> imu_preintegrated_ = nullptr;
|
||||
std::shared_ptr<PreintegrationType> preintegrated = nullptr;
|
||||
if (use_combined_imu) {
|
||||
imu_preintegrated_ =
|
||||
preintegrated =
|
||||
std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
|
||||
} else {
|
||||
imu_preintegrated_ =
|
||||
preintegrated =
|
||||
std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
|
||||
}
|
||||
assert(imu_preintegrated_);
|
||||
assert(preintegrated);
|
||||
|
||||
// Store previous state for the imu integration and the latest predicted outcome.
|
||||
// Store previous state for imu integration and latest predicted outcome.
|
||||
NavState prev_state(prior_pose, prior_velocity);
|
||||
NavState prop_state = prev_state;
|
||||
imuBias::ConstantBias prev_bias = prior_imu_bias;
|
||||
|
||||
// Keep track of the total error over the entire run for a simple performance metric.
|
||||
// Keep track of total error over the entire run as simple performance metric.
|
||||
double current_position_error = 0.0, current_orientation_error = 0.0;
|
||||
|
||||
double output_time = 0.0;
|
||||
|
|
@ -180,14 +188,13 @@ int main(int argc, char* argv[])
|
|||
|
||||
// All priors have been set up, now iterate through the data file.
|
||||
while (file.good()) {
|
||||
|
||||
// Parse out first value
|
||||
getline(file, value, ',');
|
||||
int type = atoi(value.c_str());
|
||||
|
||||
if (type == 0) { // IMU measurement
|
||||
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
|
||||
for (int i=0; i<5; ++i) {
|
||||
if (type == 0) { // IMU measurement
|
||||
Vector6 imu;
|
||||
for (int i = 0; i < 5; ++i) {
|
||||
getline(file, value, ',');
|
||||
imu(i) = atof(value.c_str());
|
||||
}
|
||||
|
|
@ -195,11 +202,11 @@ int main(int argc, char* argv[])
|
|||
imu(5) = atof(value.c_str());
|
||||
|
||||
// Adding the IMU preintegration.
|
||||
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
|
||||
preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
|
||||
|
||||
} else if (type == 1) { // GPS measurement
|
||||
Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
|
||||
for (int i=0; i<6; ++i) {
|
||||
} else if (type == 1) { // GPS measurement
|
||||
Vector7 gps;
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
getline(file, value, ',');
|
||||
gps(i) = atof(value.c_str());
|
||||
}
|
||||
|
|
@ -210,39 +217,37 @@ int main(int argc, char* argv[])
|
|||
|
||||
// Adding IMU factor and GPS factor and optimizing.
|
||||
if (use_combined_imu) {
|
||||
const PreintegratedCombinedMeasurements& preint_imu_combined =
|
||||
auto preint_imu_combined =
|
||||
dynamic_cast<const PreintegratedCombinedMeasurements&>(
|
||||
*imu_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);
|
||||
*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 {
|
||||
const PreintegratedImuMeasurements& preint_imu =
|
||||
dynamic_cast<const PreintegratedImuMeasurements&>(
|
||||
*imu_preintegrated_);
|
||||
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
|
||||
X(correction_count ), V(correction_count ),
|
||||
B(correction_count-1),
|
||||
preint_imu);
|
||||
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));
|
||||
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);
|
||||
auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
GPSFactor gps_factor(X(correction_count),
|
||||
Point3(gps(0), // N,
|
||||
gps(1), // E,
|
||||
gps(2)), // D,
|
||||
Point3(gps(0), // N,
|
||||
gps(1), // E,
|
||||
gps(2)), // D,
|
||||
correction_noise);
|
||||
graph->add(gps_factor);
|
||||
|
||||
// Now optimize and compare results.
|
||||
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
|
||||
prop_state = preintegrated->predict(prev_state, prev_bias);
|
||||
initial_values.insert(X(correction_count), prop_state.pose());
|
||||
initial_values.insert(V(correction_count), prop_state.v());
|
||||
initial_values.insert(B(correction_count), prev_bias);
|
||||
|
|
@ -256,7 +261,7 @@ int main(int argc, char* argv[])
|
|||
prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
|
||||
|
||||
// Reset the preintegration object.
|
||||
imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
|
||||
preintegrated->resetIntegrationAndSetBias(prev_bias);
|
||||
|
||||
// Print out the position and orientation error for comparison.
|
||||
Vector3 gtsam_position = prev_state.pose().translation();
|
||||
|
|
@ -267,19 +272,19 @@ int main(int argc, char* argv[])
|
|||
Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
|
||||
Quaternion quat_error = gtsam_quat * gps_quat.inverse();
|
||||
quat_error.normalize();
|
||||
Vector3 euler_angle_error(quat_error.x()*2,
|
||||
quat_error.y()*2,
|
||||
quat_error.z()*2);
|
||||
Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
|
||||
quat_error.z() * 2);
|
||||
current_orientation_error = euler_angle_error.norm();
|
||||
|
||||
// display statistics
|
||||
cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
|
||||
cout << "Position error:" << current_position_error << "\t "
|
||||
<< "Angular error:" << current_orientation_error << "\n";
|
||||
|
||||
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
|
||||
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
|
||||
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
|
||||
gps(0), gps(1), gps(2),
|
||||
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
|
||||
output_time, gtsam_position(0), gtsam_position(1),
|
||||
gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
|
||||
gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
|
||||
gps_quat.y(), gps_quat.z(), gps_quat.w());
|
||||
|
||||
output_time += 1.0;
|
||||
|
||||
|
|
@ -289,6 +294,7 @@ int main(int argc, char* argv[])
|
|||
}
|
||||
}
|
||||
fclose(fp_out);
|
||||
cout << "Complete, results written to " << output_filename << "\n\n";;
|
||||
cout << "Complete, results written to " << output_filename << "\n\n";
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue