diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py new file mode 100644 index 000000000..3aee7daff --- /dev/null +++ b/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() diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py new file mode 100644 index 000000000..38c5db275 --- /dev/null +++ b/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() diff --git a/gtsam.h b/gtsam.h index 64f025972..3d4ee5c29 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2464,6 +2464,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, @@ -2480,6 +2481,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);