12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

address review comments

release/4.3a0
Varun Agrawal 2021-10-27 09:58:07 -04:00
parent 19850425b8
commit e4a2af5f3f
2 changed files with 51 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
python/gtsam/examples/ImuFactorExample.py
View File

@ -31,9 +31,28 @@ BIAS_KEY = B(0)
np.set_printoptions(precision=3, suppress=True)
def parse_args():
"""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 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()
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)
@ -65,7 +84,7 @@ class ImuFactorExample(PreintegrationExample):
super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
bias, params, dt)
def addPrior(self, i, graph):
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(
@ -73,7 +92,8 @@ class ImuFactorExample(PreintegrationExample):
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")
@ -82,10 +102,10 @@ class ImuFactorExample(PreintegrationExample):
return result
def plot(self,
values,
title="Estimated Trajectory",
fignum=POSES_FIG + 1,
show=False):
values: gtsam.Values,
title: str = "Estimated Trajectory",
fignum: int = POSES_FIG + 1,
show: bool = False):
"""Plot poses in values."""
i = 0
while values.exists(X(i)):
@ -103,7 +123,10 @@ class ImuFactorExample(PreintegrationExample):
if show:
plt.show()
def run(self, T=12, compute_covariances=False, verbose=True):
def run(self,
T: int = 12,
compute_covariances: bool = False,
verbose: bool = True):
"""Main runner."""
graph = gtsam.NonlinearFactorGraph()
@ -193,21 +216,7 @@ class ImuFactorExample(PreintegrationExample):
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,

30
python/gtsam/examples/PreintegrationExample.py
View File

@ -12,7 +12,7 @@ Authors: Frank Dellaert, Varun Agrawal.
# pylint: disable=invalid-name,unused-import,wrong-import-order
import math
from typing import Sequence
import gtsam
import matplotlib.pyplot as plt
@ -24,13 +24,13 @@ IMU_FIG = 1
POSES_FIG = 2
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 *
@ -38,7 +38,11 @@ class PreintegrationExample(object):
params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float))
return params
def __init__(self, twist=None, bias=None, params=None, dt=1e-2):
def __init__(self,
twist: np.ndarray = None,
bias: gtsam.imuBias.ConstantBias = None,
params: gtsam.PreintegrationParams = None,
dt: float = 1e-2):
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
# setup interactive plotting
@ -50,7 +54,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)
@ -80,7 +84,8 @@ class PreintegrationExample(object):
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."""
plt.figure(IMU_FIG)
@ -114,12 +119,15 @@ 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):
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."""
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)
@ -127,7 +135,7 @@ class PreintegrationExample(object):
plt.pause(time_interval)
def run(self, T=12):
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)