From 19850425b8e4e3b30c8d4a7ea21974ed83484d8c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:02:14 -0400 Subject: [PATCH 1/6] clean up and refactoring to use custom preintegration params --- gtsam/navigation/navigation.i | 2 + python/gtsam/examples/ImuFactorExample.py | 48 ++++++++++++------- .../gtsam/examples/PreintegrationExample.py | 38 ++++++++------- 3 files changed, 56 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 48a5a35de..7a879c3ef 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -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 diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 86613234d..d6e6793f2 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,20 +10,19 @@ 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 @@ -51,12 +50,23 @@ class ImuFactorExample(PreintegrationExample): gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) + g = 9.81 + params = gtsam.PreintegrationParams.MakeSharedU(g) + + # 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`.""" + """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)) @@ -71,21 +81,27 @@ class ImuFactorExample(PreintegrationExample): result = optimizer.optimize() return result - def plot(self, result): - """Plot resulting poses.""" + def plot(self, + values, + title="Estimated Trajectory", + fignum=POSES_FIG + 1, + show=False): + """Plot poses in values.""" 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() + + if show: + plt.show() def run(self, T=12, compute_covariances=False, verbose=True): """Main runner.""" @@ -173,7 +189,7 @@ 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__': diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 458248c30..f38ff7bc9 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -5,9 +5,13 @@ 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. """ +# pylint: disable=invalid-name,unused-import,wrong-import-order + import math import gtsam @@ -21,22 +25,20 @@ POSES_FIG = 2 class PreintegrationExample(object): - + """Base class for all preintegration examples.""" @staticmethod def defaultParams(g): """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 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=None, bias=None, params=None, dt=1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -58,9 +60,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=10) if bias is not None: self.actualBias = bias @@ -69,13 +73,15 @@ 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): + """Plot IMU measurements.""" plt.figure(IMU_FIG) # plot angular velocity @@ -109,7 +115,7 @@ class PreintegrationExample(object): 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 + """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() @@ -122,7 +128,7 @@ class PreintegrationExample(object): plt.pause(time_interval) def run(self, T=12): - # simulate the loop + """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) From e4a2af5f3fd8ff206b8c8963b67b61adc04e08fb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 09:58:07 -0400 Subject: [PATCH 2/6] address review comments --- python/gtsam/examples/ImuFactorExample.py | 55 +++++++++++-------- .../gtsam/examples/PreintegrationExample.py | 30 ++++++---- 2 files changed, 51 insertions(+), 34 deletions(-) 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) From 755484c5798e0d41bd225a2978e57b4dec171cab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 10:01:50 -0400 Subject: [PATCH 3/6] fix small bug --- python/gtsam/examples/ImuFactorExample.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index dda0638a1..5b4b06b55 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -31,7 +31,7 @@ BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) -def parse_args(): +def parse_args() -> argparse.Namespace: """Parse command line arguments.""" parser = argparse.ArgumentParser("ImuFactorExample.py") parser.add_argument("--twist_scenario", @@ -48,6 +48,7 @@ def parse_args(): action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() + return args class ImuFactorExample(PreintegrationExample): From 15e57c7ec8c5d8ada86346bd46e9a8ec82ff76b7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 10:03:31 -0400 Subject: [PATCH 4/6] specify optional args as Optional type --- python/gtsam/examples/PreintegrationExample.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 95a15c077..d3c0f79b8 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 -from typing import Sequence +from typing import Optional, Sequence import gtsam import matplotlib.pyplot as plt @@ -39,9 +39,9 @@ class PreintegrationExample: return params def __init__(self, - twist: np.ndarray = None, - bias: gtsam.imuBias.ConstantBias = None, - params: gtsam.PreintegrationParams = None, + 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).""" From d98e6e500a20d72d9920a2b016bfea108fc1d3ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Oct 2021 09:51:31 -0400 Subject: [PATCH 5/6] address review comments --- python/gtsam/examples/ImuFactorExample.py | 17 +++++++++++++---- .../gtsam/examples/PreintegrationExample.py | 19 ++++++++++++++++--- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 5b4b06b55..13ed24c6c 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -27,6 +27,7 @@ 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) @@ -42,7 +43,7 @@ def parse_args() -> argparse.Namespace: "-T", default=12, type=int, - help="Total time in seconds") + help="Total navigation time in seconds") parser.add_argument("--compute_covariances", default=False, action='store_true') @@ -70,8 +71,7 @@ class ImuFactorExample(PreintegrationExample): gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - g = 9.81 - params = gtsam.PreintegrationParams.MakeSharedU(g) + params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) # Some arbitrary noise sigmas gyro_sigma = 1e-3 @@ -107,7 +107,16 @@ class ImuFactorExample(PreintegrationExample): title: str = "Estimated Trajectory", fignum: int = POSES_FIG + 1, show: bool = False): - """Plot poses in values.""" + """ + 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 values.exists(X(i)): pose_i = values.atPose3(X(i)) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index d3c0f79b8..611c536c7 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -22,6 +22,7 @@ from mpl_toolkits.mplot3d import Axes3D IMU_FIG = 1 POSES_FIG = 2 +GRAVITY = 10 class PreintegrationExample: @@ -68,7 +69,7 @@ class PreintegrationExample: self.params = params else: # Default params with simple gravity constant - self.params = self.defaultParams(g=10) + self.params = self.defaultParams(g=GRAVITY) if bias is not None: self.actualBias = bias @@ -86,7 +87,13 @@ class PreintegrationExample: def plotImu(self, t: float, measuredOmega: Sequence, measuredAcc: Sequence): - """Plot IMU measurements.""" + """ + 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 @@ -123,7 +130,13 @@ class PreintegrationExample: t: float, scale: float = 0.3, time_interval: float = 0.01): - """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" + """ + 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) translation = actualPose.translation() From b15297ae4096c9f8f0e745a8dd3b4a4505ae63db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Oct 2021 15:19:36 -0400 Subject: [PATCH 6/6] address review comments --- python/gtsam/examples/ImuFactorExample.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 13ed24c6c..c86a4e216 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -137,7 +137,14 @@ class ImuFactorExample(PreintegrationExample): T: int = 12, compute_covariances: bool = False, verbose: bool = True): - """Main runner.""" + """ + 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