From c428e307843561f8188ee5be06dab1b0a0dc8f64 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 15:29:07 -0400 Subject: [PATCH 01/10] 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; +} +/* ************************************************************************* */ + From 18234f68fd0a694e0d14c07499fc910bbd06ffff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 16:25:58 -0400 Subject: [PATCH 02/10] Cleanup, c++11 --- examples/ImuFactorExample2.cpp | 96 ++++++++++++++++++---------------- 1 file changed, 51 insertions(+), 45 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 059c72d83..1d6b32685 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,11 +1,13 @@ -#include -#include +#include +#include #include #include +#include #include -#include -#include +#include + +#include #define GTSAM4 @@ -13,36 +15,32 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -std::vector createPoses() { - +vector createPoses() { // Create the set of ground-truth poses - std::vector poses; + 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); + Point3 up(0, 0, 1); + Point3 target(0, 0, 0); + for (; i < 80; ++i, theta += 2 * M_PI / 8) { + Point3 position(radius * cos(theta), radius * sin(theta), 0.0); + SimpleCamera camera = 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; + NonlinearFactorGraph newgraph, totalgraph; - // Create ISAM2 solver - ISAM2 isam, isam_full; + // Create (incremental) ISAM2 solver + ISAM2 isam; // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth @@ -50,41 +48,46 @@ int main(int argc, char* argv[]) { // 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( + auto 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); + auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); + 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()); + auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + + Vector gravity(3), zero(3); + gravity << 0, 0, -9.8; + zero << 0, 0, 0; #ifdef GTSAM4 - PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); + PriorFactor velprior(100, zero, velnoise); #else - PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); + PriorFactor velprior(100, zero, 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()); + initialEstimate.insert(100, zero); + totalEstimate.insert(100, zero); #else - initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); - totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); + initialEstimate.insert(100, LieVector(zero)); + totalEstimate.insert(100, LieVector(zero)); #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; + Matrix3 accCov = I * 0.1; + Matrix3 gyroCov = I * 0.1; + Matrix3 intCov = I * 0.1; bool secOrder = false; #ifdef GTSAM4 // IMU preintegrator @@ -95,7 +98,8 @@ int main(int argc, char* argv[]) { accum.params()->setUse2ndOrderCoriolis(secOrder); accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); #else - ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder); + ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, + gyroCov, intCov, secOrder); #endif // Simulate poses and imu measurements, adding them to the factor graph @@ -119,42 +123,45 @@ int main(int argc, char* argv[]) { // Add Bias variables periodically if (i % 5 == 0) { biasidx++; - Symbol b1 = biasidx-1; + 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); + auto 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)); + auto f = boost::make_shared >( + 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); + accum.integrateMeasurement(gravity, zero, 0.5); #ifdef GTSAM4 - ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum); + 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()); + ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum, gravity, + zero); #endif newgraph.add(imufac); totalgraph.add(imufac); + + // insert new velocity #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 + initialEstimate.insert(100 + i, gravity); + totalEstimate.insert(100 + i, gravity); #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 + initialEstimate.insert(100 + i, LieVector(gravity)); + totalEstimate.insert(100 + i, LieVector(gravity)); #endif accum.resetIntegration(); } // Batch solution - isam_full = ISAM2(); + ISAM2 isam_full; isam_full.update(totalgraph, totalEstimate); result = isam_full.calculateEstimate(); @@ -168,4 +175,3 @@ int main(int argc, char* argv[]) { return 0; } /* ************************************************************************* */ - From eb447d28a11aa00a34942e6a1a6b966dbc200608 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 17:48:36 -0400 Subject: [PATCH 03/10] Added symbol keys --- examples/ImuFactorExample2.cpp | 75 +++++++++++++++++----------------- 1 file changed, 38 insertions(+), 37 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 1d6b32685..97002eb2a 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -18,12 +18,9 @@ using namespace gtsam; vector createPoses() { // Create the set of ground-truth poses vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - Point3 up(0, 0, 1); - Point3 target(0, 0, 0); - for (; i < 80; ++i, theta += 2 * M_PI / 8) { + double radius = 30.0, theta = 0.0; + Point3 up(0, 0, 1), target(0, 0, 0); + for (size_t i = 0; i < 80; ++i, theta += 2 * M_PI / 8) { Point3 position(radius * cos(theta), radius * sin(theta), 0.0); SimpleCamera camera = SimpleCamera::Lookat(position, target, up); poses.push_back(camera.pose()); @@ -33,6 +30,10 @@ vector createPoses() { /* ************************************************************************* */ int main(int argc, char* argv[]) { + // Shorthand for velocity and pose variables + using symbol_shorthand::V; + using symbol_shorthand::X; + // Create the set of ground-truth landmarks and poses vector poses = createPoses(); @@ -50,37 +51,38 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw auto 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)); + newgraph.push_back(PriorFactor(X(0), poses[0], noise)); + totalgraph.push_back(PriorFactor(X(0), poses[0], noise)); // Add imu priors - int biasidx = 1000; + Key biasKey = Symbol('b', 0); auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); - PriorFactor biasprior(biasidx, imuBias::ConstantBias(), + PriorFactor biasprior(biasKey, imuBias::ConstantBias(), biasnoise); newgraph.push_back(biasprior); totalgraph.push_back(biasprior); - initialEstimate.insert(biasidx, imuBias::ConstantBias()); - totalEstimate.insert(biasidx, imuBias::ConstantBias()); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + totalEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - Vector gravity(3), zero(3); + Vector gravity(3), zero(3), velocity(3); gravity << 0, 0, -9.8; zero << 0, 0, 0; + velocity << 0, 0, 0; #ifdef GTSAM4 - PriorFactor velprior(100, zero, velnoise); + PriorFactor velprior(V(0), zero, velnoise); #else - PriorFactor velprior(100, zero, velnoise); + PriorFactor velprior(V(0), zero, velnoise); #endif newgraph.push_back(velprior); totalgraph.push_back(velprior); #ifdef GTSAM4 - initialEstimate.insert(100, zero); - totalEstimate.insert(100, zero); + initialEstimate.insert(V(0), zero); + totalEstimate.insert(V(0), zero); #else - initialEstimate.insert(100, LieVector(zero)); - totalEstimate.insert(100, LieVector(zero)); + initialEstimate.insert(V(0), LieVector(zero)); + totalEstimate.insert(V(0), LieVector(zero)); #endif Matrix3 I; @@ -110,21 +112,21 @@ int main(int argc, char* argv[]) { 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)); + initialEstimate.insert(X(0), poses[0].compose(delta)); + initialEstimate.insert(X(1), poses[1].compose(delta)); + totalEstimate.insert(X(0), poses[0].compose(delta)); + totalEstimate.insert(X(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)); + initialEstimate.insert(X(i), poses[i].compose(delta)); + totalEstimate.insert(X(i), poses[i].compose(delta)); } if (i > 0) { // Add Bias variables periodically if (i % 5 == 0) { - biasidx++; - Symbol b1 = biasidx - 1; - Symbol b2 = biasidx; + biasKey++; + Symbol b1 = biasKey - 1; + Symbol b2 = biasKey; Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; auto cov = noiseModel::Diagonal::Variances(covvec); @@ -134,28 +136,27 @@ int main(int argc, char* argv[]) { b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); totalgraph.add(f); - initialEstimate.insert(biasidx, imuBias::ConstantBias()); - totalEstimate.insert(biasidx, imuBias::ConstantBias()); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + totalEstimate.insert(biasKey, imuBias::ConstantBias()); } // Add Imu Factor accum.integrateMeasurement(gravity, zero, 0.5); #ifdef GTSAM4 - ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum); + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); #else - ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum, gravity, + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity, zero); #endif - newgraph.add(imufac); totalgraph.add(imufac); // insert new velocity #ifdef GTSAM4 - initialEstimate.insert(100 + i, gravity); - totalEstimate.insert(100 + i, gravity); + initialEstimate.insert(V(i), velocity); + totalEstimate.insert(V(i), velocity); #else - initialEstimate.insert(100 + i, LieVector(gravity)); - totalEstimate.insert(100 + i, LieVector(gravity)); + initialEstimate.insert(V(i), LieVector(velocity)); + totalEstimate.insert(V(i), LieVector(velocity)); #endif accum.resetIntegration(); } From 97a5d5a64a97d13320defc5330f7ea35abfc8b4e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 14:53:20 -0400 Subject: [PATCH 04/10] Python example with reasonable measurements, in body frame. Still a TODO left. --- cython/gtsam/examples/ImuFactorExample2.py | 207 +++++++++++++++++++++ 1 file changed, 207 insertions(+) create mode 100644 cython/gtsam/examples/ImuFactorExample2.py diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py new file mode 100644 index 000000000..41b06607b --- /dev/null +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -0,0 +1,207 @@ +""" +iSAM2 example with ImuFactor. +Author: Robert Truax (C++), Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import math + +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 + +import gtsam +import gtsam.utils.plot as gtsam_plot + + +def X(key): + """Create symbol for pose key.""" + return gtsam.symbol(ord('x'), key) + + +def V(key): + """Create symbol for velocity key.""" + return gtsam.symbol(ord('v'), key) + + +def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=np.float) + + +def create_poses(angular_velocity=np.pi, + delta_t=0.01, radius=30.0): + # Create the set of ground-truth poses + poses = [] + theta = 0.0 + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + for i in range(80): + position = gtsam.Point3(radius * math.cos(theta), + radius * math.sin(theta), 0.0) + camera = gtsam.SimpleCamera.Lookat( + position, target, up, gtsam.Cal3_S2()) + poses.append(camera.pose()) + theta += delta_t * angular_velocity + + return poses + + +def ISAM2_plot(values): + """Plot poses.""" + + # Declare an id for the figure + fignum = 0 + + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plt.cla() + + i = 0 + min_bounds = 0, 0, 0 + max_bounds = 0, 0, 0 + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + gtsam_plot.plot_pose3(fignum, pose_i, 10) + position = pose_i.translation().vector() + min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)] + max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)] + # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0 + i += 1 + + # draw + axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1) + axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1) + axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1) + plt.pause(1) + + +I = np.eye(3) +accCov = I * 0.1 +gyroCov = I * 0.1 +intCov = I * 0.1 +secOrder = False + +# IMU preintegration parameters +# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis +g = 9.81 +PARAMS = gtsam.PreintegrationParams.MakeSharedU() +PARAMS.setAccelerometerCovariance(accCov) +PARAMS.setGyroscopeCovariance(gyroCov) +PARAMS.setIntegrationCovariance(intCov) +PARAMS.setUse2ndOrderCoriolis(secOrder) +PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) + + +def IMU_example(): + + # Create the set of ground-truth landmarks and poses + angular_velocity = math.radians(180) # rad/sec + delta_t = 1.0/18 # makes for 10 degrees per step + radius = 30 + poses = create_poses(angular_velocity, delta_t, radius) + + # Create a factor graph + newgraph = gtsam.NonlinearFactorGraph() + totalgraph = gtsam.NonlinearFactorGraph() + + # Create (incremental) ISAM2 solver + isam = gtsam.ISAM2() + + # Create the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initialEstimate = gtsam.Values() + totalEstimate = gtsam.Values() + + # 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 + noise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + totalgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + + # Add imu priors + biasKey = gtsam.symbol(ord('b'), 0) + biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), + biasnoise) + newgraph.push_back(biasprior) + totalgraph.push_back(biasprior) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + + # Calculate with correct initial velocity + velocity = vector3(0, angular_velocity * radius, 0) + velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) + newgraph.push_back(velprior) + totalgraph.push_back(velprior) + initialEstimate.insert(V(0), velocity) + totalEstimate.insert(V(0), velocity) + + accum = gtsam.PreintegratedImuMeasurements(PARAMS) + + # Simulate poses and imu measurements, adding them to the factor graph + for i, pose_i in enumerate(poses): + delta = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), + gtsam.Point3(0.05, -0.10, 0.20)) + if i == 0: # First time add two poses + initialEstimate.insert(X(0), poses[0].compose(delta)) + initialEstimate.insert(X(1), poses[1].compose(delta)) + totalEstimate.insert(X(0), poses[0].compose(delta)) + totalEstimate.insert(X(1), poses[1].compose(delta)) + elif i >= 2: # Add more poses as necessary + initialEstimate.insert(X(i), pose_i.compose(delta)) + totalEstimate.insert(X(i), pose_i.compose(delta)) + + if i > 0: + # Add Bias variables periodically + if i % 5 == 0: + biasKey += 1 + b1 = biasKey - 1 + b2 = biasKey + cov = gtsam.noiseModel_Isotropic.Variance(6, 0.1) + f = gtsam.BetweenFactorConstantBias( + b1, b2, gtsam.imuBias_ConstantBias(), cov) + newgraph.add(f) + totalgraph.add(f) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + + # Predict acceleration and gyro measurements in (actual) body frame + # TODO: calculate correct acceleration due to circular trajectory + nRb = pose_i.rotation().matrix() + bRn = np.transpose(nRb) + measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) + + # Add Imu Factor + imufac = gtsam.ImuFactor( + X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + newgraph.add(imufac) + totalgraph.add(imufac) + + # insert new velocity + initialEstimate.insert(V(i), velocity) + totalEstimate.insert(V(i), velocity) + accum.resetIntegration() + + # Batch solution + isam_full = gtsam.ISAM2() + isam_full.update(totalgraph, totalEstimate) + result = isam_full.calculateEstimate() + + # Incremental solution + isam.update(newgraph, initialEstimate) + result = isam.calculateEstimate() + newgraph = gtsam.NonlinearFactorGraph() + initialEstimate.clear() + + # ISAM2_plot(result) + + +if __name__ == '__main__': + IMU_example() From f7c3f1da3f16e0b6fde0cb8be4e0806f8691081b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 11:30:01 -0400 Subject: [PATCH 05/10] Fixed acceleration measurement --- cython/gtsam/examples/ImuFactorExample2.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 41b06607b..9d4365b46 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -49,12 +49,8 @@ def create_poses(angular_velocity=np.pi, return poses -def ISAM2_plot(values): +def ISAM2_plot(values, fignum=0): """Plot poses.""" - - # Declare an id for the figure - fignum = 0 - fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() @@ -101,6 +97,7 @@ def IMU_example(): angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step radius = 30 + acceleration = radius * angular_velocity**2 poses = create_poses(angular_velocity, delta_t, radius) # Create a factor graph @@ -171,10 +168,10 @@ def IMU_example(): totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame - # TODO: calculate correct acceleration due to circular trajectory nRb = pose_i.rotation().matrix() bRn = np.transpose(nRb) - measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + \ + vector3(0, 0, acceleration) # in body measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) @@ -194,13 +191,16 @@ def IMU_example(): isam_full.update(totalgraph, totalEstimate) result = isam_full.calculateEstimate() + ISAM2_plot(totalEstimate,0) + ISAM2_plot(result,1) + # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear() - # ISAM2_plot(result) + ISAM2_plot(result,2) if __name__ == '__main__': From d751c65fab8a8f4825f7a224c985d4df9beff0b0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 15:09:56 -0400 Subject: [PATCH 06/10] Removed batch solution (as identical) --- cython/gtsam/examples/ImuFactorExample2.py | 57 ++++++---------------- 1 file changed, 15 insertions(+), 42 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 9d4365b46..ebaac166a 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -33,7 +33,7 @@ def vector3(x, y, z): def create_poses(angular_velocity=np.pi, delta_t=0.01, radius=30.0): - # Create the set of ground-truth poses + """Create the set of ground-truth poses.""" poses = [] theta = 0.0 up = gtsam.Point3(0, 0, 1) @@ -74,22 +74,19 @@ def ISAM2_plot(values, fignum=0): plt.pause(1) -I = np.eye(3) -accCov = I * 0.1 -gyroCov = I * 0.1 -intCov = I * 0.1 -secOrder = False - # IMU preintegration parameters # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis g = 9.81 -PARAMS = gtsam.PreintegrationParams.MakeSharedU() -PARAMS.setAccelerometerCovariance(accCov) -PARAMS.setGyroscopeCovariance(gyroCov) -PARAMS.setIntegrationCovariance(intCov) -PARAMS.setUse2ndOrderCoriolis(secOrder) +I = np.eye(3) +PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) +PARAMS.setAccelerometerCovariance(I * 0.1) +PARAMS.setGyroscopeCovariance(I * 0.1) +PARAMS.setIntegrationCovariance(I * 0.1) +PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) +BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) + def IMU_example(): @@ -102,7 +99,6 @@ def IMU_example(): # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() - totalgraph = gtsam.NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = gtsam.ISAM2() @@ -110,14 +106,12 @@ def IMU_example(): # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initialEstimate = gtsam.Values() - totalEstimate = gtsam.Values() # 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 noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) - totalgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) @@ -125,18 +119,14 @@ def IMU_example(): biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) newgraph.push_back(biasprior) - totalgraph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) - totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity velocity = vector3(0, angular_velocity * radius, 0) velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) newgraph.push_back(velprior) - totalgraph.push_back(velprior) initialEstimate.insert(V(0), velocity) - totalEstimate.insert(V(0), velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) @@ -147,25 +137,17 @@ def IMU_example(): if i == 0: # First time add two poses initialEstimate.insert(X(0), poses[0].compose(delta)) initialEstimate.insert(X(1), poses[1].compose(delta)) - totalEstimate.insert(X(0), poses[0].compose(delta)) - totalEstimate.insert(X(1), poses[1].compose(delta)) elif i >= 2: # Add more poses as necessary initialEstimate.insert(X(i), pose_i.compose(delta)) - totalEstimate.insert(X(i), pose_i.compose(delta)) if i > 0: # Add Bias variables periodically if i % 5 == 0: biasKey += 1 - b1 = biasKey - 1 - b2 = biasKey - cov = gtsam.noiseModel_Isotropic.Variance(6, 0.1) - f = gtsam.BetweenFactorConstantBias( - b1, b2, gtsam.imuBias_ConstantBias(), cov) - newgraph.add(f) - totalgraph.add(f) + factor = gtsam.BetweenFactorConstantBias( + biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) + newgraph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) - totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = pose_i.rotation().matrix() @@ -179,29 +161,20 @@ def IMU_example(): imufac = gtsam.ImuFactor( X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) - totalgraph.add(imufac) # insert new velocity initialEstimate.insert(V(i), velocity) - totalEstimate.insert(V(i), velocity) accum.resetIntegration() - # Batch solution - isam_full = gtsam.ISAM2() - isam_full.update(totalgraph, totalEstimate) - result = isam_full.calculateEstimate() - - ISAM2_plot(totalEstimate,0) - ISAM2_plot(result,1) - # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() + ISAM2_plot(result) + + # reset newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear() - ISAM2_plot(result,2) - if __name__ == '__main__': IMU_example() From 4868a36b6cfa4f542ac6bdbb401fe270d9b40630 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 16:14:24 -0400 Subject: [PATCH 07/10] Added optional initial pose in ConstantTwistScenario --- gtsam/navigation/Scenario.h | 10 +++++++--- gtsam/navigation/tests/testScenario.cpp | 25 ++++++++++++++++++++++++- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index e4b380ff9..acb8f46f5 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,10 +57,13 @@ class Scenario { class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] - ConstantTwistScenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} + ConstantTwistScenario(const Vector3& w, const Vector3& v, + const Pose3& nTb0 = Pose3()) + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {} - Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } + Pose3 pose(double t) const override { + return nTb0_ * Pose3::Expmap(twist_ * t); + } Vector3 omega_b(double t) const override { return twist_.head<3>(); } Vector3 velocity_n(double t) const override { return rotation(t).matrix() * twist_.tail<3>(); @@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario { private: const Vector6 twist_; const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b + const Pose3 nTb0_; }; /// Accelerating from an arbitrary initial state, with optional rotation diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index bc965b058..ef30552ea 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include #include @@ -99,6 +99,29 @@ TEST(Scenario, Loop) { EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, LoopWithInitialPose) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + const Vector3 W(0, -w, 0), V(v, 0, 0); + const Rot3 nRb0 = Rot3::yaw(M_PI); + const Pose3 nTb0(nRb0, Point3(1, 2, 3)); + const ConstantTwistScenario scenario(W, V, nTb0); + + const double T = 30; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = scenario.pose(30); + EXPECT( + assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9)); +} + /* ************************************************************************* */ TEST(Scenario, Accelerating) { // Set up body pointing towards y axis, and start at 10,20,0 with velocity From 1d214d4529488b3e003b77b6529c0836c9d6128c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 17:01:47 -0400 Subject: [PATCH 08/10] Successful wrap of Scenario --- cython/gtsam/tests/test_Scenario.py | 36 +++++++++++++++++++++++++ gtsam.h | 29 +++++++++++--------- gtsam/navigation/tests/testScenario.cpp | 1 + 3 files changed, 54 insertions(+), 12 deletions(-) create mode 100644 cython/gtsam/tests/test_Scenario.py diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py new file mode 100644 index 000000000..4cca1400b --- /dev/null +++ b/cython/gtsam/tests/test_Scenario.py @@ -0,0 +1,36 @@ +import math +import unittest +import numpy as np + +import gtsam + + +class TestScenario(unittest.TestCase): + def setUp(self): + pass + + def test_loop(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + T = 30 + np.testing.assert_almost_equal(W, scenario.omega_b(T)) + np.testing.assert_almost_equal(V, scenario.velocity_b(T)) + np.testing.assert_almost_equal( + np.cross(W, V), scenario.acceleration_b(T)) + + # R = v/w, so test if loop crests at 2*R + R = v / w + T30 = scenario.pose(T) + np.testing.assert_almost_equal( + np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) + self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + + +if __name__ == '__main__': + unittest.main() diff --git a/gtsam.h b/gtsam.h index 6e828486f..bce98b35f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2665,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ gtsam::Unit3 bRef() const; }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); void print(string s) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; @@ -2677,24 +2679,27 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ }; #include -virtual class Scenario {}; - -virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(const Vector& w, const Vector& v); +virtual class Scenario { gtsam::Pose3 pose(double t) const; Vector omega_b(double t) const; Vector velocity_n(double t) const; Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(const Vector& w, const Vector& v); + ConstantTwistScenario(const Vector& w, const Vector& v, + const Pose3& nTb0); }; virtual class AcceleratingScenario : gtsam::Scenario { AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, const Vector& v0, const Vector& a_n, const Vector& omega_b); - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; }; //************************************************************************* diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ef30552ea..161b8841a 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -96,6 +96,7 @@ TEST(Scenario, Loop) { const double R = v / w; const Pose3 T30 = scenario.pose(30); EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz())); EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } From 264a240094d62a56c3b2d364c8c08fd158e898f7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 19:01:28 -0400 Subject: [PATCH 09/10] Fixed both C++ and python examples --- cython/gtsam/examples/ImuFactorExample2.py | 72 +++++----- examples/ImuFactorExample2.cpp | 152 ++++++++------------- 2 files changed, 89 insertions(+), 135 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index ebaac166a..10bd3de11 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -31,24 +31,6 @@ def vector3(x, y, z): return np.array([x, y, z], dtype=np.float) -def create_poses(angular_velocity=np.pi, - delta_t=0.01, radius=30.0): - """Create the set of ground-truth poses.""" - poses = [] - theta = 0.0 - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - for i in range(80): - position = gtsam.Point3(radius * math.cos(theta), - radius * math.sin(theta), 0.0) - camera = gtsam.SimpleCamera.Lookat( - position, target, up, gtsam.Cal3_S2()) - poses.append(camera.pose()) - theta += delta_t * angular_velocity - - return poses - - def ISAM2_plot(values, fignum=0): """Plot poses.""" fig = plt.figure(fignum) @@ -77,8 +59,9 @@ def ISAM2_plot(values, fignum=0): # IMU preintegration parameters # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis g = 9.81 -I = np.eye(3) +n_gravity = vector3(0, 0, -g) PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) +I = np.eye(3) PARAMS.setAccelerometerCovariance(I * 0.1) PARAMS.setGyroscopeCovariance(I * 0.1) PARAMS.setIntegrationCovariance(I * 0.1) @@ -86,16 +69,29 @@ PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) +DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), + gtsam.Point3(0.05, -0.10, 0.20)) def IMU_example(): + """Run iSAM 2 example with IMU factor.""" + + # Start with a camera on x-axis looking at origin + radius = 30 + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + position = gtsam.Point3(radius, 0, 0) + camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) + pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step - radius = 30 - acceleration = radius * angular_velocity**2 - poses = create_poses(angular_velocity, delta_t, radius) + + angular_velocity_vector = vector3(0, -angular_velocity, 0) + linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) + scenario = gtsam.ConstantTwistScenario( + angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() @@ -111,7 +107,7 @@ def IMU_example(): # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) @@ -123,22 +119,23 @@ def IMU_example(): velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity - velocity = vector3(0, angular_velocity * radius, 0) - velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) + n_velocity = vector3(0, angular_velocity * radius, 0) + velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) - initialEstimate.insert(V(0), velocity) + initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) # Simulate poses and imu measurements, adding them to the factor graph - for i, pose_i in enumerate(poses): - delta = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), - gtsam.Point3(0.05, -0.10, 0.20)) + for i in range(80): + t = i * delta_t # simulation time if i == 0: # First time add two poses - initialEstimate.insert(X(0), poses[0].compose(delta)) - initialEstimate.insert(X(1), poses[1].compose(delta)) + pose_1 = scenario.pose(delta_t) + initialEstimate.insert(X(0), pose_0.compose(DELTA)) + initialEstimate.insert(X(1), pose_1.compose(DELTA)) elif i >= 2: # Add more poses as necessary - initialEstimate.insert(X(i), pose_i.compose(delta)) + pose_i = scenario.pose(t) + initialEstimate.insert(X(i), pose_i.compose(DELTA)) if i > 0: # Add Bias variables periodically @@ -150,11 +147,10 @@ def IMU_example(): initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame - nRb = pose_i.rotation().matrix() + nRb = scenario.rotation(t).matrix() bRn = np.transpose(nRb) - measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + \ - vector3(0, 0, acceleration) # in body - measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) + measuredOmega = scenario.omega_b(t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor @@ -162,8 +158,8 @@ def IMU_example(): X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) - # insert new velocity - initialEstimate.insert(V(i), velocity) + # insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 97002eb2a..1137ca214 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -3,42 +3,51 @@ #include #include #include +#include #include #include #include #include -#define GTSAM4 - using namespace std; using namespace gtsam; -/* ************************************************************************* */ -vector createPoses() { - // Create the set of ground-truth poses - vector poses; - double radius = 30.0, theta = 0.0; - Point3 up(0, 0, 1), target(0, 0, 0); - for (size_t i = 0; i < 80; ++i, theta += 2 * M_PI / 8) { - Point3 position(radius * cos(theta), radius * sin(theta), 0.0); - SimpleCamera camera = SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); - } - return poses; -} +// Shorthand for velocity and pose variables +using symbol_shorthand::V; +using symbol_shorthand::X; + +const double kGravity = 9.81; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Shorthand for velocity and pose variables - using symbol_shorthand::V; - using symbol_shorthand::X; + auto params = PreintegrationParams::MakeSharedU(kGravity); + params->setAccelerometerCovariance(I_3x3 * 0.1); + params->setGyroscopeCovariance(I_3x3 * 0.1); + params->setIntegrationCovariance(I_3x3 * 0.1); + params->setUse2ndOrderCoriolis(false); + params->setOmegaCoriolis(Vector3(0, 0, 0)); - // Create the set of ground-truth landmarks and poses - vector poses = createPoses(); + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + + // Start with a camera on x-axis looking at origin + double radius = 30; + const Point3 up(0, 0, 1), target(0, 0, 0); + const Point3 position(radius, 0, 0); + const SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + const auto pose_0 = camera.pose(); + + // Now, create a constant-twist scenario that makes the camera orbit the + // origin + double angular_velocity = M_PI, // rad/sec + delta_t = 1.0 / 18; // makes for 10 degrees per step + Vector3 angular_velocity_vector(0, -angular_velocity, 0); + Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0); + auto scenario = ConstantTwistScenario(angular_velocity_vector, + linear_velocity_vector, pose_0); // Create a factor graph - NonlinearFactorGraph newgraph, totalgraph; + NonlinearFactorGraph newgraph; // Create (incremental) ISAM2 solver ISAM2 isam; @@ -51,8 +60,7 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - newgraph.push_back(PriorFactor(X(0), poses[0], noise)); - totalgraph.push_back(PriorFactor(X(0), poses[0], noise)); + newgraph.push_back(PriorFactor(X(0), pose_0, noise)); // Add imu priors Key biasKey = Symbol('b', 0); @@ -60,65 +68,29 @@ int main(int argc, char* argv[]) { PriorFactor biasprior(biasKey, imuBias::ConstantBias(), biasnoise); newgraph.push_back(biasprior); - totalgraph.push_back(biasprior); initialEstimate.insert(biasKey, imuBias::ConstantBias()); - totalEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - Vector gravity(3), zero(3), velocity(3); - gravity << 0, 0, -9.8; - zero << 0, 0, 0; - velocity << 0, 0, 0; -#ifdef GTSAM4 - PriorFactor velprior(V(0), zero, velnoise); -#else - PriorFactor velprior(V(0), zero, velnoise); -#endif + Vector n_velocity(3); + n_velocity << 0, angular_velocity * radius, 0; + PriorFactor velprior(V(0), n_velocity, velnoise); newgraph.push_back(velprior); - totalgraph.push_back(velprior); -#ifdef GTSAM4 - initialEstimate.insert(V(0), zero); - totalEstimate.insert(V(0), zero); -#else - initialEstimate.insert(V(0), LieVector(zero)); - totalEstimate.insert(V(0), LieVector(zero)); -#endif + initialEstimate.insert(V(0), n_velocity); - 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 + PreintegratedImuMeasurements accum(params); // 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 + for (size_t i = 0; i < 36; ++i) { + double t = i * delta_t; if (i == 0) { // First time add two poses - initialEstimate.insert(X(0), poses[0].compose(delta)); - initialEstimate.insert(X(1), poses[1].compose(delta)); - totalEstimate.insert(X(0), poses[0].compose(delta)); - totalEstimate.insert(X(1), poses[1].compose(delta)); + auto pose_1 = scenario.pose(delta_t); + initialEstimate.insert(X(0), pose_0.compose(delta)); + initialEstimate.insert(X(1), pose_1.compose(delta)); } else if (i >= 2) { // Add more poses as necessary - initialEstimate.insert(X(i), poses[i].compose(delta)); - totalEstimate.insert(X(i), poses[i].compose(delta)); + auto pose_i = scenario.pose(t); + initialEstimate.insert(X(i), pose_i.compose(delta)); } if (i > 0) { @@ -135,44 +107,30 @@ int main(int argc, char* argv[]) { auto f = boost::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); - totalgraph.add(f); initialEstimate.insert(biasKey, imuBias::ConstantBias()); - totalEstimate.insert(biasKey, imuBias::ConstantBias()); } - // Add Imu Factor - accum.integrateMeasurement(gravity, zero, 0.5); -#ifdef GTSAM4 - ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); -#else - ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity, - zero); -#endif - newgraph.add(imufac); - totalgraph.add(imufac); + // Predict acceleration and gyro measurements in (actual) body frame + auto measuredAcc = scenario.acceleration_b(t) - + scenario.rotation(t).transpose() * params->n_gravity; + auto measuredOmega = scenario.omega_b(t); + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t); - // insert new velocity -#ifdef GTSAM4 - initialEstimate.insert(V(i), velocity); - totalEstimate.insert(V(i), velocity); -#else - initialEstimate.insert(V(i), LieVector(velocity)); - totalEstimate.insert(V(i), LieVector(velocity)); -#endif + // Add Imu Factor + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); + newgraph.add(imufac); + + // insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity); accum.resetIntegration(); } - // Batch solution - ISAM2 isam_full; - isam_full.update(totalgraph, totalEstimate); - result = isam_full.calculateEstimate(); - // Incremental solution isam.update(newgraph, initialEstimate); result = isam.calculateEstimate(); newgraph = NonlinearFactorGraph(); initialEstimate.clear(); } - + GTSAM_PRINT(result); return 0; } /* ************************************************************************* */ From 15cc9a51b3b66eb1a6d3f3b201000141bed37b3e Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Wed, 17 Oct 2018 06:01:14 -0400 Subject: [PATCH 10/10] fix "const Vector&" in gtsam.h --- gtsam.h | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam.h b/gtsam.h index bce98b35f..ca37d2d62 100644 --- a/gtsam.h +++ b/gtsam.h @@ -568,7 +568,7 @@ class Pose2 { static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; + Vector Adjoint(Vector xi) const; static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 @@ -865,7 +865,7 @@ class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); + CalibratedCamera(Vector v); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable @@ -875,7 +875,7 @@ class CalibratedCamera { // Manifold static size_t Dim(); size_t dim() const; - gtsam::CalibratedCamera retract(const Vector& d) const; + gtsam::CalibratedCamera retract(Vector d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; // Action on Point3 @@ -911,7 +911,7 @@ class PinholeCamera { CALIBRATION calibration() const; // Manifold - This retract(const Vector& d) const; + This retract(Vector d) const; Vector localCoordinates(const This& T2) const; size_t dim() const; static size_t Dim(); @@ -950,7 +950,7 @@ virtual class SimpleCamera { gtsam::Cal3_S2 calibration() const; // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; + gtsam::SimpleCamera retract(Vector d) const; Vector localCoordinates(const gtsam::SimpleCamera& T2) const; size_t dim() const; static size_t Dim(); @@ -992,7 +992,7 @@ class StereoCamera { gtsam::Cal3_S2Stereo calibration() const; // Manifold - gtsam::StereoCamera retract(const Vector& d) const; + gtsam::StereoCamera retract(Vector d) const; Vector localCoordinates(const gtsam::StereoCamera& T2) const; size_t dim() const; static size_t Dim(); @@ -1227,12 +1227,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { }; virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); static gtsam::noiseModel::Constrained* All(size_t dim); static gtsam::noiseModel::Constrained* All(size_t dim, double mu); @@ -1415,12 +1415,12 @@ virtual class JacobianFactor : gtsam::GaussianFactor { pair jacobianUnweighted() const; Matrix augmentedJacobianUnweighted() const; - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; pair eliminate(const gtsam::Ordering& keys) const; - void setModel(bool anyConstrained, const Vector& sigmas); + void setModel(bool anyConstrained, Vector sigmas); gtsam::noiseModel::Diagonal* get_model() const; @@ -2691,15 +2691,15 @@ virtual class Scenario { }; virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(const Vector& w, const Vector& v); - ConstantTwistScenario(const Vector& w, const Vector& v, + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, const Pose3& nTb0); }; virtual class AcceleratingScenario : gtsam::Scenario { AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, - const Vector& v0, const Vector& a_n, - const Vector& omega_b); + Vector v0, Vector a_n, + Vector omega_b); }; //*************************************************************************