From 97a5d5a64a97d13320defc5330f7ea35abfc8b4e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 14:53:20 -0400 Subject: [PATCH] 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()