2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 A script validating and demonstrating the ImuFactor inference.
10 Author: Frank Dellaert, Varun Agrawal
15 from __future__
import print_function
20 import matplotlib.pyplot
as plt
24 from mpl_toolkits.mplot3d
import Axes3D
25 from PreintegrationExample
import POSES_FIG, PreintegrationExample
32 np.set_printoptions(precision=3, suppress=
True)
36 """Parse command line arguments."""
37 parser = argparse.ArgumentParser(
"ImuFactorExample.py")
38 parser.add_argument(
"--twist_scenario",
40 choices=(
"zero_twist",
"forward_twist",
"loop_twist",
42 parser.add_argument(
"--time",
46 help=
"Total navigation time in seconds")
47 parser.add_argument(
"--compute_covariances",
50 parser.add_argument(
"--verbose", default=
False, action=
'store_true')
51 args = parser.parse_args()
56 """Class to run example of the Imu Factor."""
57 def __init__(self, twist_scenario: str =
"sick_twist"):
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),
70 accBias = np.array([-0.3, 0.1, 0.2])
71 gyroBias = np.array([0.1, 0.3, -0.1])
80 params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
81 params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
82 params.setIntegrationCovariance(1e-7**2 * I_3x3)
85 super(ImuFactorExample, self).
__init__(twist_scenarios[twist_scenario],
89 """Add a prior on the navigation state at time `i`."""
92 gtsam.PriorFactorPose3(
X(i), state.pose(), self.
priorNoise))
94 gtsam.PriorFactorVector(
V(i), state.velocity(), self.
velNoise))
96 def optimize(self, graph: gtsam.NonlinearFactorGraph,
98 """Optimize using Levenberg-Marquardt optimization."""
100 params.setVerbosityLM(
"SUMMARY")
102 result = optimizer.optimize()
106 values: gtsam.Values,
107 title: str =
"Estimated Trajectory",
108 fignum: int = POSES_FIG + 1,
111 Plot poses in values.
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.
121 while values.exists(
X(i)):
122 pose_i = values.atPose3(
X(i))
129 print(
"Bias Values", values.atConstantBias(BIAS_KEY))
138 compute_covariances: bool =
False,
139 verbose: bool =
True):
144 T: Total trajectory time.
145 compute_covariances: Flag indicating whether to compute marginal covariances.
146 verbose: Flag indicating if printing should be verbose.
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())
166 for k, t
in enumerate(np.arange(0, T, self.
dt)):
168 measuredOmega = self.
runner.measuredAngularVelocity(t)
169 measuredAcc = self.
runner.measuredSpecificForce(t)
170 pim.integrateMeasurement(measuredAcc, measuredOmega, self.
dt)
174 self.
plotImu(t, measuredOmega, measuredAcc)
176 if (k + 1) %
int(1 / self.
dt) == 0:
179 plt.title(
"Ground Truth Trajectory")
184 graph.push_back(factor)
190 pim.predict(initial_state_i, self.
actualBias)))
192 pim.resetIntegration()
195 translationNoise =
gtsam.Point3(*np.random.randn(3) * 1)
196 poseNoise =
gtsam.Pose3(rotationNoise, translationNoise)
198 actual_state_i = self.
scenario.navState(t + self.
dt)
200 t + self.
dt, actual_state_i))
203 initial_state_i = actual_state_i
206 actual_state_i.pose().
compose(poseNoise),
207 actual_state_i.velocity() + np.random.randn(3) * 0.1)
209 initial.insert(
X(i + 1), noisy_state_i.pose())
210 initial.insert(
V(i + 1), noisy_state_i.velocity())
216 initial.print(
"Initial values:")
218 result = self.
optimize(graph, initial)
220 result.print(
"Optimized values:")
221 print(
"------------------")
222 print(graph.error(initial))
223 print(graph.error(result))
224 print(
"------------------")
226 if compute_covariances:
229 print(
"Covariance on bias:\n",
230 marginals.marginalCovariance(BIAS_KEY))
231 for i
in range(num_poses):
233 i, marginals.marginalCovariance(
X(i))))
235 i, marginals.marginalCovariance(
V(i))))
237 self.
plot(result, show=
True)
240 if __name__ ==
'__main__':
244 args.compute_covariances,