Switched back to gtsam. pattern, added pylint exceptions

cython/gtsam/examples/OdometryExample.py

@@ -1,26 +1,40 @@
-#!/usr/bin/env python
+"""
+GTSAM Copyright 2010, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Simple robot motion example, with prior and two odometry measurements
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, E1101
+
 from __future__ import print_function
 
 import numpy as np
 
 import gtsam
 
+# 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]))
+
 # Create an empty nonlinear factor graph
 graph = gtsam.NonlinearFactorGraph()
 
 # Add a prior on the first pose, setting it to the origin
 # A prior factor consists of a mean and a noise model (covariance matrix)
 priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
-priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
-graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
 
 # Add odometry factors
 odometry = gtsam.Pose2(2.0, 0.0, 0.0)
 # For simplicity, we will use the same noise model for each odometry factor
-odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
 # Create odometry (Between) factors between consecutive poses
-graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
+graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
-graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
+graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
 graph.print_("\nFactor Graph:\n")
 
 # Create the data structure to hold the initialEstimate estimate to the solution

cython/gtsam/examples/PlanarSLAMExample.py

@@ -9,54 +9,56 @@ See LICENSE for the license information
 Simple robotics example using odometry measurements and bearing-range (laser) measurements
 Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
 """
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
 
 import numpy as np
 
-from gtsam import (BearingRangeFactor2D, BetweenFactorPose2,
+import gtsam
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   Marginals, NonlinearFactorGraph, Point2, Pose2,
-                   PriorFactorPose2, Rot2, Values, noiseModel_Diagonal, symbol)
 
 # Create noise models
-PRIOR_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
-ODOMETRY_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
-MEASUREMENT_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
+MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
 
 # Create an empty nonlinear factor graph
-graph = NonlinearFactorGraph()
+graph = gtsam.NonlinearFactorGraph()
 
 # Create the keys corresponding to unknown variables in the factor graph
-X1 = symbol(ord('x'), 1)
+X1 = gtsam.symbol(ord('x'), 1)
-X2 = symbol(ord('x'), 2)
+X2 = gtsam.symbol(ord('x'), 2)
-X3 = symbol(ord('x'), 3)
+X3 = gtsam.symbol(ord('x'), 3)
-L1 = symbol(ord('l'), 4)
+L1 = gtsam.symbol(ord('l'), 4)
-L2 = symbol(ord('l'), 5)
+L2 = gtsam.symbol(ord('l'), 5)
 
 # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
-graph.add(PriorFactorPose2(X1, 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(BetweenFactorPose2(X1, X2, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
+graph.add(gtsam.BetweenFactorPose2(
-graph.add(BetweenFactorPose2(X2, X3, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
+    X1, X2, 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(BearingRangeFactor2D(
+graph.add(gtsam.BearingRangeFactor2D(
-    X1, L1, Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
+    X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
-graph.add(BearingRangeFactor2D(
+graph.add(gtsam.BearingRangeFactor2D(
-    X2, L1, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
+    X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
-graph.add(BearingRangeFactor2D(
+graph.add(gtsam.BearingRangeFactor2D(
-    X3, L2, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
+    X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
 
 # Print graph
 graph.print_("Factor Graph:\n")
 
 # Create (deliberately inaccurate) initial estimate
-initial_estimate = Values()
+initial_estimate = gtsam.Values()
-initial_estimate.insert(X1, Pose2(-0.25, 0.20, 0.15))
+initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
-initial_estimate.insert(X2, Pose2(2.30, 0.10, -0.20))
+initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
-initial_estimate.insert(X3, Pose2(4.10, 0.10, 0.10))
+initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
-initial_estimate.insert(L1, Point2(1.80, 2.10))
+initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
-initial_estimate.insert(L2, Point2(4.10, 1.80))
+initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
 
 # Print
 initial_estimate.print_("Initial Estimate:\n")
@@ -67,12 +69,12 @@ initial_estimate.print_("Initial Estimate:\n")
 # to use, and the amount of information displayed during optimization.
 # Here we will use the default set of parameters.  See the
 # documentation for the full set of parameters.
-params = LevenbergMarquardtParams()
+params = gtsam.LevenbergMarquardtParams()
-optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
+optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
 result = optimizer.optimize()
 result.print_("\nFinal Result:\n")
 
 # Calculate and print marginal covariances for all variables
-marginals = Marginals(graph, result)
+marginals = gtsam.Marginals(graph, result)
-for (key, str) in [(X1,"X1"),(X2,"X2"),(X3,"X3"),(L1,"L1"),(L2,"L2")]:
+for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
-    print("{} covariance:\n{}\n".format(str,marginals.marginalCovariance(key)))
+    print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))

cython/gtsam/examples/Pose2SLAMExample.py

@@ -1,3 +1,16 @@
+"""
+GTSAM Copyright 2010, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Simple robotics example using odometry measurements and bearing-range (laser) measurements
+Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
 from __future__ import print_function
 
 import math
@@ -7,36 +20,38 @@ import numpy as np
 import gtsam
 
 
-def Vector3(x, y, z): return np.array([x, y, z])
+def Vector3(x, y, z):
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=np.float)
 
 
+# Create noise models
+PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1))
+ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1))
+
 # 1. Create a factor graph container and add factors to it
 graph = gtsam.NonlinearFactorGraph()
 
 # 2a. Add a prior on the first pose, setting it to the origin
-# A prior factor consists of a mean and a noise model (covariance matrix)
+# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
-priorNoise = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1))
+graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
-graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise))
-
-# For simplicity, we will use the same noise model for odometry and loop closures
-model = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1))
 
 # 2b. Add odometry factors
 # Create odometry (Between) factors between consecutive poses
-graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model))
+graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
 graph.add(gtsam.BetweenFactorPose2(
-    2, 3, gtsam.Pose2(2, 0, math.pi / 2), model))
+    2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
 graph.add(gtsam.BetweenFactorPose2(
-    3, 4, gtsam.Pose2(2, 0, math.pi / 2), model))
+    3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
 graph.add(gtsam.BetweenFactorPose2(
-    4, 5, gtsam.Pose2(2, 0, math.pi / 2), model))
+    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(
-    5, 2, gtsam.Pose2(2, 0, math.pi / 2), model))
+    5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
 graph.print_("\nFactor Graph:\n")  # print
 
 # 3. Create the data structure to hold the initial_estimate estimate to the
@@ -68,8 +83,5 @@ result.print_("Final Result:\n")
 
 # 5. Calculate and print marginal covariances for all variables
 marginals = gtsam.Marginals(graph, result)
-print("x1 covariance:\n", marginals.marginalCovariance(1))
+for i in range(1, 6):
-print("x2 covariance:\n", marginals.marginalCovariance(2))
+    print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
-print("x3 covariance:\n", marginals.marginalCovariance(3))
-print("x4 covariance:\n", marginals.marginalCovariance(4))
-print("x5 covariance:\n", marginals.marginalCovariance(5))

cython/gtsam/examples/SFMdata.py

@@ -3,6 +3,7 @@ A structure-from-motion example with landmarks
  - The landmarks form a 10 meter cube
  - The robot rotates around the landmarks, always facing towards the cube
 """
+# pylint: disable=invalid-name, E1101
 
 import numpy as np
 

cython/gtsam/examples/VisualISAM2Example.py

@@ -1,5 +1,15 @@
-"""An example of running visual SLAM using iSAM2."""
+"""
-# pylint: disable=invalid-name
+GTSAM Copyright 2010, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+An example of running visual SLAM using iSAM2.
+Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
 
 from __future__ import print_function