ImuFactorExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 A script validating and demonstrating the ImuFactor inference.
9 
10 Author: Frank Dellaert, Varun Agrawal
11 """
12 
13 # pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order
14 
15 from __future__ import print_function
16 
17 import argparse
18 import math
19 
20 import matplotlib.pyplot as plt
21 import numpy as np
22 from gtsam.symbol_shorthand import B, V, X
23 from gtsam.utils.plot import plot_pose3
24 from mpl_toolkits.mplot3d import Axes3D
25 from PreintegrationExample import POSES_FIG, PreintegrationExample
26 
27 import gtsam
28 
29 BIAS_KEY = B(0)
30 GRAVITY = 9.81
31 
32 np.set_printoptions(precision=3, suppress=True)
33 
34 
35 def parse_args() -> argparse.Namespace:
36  """Parse command line arguments."""
37  parser = argparse.ArgumentParser("ImuFactorExample.py")
38  parser.add_argument("--twist_scenario",
39  default="sick_twist",
40  choices=("zero_twist", "forward_twist", "loop_twist",
41  "sick_twist"))
42  parser.add_argument("--time",
43  "-T",
44  default=12,
45  type=int,
46  help="Total navigation time in seconds")
47  parser.add_argument("--compute_covariances",
48  default=False,
49  action='store_true')
50  parser.add_argument("--verbose", default=False, action='store_true')
51  args = parser.parse_args()
52  return args
53 
54 
56  """Class to run example of the Imu Factor."""
57  def __init__(self, twist_scenario: str = "sick_twist"):
58  self.velocity = np.array([2, 0, 0])
61 
62  # Choose one of these twists to change scenario:
63  twist_scenarios = dict(
64  zero_twist=(np.zeros(3), np.zeros(3)),
65  forward_twist=(np.zeros(3), self.velocity),
66  loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
67  sick_twist=(np.array([math.radians(30), -math.radians(30),
68  0]), self.velocity))
69 
70  accBias = np.array([-0.3, 0.1, 0.2])
71  gyroBias = np.array([0.1, 0.3, -0.1])
72  bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
73 
75 
76  # Some arbitrary noise sigmas
77  gyro_sigma = 1e-3
78  accel_sigma = 1e-3
79  I_3x3 = np.eye(3)
80  params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
81  params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
82  params.setIntegrationCovariance(1e-7**2 * I_3x3)
83 
84  dt = 1e-2
85  super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
86  bias, params, dt)
87 
88  def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
89  """Add a prior on the navigation state at time `i`."""
90  state = self.scenario.navState(i)
91  graph.push_back(
92  gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
93  graph.push_back(
94  gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
95 
96  def optimize(self, graph: gtsam.NonlinearFactorGraph,
97  initial: gtsam.Values):
98  """Optimize using Levenberg-Marquardt optimization."""
100  params.setVerbosityLM("SUMMARY")
101  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
102  result = optimizer.optimize()
103  return result
104 
105  def plot(self,
106  values: gtsam.Values,
107  title: str = "Estimated Trajectory",
108  fignum: int = POSES_FIG + 1,
109  show: bool = False):
110  """
111  Plot poses in values.
112 
113  Args:
114  values: The values object with the poses to plot.
115  title: The title of the plot.
116  fignum: The matplotlib figure number.
117  POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure.
118  show: Flag indicating whether to display the figure.
119  """
120  i = 0
121  while values.exists(X(i)):
122  pose_i = values.atPose3(X(i))
123  plot_pose3(fignum, pose_i, 1)
124  i += 1
125  plt.title(title)
126 
128 
129  print("Bias Values", values.atConstantBias(BIAS_KEY))
130 
131  plt.ioff()
132 
133  if show:
134  plt.show()
135 
136  def run(self,
137  T: int = 12,
138  compute_covariances: bool = False,
139  verbose: bool = True):
140  """
141  Main runner.
142 
143  Args:
144  T: Total trajectory time.
145  compute_covariances: Flag indicating whether to compute marginal covariances.
146  verbose: Flag indicating if printing should be verbose.
147  """
149 
150  # initialize data structure for pre-integrated IMU measurements
152 
153  num_poses = T # assumes 1 factor per second
154  initial = gtsam.Values()
155  initial.insert(BIAS_KEY, self.actualBias)
156 
157  # simulate the loop
158  i = 0 # state index
159  initial_state_i = self.scenario.navState(0)
160  initial.insert(X(i), initial_state_i.pose())
161  initial.insert(V(i), initial_state_i.velocity())
162 
163  # add prior on beginning
164  self.addPrior(0, graph)
165 
166  for k, t in enumerate(np.arange(0, T, self.dt)):
167  # get measurements and add them to PIM
168  measuredOmega = self.runner.measuredAngularVelocity(t)
169  measuredAcc = self.runner.measuredSpecificForce(t)
170  pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
171 
172  # Plot IMU many times
173  if k % 10 == 0:
174  self.plotImu(t, measuredOmega, measuredAcc)
175 
176  if (k + 1) % int(1 / self.dt) == 0:
177  # Plot every second
178  self.plotGroundTruthPose(t, scale=1)
179  plt.title("Ground Truth Trajectory")
180 
181  # create IMU factor every second
182  factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
183  BIAS_KEY, pim)
184  graph.push_back(factor)
185 
186  if verbose:
187  print(factor)
188  print("Predicted state at {0}:\n{1}".format(
189  t + self.dt,
190  pim.predict(initial_state_i, self.actualBias)))
191 
192  pim.resetIntegration()
193 
194  rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
195  translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
196  poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
197 
198  actual_state_i = self.scenario.navState(t + self.dt)
199  print("Actual state at {0}:\n{1}".format(
200  t + self.dt, actual_state_i))
201 
202  # Set initial state to current
203  initial_state_i = actual_state_i
204 
205  noisy_state_i = gtsam.NavState(
206  actual_state_i.pose().compose(poseNoise),
207  actual_state_i.velocity() + np.random.randn(3) * 0.1)
208 
209  initial.insert(X(i + 1), noisy_state_i.pose())
210  initial.insert(V(i + 1), noisy_state_i.velocity())
211  i += 1
212 
213  # add priors on end
214  self.addPrior(num_poses - 1, graph)
215 
216  initial.print("Initial values:")
217 
218  result = self.optimize(graph, initial)
219 
220  result.print("Optimized values:")
221  print("------------------")
222  print(graph.error(initial))
223  print(graph.error(result))
224  print("------------------")
225 
226  if compute_covariances:
227  # Calculate and print marginal covariances
228  marginals = gtsam.Marginals(graph, result)
229  print("Covariance on bias:\n",
230  marginals.marginalCovariance(BIAS_KEY))
231  for i in range(num_poses):
232  print("Covariance on pose {}:\n{}\n".format(
233  i, marginals.marginalCovariance(X(i))))
234  print("Covariance on vel {}:\n{}\n".format(
235  i, marginals.marginalCovariance(V(i))))
236 
237  self.plot(result, show=True)
238 
239 
240 if __name__ == '__main__':
241  args = parse_args()
242 
243  ImuFactorExample(args.twist_scenario).run(args.time,
244  args.compute_covariances,
245  args.verbose)
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
gtsam::utils.plot.plot_pose3
plt.Figure plot_pose3(int fignum, Pose3 pose, float axis_length=0.1, np.ndarray P=None, Iterable[str] axis_labels=("X axis", "Y axis", "Z axis"))
Definition: plot.py:436
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Definition: openglsupport.cpp:226
gtsam::Marginals
Definition: Marginals.h:32
gtsam.examples.ImuFactorExample.ImuFactorExample.velNoise
velNoise
Definition: ImuFactorExample.py:60
gtsam::NavState
Definition: NavState.h:34
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
X
#define X
Definition: icosphere.cpp:20
gtsam.examples.PreintegrationExample.PreintegrationExample.runner
runner
Definition: PreintegrationExample.py:78
gtsam.examples.PreintegrationExample.PreintegrationExample.plotImu
def plotImu(self, float t, Sequence measuredOmega, Sequence measuredAcc)
Definition: PreintegrationExample.py:88
gtsam::utils.plot.set_axes_equal
None set_axes_equal(int fignum)
Definition: plot.py:36
gtsam.examples.PreintegrationExample.PreintegrationExample.dt
dt
Definition: PreintegrationExample.py:58
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam.examples.PreintegrationExample.PreintegrationExample.actualBias
actualBias
Definition: PreintegrationExample.py:71
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
dict
Definition: pytypes.h:2107
gtsam.examples.ImuFactorExample.ImuFactorExample.__init__
def __init__(self, str twist_scenario="sick_twist")
Definition: ImuFactorExample.py:57
gtsam.examples.ImuFactorExample.ImuFactorExample.priorNoise
priorNoise
Definition: ImuFactorExample.py:59
gtsam.examples.PreintegrationExample.PreintegrationExample.params
params
Definition: PreintegrationExample.py:65
gtsam::Pose3
Definition: Pose3.h:37
gtsam.examples.PreintegrationExample.PreintegrationExample.scenario
scenario
Definition: PreintegrationExample.py:57
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam.examples.ImuFactorExample.ImuFactorExample.run
def run(self, int T=12, bool compute_covariances=False, bool verbose=True)
Definition: ImuFactorExample.py:136
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam.examples.ImuFactorExample.parse_args
argparse.Namespace parse_args()
Definition: ImuFactorExample.py:35
gtsam::ImuFactor
Definition: ImuFactor.h:169
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
gtsam.examples.ImuFactorExample.ImuFactorExample.velocity
velocity
Definition: ImuFactorExample.py:58
gtsam.examples.ImuFactorExample.ImuFactorExample.plot
def plot(self, gtsam.Values values, str title="Estimated Trajectory", int fignum=POSES_FIG+1, bool show=False)
Definition: ImuFactorExample.py:105
gtsam::PreintegrationParams::MakeSharedU
static std::shared_ptr< PreintegrationParams > MakeSharedU(double g=9.81)
Definition: PreintegrationParams.h:56
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam.examples.PreintegrationExample.PreintegrationExample
Definition: PreintegrationExample.py:28
gtsam::Values
Definition: Values.h:65
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam.examples.PreintegrationExample.PreintegrationExample.plotGroundTruthPose
def plotGroundTruthPose(self, float t, float scale=0.3, float time_interval=0.01)
Definition: PreintegrationExample.py:129
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam.examples.ImuFactorExample.ImuFactorExample.addPrior
def addPrior(self, int i, gtsam.NonlinearFactorGraph graph)
Definition: ImuFactorExample.py:88
gtsam::utils.plot
Definition: plot.py:1
gtsam.examples.ImuFactorExample.ImuFactorExample.optimize
def optimize(self, gtsam.NonlinearFactorGraph graph, gtsam.Values initial)
Definition: ImuFactorExample.py:96
gtsam.examples.ImuFactorExample.ImuFactorExample
Definition: ImuFactorExample.py:55


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:45