ImuFactorISAM2Example.py
Go to the documentation of this file.
1 """
2 ImuFactor example with iSAM2.
3 Authors: Robert Truax (C++), Frank Dellaert, Varun Agrawal (Python)
4 """
5 # pylint: disable=invalid-name, E1101
6 
7 from __future__ import print_function
8 
9 import math
10 
11 import matplotlib.pyplot as plt
12 import numpy as np
13 from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
14 
15 import gtsam
16 from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
17  ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
18  PinholeCameraCal3_S2, Point3, Pose3,
19  PriorFactorConstantBias, PriorFactorPose3,
20  PriorFactorVector, Rot3, Values)
21 from gtsam.symbol_shorthand import B, V, X
22 from gtsam.utils import plot
23 
24 
25 def vector3(x, y, z):
26  """Create 3d double numpy array."""
27  return np.array([x, y, z], dtype=float)
28 
29 
30 g = 9.81
31 n_gravity = vector3(0, 0, -g)
32 
33 
35  # IMU preintegration parameters
36  # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
38  I = np.eye(3)
39  PARAMS.setAccelerometerCovariance(I * 0.1)
40  PARAMS.setGyroscopeCovariance(I * 0.1)
41  PARAMS.setIntegrationCovariance(I * 0.1)
42  PARAMS.setUse2ndOrderCoriolis(False)
43  PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
44 
45  BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
46  DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
47  Point3(0.05, -0.10, 0.20))
48 
49  return PARAMS, BIAS_COVARIANCE, DELTA
50 
51 
52 def get_camera(radius):
53  up = Point3(0, 0, 1)
54  target = Point3(0, 0, 0)
55  position = Point3(radius, 0, 0)
56  camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
57  return camera
58 
59 
60 def get_scenario(radius, pose_0, angular_velocity, delta_t):
61  """Create the set of ground-truth landmarks and poses"""
62  angular_velocity_vector = vector3(0, -angular_velocity, 0)
63  linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
64  scenario = ConstantTwistScenario(
65  angular_velocity_vector, linear_velocity_vector, pose_0)
66 
67  return scenario
68 
69 
71  """Run iSAM 2 example with IMU factor."""
72 
73  # Start with a camera on x-axis looking at origin
74  radius = 30
75  camera = get_camera(radius)
76  pose_0 = camera.pose()
77 
78  delta_t = 1.0/18 # makes for 10 degrees per step
79  angular_velocity = math.radians(180) # rad/sec
80  scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
81 
82  PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
83 
84  # Create a factor graph
85  graph = NonlinearFactorGraph()
86 
87  # Create (incremental) ISAM2 solver
88  isam = ISAM2()
89 
90  # Create the initial estimate to the solution
91  # Intentionally initialize the variables off from the ground truth
92  initialEstimate = Values()
93 
94  # Add a prior on pose x0. This indirectly specifies where the origin is.
95  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
97  np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
98  graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
99 
100  # Add imu priors
101  biasKey = B(0)
102  biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
103  biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
104  biasnoise)
105  graph.push_back(biasprior)
106  initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
107  velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
108 
109  # Calculate with correct initial velocity
110  n_velocity = vector3(0, angular_velocity * radius, 0)
111  velprior = PriorFactorVector(V(0), n_velocity, velnoise)
112  graph.push_back(velprior)
113  initialEstimate.insert(V(0), n_velocity)
114 
115  accum = gtsam.PreintegratedImuMeasurements(PARAMS)
116 
117  # Simulate poses and imu measurements, adding them to the factor graph
118  for i in range(80):
119  t = i * delta_t # simulation time
120  if i == 0: # First time add two poses
121  pose_1 = scenario.pose(delta_t)
122  initialEstimate.insert(X(0), pose_0.compose(DELTA))
123  initialEstimate.insert(X(1), pose_1.compose(DELTA))
124  elif i >= 2: # Add more poses as necessary
125  pose_i = scenario.pose(t)
126  initialEstimate.insert(X(i), pose_i.compose(DELTA))
127 
128  if i > 0:
129  # Add Bias variables periodically
130  if i % 5 == 0:
131  biasKey += 1
132  factor = BetweenFactorConstantBias(
133  biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
134  graph.add(factor)
135  initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
136 
137  # Predict acceleration and gyro measurements in (actual) body frame
138  nRb = scenario.rotation(t).matrix()
139  bRn = np.transpose(nRb)
140  measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
141  measuredOmega = scenario.omega_b(t)
142  accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
143 
144  # Add Imu Factor
145  imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
146  graph.add(imufac)
147 
148  # insert new velocity, which is wrong
149  initialEstimate.insert(V(i), n_velocity)
150  accum.resetIntegration()
151 
152  # Incremental solution
153  isam.update(graph, initialEstimate)
154  result = isam.calculateEstimate()
155  plot.plot_incremental_trajectory(0, result,
156  start=i, scale=3, time_interval=0.01)
157 
158  # reset
159  graph = NonlinearFactorGraph()
160  initialEstimate.clear()
161 
162  plt.show()
163 
164 
165 if __name__ == '__main__':
166  IMU_example()
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
Definition: NoiseModel.cpp:573
static boost::shared_ptr< PreintegrationParams > MakeSharedU(double g=9.81)
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
#define X
Definition: icosphere.cpp:20
def get_scenario(radius, pose_0, angular_velocity, delta_t)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:567


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:13