Addressing Duy's comments about argparse and PEP8. Adding plot flag to the examples

cython/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -6,66 +6,61 @@ Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 See LICENSE for the license information
 
-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 
+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
-Syntax for the script is "python ./Pose2SLAMExample_g2o.py input.g2o output.g2o"
 """
 # 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 sys
+from utils import plot
 
 
 def vector3(x, y, z):
     """Create 3d double numpy array."""
     return np.array([x, y, z], dtype=np.float)
 
-kernelType = "none"
-maxIterations = 100 # default
-g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") # default
 
-if len(sys.argv) > 1:
+parser = argparse.ArgumentParser(
-    g2ofile = str(sys.argv[1])
+    description="A 2D Pose SLAM example that reads input from g2o, "
-    print ("Input file: ",g2ofile)
+                "converts it to a factor graph and does the optimization. "
-if len(sys.argv) > 3:
+                "Output is written on a file, in g2o format")
-    maxIterations = int(sys.argv[3])
+parser.add_argument('-i', '--input', help='input file g2o format')
-    print("Sepficied max iterations: ", maxIterations)
+parser.add_argument('-o', '--output',
-if len(sys.argv) > 4:
+                    help="the path to the output file with optimized graph")
-    kernelType = sys.argv[4]
+parser.add_argument('-m', '--maxiter', type=int,
+                    help="maximum number of iterations for optimizer")
+parser.add_argument('-k', '--kernel', 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()
 
-is3D = False # readG2o 3d parameter not available at time of this writing
+g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
+    else args.input
 
-if kernelType is "none":
+maxIterations = 100 if args.maxiter is None else args.maxiter
-    graph, initial = gtsam.readG2o(g2oFile)
+
-if kernelType is "huber":
+is3D = False
-    print("Using robust kernel: huber - NOT CURRENTLY IMPLEMENTED IN PYTHON")
+
-    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeHUBER)
+graph, initial = gtsam.readG2o(g2oFile, is3D)
-if kernelType is "tukey":
+
-    print("Using robust kernel: tukey - NOT CURRENTLY IMPLEMENTED IN PYTHON")
+assert args.kernel == "none", "Supplied kernel type is not yet implemented"
-    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY)
 
 # Add prior on the pose having index (key) = 0
 graphWithPrior = graph
 priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
 graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
-print("Adding prior on pose 0 ")
-
-print("\nFactor Graph:\n{}".format(graph)) 
-
-print("\nInitial Estimate:\n{}".format(initial))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")
-
+params.setMaxIterations(maxIterations)
-if (sys.argv > 3):
+# parameters.setRelativeErrorTol(1e-5)
-    params.setMaxIterations(maxIterations)
-    print("User setting: required to perform maximum ", maxIterations," iterations ")
-
-#parameters.setRelativeErrorTol(1e-5)
-
 # Create the optimizer ...
 optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
 # ... and optimize
@@ -75,11 +70,20 @@ print("Optimization complete")
 print("initial error = ", graph.error(initial))
 print("final error = ", graph.error(result))
 
-if len(sys.argv) < 3:
+
+if args.output is None:
+    print("\nFactor Graph:\n{}".format(graph))
+    print("\nInitial Estimate:\n{}".format(initial))
     print("Final Result:\n{}".format(result))
 else:
-    outputFile = sys.argv[2]
+    outputFile = args.output
     print("Writing results to file: ", outputFile)
-    graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
+    graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
     gtsam.writeG2o(graphNoKernel, result, outputFile)
     print ("Done!")
+
+if args.plot:
+    resultPoses = gtsam.extractPose2(result)
+    for i in range(resultPoses.shape[0]):
+        plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
+    plt.show()

cython/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -1,35 +1,45 @@
 """
  * @file Pose3SLAMExample_initializePose3.cpp
- * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ *  Pose3 using InitializePose3
- * @date Jan 17, 2019 
+ * @date Jan 17, 2019
  * @author Vikrant Shah based on CPP example by Luca Carlone
 """
 # pylint: disable=invalid-name, E1101
 
 from __future__ import print_function
-
+import argparse
-import math
-
 import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
 
 import gtsam
+from utils import plot
 
-import sys
 
 def vector6(x, y, z, a, b, c):
     """Create 6d double numpy array."""
     return np.array([x, y, z, a, b, c], dtype=np.float)
 
-if len(sys.argv) < 2:
+
-    g2oFile = gtsam.findExampleDataFile("pose3example.txt")
+parser = argparse.ArgumentParser(
-else:
+    description="A 3D Pose SLAM example that reads input from g2o, and "
-    g2oFile = str(sys.argv[1])
+                "initializes Pose3")
+parser.add_argument('-i', '--input', help='input file g2o format')
+parser.add_argument('-o', '--output',
+                    help="the path to the output file with optimized graph")
+parser.add_argument("-p", "--plot", action="store_true",
+                    help="Flag to plot results")
+args = parser.parse_args()
+
+g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
+    else args.input
+
 is3D = True
-graph, initial = gtsam.readG2o(g2oFile,is3D)
+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(vector6(1e-6, 1e-6, 1e-6,
                                                          1e-4, 1e-4, 1e-4))
 
 print("Adding prior to g2o file ")
@@ -38,19 +48,25 @@ firstKey = initial.keys().at(0)
 graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
-params.setVerbosity("Termination") # this will show info about stopping conditions
+params.setVerbosity("Termination")  # this will show info about stopping conds
 optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
 result = optimizer.optimize()
 print("Optimization complete")
 
-print("initial error = ",graphWithPrior.error(initial))
+print("initial error = ", graphWithPrior.error(initial))
-print("final error = ",graphWithPrior.error(result))
+print("final error = ", graphWithPrior.error(result))
 
-if len(sys.argv) < 3:
+if args.output is None:
     print("Final Result:\n{}".format(result))
 else:
-    outputFile = sys.argv[2]
+    outputFile = args.output
     print("Writing results to file: ", outputFile)
-    graphNoKernel, initial2 = gtsam.readG2o(g2oFile,is3D)
+    graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
     gtsam.writeG2o(graphNoKernel, result, outputFile)
     print ("Done!")
+
+if args.plot:
+    resultPoses = gtsam.allPose3s(result)
+    for i in range(resultPoses.size()):
+        plot.plot_pose3(1, resultPoses.atPose3(i))
+    plt.show()