Merged in vik748/gtsam (pull request #373)

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

Approved-by: Duy-Nguyen Ta <thduynguyen@gmail.com>
Approved-by: Vikrant Shah <vikrantshah@gmail.com>

cython/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -0,0 +1,89 @@
+"""
+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
+"""
+# 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
+from gtsam.utils import plot
+
+
+def vector3(x, y, z):
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=np.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',
+                    help="the path to the output file with optimized graph")
+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()
+
+g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
+    else args.input
+
+maxIterations = 100 if args.maxiter is None else args.maxiter
+
+is3D = False
+
+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
+graphWithPrior = graph
+priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
+graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
+
+params = gtsam.GaussNewtonParams()
+params.setVerbosity("Termination")
+params.setMaxIterations(maxIterations)
+# 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 args.output is None:
+    print("\nFactor Graph:\n{}".format(graph))
+    print("\nInitial Estimate:\n{}".format(initial))
+    print("Final Result:\n{}".format(result))
+else:
+    outputFile = args.output
+    print("Writing results to file: ", outputFile)
+    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

@@ -0,0 +1,72 @@
+"""
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
+ *  Pose3 using InitializePose3
+ * @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 numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
+from gtsam.utils import plot
+
+
+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)
+
+
+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',
+                    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)
+
+# 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 conds
+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 args.output is None:
+    print("Final Result:\n{}".format(result))
+else:
+    outputFile = args.output
+    print("Writing results to file: ", outputFile)
+    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()

gtsam.h

@@ -2464,6 +2464,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,
@@ -2480,6 +2481,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);