diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index 03b16098d..f9ff44738 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -15,14 +15,12 @@ from __future__ import print_function import argparse import math -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -import numpy as np - import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam.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 diff --git a/python/gtsam_py/python/examples/ImuFactorISAM2Example.py b/python/gtsam_py/python/examples/ImuFactorISAM2Example.py index 969d4fe6b..6556eb554 100644 --- a/python/gtsam_py/python/examples/ImuFactorISAM2Example.py +++ b/python/gtsam_py/python/examples/ImuFactorISAM2Example.py @@ -8,11 +8,9 @@ from __future__ import print_function import math -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 -import numpy as np - import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, @@ -20,6 +18,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PriorFactorVector, Rot3, Values) from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.utils import plot +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def vector3(x, y, z): diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py index e35722571..e308bc933 100644 --- a/python/gtsam_py/python/examples/PreintegrationExample.py +++ b/python/gtsam_py/python/examples/PreintegrationExample.py @@ -10,12 +10,11 @@ A script validating the Preintegration of IMU measurements import math +import gtsam import matplotlib.pyplot as plt import numpy as np -from mpl_toolkits.mplot3d import Axes3D - -import gtsam from gtsam.utils.plot import plot_pose3 +from mpl_toolkits.mplot3d import Axes3D IMU_FIG = 1 POSES_FIG = 2 diff --git a/python/gtsam_py/python/examples/VisualISAMExample.py b/python/gtsam_py/python/examples/VisualISAMExample.py index 2e90ce21b..7d562c9b4 100644 --- a/python/gtsam_py/python/examples/VisualISAMExample.py +++ b/python/gtsam_py/python/examples/VisualISAMExample.py @@ -16,9 +16,9 @@ import numpy as np import gtsam from gtsam.examples import SFMdata from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, - NonlinearFactorGraph, NonlinearISAM, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values, Point3) + NonlinearFactorGraph, NonlinearISAM, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, + PinholeCameraCal3_S2, Values, Point3) def symbol(name: str, index: int) -> int: @@ -37,7 +37,8 @@ def main(): K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model - camera_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + camera_noise = gtsam.noiseModel.Isotropic.Sigma( + 2, 1.0) # one pixel in u and v # Create the set of ground-truth landmarks points = SFMdata.createPoints() @@ -54,15 +55,17 @@ def main(): # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) # Add factors for each landmark observation for j, point in enumerate(points): measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K) + factor = GenericProjectionFactorCal3_S2( + measurement, camera_noise, symbol('x', i), symbol('l', j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth - noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) + noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), + t=Point3(0.05, -0.10, 0.20)) initial_xi = pose.compose(noise) # Add an initial guess for the current pose @@ -74,7 +77,8 @@ def main(): # adding it to iSAM. if i == 0: # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + pose_noise = gtsam.noiseModel.Diagonal.Sigmas( + np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) graph.push_back(factor)