shorten parameter values
Nghia Ho
parent
224af650bb
commit
937cdcf4d9
|
|
@ -0,0 +1,236 @@
|
|||
/**
|
||||
* @file ISAM2_SmartFactorStereo_IMU.cpp
|
||||
* @brief test of iSAM2 with smart stereo factors and IMU preintegration,
|
||||
* originally used to debug valgrind invalid reads with Eigen
|
||||
* @author Nghia Ho
|
||||
*
|
||||
* Setup is a stationary stereo camera with an IMU attached.
|
||||
* The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt
|
||||
* It contains 5 frames of stereo matches and IMU data.
|
||||
*/
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::B;
|
||||
|
||||
struct IMUHelper {
|
||||
IMUHelper() {
|
||||
{
|
||||
auto gaussian = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
|
||||
.finished());
|
||||
|
||||
auto huber = noiseModel::Robust::Create(
|
||||
noiseModel::mEstimator::Huber::Create(1.345), gaussian);
|
||||
|
||||
biasNoiseModel = huber;
|
||||
}
|
||||
|
||||
{
|
||||
auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
|
||||
auto huber = noiseModel::Robust::Create(
|
||||
noiseModel::mEstimator::Huber::Create(1.345), gaussian);
|
||||
|
||||
velocityNoiseModel = huber;
|
||||
}
|
||||
|
||||
// expect IMU to be rotated in image space co-ords
|
||||
auto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>(
|
||||
Vector3(0.0, 9.8, 0.0));
|
||||
|
||||
p->accelerometerCovariance =
|
||||
I_3x3 * pow(0.0565, 2.0); // acc white noise in continuous
|
||||
p->integrationCovariance =
|
||||
I_3x3 * 1e-9; // integration uncertainty continuous
|
||||
p->gyroscopeCovariance =
|
||||
I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuous
|
||||
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
|
||||
p->biasOmegaCovariance =
|
||||
I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
|
||||
p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
|
||||
|
||||
// body to IMU rotation
|
||||
Rot3 iRb(
|
||||
0.036129, -0.998727, 0.035207,
|
||||
0.045417, -0.033553, -0.99840,4
|
||||
0.998315, 0.037670, 0.044147);
|
||||
|
||||
// body to IMU translation (meters)
|
||||
Point3 iTb(0.03, -0.025, -0.06);
|
||||
|
||||
// body in this example is the left camera
|
||||
p->body_P_sensor = Pose3(iRb, iTb);
|
||||
|
||||
Rot3 prior_rotation = Rot3(I_3x3);
|
||||
Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));
|
||||
|
||||
Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame
|
||||
Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
|
||||
|
||||
priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);
|
||||
|
||||
prevState = NavState(prior_pose, Vector3(0, 0, 0));
|
||||
propState = prevState;
|
||||
prevBias = priorImuBias;
|
||||
|
||||
preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
|
||||
}
|
||||
|
||||
imuBias::ConstantBias priorImuBias; // assume zero initial bias
|
||||
noiseModel::Robust::shared_ptr velocityNoiseModel;
|
||||
noiseModel::Robust::shared_ptr biasNoiseModel;
|
||||
NavState prevState;
|
||||
NavState propState;
|
||||
imuBias::ConstantBias prevBias;
|
||||
PreintegratedCombinedMeasurements* preintegrated;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
if (argc != 2) {
|
||||
cout << "./main [data.txt]\n";
|
||||
return 0;
|
||||
}
|
||||
|
||||
ifstream in(argv[1]);
|
||||
|
||||
if (!in) {
|
||||
cerr << "error opening: " << argv[1] << "\n";
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Camera parameters
|
||||
double fx = 822.37;
|
||||
double fy = 822.37;
|
||||
double cx = 538.73;
|
||||
double cy = 579.10;
|
||||
double baseline = 0.372; // meters
|
||||
|
||||
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
|
||||
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.1;
|
||||
ISAM2 isam(parameters);
|
||||
|
||||
// Create a factor graph
|
||||
std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
IMUHelper imu;
|
||||
|
||||
// Pose prior - at identity
|
||||
auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
|
||||
priorPoseNoise);
|
||||
initialEstimate.insert(X(0), Pose3::identity());
|
||||
|
||||
// Bias prior
|
||||
graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
|
||||
imu.biasNoiseModel));
|
||||
initialEstimate.insert(B(0), imu.priorImuBias);
|
||||
|
||||
// Velocity prior - assume stationary
|
||||
graph.add(
|
||||
PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
|
||||
initialEstimate.insert(V(0), Vector3(0, 0, 0));
|
||||
|
||||
int lastFrame = 1;
|
||||
int frame;
|
||||
|
||||
while (true) {
|
||||
char line[1024];
|
||||
|
||||
in.getline(line, sizeof(line));
|
||||
stringstream ss(line);
|
||||
char type;
|
||||
|
||||
ss >> type;
|
||||
ss >> frame;
|
||||
|
||||
if (frame != lastFrame || in.eof()) {
|
||||
cout << "Running iSAM for frame: " << lastFrame << "\n";
|
||||
|
||||
initialEstimate.insert(X(lastFrame), Pose3::identity());
|
||||
initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
|
||||
initialEstimate.insert(B(lastFrame), imu.prevBias);
|
||||
|
||||
CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),
|
||||
X(lastFrame), V(lastFrame), B(lastFrame - 1),
|
||||
B(lastFrame), *imu.preintegrated);
|
||||
|
||||
graph.add(imuFactor);
|
||||
|
||||
isam.update(graph, initialEstimate);
|
||||
|
||||
Values currentEstimate = isam.calculateEstimate();
|
||||
|
||||
imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);
|
||||
imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),
|
||||
currentEstimate.at<Vector3>(V(lastFrame)));
|
||||
imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));
|
||||
imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias);
|
||||
|
||||
graph.resize(0);
|
||||
initialEstimate.clear();
|
||||
|
||||
if (in.eof()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (type == 'i') { // Process IMU measurement
|
||||
double ax, ay, az;
|
||||
double gx, gy, gz;
|
||||
double dt = 1 / 800.0; // IMU at ~800Hz
|
||||
|
||||
ss >> ax;
|
||||
ss >> ay;
|
||||
ss >> az;
|
||||
|
||||
ss >> gx;
|
||||
ss >> gy;
|
||||
ss >> gz;
|
||||
|
||||
Vector3 acc(ax, ay, az);
|
||||
Vector3 gyr(gx, gy, gz);
|
||||
|
||||
imu.preintegrated->integrateMeasurement(acc, gyr, dt);
|
||||
} else if (type == 's') { // Process stereo measurement
|
||||
int landmark;
|
||||
double xl, xr, y;
|
||||
|
||||
ss >> landmark;
|
||||
ss >> xl;
|
||||
ss >> xr;
|
||||
ss >> y;
|
||||
|
||||
if (smartFactors.count(landmark) == 0) {
|
||||
auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
|
||||
|
||||
smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(
|
||||
new SmartStereoProjectionPoseFactor(gaussian, params));
|
||||
graph.push_back(smartFactors[landmark]);
|
||||
}
|
||||
|
||||
smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
|
||||
} else {
|
||||
throw runtime_error("unexpected data type: " + type);
|
||||
}
|
||||
|
||||
lastFrame = frame;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,222 +0,0 @@
|
|||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <sstream>
|
||||
#include <fstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::B;
|
||||
|
||||
struct IMUHelper
|
||||
{
|
||||
IMUHelper()
|
||||
{
|
||||
gtsam::Matrix33 measured_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(0.113, 2.0);
|
||||
gtsam::Matrix33 measured_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(4.0e-5, 2.0);
|
||||
gtsam::Matrix33 bias_acc_cov = gtsam::Matrix33::Identity(3,3) * pow( 0.00002, 2.0);
|
||||
gtsam::Matrix33 bias_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(0.001, 2.0);
|
||||
gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-9; // error committed in integrating position from velocities
|
||||
gtsam::Matrix bias_acc_omega_int = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
|
||||
|
||||
{
|
||||
auto gaussian = gtsam::noiseModel::Diagonal::Sigmas(
|
||||
(gtsam::Vector(6) <<
|
||||
gtsam::Vector3::Constant(5.0e-2),
|
||||
gtsam::Vector3::Constant(5.0e-3)).finished());
|
||||
|
||||
auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
|
||||
|
||||
bias_noise_model = huber;
|
||||
}
|
||||
|
||||
{
|
||||
auto gaussian = gtsam::noiseModel::Isotropic::Sigma(3,0.01);
|
||||
auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
|
||||
|
||||
velocity_noise_model = huber;
|
||||
}
|
||||
|
||||
// expect IMU to be rotated in image space co-ords
|
||||
auto p = boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(gtsam::Vector3(0.0, 9.8, 0.0));
|
||||
|
||||
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
|
||||
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
|
||||
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise 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;
|
||||
|
||||
// from SVD comparing gyro and vision estimates, no bias modelled
|
||||
gtsam::Rot3 body_to_imu_rot(
|
||||
3.612861008216737e-02, -9.987267865568920e-01, 3.520695026944293e-02,
|
||||
4.541686330383411e-02, -3.355264881270600e-02, -9.984044913186698e-01,
|
||||
9.983145957368207e-01, 3.766995581886975e-02, 4.414682737675374e-02);
|
||||
|
||||
gtsam::Point3 body_to_imu_trans(0.03, -0.025, -0.06);
|
||||
|
||||
p->body_P_sensor = gtsam::Pose3(body_to_imu_rot, body_to_imu_trans);
|
||||
|
||||
gtsam::Rot3 prior_rotation = gtsam::Rot3(gtsam::Matrix33::Identity());
|
||||
gtsam::Pose3 prior_pose(prior_rotation, gtsam::Point3(0,0,0));
|
||||
|
||||
gtsam::Vector3 acc_bias(2.05998e-18, -0.0942015, 1.17663e-16);
|
||||
gtsam::Vector3 gyro_bias(0,0,0);
|
||||
|
||||
prior_imu_bias = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias);
|
||||
|
||||
prev_state = gtsam::NavState(prior_pose, gtsam::Vector3(0,0,0));
|
||||
prop_state = prev_state;
|
||||
prev_bias = prior_imu_bias;
|
||||
|
||||
preintegrated = new gtsam::PreintegratedCombinedMeasurements(p, prior_imu_bias);
|
||||
}
|
||||
|
||||
gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
|
||||
gtsam::noiseModel::Robust::shared_ptr velocity_noise_model;
|
||||
gtsam::noiseModel::Robust::shared_ptr bias_noise_model;
|
||||
gtsam::NavState prev_state;
|
||||
gtsam::NavState prop_state;
|
||||
gtsam::imuBias::ConstantBias prev_bias;
|
||||
gtsam::PreintegratedCombinedMeasurements *preintegrated;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
if (argc != 2) {
|
||||
cout << "./main [data.txt]\n";
|
||||
return 0;
|
||||
}
|
||||
|
||||
double fx = 8.2237229542137766e+02;
|
||||
double fy = fx;
|
||||
double cx = 5.3872524261474609e+02;
|
||||
double cy = 5.7909587860107422e+02;
|
||||
double baseline = 7.4342307430851651e-01;
|
||||
|
||||
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
|
||||
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.1;
|
||||
ISAM2 isam(parameters);
|
||||
|
||||
// Create a factor graph
|
||||
std::map<size_t, gtsam::SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
IMUHelper imu;
|
||||
|
||||
// Pose prior
|
||||
auto priorPoseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(), priorPoseNoise);
|
||||
initialEstimate.insert(X(0), Pose3::identity());
|
||||
|
||||
// Bias prior
|
||||
graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.prior_imu_bias, imu.bias_noise_model));
|
||||
initialEstimate.insert(B(0), imu.prior_imu_bias);
|
||||
|
||||
// Velocity prior
|
||||
graph.add(PriorFactor<Vector3>(V(1), Vector3(0,0,0), imu.velocity_noise_model));
|
||||
initialEstimate.insert(V(0), Vector3(0,0,0));
|
||||
|
||||
ifstream in(argv[1]);
|
||||
|
||||
if (!in) {
|
||||
cerr << "error opening: " << argv[1] << "\n";
|
||||
return 1;
|
||||
}
|
||||
|
||||
int last_frame = 1;
|
||||
int frame;
|
||||
|
||||
while (true) {
|
||||
char line[1024];
|
||||
|
||||
in.getline(line, sizeof(line));
|
||||
stringstream ss(line);
|
||||
char type;
|
||||
|
||||
ss >> type;
|
||||
ss >> frame;
|
||||
|
||||
if (frame != last_frame || in.eof()) {
|
||||
cout << "Running isam for frame: " << last_frame << "\n";
|
||||
|
||||
initialEstimate.insert(X(last_frame), Pose3::identity());
|
||||
initialEstimate.insert(V(last_frame), Vector3(0,0,0));
|
||||
initialEstimate.insert(B(last_frame), imu.prev_bias);
|
||||
|
||||
CombinedImuFactor imu_factor(
|
||||
X(last_frame-1), V(last_frame-1),
|
||||
X(last_frame), V(last_frame),
|
||||
B(last_frame-1), B(last_frame),
|
||||
*imu.preintegrated);
|
||||
|
||||
graph.add(imu_factor);
|
||||
|
||||
isam.update(graph, initialEstimate);
|
||||
|
||||
Values currentEstimate = isam.calculateEstimate();
|
||||
//currentEstimate.print("Current estimate: ");
|
||||
|
||||
imu.prop_state = imu.preintegrated->predict(imu.prev_state, imu.prev_bias);
|
||||
imu.prev_state = NavState(currentEstimate.at<Pose3>(X(last_frame)), currentEstimate.at<Vector3>(V(last_frame)));
|
||||
imu.prev_bias = currentEstimate.at<imuBias::ConstantBias>(B(last_frame));
|
||||
imu.preintegrated->resetIntegrationAndSetBias(imu.prev_bias);
|
||||
|
||||
graph.resize(0);
|
||||
initialEstimate.clear();
|
||||
|
||||
if (in.eof()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (type == 'i') {
|
||||
double ax, ay, az;
|
||||
double gx, gy, gz;
|
||||
double dt = 1/800.0;
|
||||
|
||||
ss >> ax;
|
||||
ss >> ay;
|
||||
ss >> az;
|
||||
|
||||
ss >> gx;
|
||||
ss >> gy;
|
||||
ss >> gz;
|
||||
|
||||
Vector3 acc(ax, ay, az);
|
||||
Vector3 gyr(gx, gy, gz);
|
||||
|
||||
imu.preintegrated->integrateMeasurement(acc, gyr, dt);
|
||||
} else if (type == 's') {
|
||||
int landmark;
|
||||
double xl, xr, y;
|
||||
|
||||
ss >> landmark;
|
||||
ss >> xl;
|
||||
ss >> xr;
|
||||
ss >> y;
|
||||
|
||||
if (smartFactors.count(landmark) == 0) {
|
||||
auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
|
||||
|
||||
smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(new SmartStereoProjectionPoseFactor(gaussian, params));
|
||||
graph.push_back(smartFactors[landmark]);
|
||||
}
|
||||
|
||||
smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
|
||||
}
|
||||
|
||||
last_frame = frame;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue