diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py new file mode 100644 index 000000000..10bd3de11 --- /dev/null +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -0,0 +1,176 @@ +""" +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 ISAM2_plot(values, fignum=0): + """Plot poses.""" + 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) + + +# IMU preintegration parameters +# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis +g = 9.81 +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) +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 + + 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() + + # 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() + + # 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), pose_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) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + + # Calculate with correct initial velocity + n_velocity = vector3(0, angular_velocity * radius, 0) + velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) + newgraph.push_back(velprior) + initialEstimate.insert(V(0), n_velocity) + + accum = gtsam.PreintegratedImuMeasurements(PARAMS) + + # Simulate poses and imu measurements, adding them to the factor graph + for i in range(80): + t = i * delta_t # simulation time + if i == 0: # First time add two poses + 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 + pose_i = scenario.pose(t) + initialEstimate.insert(X(i), pose_i.compose(DELTA)) + + if i > 0: + # Add Bias variables periodically + if i % 5 == 0: + biasKey += 1 + factor = gtsam.BetweenFactorConstantBias( + biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) + newgraph.add(factor) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + + # Predict acceleration and gyro measurements in (actual) body frame + nRb = scenario.rotation(t).matrix() + bRn = np.transpose(nRb) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) + measuredOmega = scenario.omega_b(t) + 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) + + # insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity) + accum.resetIntegration() + + # Incremental solution + isam.update(newgraph, initialEstimate) + result = isam.calculateEstimate() + ISAM2_plot(result) + + # reset + newgraph = gtsam.NonlinearFactorGraph() + initialEstimate.clear() + + +if __name__ == '__main__': + IMU_example() 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/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp new file mode 100644 index 000000000..1137ca214 --- /dev/null +++ b/examples/ImuFactorExample2.cpp @@ -0,0 +1,136 @@ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// 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[]) { + 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)); + + 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; + + // Create (incremental) ISAM2 solver + ISAM2 isam; + + // 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 + auto noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + newgraph.push_back(PriorFactor(X(0), pose_0, noise)); + + // Add imu priors + Key biasKey = Symbol('b', 0); + auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); + PriorFactor biasprior(biasKey, imuBias::ConstantBias(), + biasnoise); + newgraph.push_back(biasprior); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + + Vector n_velocity(3); + n_velocity << 0, angular_velocity * radius, 0; + PriorFactor velprior(V(0), n_velocity, velnoise); + newgraph.push_back(velprior); + + initialEstimate.insert(V(0), n_velocity); + + // IMU preintegrator + PreintegratedImuMeasurements accum(params); + + // Simulate poses and imu measurements, adding them to the factor graph + for (size_t i = 0; i < 36; ++i) { + double t = i * delta_t; + if (i == 0) { // First time add two poses + 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 + auto pose_i = scenario.pose(t); + initialEstimate.insert(X(i), pose_i.compose(delta)); + } + + if (i > 0) { + // Add Bias variables periodically + if (i % 5 == 0) { + 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); + Vector6 zerovec; + zerovec << 0, 0, 0, 0, 0, 0; + auto f = boost::make_shared >( + b1, b2, imuBias::ConstantBias(), cov); + newgraph.add(f); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + } + // 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); + + // 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(); + } + + // Incremental solution + isam.update(newgraph, initialEstimate); + result = isam.calculateEstimate(); + newgraph = NonlinearFactorGraph(); + initialEstimate.clear(); + } + GTSAM_PRINT(result); + return 0; +} +/* ************************************************************************* */ diff --git a/gtsam.h b/gtsam.h index 6e828486f..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; @@ -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(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); - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; + Vector v0, Vector a_n, + Vector omega_b); }; //************************************************************************* 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..161b8841a 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 @@ -96,9 +96,33 @@ 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)); } +/* ************************************************************************* */ +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