Adding wrapper for Pose3 for reading g2o files and examples for Pose2 and Pose3 slam using g2o file

cython/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -0,0 +1,85 @@
+"""
+GTSAM Copyright 2010-2018, 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
+
+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 math
+import numpy as np
+import gtsam
+import sys
+
+
+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:
+    g2ofile = str(sys.argv[1])
+    print ("Input file: ",g2ofile)
+if len(sys.argv) > 3:
+    maxIterations = int(sys.argv[3])
+    print("Sepficied max iterations: ", maxIterations)
+if len(sys.argv) > 4:
+    kernelType = sys.argv[4]
+
+is3D = False # readG2o 3d parameter not available at time of this writing
+
+if kernelType is "none":
+    graph, initial = gtsam.readG2o(g2oFile)
+if kernelType is "huber":
+    print("Using robust kernel: huber - NOT CURRENTLY IMPLEMENTED IN PYTHON")
+    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeHUBER)
+if kernelType is "tukey":
+    print("Using robust kernel: tukey - NOT CURRENTLY IMPLEMENTED IN PYTHON")
+    #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")
+
+if (sys.argv > 3):
+    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
+result = optimizer.optimize()
+
+print("Optimization complete")
+print("initial error = ", graph.error(initial))
+print("final error = ", graph.error(result))
+
+if len(sys.argv) < 3:
+    print("Final Result:\n{}".format(result))
+else:
+    outputFile = sys.argv[2]
+    print("Writing results to file: ", outputFile)
+    graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
+    gtsam.writeG2o(graphNoKernel, result, outputFile)
+    print ("Done!")

cython/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -0,0 +1,56 @@
+"""
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * @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 math
+
+import numpy as np
+
+import gtsam
+
+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")
+else:
+    g2oFile = str(sys.argv[1])
+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, 
+                                                         1e-4, 1e-4, 1e-4))
+
+print("Adding prior to g2o file ")
+graphWithPrior = graph
+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
+optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+result = optimizer.optimize()
+print("Optimization complete")
+
+print("initial error = ",graphWithPrior.error(initial))
+print("final error = ",graphWithPrior.error(result))
+
+if len(sys.argv) < 3:
+    print("Final Result:\n{}".format(result))
+else:
+    outputFile = sys.argv[2]
+    print("Writing results to file: ", outputFile)
+    graphNoKernel, initial2 = gtsam.readG2o(g2oFile,is3D)
+    gtsam.writeG2o(graphNoKernel, result, outputFile)
+    print ("Done!")

gtsam.h

@@ -2449,6 +2449,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+string findExampleDataFile(string name);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
@@ -2465,6 +2466,7 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     string filename);
 
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& estimate, string filename);