Merge pull request #872 from borglab/fix/imu-examples
Varun Agrawal
GitHub
commit
41dc3f876b
|
|
@ -88,6 +88,8 @@ virtual class PreintegratedRotationParams {
|
|||
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
|
||||
PreintegrationParams(Vector n_gravity);
|
||||
|
||||
gtsam::Vector n_gravity;
|
||||
|
||||
static gtsam::PreintegrationParams* MakeSharedD(double g);
|
||||
static gtsam::PreintegrationParams* MakeSharedU(double g);
|
||||
static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81
|
||||
|
|
|
|||
|
|
@ -10,31 +10,51 @@ A script validating and demonstrating the ImuFactor inference.
|
|||
Author: Frank Dellaert, Varun Agrawal
|
||||
"""
|
||||
|
||||
# pylint: disable=no-name-in-module,unused-import,arguments-differ
|
||||
# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import math
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
import gtsam
|
||||
from gtsam.symbol_shorthand import B, V, X
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
from PreintegrationExample import POSES_FIG, PreintegrationExample
|
||||
|
||||
BIAS_KEY = B(0)
|
||||
GRAVITY = 9.81
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
"""Parse command line arguments."""
|
||||
parser = argparse.ArgumentParser("ImuFactorExample.py")
|
||||
parser.add_argument("--twist_scenario",
|
||||
default="sick_twist",
|
||||
choices=("zero_twist", "forward_twist", "loop_twist",
|
||||
"sick_twist"))
|
||||
parser.add_argument("--time",
|
||||
"-T",
|
||||
default=12,
|
||||
type=int,
|
||||
help="Total navigation time in seconds")
|
||||
parser.add_argument("--compute_covariances",
|
||||
default=False,
|
||||
action='store_true')
|
||||
parser.add_argument("--verbose", default=False, action='store_true')
|
||||
args = parser.parse_args()
|
||||
return args
|
||||
|
||||
|
||||
class ImuFactorExample(PreintegrationExample):
|
||||
"""Class to run example of the Imu Factor."""
|
||||
def __init__(self, twist_scenario="sick_twist"):
|
||||
def __init__(self, twist_scenario: str = "sick_twist"):
|
||||
self.velocity = np.array([2, 0, 0])
|
||||
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
|
|
@ -51,19 +71,30 @@ class ImuFactorExample(PreintegrationExample):
|
|||
gyroBias = np.array([0.1, 0.3, -0.1])
|
||||
bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
|
||||
|
||||
params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY)
|
||||
|
||||
# Some arbitrary noise sigmas
|
||||
gyro_sigma = 1e-3
|
||||
accel_sigma = 1e-3
|
||||
I_3x3 = np.eye(3)
|
||||
params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
|
||||
params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
|
||||
params.setIntegrationCovariance(1e-7**2 * I_3x3)
|
||||
|
||||
dt = 1e-2
|
||||
super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
|
||||
bias, dt)
|
||||
bias, params, dt)
|
||||
|
||||
def addPrior(self, i, graph):
|
||||
"""Add priors at time step `i`."""
|
||||
def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
|
||||
"""Add a prior on the navigation state at time `i`."""
|
||||
state = self.scenario.navState(i)
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
|
||||
|
||||
def optimize(self, graph, initial):
|
||||
def optimize(self, graph: gtsam.NonlinearFactorGraph,
|
||||
initial: gtsam.Values):
|
||||
"""Optimize using Levenberg-Marquardt optimization."""
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
params.setVerbosityLM("SUMMARY")
|
||||
|
|
@ -71,24 +102,49 @@ class ImuFactorExample(PreintegrationExample):
|
|||
result = optimizer.optimize()
|
||||
return result
|
||||
|
||||
def plot(self, result):
|
||||
"""Plot resulting poses."""
|
||||
def plot(self,
|
||||
values: gtsam.Values,
|
||||
title: str = "Estimated Trajectory",
|
||||
fignum: int = POSES_FIG + 1,
|
||||
show: bool = False):
|
||||
"""
|
||||
Plot poses in values.
|
||||
|
||||
Args:
|
||||
values: The values object with the poses to plot.
|
||||
title: The title of the plot.
|
||||
fignum: The matplotlib figure number.
|
||||
POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure.
|
||||
show: Flag indicating whether to display the figure.
|
||||
"""
|
||||
i = 0
|
||||
while result.exists(X(i)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
plot_pose3(POSES_FIG + 1, pose_i, 1)
|
||||
while values.exists(X(i)):
|
||||
pose_i = values.atPose3(X(i))
|
||||
plot_pose3(fignum, pose_i, 1)
|
||||
i += 1
|
||||
plt.title("Estimated Trajectory")
|
||||
plt.title(title)
|
||||
|
||||
gtsam.utils.plot.set_axes_equal(POSES_FIG + 1)
|
||||
gtsam.utils.plot.set_axes_equal(fignum)
|
||||
|
||||
print("Bias Values", result.atConstantBias(BIAS_KEY))
|
||||
print("Bias Values", values.atConstantBias(BIAS_KEY))
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
def run(self, T=12, compute_covariances=False, verbose=True):
|
||||
"""Main runner."""
|
||||
if show:
|
||||
plt.show()
|
||||
|
||||
def run(self,
|
||||
T: int = 12,
|
||||
compute_covariances: bool = False,
|
||||
verbose: bool = True):
|
||||
"""
|
||||
Main runner.
|
||||
|
||||
Args:
|
||||
T: Total trajectory time.
|
||||
compute_covariances: Flag indicating whether to compute marginal covariances.
|
||||
verbose: Flag indicating if printing should be verbose.
|
||||
"""
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# initialize data structure for pre-integrated IMU measurements
|
||||
|
|
@ -173,25 +229,11 @@ class ImuFactorExample(PreintegrationExample):
|
|||
print("Covariance on vel {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(V(i))))
|
||||
|
||||
self.plot(result)
|
||||
self.plot(result, show=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser("ImuFactorExample.py")
|
||||
parser.add_argument("--twist_scenario",
|
||||
default="sick_twist",
|
||||
choices=("zero_twist", "forward_twist", "loop_twist",
|
||||
"sick_twist"))
|
||||
parser.add_argument("--time",
|
||||
"-T",
|
||||
default=12,
|
||||
type=int,
|
||||
help="Total time in seconds")
|
||||
parser.add_argument("--compute_covariances",
|
||||
default=False,
|
||||
action='store_true')
|
||||
parser.add_argument("--verbose", default=False, action='store_true')
|
||||
args = parser.parse_args()
|
||||
args = parse_args()
|
||||
|
||||
ImuFactorExample(args.twist_scenario).run(args.time,
|
||||
args.compute_covariances,
|
||||
|
|
|
|||
|
|
@ -5,10 +5,14 @@ All Rights Reserved
|
|||
|
||||
See LICENSE for the license information
|
||||
|
||||
A script validating the Preintegration of IMU measurements
|
||||
A script validating the Preintegration of IMU measurements.
|
||||
|
||||
Authors: Frank Dellaert, Varun Agrawal.
|
||||
"""
|
||||
|
||||
import math
|
||||
# pylint: disable=invalid-name,unused-import,wrong-import-order
|
||||
|
||||
from typing import Optional, Sequence
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
|
|
@ -18,25 +22,28 @@ from mpl_toolkits.mplot3d import Axes3D
|
|||
|
||||
IMU_FIG = 1
|
||||
POSES_FIG = 2
|
||||
GRAVITY = 10
|
||||
|
||||
|
||||
class PreintegrationExample(object):
|
||||
|
||||
class PreintegrationExample:
|
||||
"""Base class for all preintegration examples."""
|
||||
@staticmethod
|
||||
def defaultParams(g):
|
||||
def defaultParams(g: float):
|
||||
"""Create default parameters with Z *up* and realistic noise parameters"""
|
||||
params = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||
kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
|
||||
kGyroSigma = np.radians(0.5) / 60 # 0.5 degree ARW
|
||||
kAccelSigma = 0.1 / 60 # 10 cm VRW
|
||||
params.setGyroscopeCovariance(
|
||||
kGyroSigma ** 2 * np.identity(3, float))
|
||||
params.setAccelerometerCovariance(
|
||||
kAccelSigma ** 2 * np.identity(3, float))
|
||||
params.setIntegrationCovariance(
|
||||
0.0000001 ** 2 * np.identity(3, float))
|
||||
params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float))
|
||||
params.setAccelerometerCovariance(kAccelSigma**2 *
|
||||
np.identity(3, float))
|
||||
params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float))
|
||||
return params
|
||||
|
||||
def __init__(self, twist=None, bias=None, dt=1e-2):
|
||||
def __init__(self,
|
||||
twist: Optional[np.ndarray] = None,
|
||||
bias: Optional[gtsam.imuBias.ConstantBias] = None,
|
||||
params: Optional[gtsam.PreintegrationParams] = None,
|
||||
dt: float = 1e-2):
|
||||
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
|
||||
|
||||
# setup interactive plotting
|
||||
|
|
@ -48,7 +55,7 @@ class PreintegrationExample(object):
|
|||
else:
|
||||
# default = loop with forward velocity 2m/s, while pitching up
|
||||
# with angular velocity 30 degree/sec (negative in FLU)
|
||||
W = np.array([0, -math.radians(30), 0])
|
||||
W = np.array([0, -np.radians(30), 0])
|
||||
V = np.array([2, 0, 0])
|
||||
|
||||
self.scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
|
|
@ -58,9 +65,11 @@ class PreintegrationExample(object):
|
|||
self.labels = list('xyz')
|
||||
self.colors = list('rgb')
|
||||
|
||||
# Create runner
|
||||
self.g = 10 # simple gravity constant
|
||||
self.params = self.defaultParams(self.g)
|
||||
if params:
|
||||
self.params = params
|
||||
else:
|
||||
# Default params with simple gravity constant
|
||||
self.params = self.defaultParams(g=GRAVITY)
|
||||
|
||||
if bias is not None:
|
||||
self.actualBias = bias
|
||||
|
|
@ -69,13 +78,22 @@ class PreintegrationExample(object):
|
|||
gyroBias = np.array([0, 0, 0])
|
||||
self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
|
||||
|
||||
self.runner = gtsam.ScenarioRunner(
|
||||
self.scenario, self.params, self.dt, self.actualBias)
|
||||
# Create runner
|
||||
self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt,
|
||||
self.actualBias)
|
||||
|
||||
fig, self.axes = plt.subplots(4, 3)
|
||||
fig.set_tight_layout(True)
|
||||
|
||||
def plotImu(self, t, measuredOmega, measuredAcc):
|
||||
def plotImu(self, t: float, measuredOmega: Sequence,
|
||||
measuredAcc: Sequence):
|
||||
"""
|
||||
Plot IMU measurements.
|
||||
Args:
|
||||
t: The time at which the measurement was recoreded.
|
||||
measuredOmega: Measured angular velocity.
|
||||
measuredAcc: Measured linear acceleration.
|
||||
"""
|
||||
plt.figure(IMU_FIG)
|
||||
|
||||
# plot angular velocity
|
||||
|
|
@ -108,12 +126,21 @@ class PreintegrationExample(object):
|
|||
ax.scatter(t, measuredAcc[i], color=color, marker='.')
|
||||
ax.set_xlabel('specific force ' + label)
|
||||
|
||||
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
|
||||
# plot ground truth pose, as well as prediction from integrated IMU measurements
|
||||
def plotGroundTruthPose(self,
|
||||
t: float,
|
||||
scale: float = 0.3,
|
||||
time_interval: float = 0.01):
|
||||
"""
|
||||
Plot ground truth pose, as well as prediction from integrated IMU measurements.
|
||||
Args:
|
||||
t: Time at which the pose was obtained.
|
||||
scale: The scaling factor for the pose axes.
|
||||
time_interval: The time to wait before showing the plot.
|
||||
"""
|
||||
actualPose = self.scenario.pose(t)
|
||||
plot_pose3(POSES_FIG, actualPose, scale)
|
||||
t = actualPose.translation()
|
||||
self.maxDim = max([max(np.abs(t)), self.maxDim])
|
||||
translation = actualPose.translation()
|
||||
self.maxDim = max([max(np.abs(translation)), self.maxDim])
|
||||
ax = plt.gca()
|
||||
ax.set_xlim3d(-self.maxDim, self.maxDim)
|
||||
ax.set_ylim3d(-self.maxDim, self.maxDim)
|
||||
|
|
@ -121,8 +148,8 @@ class PreintegrationExample(object):
|
|||
|
||||
plt.pause(time_interval)
|
||||
|
||||
def run(self, T=12):
|
||||
# simulate the loop
|
||||
def run(self, T: int = 12):
|
||||
"""Simulate the loop."""
|
||||
for i, t in enumerate(np.arange(0, T, self.dt)):
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||
|
|
|
|||
Loading…
Reference in New Issue