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()