From c428e307843561f8188ee5be06dab1b0a0dc8f64 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 15:29:07 -0400 Subject: [PATCH] Example due to Robert Truax in Issue #280 --- examples/ImuFactorExample2.cpp | 171 +++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) create mode 100644 examples/ImuFactorExample2.cpp diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp new file mode 100644 index 000000000..059c72d83 --- /dev/null +++ b/examples/ImuFactorExample2.cpp @@ -0,0 +1,171 @@ + +#include +#include +#include +#include +#include +#include +#include + +#define GTSAM4 + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +std::vector createPoses() { + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 80; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); + } + return poses; +} +/* ************************************************************************* */ + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Create the set of ground-truth landmarks and poses + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph newgraph; + NonlinearFactorGraph totalgraph; + + // Create ISAM2 solver + ISAM2 isam, isam_full; + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate, totalEstimate, result; + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + newgraph.push_back(PriorFactor(0, poses[0], noise)); + totalgraph.push_back(PriorFactor(0, poses[0], noise)); + + // Add imu priors + int biasidx = 1000; + noiseModel::Diagonal::shared_ptr biasnoise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); + PriorFactor biasprior(biasidx,imuBias::ConstantBias(),biasnoise); + newgraph.push_back(biasprior); + totalgraph.push_back(biasprior); + initialEstimate.insert(biasidx, imuBias::ConstantBias()); + totalEstimate.insert(biasidx, imuBias::ConstantBias()); + noiseModel::Diagonal::shared_ptr velnoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); +#ifdef GTSAM4 + PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); +#else + PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); +#endif + newgraph.push_back(velprior); + totalgraph.push_back(velprior); + +#ifdef GTSAM4 + initialEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); + totalEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); +#else + initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); + totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); +#endif + + Matrix3 I; + I << 1, 0, 0, 0, 1, 0, 0, 0, 1; + Matrix3 accCov = I*0.1; + Matrix3 gyroCov = I*0.1; + Matrix3 intCov = I*0.1; + bool secOrder = false; +#ifdef GTSAM4 + // IMU preintegrator + PreintegratedImuMeasurements accum(PreintegrationParams::MakeSharedD()); + accum.params()->setAccelerometerCovariance(accCov); + accum.params()->setGyroscopeCovariance(gyroCov); + accum.params()->setIntegrationCovariance(intCov); + accum.params()->setUse2ndOrderCoriolis(secOrder); + accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); +#else + ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder); +#endif + + // Simulate poses and imu measurements, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { +#ifdef GTSAM4 + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); +#else + Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); +#endif + if (i == 0) { // First time add two poses + initialEstimate.insert(0, poses[0].compose(delta)); + initialEstimate.insert(1, poses[1].compose(delta)); + totalEstimate.insert(0, poses[0].compose(delta)); + totalEstimate.insert(1, poses[1].compose(delta)); + } else if (i >= 2) { // Add more poses as necessary + initialEstimate.insert(i, poses[i].compose(delta)); + totalEstimate.insert(i, poses[i].compose(delta)); + } + + if (i > 0) { + // Add Bias variables periodically + if (i % 5 == 0) { + biasidx++; + Symbol b1 = biasidx-1; + Symbol b2 = biasidx; + imuBias::ConstantBias basebias = imuBias::ConstantBias(); + Vector6 covvec; + covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; + noiseModel::Diagonal::shared_ptr cov = noiseModel::Diagonal::Variances(covvec); + Vector6 zerovec; + zerovec << 0, 0, 0, 0, 0, 0; + BetweenFactor::shared_ptr f(new BetweenFactor(b1, b2, imuBias::ConstantBias(), cov)); + newgraph.add(f); + totalgraph.add(f); + initialEstimate.insert(biasidx, imuBias::ConstantBias()); + totalEstimate.insert(biasidx, imuBias::ConstantBias()); + } + // Add Imu Factor + accum.integrateMeasurement((Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0, 0, 0).finished(), 0.5); +#ifdef GTSAM4 + ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum); +#else + ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum, (Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0.0, 0.0, 0.0).finished()); +#endif + + newgraph.add(imufac); + totalgraph.add(imufac); +#ifdef GTSAM4 + initialEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity + totalEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity +#else + initialEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity + totalEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity +#endif + accum.resetIntegration(); + } + + // Batch solution + isam_full = ISAM2(); + isam_full.update(totalgraph, totalEstimate); + result = isam_full.calculateEstimate(); + + // Incremental solution + isam.update(newgraph, initialEstimate); + result = isam.calculateEstimate(); + newgraph = NonlinearFactorGraph(); + initialEstimate.clear(); + } + + return 0; +} +/* ************************************************************************* */ +