diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index d6e6793f2..dda0638a1 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -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, diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index f38ff7bc9..95a15c077 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -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)