From 223020ec8200ed6e0aed771f8ee1959180aa8c4f Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Fri, 18 Jan 2019 22:05:57 -0500 Subject: [PATCH 1/3] Adding wrapper for Pose3 for reading g2o files and examples for Pose2 and Pose3 slam using g2o file --- cython/gtsam/examples/Pose2SLAMExample_g2o.py | 85 +++++++++++++++++++ cython/gtsam/examples/Pose3SLAMExample_g2o.py | 56 ++++++++++++ gtsam.h | 2 + 3 files changed, 143 insertions(+) create mode 100644 cython/gtsam/examples/Pose2SLAMExample_g2o.py create mode 100644 cython/gtsam/examples/Pose3SLAMExample_g2o.py diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py new file mode 100644 index 000000000..7920a4aa2 --- /dev/null +++ b/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!") diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py new file mode 100644 index 000000000..efafe0ecf --- /dev/null +++ b/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!") diff --git a/gtsam.h b/gtsam.h index 8097cc6c9..bcf8f90f4 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2449,6 +2449,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +string findExampleDataFile(string name); pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); pair load2D(string filename, @@ -2465,6 +2466,7 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, string filename); pair readG2o(string filename); +pair readG2o(string filename, bool is3D); void writeG2o(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& estimate, string filename); From b3b69bfd21c7d1dffcee5bc595ec5f4e13e4a265 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Mon, 21 Jan 2019 00:46:57 -0500 Subject: [PATCH 2/3] Addressing Duy's comments about argparse and PEP8. Adding plot flag to the examples --- cython/gtsam/examples/Pose2SLAMExample_g2o.py | 82 ++++++++++--------- cython/gtsam/examples/Pose3SLAMExample_g2o.py | 54 +++++++----- 2 files changed, 78 insertions(+), 58 deletions(-) diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 7920a4aa2..0ec80d169 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/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 - -Syntax for the script is "python ./Pose2SLAMExample_g2o.py input.g2o output.g2o" +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 -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: - 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] +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() -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": - 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) +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)) -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) - +params.setMaxIterations(maxIterations) +# 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() diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index efafe0ecf..787dc5d99 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/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 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o - * @date Jan 17, 2019 + * @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 math - +import argparse 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") -else: - g2oFile = str(sys.argv[1]) + +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) +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("final error = ",graphWithPrior.error(result)) +print("initial error = ", graphWithPrior.error(initial)) +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() From ddf0c839181bf5b3e284a9fbe92e4af04ecb44de Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Mon, 21 Jan 2019 13:29:46 -0500 Subject: [PATCH 3/3] Fixing gtsam.utils import --- cython/gtsam/examples/Pose2SLAMExample_g2o.py | 2 +- cython/gtsam/examples/Pose3SLAMExample_g2o.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 0ec80d169..3aee7daff 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py @@ -18,7 +18,7 @@ import numpy as np import matplotlib.pyplot as plt import gtsam -from utils import plot +from gtsam.utils import plot def vector3(x, y, z): diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index 787dc5d99..38c5db275 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py @@ -14,7 +14,7 @@ import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import gtsam -from utils import plot +from gtsam.utils import plot def vector6(x, y, z, a, b, c):