Merge pull request #903 from borglab/feature/python-examples

python/gtsam/examples/CustomFactorExample.py

@@ -9,15 +9,17 @@ CustomFactor demo that simulates a 1-D sensor fusion task.
 Author: Fan Jiang, Frank Dellaert
 """
 
+from functools import partial
+from typing import List, Optional
+
 import gtsam
 import numpy as np
 
-from typing import List, Optional
+I = np.eye(1)
-from functools import partial
 
 
-def simulate_car():
+def simulate_car() -> List[float]:
-    # Simulate a car for one second
+    """Simulate a car for one second"""
     x0 = 0
     dt = 0.25  # 4 Hz, typical GPS
     v = 144 * 1000 / 3600  # 144 km/hour = 90mph, pretty fast
@@ -26,21 +28,87 @@ def simulate_car():
     return x
 
 
+def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor,
+              values: gtsam.Values,
+              jacobians: Optional[List[np.ndarray]]) -> float:
+    """GPS Factor error function
+    :param measurement: GPS measurement, to be filled with `partial`
+    :param this: gtsam.CustomFactor handle
+    :param values: gtsam.Values
+    :param jacobians: Optional list of Jacobians
+    :return: the unwhitened error
+    """
+    key = this.keys()[0]
+    estimate = values.atVector(key)
+    error = estimate - measurement
+    if jacobians is not None:
+        jacobians[0] = I
+
+    return error
+
+
+def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor,
+               values: gtsam.Values,
+               jacobians: Optional[List[np.ndarray]]) -> float:
+    """Odometry Factor error function
+    :param measurement: Odometry measurement, to be filled with `partial`
+    :param this: gtsam.CustomFactor handle
+    :param values: gtsam.Values
+    :param jacobians: Optional list of Jacobians
+    :return: the unwhitened error
+    """
+    key1 = this.keys()[0]
+    key2 = this.keys()[1]
+    pos1, pos2 = values.atVector(key1), values.atVector(key2)
+    error = measurement - (pos1 - pos2)
+    if jacobians is not None:
+        jacobians[0] = I
+        jacobians[1] = -I
+
+    return error
+
+
+def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor,
+             values: gtsam.Values,
+             jacobians: Optional[List[np.ndarray]]) -> float:
+    """Landmark Factor error function
+    :param measurement: Landmark measurement, to be filled with `partial`
+    :param this: gtsam.CustomFactor handle
+    :param values: gtsam.Values
+    :param jacobians: Optional list of Jacobians
+    :return: the unwhitened error
+    """
+    key = this.keys()[0]
+    pos = values.atVector(key)
+    error = pos - measurement
+    if jacobians is not None:
+        jacobians[0] = I
+
+    return error
+
+
+def main():
+    """Main runner."""
+
     x = simulate_car()
     print(f"Simulated car trajectory: {x}")
 
-# %%
     add_noise = True  # set this to False to run with "perfect" measurements
 
     # GPS measurements
     sigma_gps = 3.0  # assume GPS is +/- 3m
-g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
+    g = [
-     for k in range(5)]
+        x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
+        for k in range(5)
+    ]
 
     # Odometry measurements
     sigma_odo = 0.1  # assume Odometry is 10cm accurate at 4Hz
-o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
+    o = [
-     for k in range(4)]
+        x[k + 1] - x[k] +
+        (np.random.normal(scale=sigma_odo) if add_noise else 0)
+        for k in range(4)
+    ]
 
     # Landmark measurements:
     sigma_lm = 1  # assume landmark measurement is accurate up to 1m
@@ -61,30 +129,12 @@ print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
     factor_graph = gtsam.NonlinearFactorGraph()
 
     # Add factors for GPS measurements
-I = np.eye(1)
     gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
 
-
-def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
-    """GPS Factor error function
-    :param measurement: GPS measurement, to be filled with `partial`
-    :param this: gtsam.CustomFactor handle
-    :param values: gtsam.Values
-    :param jacobians: Optional list of Jacobians
-    :return: the unwhitened error
-    """
-    key = this.keys()[0]
-    estimate = values.atVector(key)
-    error = estimate - measurement
-    if jacobians is not None:
-        jacobians[0] = I
-
-    return error
-
-
     # Add the GPS factors
     for k in range(5):
-    gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]])))
+        gf = gtsam.CustomFactor(gps_model, [unknown[k]],
+                                partial(error_gps, np.array([g[k]])))
         factor_graph.add(gf)
 
     # New Values container
@@ -102,36 +152,19 @@ optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
     result = optimizer.optimize()
 
     # calculate the error from ground truth
-error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
+    error = np.array([(result.atVector(unknown[k]) - x[k])[0]
+                      for k in range(5)])
 
     print("Result with only GPS")
-print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
+    print(result, np.round(error, 2),
+          f"\nJ(X)={0.5 * np.sum(np.square(error))}")
 
     # Adding odometry will improve things a lot
     odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
 
-
-def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
-    """Odometry Factor error function
-    :param measurement: Odometry measurement, to be filled with `partial`
-    :param this: gtsam.CustomFactor handle
-    :param values: gtsam.Values
-    :param jacobians: Optional list of Jacobians
-    :return: the unwhitened error
-    """
-    key1 = this.keys()[0]
-    key2 = this.keys()[1]
-    pos1, pos2 = values.atVector(key1), values.atVector(key2)
-    error = measurement - (pos1 - pos2)
-    if jacobians is not None:
-        jacobians[0] = I
-        jacobians[1] = -I
-
-    return error
-
-
     for k in range(4):
-    odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]])))
+        odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]],
+                                  partial(error_odom, np.array([o[k]])))
         factor_graph.add(odof)
 
     params = gtsam.GaussNewtonParams()
@@ -139,41 +172,35 @@ optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
 
     result = optimizer.optimize()
 
-error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
+    error = np.array([(result.atVector(unknown[k]) - x[k])[0]
+                      for k in range(5)])
 
     print("Result with GPS+Odometry")
-print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
+    print(result, np.round(error, 2),
+          f"\nJ(X)={0.5 * np.sum(np.square(error))}")
 
     # This is great, but GPS noise is still apparent, so now we add the two landmarks
     lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
 
-
+    factor_graph.add(
-def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
+        gtsam.CustomFactor(lm_model, [unknown[0]],
-    """Landmark Factor error function
+                           partial(error_lm, np.array([lm_0 + z_0]))))
-    :param measurement: Landmark measurement, to be filled with `partial`
+    factor_graph.add(
-    :param this: gtsam.CustomFactor handle
+        gtsam.CustomFactor(lm_model, [unknown[3]],
-    :param values: gtsam.Values
+                           partial(error_lm, np.array([lm_3 + z_3]))))
-    :param jacobians: Optional list of Jacobians
-    :return: the unwhitened error
-    """
-    key = this.keys()[0]
-    pos = values.atVector(key)
-    error = pos - measurement
-    if jacobians is not None:
-        jacobians[0] = I
-
-    return error
-
-
-factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0]))))
-factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3]))))
 
     params = gtsam.GaussNewtonParams()
     optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
 
     result = optimizer.optimize()
 
-error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
+    error = np.array([(result.atVector(unknown[k]) - x[k])[0]
+                      for k in range(5)])
 
     print("Result with GPS+Odometry+Landmark")
-print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
+    print(result, np.round(error, 2),
+          f"\nJ(X)={0.5 * np.sum(np.square(error))}")
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/GPSFactorExample.py

@@ -13,13 +13,8 @@ Author: Mandy Xie
 
 from __future__ import print_function
 
-import numpy as np
-
 import gtsam
 
-import matplotlib.pyplot as plt
-import gtsam.utils.plot as gtsam_plot
-
 # ENU Origin is where the plane was in hold next to runway
 lat0 = 33.86998
 lon0 = -84.30626
@@ -29,6 +24,9 @@ h0 = 274
 GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
 PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)
 
+
+def main():
+    """Main runner."""
     # Create an empty nonlinear factor graph
     graph = gtsam.NonlinearFactorGraph()
 
@@ -54,3 +52,6 @@ optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
     result = optimizer.optimize()
     print("\nFinal Result:\n{}".format(result))
 
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/OdometryExample.py

@@ -13,17 +13,18 @@ Author: Frank Dellaert
 
 from __future__ import print_function
 
-import numpy as np
-
 import gtsam
-
-import matplotlib.pyplot as plt
 import gtsam.utils.plot as gtsam_plot
+import matplotlib.pyplot as plt
+import numpy as np
 
 # Create noise models
 ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
 PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
 
+
+def main():
+    """Main runner"""
     # Create an empty nonlinear factor graph
     graph = gtsam.NonlinearFactorGraph()
 
@@ -57,13 +58,15 @@ print("\nFinal Result:\n{}".format(result))
     # 5. Calculate and print marginal covariances for all variables
     marginals = gtsam.Marginals(graph, result)
     for i in range(1, 4):
-    print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
+        print("X{} covariance:\n{}\n".format(i,
+                                             marginals.marginalCovariance(i)))
 
-fig = plt.figure(0)
     for i in range(1, 4):
-    gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
+        gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
+                              marginals.marginalCovariance(i))
     plt.axis('equal')
     plt.show()
 
 
-
+if __name__ == "__main__":
+    main()

python/gtsam/examples/PlanarSLAMExample.py

@@ -13,16 +13,19 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
 
 from __future__ import print_function
 
-import numpy as np
-
 import gtsam
-from gtsam.symbol_shorthand import X, L
+import numpy as np
+from gtsam.symbol_shorthand import L, X
 
 # Create noise models
 PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
 ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
 MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
 
+
+def main():
+    """Main runner"""
+
     # Create an empty nonlinear factor graph
     graph = gtsam.NonlinearFactorGraph()
 
@@ -34,21 +37,27 @@ L1 = L(4)
     L2 = L(5)
 
     # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
-graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
+    graph.add(
+        gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
 
     # Add odometry factors between X1,X2 and X2,X3, respectively
-graph.add(gtsam.BetweenFactorPose2(
+    graph.add(
-    X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
+        gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
-graph.add(gtsam.BetweenFactorPose2(
+                                 ODOMETRY_NOISE))
-    X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
+    graph.add(
+        gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
+                                 ODOMETRY_NOISE))
 
     # Add Range-Bearing measurements to two different landmarks L1 and L2
-graph.add(gtsam.BearingRangeFactor2D(
+    graph.add(
-    X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
+        gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
-graph.add(gtsam.BearingRangeFactor2D(
+                                   np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
-    X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
+    graph.add(
-graph.add(gtsam.BearingRangeFactor2D(
+        gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
-    X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
+                                   MEASUREMENT_NOISE))
+    graph.add(
+        gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
+                                   MEASUREMENT_NOISE))
 
     # Print graph
     print("Factor Graph:\n{}".format(graph))
@@ -71,11 +80,18 @@ print("Initial Estimate:\n{}".format(initial_estimate))
     # Here we will use the default set of parameters.  See the
     # documentation for the full set of parameters.
     params = gtsam.LevenbergMarquardtParams()
-optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
+    optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
+                                                  params)
     result = optimizer.optimize()
     print("\nFinal Result:\n{}".format(result))
 
     # Calculate and print marginal covariances for all variables
     marginals = gtsam.Marginals(graph, result)
-for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
+    for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
-    print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))
+                     (L2, "L2")]:
+        print("{} covariance:\n{}\n".format(s,
+                                            marginals.marginalCovariance(key)))
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/Pose2SLAMExample.py

@@ -15,21 +15,17 @@ from __future__ import print_function
 
 import math
 
-import numpy as np
-
 import gtsam
-
-import matplotlib.pyplot as plt
 import gtsam.utils.plot as gtsam_plot
+import matplotlib.pyplot as plt
 
 
-def vector3(x, y, z):
+def main():
-    """Create 3d double numpy array."""
+    """Main runner."""
-    return np.array([x, y, z], dtype=float)
-
     # Create noise models
-PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
+    PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1))
-ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
+    ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(
+        gtsam.Point3(0.2, 0.2, 0.1))
 
     # 1. Create a factor graph container and add factors to it
     graph = gtsam.NonlinearFactorGraph()
@@ -40,20 +36,25 @@ graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
 
     # 2b. Add odometry factors
     # Create odometry (Between) factors between consecutive poses
-graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
+    graph.add(
-graph.add(gtsam.BetweenFactorPose2(
+        gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
-    2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
+    graph.add(
-graph.add(gtsam.BetweenFactorPose2(
+        gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2),
-    3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
+                                 ODOMETRY_NOISE))
-graph.add(gtsam.BetweenFactorPose2(
+    graph.add(
-    4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
+        gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2),
+                                 ODOMETRY_NOISE))
+    graph.add(
+        gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2),
+                                 ODOMETRY_NOISE))
 
     # 2c. Add the loop closure constraint
     # This factor encodes the fact that we have returned to the same pose. In real
     # systems, these constraints may be identified in many ways, such as appearance-based
     # techniques with camera images. We will use another Between Factor to enforce this constraint:
-graph.add(gtsam.BetweenFactorPose2(
+    graph.add(
-    5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
+        gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2),
+                                 ODOMETRY_NOISE))
     print("\nFactor Graph:\n{}".format(graph))  # print
 
     # 3. Create the data structure to hold the initial_estimate estimate to the
@@ -86,11 +87,16 @@ print("Final Result:\n{}".format(result))
     # 5. Calculate and print marginal covariances for all variables
     marginals = gtsam.Marginals(graph, result)
     for i in range(1, 6):
-    print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
+        print("X{} covariance:\n{}\n".format(i,
+                                             marginals.marginalCovariance(i)))
 
-fig = plt.figure(0)
     for i in range(1, 6):
-    gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
+        gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
+                              marginals.marginalCovariance(i))
 
     plt.axis('equal')
     plt.show()
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -12,32 +12,38 @@ and does the optimization. Output is written on a file, in g2o format
 # pylint: disable=invalid-name, E1101
 
 from __future__ import print_function
+
 import argparse
-import math
-import numpy as np
-import matplotlib.pyplot as plt
 
 import gtsam
+import matplotlib.pyplot as plt
 from gtsam.utils import plot
 
 
-def vector3(x, y, z):
+def main():
-    """Create 3d double numpy array."""
+    """Main runner."""
-    return np.array([x, y, z], dtype=float)
-
 
     parser = argparse.ArgumentParser(
         description="A 2D Pose SLAM example that reads input from g2o, "
         "converts it to a factor graph and does the optimization. "
         "Output is written on a file, in g2o format")
     parser.add_argument('-i', '--input', help='input file g2o format')
-parser.add_argument('-o', '--output',
+    parser.add_argument(
+        '-o',
+        '--output',
         help="the path to the output file with optimized graph")
-parser.add_argument('-m', '--maxiter', type=int,
+    parser.add_argument('-m',
+                        '--maxiter',
+                        type=int,
                         help="maximum number of iterations for optimizer")
-parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
+    parser.add_argument('-k',
-                    default="none", help="Type of kernel used")
+                        '--kernel',
-parser.add_argument("-p", "--plot", action="store_true",
+                        choices=['none', 'huber', 'tukey'],
+                        default="none",
+                        help="Type of kernel used")
+    parser.add_argument("-p",
+                        "--plot",
+                        action="store_true",
                         help="Flag to plot results")
     args = parser.parse_args()
 
@@ -53,7 +59,7 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
     assert args.kernel == "none", "Supplied kernel type is not yet implemented"
 
     # Add prior on the pose having index (key) = 0
-priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
+    priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8))
     graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
 
     params = gtsam.GaussNewtonParams()
@@ -69,7 +75,6 @@ print("Optimization complete")
     print("initial error = ", graph.error(initial))
     print("final error = ", graph.error(result))
 
-
     if args.output is None:
         print("\nFactor Graph:\n{}".format(graph))
         print("\nInitial Estimate:\n{}".format(initial))
@@ -86,3 +91,7 @@ if args.plot:
         for i in range(resultPoses.shape[0]):
             plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
         plt.show()
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -8,13 +8,14 @@
 # pylint: disable=invalid-name, E1101
 
 from __future__ import print_function
+
 import argparse
-import numpy as np
-import matplotlib.pyplot as plt
-from mpl_toolkits.mplot3d import Axes3D
 
 import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
 from gtsam.utils import plot
+from mpl_toolkits.mplot3d import Axes3D
 
 
 def vector6(x, y, z, a, b, c):
@@ -22,13 +23,20 @@ def vector6(x, y, z, a, b, c):
     return np.array([x, y, z, a, b, c], dtype=float)
 
 
+def main():
+    """Main runner."""
+
     parser = argparse.ArgumentParser(
         description="A 3D Pose SLAM example that reads input from g2o, and "
         "initializes Pose3")
     parser.add_argument('-i', '--input', help='input file g2o format')
-parser.add_argument('-o', '--output',
+    parser.add_argument(
+        '-o',
+        '--output',
         help="the path to the output file with optimized graph")
-parser.add_argument("-p", "--plot", action="store_true",
+    parser.add_argument("-p",
+                        "--plot",
+                        action="store_true",
                         help="Flag to plot results")
     args = parser.parse_args()
 
@@ -39,15 +47,16 @@ is3D = True
     graph, initial = gtsam.readG2o(g2oFile, is3D)
 
     # Add Prior on the first key
-priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
+    priorModel = gtsam.noiseModel.Diagonal.Variances(
-                                                         1e-4, 1e-4, 1e-4))
+        vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
 
     print("Adding prior to g2o file ")
     firstKey = initial.keys()[0]
     graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
     params = gtsam.GaussNewtonParams()
-params.setVerbosity("Termination")  # this will show info about stopping conds
+    params.setVerbosity(
+        "Termination")  # this will show info about stopping conds
     optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
     result = optimizer.optimize()
     print("Optimization complete")
@@ -69,3 +78,7 @@ if args.plot:
         for i in range(resultPoses.size()):
             plot.plot_pose3(1, resultPoses.atPose3(i))
         plt.show()
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py

@@ -13,10 +13,12 @@ Author: Luca Carlone, Frank Dellaert (python port)
 
 from __future__ import print_function
 
+import gtsam
 import numpy as np
 
-import gtsam
 
+def main():
+    """Main runner."""
     # Read graph from file
     g2oFile = gtsam.findExampleDataFile("pose3example.txt")
 
@@ -29,7 +31,11 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(
     firstKey = initial.keys()[0]
     graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
 
-# Initializing Pose3 - chordal relaxation"
+    # Initializing Pose3 - chordal relaxation
     initialization = gtsam.InitializePose3.initialize(graph)
 
     print(initialization)
+
+
+if __name__ == "__main__":
+    main()