From 223020ec8200ed6e0aed771f8ee1959180aa8c4f Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Fri, 18 Jan 2019 22:05:57 -0500 Subject: [PATCH 01/97] 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 edb94a6e938d2166aa30fe5adcec552930bcd299 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 20 Jan 2019 16:52:44 -0500 Subject: [PATCH 02/97] Better error message. --- wrap/Method.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 1a58692b6..8f0ef17b9 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -200,6 +200,6 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const { file.oss << " pass\n"; } file.oss - << " raise TypeError('Incorrect arguments for method call.')\n\n"; + << " raise TypeError('Incorrect arguments or types for method call.')\n\n"; } /* ************************************************************************* */ From b3b69bfd21c7d1dffcee5bc595ec5f4e13e4a265 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Mon, 21 Jan 2019 00:46:57 -0500 Subject: [PATCH 03/97] 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 04/97] 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): From 1aa36b2ea9f60848d4d25d7d2517d5d36b5d2c6e Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Mon, 21 Jan 2019 23:27:58 -0800 Subject: [PATCH 05/97] Fix testPose2 failure by forcing eval in wedge expression. Always align Pose2. --- gtsam/geometry/Pose2.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 079c649f3..603c3a421 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -295,17 +295,16 @@ private: ar & BOOST_SERIALIZATION_NVP(r_); } -#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS public: - // Make sure Pose2 is aligned if it contains a Vector2 + // Align for Point2, which is either derived from, or is typedef, of Vector2 EIGEN_MAKE_ALIGNED_OPERATOR_NEW -#endif }; // Pose2 /** specialization for pose2 wedge function (generic template in Lie.h) */ template <> inline Matrix wedge(const Vector& xi) { - return Pose2::wedge(xi(0),xi(1),xi(2)); + // NOTE(chris): Need eval() as workaround for Apple clang + avx2. + return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval(); } /** From 9ea90b5b451ada9d5282c8c9e4661cb5f9e53b44 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 15 Jan 2019 00:38:14 +0100 Subject: [PATCH 06/97] port to target-based compile options and definitions This also fixes CMake warnings on Windows --- CMakeLists.txt | 38 +++++++++++++++------------- cmake/example_project/CMakeLists.txt | 2 +- gtsam/CMakeLists.txt | 7 ++--- gtsam_unstable/CMakeLists.txt | 8 ++---- 4 files changed, 28 insertions(+), 27 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 972c40650..673f9a45f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ project(GTSAM CXX C) -cmake_minimum_required(VERSION 2.8.4) +cmake_minimum_required(VERSION 3.0) # new feature to Cmake Version > 2.8.12 # Mac ONLY. Define Relative Path on Mac OS @@ -121,12 +121,11 @@ if(MSVC) # If we use Boost shared libs, disable auto linking. # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. if(NOT Boost_USE_STATIC_LIBS) - add_definitions(-DBOOST_ALL_NO_LIB) - add_definitions(-DBOOST_ALL_DYN_LINK) + list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) endif() # Virtual memory range for PCH exceeded on VS2015 if(MSVC_VERSION LESS 1910) # older than VS2017 - add_definitions(-Zm295) + list(APPEND GTSAM_COMPILE_OPTIONS -Zm295) endif() endif() @@ -141,14 +140,19 @@ endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) set(GTSAM_BOOST_LIBRARIES - ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) + Boost::serialization + Boost::system + Boost::filesystem + Boost::thread + Boost::date_time + Boost::regex +) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") - add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) + list(APPEND GTSAM_COMPILE_DEFINITIONS DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) + list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) else() list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") @@ -158,7 +162,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) + list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -279,9 +283,9 @@ endif() if (MSVC) if (BUILD_SHARED_LIBS) # mute eigen static assert to avoid errors in shared lib - add_definitions(-DEIGEN_NO_STATIC_ASSERT) + list(APPEND GTSAM_COMPILE_DEFINITIONS DEIGEN_NO_STATIC_ASSERT) endif() - add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen + list(APPEND GTSAM_COMPILE_OPTIONS "/wd4244") # Disable loss of precision which is thrown all over our Eigen endif() ############################################################################### @@ -347,27 +351,27 @@ include_directories(BEFORE CppUnitLite) if(MSVC) - add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) - add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - add_definitions(/bigobj) # Allow large object files for template-based code + list(APPEND GTSAM_COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) + list(APPEND GTSAM_COMPILE_OPTIONS /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings + list(APPEND GTSAM_COMPILE_OPTIONS /bigobj) # Allow large object files for template-based code endif() # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) - add_definitions(-Wno-unused-local-typedefs) + list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs) endif() endif() # As of XCode 7, clang also complains about this if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) - add_definitions(-Wno-unused-local-typedefs) + list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs) endif() endif() if(GTSAM_ENABLE_CONSISTENCY_CHECKS) - add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) + list(APPEND GTSAM_COMPILE_DEFINITIONS GTSAM_EXTRA_CONSISTENCY_CHECKS) endif() ############################################################################### diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 548d56acd..e287087d1 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -7,7 +7,7 @@ ################################################################################### # To create your own project, replace "example" with the actual name of your project -cmake_minimum_required(VERSION 2.6) +cmake_minimum_required(VERSION 3.0) project(example CXX C) # Include GTSAM CMake tools diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 7fa9859ea..d369009c0 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -101,9 +101,10 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam ${gtsam_srcs}) -target_link_libraries(gtsam - PUBLIC - ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) +target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) +target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) +target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS}) +target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS}) set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 1494dcf41..327326201 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -14,8 +14,6 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION) # Only build partition if metis is built set (gtsam_unstable_subdirs ${gtsam_unstable_subdirs} partition) endif(GTSAM_SUPPORT_NESTED_DISSECTION) -set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES}) - add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) # To exclude a source from the library build (in any subfolder) @@ -72,10 +70,8 @@ set_target_properties(gtsam_unstable PROPERTIES CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_unstable_version} SOVERSION ${gtsam_unstable_soversion}) -target_link_libraries(gtsam_unstable - PUBLIC - gtsam - ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) +target_link_libraries(gtsam_unstable PUBLIC gtsam) +# No need to link against Boost here, it's inherited from gtsam PUBLIC interface if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if (NOT BUILD_SHARED_LIBS) From ca9c9720281eee008c3ad4a6e9b061482ba844a9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 25 Jan 2019 15:20:54 -0500 Subject: [PATCH 07/97] Wrapping BearingRange --- gtsam.h | 15 +++++++++++++++ gtsam/geometry/BearingRange.h | 24 +++++++++++++++++------- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8097cc6c9..64f025972 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2316,6 +2316,21 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { typedef gtsam::BearingFactor BearingFactor2D; typedef gtsam::BearingFactor BearingFactorPose2; +#include +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + // TODO(frank): can't class instance itself? + // static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s) const; +}; + +typedef gtsam::BearingRange BearingRange2D; + #include template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index ffa373027..8214b0727 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -44,11 +44,11 @@ struct Range; * For example BearingRange(pose,point) will return pair * and BearingRange(pose,point) will return pair */ -template +template ::result_type, + typename R = typename Range::result_type> struct BearingRange { private: - typedef typename Bearing::result_type B; - typedef typename Range::result_type R; B bearing_; R range_; @@ -91,6 +91,16 @@ public: return BearingRange(b, r); } + /// Predict bearing + static B MeasureBearing(const A1& a1, const A2& a2) { + return Bearing()(a1, a2); + } + + /// Predict range + static R MeasureRange(const A1& a1, const A2& a2) { + return Range()(a1, a2); + } + /// @} /// @name Testable /// @{ @@ -170,8 +180,8 @@ struct HasBearing { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1, - OptionalJacobian::dimension, traits::dimension> H2) { + OptionalJacobian::dimension, traits::dimension> H1=boost::none, + OptionalJacobian::dimension, traits::dimension> H2=boost::none) { return a1.bearing(a2, H1, H2); } }; @@ -184,8 +194,8 @@ struct HasRange { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1, - OptionalJacobian::dimension, traits::dimension> H2) { + OptionalJacobian::dimension, traits::dimension> H1=boost::none, + OptionalJacobian::dimension, traits::dimension> H2=boost::none) { return a1.range(a2, H1, H2); } }; From 5acd89f6344cd0fa81520c2f7c4ddc1c3eb3768a Mon Sep 17 00:00:00 2001 From: Mike Sheffler Date: Tue, 29 Jan 2019 00:28:54 -0800 Subject: [PATCH 08/97] Non-include of caused compilation errors related to std::cout usage --- gtsam/base/serializationTestHelpers.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 4ef09beb1..06b3462e9 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include From ca7866811336baa8212b3403989cd9951df02824 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 11 Feb 2019 10:33:16 -0500 Subject: [PATCH 09/97] Deleted legacy files --- cython/gtsam/tests/experiments.py | 112 ----- cython/gtsam/tests/gtsam_test.h | 772 ------------------------------ 2 files changed, 884 deletions(-) delete mode 100644 cython/gtsam/tests/experiments.py delete mode 100644 cython/gtsam/tests/gtsam_test.h diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py deleted file mode 100644 index 425180173..000000000 --- a/cython/gtsam/tests/experiments.py +++ /dev/null @@ -1,112 +0,0 @@ -""" -This file is not a real python unittest. It contains small experiments -to test the wrapper with gtsam_test, a short version of gtsam.h. -Its name convention is different from other tests so it won't be discovered. -""" -import gtsam -import numpy as np - -r = gtsam.Rot3() -print(r) -print(r.pitch()) -r2 = gtsam.Rot3() -r3 = r.compose(r2) -print("r3 pitch:", r3.pitch()) - -v = np.array([1, 1, 1]) -print("v = ", v) -r4 = r3.retract(v) -print("r4 pitch:", r4.pitch()) -r4.print_(b'r4: ') -r3.print_(b"r3: ") - -v = r3.localCoordinates(r4) -print("localCoordinates:", v) - -Rmat = np.array([ - [0.990074, -0.0942928, 0.104218], - [0.104218, 0.990074, -0.0942928], - [-0.0942928, 0.104218, 0.990074] - ]) -r5 = gtsam.Rot3(Rmat) -r5.print_(b"r5: ") - -l = gtsam.Rot3.Logmap(r5) -print("l = ", l) - - -noise = gtsam.noiseModel_Gaussian.Covariance(Rmat) -noise.print_(b"noise:") - -D = np.array([1.,2.,3.]) -diag = gtsam.noiseModel_Diagonal.Variances(D) -print("diag:", diag) -diag.print_(b"diag:") -print("diag R:", diag.R()) - -p = gtsam.Point3() -p.print_("p:") -factor = gtsam.BetweenFactorPoint3(1,2,p, noise) -factor.print_(b"factor:") - -vv = gtsam.VectorValues() -vv.print_(b"vv:") -vv.insert(1, np.array([1.,2.,3.])) -vv.insert(2, np.array([3.,4.])) -vv.insert(3, np.array([5.,6.,7.,8.])) -vv.print_(b"vv:") - -vv2 = gtsam.VectorValues(vv) -vv2.insert(4, np.array([4.,2.,1])) -vv2.print_(b"vv2:") -vv.print_(b"vv:") - -vv.insert(4, np.array([1.,2.,4.])) -vv.print_(b"vv:") -vv3 = vv.add(vv2) - -vv3.print_(b"vv3:") - -values = gtsam.Values() -values.insert(1, gtsam.Point3()) -values.insert(2, gtsam.Rot3()) -values.print_(b"values:") - -factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag) -print "Prior factor vector: ", factor - - - -keys = gtsam.KeyVector() - -keys.push_back(1) -keys.push_back(2) -print 'size: ', keys.size() -print keys.at(0) -print keys.at(1) - -noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) -noise.print_('noise:') -print 'noise print:', noise -f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) -print 'JacobianFactor(7):\n', f -print "A = ", f.getA() -print "b = ", f.getb() - -f = gtsam.JacobianFactor(np.ones(2)) -f.print_('jacoboian b_in:') - - -print "JacobianFactor initalized with b_in:", f - -diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) -fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag) -print "priorfactorvector: ", fv - -print "base noise: ", fv.get_noiseModel() -print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()) - -X = gtsam.symbol(65, 19) -print X -print gtsam.symbolChr(X) -print gtsam.symbolIndex(X) diff --git a/cython/gtsam/tests/gtsam_test.h b/cython/gtsam/tests/gtsam_test.h deleted file mode 100644 index 659a7cd0c..000000000 --- a/cython/gtsam/tests/gtsam_test.h +++ /dev/null @@ -1,772 +0,0 @@ -namespace gtsam { - -#include -typedef size_t Key; - -#include -template class FastVector { - FastVector(); - FastVector(const This& f); - void push_back(const T& e); - //T& operator[](int); - T at(int i); - size_t size() const; -}; - -typedef gtsam::FastVector KeyVector; - -//************************************************************************* -// geometry -//************************************************************************* - -#include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - //Point2(const gtsam::Point2& l); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point2& pose, double tol) const; - - // Group - static gtsam::Point2 identity(); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - //Point3(const gtsam::Point3& l); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point3& p, double tol) const; - - // Group - static gtsam::Point3 identity(); - - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - //Rot2(const gtsam::Rot2& l); - - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot2& rot, double tol) const; - - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; - - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; - - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; - - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - //Rot3(const gtsam::Rot3& l); - - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 Rodrigues(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; -// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly - Vector quaternion() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Pose2 { - // Standard Constructor - Pose2(); - //Pose2(const gtsam::Pose2& pose); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2& pose, double tol) const; - - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; - - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; - - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(const gtsam::Point2& p) const; - - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - - -#include -class Pose3 { - // Standard Constructors - Pose3(); - //Pose3(const gtsam::Pose3& pose); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; - - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; - - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; -}; - -//************************************************************************* -// noise -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; - bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - //Null(const gtsam::noiseModel::mEstimator::Null& other); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - //Fair(const gtsam::noiseModel::mEstimator::Fair& other); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - //Huber(const gtsam::noiseModel::mEstimator::Huber& other); - - void print(string s) const; - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - //Tukey(const gtsam::noiseModel::mEstimator::Tukey& other); - - void print(string s) const; - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - //Robust(const gtsam::noiseModel::Robust& other); - - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - //Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - Sampler(int seed); - //Sampler(const gtsam::Sampler& other); - - - //Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); -}; - -#include -class VectorValues { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; - -#include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - //JacobianFactor(const gtsam::GaussianFactorGraph& graph); - //JacobianFactor(const gtsam::JacobianFactor& other); - - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - //pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, const Vector& sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - //HessianFactor(const gtsam::GaussianFactorGraph& factors); - //HessianFactor(const gtsam::HessianFactor& other); - - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Values { - Values(); - //Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - // template - // void insert(size_t j, const T& t); - - // template - // void update(size_t j, const T& t); - void insert(size_t j, const gtsam::Point2& t); - void insert(size_t j, const gtsam::Point3& t); - void insert(size_t j, const gtsam::Rot2& t); - void insert(size_t j, const gtsam::Pose2& t); - void insert(size_t j, const gtsam::Rot3& t); - void insert(size_t j, const gtsam::Pose3& t); - void insert(size_t j, Vector t); - void insert(size_t j, Matrix t); - - void update(size_t j, const gtsam::Point2& t); - void update(size_t j, const gtsam::Point3& t); - void update(size_t j, const gtsam::Rot2& t); - void update(size_t j, const gtsam::Pose2& t); - void update(size_t j, const gtsam::Rot3& t); - void update(size_t j, const gtsam::Pose3& t); - void update(size_t j, Vector t); - void update(size_t j, Matrix t); - - template - T at(size_t j); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; - -#include -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor& other, double tol) const; - - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen -}; - -#include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - //NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - // gtsam::KeySet keys() const; - - // NonlinearFactorGraph - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - //gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - //gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - //PriorFactor(const This& other); - T prior() const; - - // enabling serialization functionality - void serialize() const; -}; - - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - //BetweenFactor(const This& other); - T measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -#include -// Default keyformatter -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); - -#include -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - -#include -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -pair readG2o(string filename); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -//************************************************************************* -// Utilities -//************************************************************************* - -namespace utilities { - - #include - // gtsam::KeyList createKeyList(Vector I); - // gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - // gtsam::KeySet createKeySet(Vector I); - // gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - // void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - // void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - // void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities - -#include -class RedirectCout { - RedirectCout(); - string str(); -}; - -} //\namespace gtsam From c0686f1f09bedda09bdef26ddc4bbb05faf49a09 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 11 Feb 2019 10:43:48 -0500 Subject: [PATCH 10/97] Fixed findExampleDataFile issue. Note the wrapped version is not the one that will be available in MATLAB. Still have to test whether we can use that or not. --- gtsam/slam/dataset.cpp | 4 ---- gtsam/slam/dataset.h | 2 -- 2 files changed, 6 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6efd01feb..34370d4e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -51,7 +51,6 @@ using namespace gtsam::symbol_shorthand; namespace gtsam { -#ifndef MATLAB_MEX_FILE /* ************************************************************************* */ string findExampleDataFile(const string& name) { // Search source tree and installed location @@ -100,9 +99,6 @@ string createRewrittenFileName(const string& name) { return newpath.string(); } -/* ************************************************************************* */ -#endif - /* ************************************************************************* */ GraphAndValues load2D(pair dataset, int maxID, bool addNoise, bool smart, NoiseFormat noiseFormat, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 929ada2c1..7c9b54a6f 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -37,7 +37,6 @@ namespace gtsam { -#ifndef MATLAB_MEX_FILE /** * Find the full path to an example dataset distributed with gtsam. The name * may be specified with or without a file extension - if no extension is @@ -56,7 +55,6 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); * for checking read-write oprations */ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); -#endif /// Indicates how noise parameters are stored in file enum NoiseFormat { From 37ed46cee1d8e4cf2a68eb4f6d61f1815aa82307 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 11 Feb 2019 10:44:24 -0500 Subject: [PATCH 11/97] Added underscore to case conflicts until wrap can handle this. --- cython/gtsam/tests/test_Pose2.py | 21 +++++++++++++++++++++ cython/gtsam/tests/test_Pose3.py | 4 ++-- gtsam.h | 8 ++++---- gtsam/geometry/Pose2.h | 8 ++++++-- gtsam/geometry/Pose3.h | 4 ++++ 5 files changed, 37 insertions(+), 8 deletions(-) create mode 100644 cython/gtsam/tests/test_Pose2.py diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py new file mode 100644 index 000000000..f43ec0683 --- /dev/null +++ b/cython/gtsam/tests/test_Pose2.py @@ -0,0 +1,21 @@ +"""Pose2 unit tests.""" +import unittest + +import numpy as np + +from gtsam import Pose2 + + +class TestPose2(unittest.TestCase): + """Test selected Pose2 methods.""" + + def test_adjoint(self): + """Test adjoint method.""" + xi = np.array([1, 2, 3]) + expected = np.dot(Pose2.adjointMap(xi), xi) + actual = Pose2.adjoint(xi, xi) + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 4752a4b02..b878a9551 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -49,8 +49,8 @@ class TestPose3(unittest.TestCase): def test_adjoint(self): """Test adjoint method.""" xi = np.array([1, 2, 3, 4, 5, 6]) - expected = np.dot(Pose3.adjointMap(xi), xi) - actual = Pose3.adjoint(xi, xi) + expected = np.dot(Pose3.adjointMap_(xi), xi) + actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) diff --git a/gtsam.h b/gtsam.h index 3d4ee5c29..4720d9d6b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -577,8 +577,8 @@ class Pose2 { static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; - static Matrix adjointMap(Vector v); - Vector adjoint(Vector xi, Vector y); + static Matrix adjointMap_(Vector v); + Vector adjoint_(Vector xi, Vector y); Vector adjointTranspose(Vector xi, Vector y); static Matrix wedge(double vx, double vy, double w); @@ -628,8 +628,8 @@ class Pose3 { static Vector Logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; - static Matrix adjointMap(Vector xi); - static Vector adjoint(Vector xi, Vector y); + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y); static Matrix ExpmapDerivative(Vector xi); static Matrix LogmapDerivative(const gtsam::Pose3& xi); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 603c3a421..6937ff78d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -146,17 +146,21 @@ public: /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - Vector3 adjoint(const Vector3& xi, const Vector3& y) { + static Vector3 adjoint(const Vector3& xi, const Vector3& y) { return adjointMap(xi) * y; } /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { + static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { return adjointMap(xi).transpose() * y; } + // temporary fix for wrappers until case issue is resolved + static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);} + static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);} + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ca0fdff10..757c4fdd4 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -160,6 +160,10 @@ public: static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6, 6> = boost::none); + // temporary fix for wrappers until case issue is resolved + static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} + static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);} + /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ From 7138236f716f767d3401da2440f36b5e7cb35fa7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 11 Feb 2019 10:52:36 -0500 Subject: [PATCH 12/97] Fixed static issue --- cython/gtsam/tests/test_Pose2.py | 4 ++-- gtsam.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py index f43ec0683..afd0c5773 100644 --- a/cython/gtsam/tests/test_Pose2.py +++ b/cython/gtsam/tests/test_Pose2.py @@ -12,8 +12,8 @@ class TestPose2(unittest.TestCase): def test_adjoint(self): """Test adjoint method.""" xi = np.array([1, 2, 3]) - expected = np.dot(Pose2.adjointMap(xi), xi) - actual = Pose2.adjoint(xi, xi) + expected = np.dot(Pose2.adjointMap_(xi), xi) + actual = Pose2.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) diff --git a/gtsam.h b/gtsam.h index 4720d9d6b..48768db40 100644 --- a/gtsam.h +++ b/gtsam.h @@ -578,8 +578,8 @@ class Pose2 { Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; static Matrix adjointMap_(Vector v); - Vector adjoint_(Vector xi, Vector y); - Vector adjointTranspose(Vector xi, Vector y); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 From 7625c2177710ef8a1b5624fcf8a94c74fce2bf86 Mon Sep 17 00:00:00 2001 From: jlblancoc Date: Fri, 8 Feb 2019 22:18:21 +0100 Subject: [PATCH 13/97] Various fixes to cmake exported targets List of changes: * -I boost is no longer required (Since the use of Boost::xxx imported targets) * fix missing Boost deps in imported gtsam by searching for Boost inside GTSAMConfig.cmake * Including the dirs for Eigen/MKL/SuiteSparse/Metis into exported targets public interface. * Fix missing cmake changes in wrap/* * Split build flags into private/public, not to expose to users flags that may be invasive. * Removed now useless include_dirs in "extra cmake" * Update cmake/example_project * Make cppunitlite to find boost headers via Boost::boost * Update README / INSTALL to reflect the updated minimum CMake >= 3.0 --- CMakeLists.txt | 64 +++++++++------------------- CppUnitLite/CMakeLists.txt | 1 + INSTALL | 64 ++++++++++++++-------------- README.md | 10 ++--- cmake/Config.cmake.in | 8 +++- cmake/example_project/CMakeLists.txt | 5 ++- gtsam/CMakeLists.txt | 60 +++++++++++++++++++++++++- gtsam_extra.cmake.in | 12 ------ wrap/CMakeLists.txt | 19 ++++++--- 9 files changed, 139 insertions(+), 104 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 673f9a45f..d084840be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,15 +121,19 @@ if(MSVC) # If we use Boost shared libs, disable auto linking. # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. if(NOT Boost_USE_STATIC_LIBS) - list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) endif() # Virtual memory range for PCH exceeded on VS2015 if(MSVC_VERSION LESS 1910) # older than VS2017 - list(APPEND GTSAM_COMPILE_OPTIONS -Zm295) + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) endif() endif() -find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) +# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. +set(BOOST_FIND_MINIMUM_VERSION 1.43) +set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) + +find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR @@ -149,7 +153,7 @@ set(GTSAM_BOOST_LIBRARIES ) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") - list(APPEND GTSAM_COMPILE_DEFINITIONS DGTSAM_DISABLE_NEW_TIMERS) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) @@ -162,7 +166,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -212,7 +216,6 @@ find_package(MKL) if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL - include_directories(${MKL_INCLUDE_DIR}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) # --no-as-needed is required with gcc according to the MKL link advisor @@ -247,10 +250,9 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) - include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") + set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches @@ -270,22 +272,19 @@ else() if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) endif() - # Add the bundled version of eigen to the include path so that it can still be included - # with #include - include_directories(BEFORE "gtsam/3rdparty/Eigen/") # set full path to be used by external projects # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in - set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/") + set(GTSAM_EIGEN_INCLUDE_PREFIX "include/gtsam/3rdparty/Eigen/") endif() if (MSVC) if (BUILD_SHARED_LIBS) # mute eigen static assert to avoid errors in shared lib - list(APPEND GTSAM_COMPILE_DEFINITIONS DEIGEN_NO_STATIC_ASSERT) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT) endif() - list(APPEND GTSAM_COMPILE_OPTIONS "/wd4244") # Disable loss of precision which is thrown all over our Eigen + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen endif() ############################################################################### @@ -326,52 +325,29 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") endif() -# Include boost - use 'BEFORE' so that a specific boost specified to CMake -# takes precedence over a system-installed one. -include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) - -if(GTSAM_SUPPORT_NESTED_DISSECTION) - set(METIS_INCLUDE_DIRECTORIES - gtsam/3rdparty/metis/include - gtsam/3rdparty/metis/libmetis - gtsam/3rdparty/metis/GKlib) -else() - set(METIS_INCLUDE_DIRECTORIES) -endif() - -# Add includes for source directories 'BEFORE' boost and any system include -# paths so that the compiler uses GTSAM headers in our source directory instead -# of any previously installed GTSAM headers. -include_directories(BEFORE - gtsam/3rdparty/SuiteSparse_config - gtsam/3rdparty/CCOLAMD/Include - ${METIS_INCLUDE_DIRECTORIES} - ${PROJECT_SOURCE_DIR} - ${PROJECT_BINARY_DIR} # So we can include generated config header files - CppUnitLite) - if(MSVC) - list(APPEND GTSAM_COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) - list(APPEND GTSAM_COMPILE_OPTIONS /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - list(APPEND GTSAM_COMPILE_OPTIONS /bigobj) # Allow large object files for template-based code + list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code endif() # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) - list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs) + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) endif() endif() # As of XCode 7, clang also complains about this if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) - list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs) + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) endif() endif() if(GTSAM_ENABLE_CONSISTENCY_CHECKS) - list(APPEND GTSAM_COMPILE_DEFINITIONS GTSAM_EXTRA_CONSISTENCY_CHECKS) + # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h + list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) endif() ############################################################################### diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index 35ecd71f8..f52841274 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp") add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) +target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure diff --git a/INSTALL b/INSTALL index c71dcd4f9..d2f0b68df 100644 --- a/INSTALL +++ b/INSTALL @@ -14,7 +14,7 @@ Important Installation Notes 1) GTSAM requires the following libraries to be installed on your system: - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) - - Cmake version 2.6 or higher + - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher Optional dependent libraries: @@ -33,20 +33,20 @@ Tested compilers: Tested systems: -- Ubuntu 11.04 - 13.10 +- Ubuntu 11.04 - 18.04 - MacOS 10.6 - 10.9 -- Windows 7, 8, 8.1 +- Windows 7, 8, 8.1, 10 Known issues: - MSVC 2013 is not yet supported because it cannot build the serialization module - of Boost 1.55 (or earlier). + of Boost 1.55 (or earlier). 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work in Debug mode while developing (enabled by default). Likewise, it is imperative that you switch to release mode when running finished code and for timing. GTSAM -will run up to 10x faster in Release mode! See the end of this document for +will run up to 10x faster in Release mode! See the end of this document for additional debugging tips. 3) @@ -55,7 +55,7 @@ build directory. 4) The instructions below install the library to the default system install path and -build all components. From a terminal, starting in the root library folder, +build all components. From a terminal, starting in the root library folder, execute commands as follows for an out-of-source build: $] mkdir build @@ -64,7 +64,7 @@ $] cmake .. $] make check (optional, runs unit tests) $] make install -This will build the library and unit tests, run all of the unit tests, +This will build the library and unit tests, run all of the unit tests, and then install the library itself. - CMake Configuration Options and Details @@ -75,12 +75,12 @@ one of the following: ccmake the curses GUI for cmake cmake-gui a real GUI for cmake -Important Options: +Important Options: CMAKE_BUILD_TYPE: We support several build configurations for GTSAM (case insensitive) Debug (default) All error checking options on, no optimization. Use for development. - Release Optimizations turned on, no debug symbols. - Timing Adds ENABLE_TIMING flag to provide statistics on operation + Release Optimizations turned on, no debug symbols. + Timing Adds ENABLE_TIMING flag to provide statistics on operation Profiling Standard configuration for use during profiling RelWithDebInfo Same as Release, but with the -g flag for debug symbols @@ -92,42 +92,42 @@ GTSAM_TOOLBOX_INSTALL_PATH: The Matlab toolbox will be installed in a subdirecto of this folder, called 'gtsam'. $] cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox .. -GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in -subfolders to be linked against convenience libraries rather than the full libgtsam. +GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in +subfolders to be linked against convenience libraries rather than the full libgtsam. Set with the command line as follows: $] cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON .. - ON (Default) This builds convenience libraries and links tests against them. This - option is suggested for gtsam developers, as it is possible to build - and run tests without first building the rest of the library, and - speeds up compilation for a single test. The downside of this option - is that it will build the entire library again to build the full + ON (Default) This builds convenience libraries and links tests against them. This + option is suggested for gtsam developers, as it is possible to build + and run tests without first building the rest of the library, and + speeds up compilation for a single test. The downside of this option + is that it will build the entire library again to build the full libgtsam library, so build/install will be slower. - OFF This will build all of libgtsam before any of the tests, and then - link all of the tests at once. This option is best for users of GTSAM, - as it avoids rebuilding the entirety of gtsam an extra time. + OFF This will build all of libgtsam before any of the tests, and then + link all of the tests at once. This option is best for users of GTSAM, + as it avoids rebuilding the entirety of gtsam an extra time. -GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library. +GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library. Set with the command line as follows: $] cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON .. - ON When enabled, libgtsam_unstable will be built and installed with the - same options as libgtsam. In addition, if tests are enabled, the + ON When enabled, libgtsam_unstable will be built and installed with the + same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also - be generated if the matlab toolbox is enabled, installing into a + be generated if the matlab toolbox is enabled, installing into a folder called "gtsam_unstable". - OFF (Default) If disabled, no gtsam_unstable code will be included in build or install. + OFF (Default) If disabled, no gtsam_unstable code will be included in build or install. Check -"make check" will build and run all of the tests. Note that the tests will only be +"make check" will build and run all of the tests. Note that the tests will only be built when using the "check" targets, to prevent "make install" from building the tests -unnecessarily. You can also run "make timing" to build all of the timing scripts. +unnecessarily. You can also run "make timing" to build all of the timing scripts. To run check on a particular module only, run "make check.[subfolder]", so to run just the geometry tests, run "make check.geometry". Individual tests can be run by -appending ".run" to the name of the test, for example, to run testMatrix, run -"make testMatrix.run". +appending ".run" to the name of the test, for example, to run testMatrix, run +"make testMatrix.run". -MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your -shell's PATH environment variable. mex is installed with matlab at +MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your +shell's PATH environment variable. mex is installed with matlab at $MATLABROOT/bin/mex $MATLABROOT can be found by executing the command 'matlabroot' in MATLAB @@ -143,4 +143,4 @@ it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it tho NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of -header-only Eigen. +header-only Eigen. diff --git a/README.md b/README.md index 02ed5149c..e815d7f4b 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ $ make install Prerequisites: - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. Optional prerequisites - used automatically if findable by CMake: @@ -54,7 +54,7 @@ GTSAM includes a state of the art IMU handling scheme based on Our implementation improves on this using integration on the manifold, as detailed in -- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. - Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. If you are using the factor in academic work, please cite the publications above. @@ -67,8 +67,8 @@ Additional Information There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion. -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, -which support (superfast) automatic differentiation, +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, +which support (superfast) automatic differentiation, can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home). See the [`INSTALL`](INSTALL) file for more detailed installation instructions. @@ -77,4 +77,4 @@ GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`L Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. -GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file +GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in index 5f3956f07..4323f760b 100644 --- a/cmake/Config.cmake.in +++ b/cmake/Config.cmake.in @@ -1,7 +1,7 @@ # - Config file for @CMAKE_PROJECT_NAME@ # It defines the following variables # @PACKAGE_NAME@_INCLUDE_DIR - include directories for @CMAKE_PROJECT_NAME@ - + # Compute paths get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt") @@ -11,7 +11,11 @@ else() # Find installed library set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory") endif() - + +# Find dependencies, required by cmake exported targets: +include(CMakeFindDependencyMacro) +find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@) + # Load exports include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake) diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index e287087d1..2afc6edc2 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -22,7 +22,10 @@ include_directories(BEFORE "${PROJECT_SOURCE_DIR}") ################################################################################### # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package -include_directories(${GTSAM_INCLUDE_DIR}) +# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets +# that automatically do include the include_directories() without the need +# to call include_directories(), just target_link_libraries(NAME gtsam) +#include_directories(${GTSAM_INCLUDE_DIR}) ################################################################################### # Build static library from common sources diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d369009c0..034268fdb 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -103,14 +103,70 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}") add_library(gtsam ${gtsam_srcs}) target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) -target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS}) -target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS}) +target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE}) +target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC}) +if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "") + target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC}) +endif() +target_compile_options(gtsam PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) +# Append Eigen include path, set in top-level CMakeLists.txt to either +# system-eigen, or GTSAM eigen path +if (GTSAM_USE_SYSTEM_EIGEN) + target_include_directories(gtsam PUBLIC + $ + ) +else() + target_include_directories(gtsam PUBLIC + $ + ) +endif() +target_include_directories(gtsam PUBLIC + $ +) +# MKL include dir: +if (GTSAM_USE_EIGEN_MKL) + target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) +endif() + +if(GTSAM_USE_TBB) + target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS}) +endif() + +# Add includes for source directories 'BEFORE' boost and any system include +# paths so that the compiler uses GTSAM headers in our source directory instead +# of any previously installed GTSAM headers. +target_include_directories(gtsam BEFORE PUBLIC + # SuiteSparse_config + $ + $ + # CCOLAMD + $ + $ + # main gtsam includes: + $ + $ + # config.h + $ + # unit tests: + $ +) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + target_include_directories(gtsam BEFORE PUBLIC + $ + $ + $ + $ + ) +endif() + + + if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if (NOT BUILD_SHARED_LIBS) set_target_properties(gtsam PROPERTIES diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index a1a8e3a90..b7990b3cc 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -9,18 +9,6 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") set (GTSAM_USE_TBB @GTSAM_USE_TBB@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) -if("@GTSAM_USE_TBB@") - list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@") -endif() - -# Append Eigen include path, set in top-level CMakeLists.txt to either -# system-eigen, or GTSAM eigen path -list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") - -if("@GTSAM_USE_EIGEN_MKL@") - list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") -endif() - if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") endif() diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 04d471b39..df8dc209f 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -1,22 +1,30 @@ # Build/install Wrap -set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY}) +set(WRAP_BOOST_LIBRARIES + Boost::system + Boost::filesystem + Boost::thread +) # Allow for disabling serialization to handle errors related to Clang's linker option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON) -if (NOT GTSAM_WRAP_SERIALIZATION) - add_definitions(-DWRAP_DISABLE_SERIALIZE) -endif() # Build the executable itself file(GLOB wrap_srcs "*.cpp") file(GLOB wrap_headers "*.h") list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp) add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers}) +target_include_directories(wrap_lib PUBLIC + $ +) +if (NOT GTSAM_WRAP_SERIALIZATION) + target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE) +endif() + target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES}) gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers}) add_executable(wrap wrap.cpp) -target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES}) +target_link_libraries(wrap PRIVATE wrap_lib) # Set folder in Visual Studio file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") @@ -32,4 +40,3 @@ install(FILES matlab.h DESTINATION include/wrap) # Build tests add_subdirectory(tests) - From 6723b481a6e2985ed6204e4520b5bc57c2dd6727 Mon Sep 17 00:00:00 2001 From: jlblancoc Date: Sun, 17 Feb 2019 00:58:35 +0100 Subject: [PATCH 14/97] fix missing Eigen in Cython wrapper --- CMakeLists.txt | 8 ++++++-- cython/gtsam_eigency/CMakeLists.txt | 11 +++++++++++ cython/gtsam_eigency/__init__.py.in | 2 +- gtsam/CMakeLists.txt | 12 ++---------- 4 files changed, 20 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d084840be..c1a421a31 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -252,7 +252,7 @@ if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") + set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}") # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches @@ -266,6 +266,8 @@ if(GTSAM_USE_SYSTEM_EIGEN) message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.") endif() + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}") else() # Use bundled Eigen include path. # Clear any variables set by FindEigen3 @@ -275,8 +277,10 @@ else() # set full path to be used by external projects # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in - set(GTSAM_EIGEN_INCLUDE_PREFIX "include/gtsam/3rdparty/Eigen/") + set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/") + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/") endif() if (MSVC) diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt index 54b7de9aa..4bff567eb 100644 --- a/cython/gtsam_eigency/CMakeLists.txt +++ b/cython/gtsam_eigency/CMakeLists.txt @@ -22,6 +22,17 @@ cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conv "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "") cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core" ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "") + +# Include Eigen headers: +target_include_directories(cythonize_eigency_conversions PUBLIC + $ + $ +) +target_include_directories(cythonize_eigency_core PUBLIC + $ + $ +) + add_dependencies(cythonize_eigency_core cythonize_eigency_conversions) add_custom_target(cythonize_eigency) add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core) diff --git a/cython/gtsam_eigency/__init__.py.in b/cython/gtsam_eigency/__init__.py.in index dd278d128..a59d51eab 100644 --- a/cython/gtsam_eigency/__init__.py.in +++ b/cython/gtsam_eigency/__init__.py.in @@ -1,7 +1,7 @@ import os import numpy as np -__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_PREFIX}" +__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}" def get_includes(include_eigen=True): root = os.path.dirname(__file__) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 034268fdb..b4adc9978 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -117,17 +117,9 @@ set_target_properties(gtsam PROPERTIES # Append Eigen include path, set in top-level CMakeLists.txt to either # system-eigen, or GTSAM eigen path -if (GTSAM_USE_SYSTEM_EIGEN) - target_include_directories(gtsam PUBLIC - $ - ) -else() - target_include_directories(gtsam PUBLIC - $ - ) -endif() target_include_directories(gtsam PUBLIC - $ + $ + $ ) # MKL include dir: if (GTSAM_USE_EIGEN_MKL) From dbc07997651b8bc7343d95ec661c1d10566d1ff9 Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 01:57:39 -0500 Subject: [PATCH 15/97] Init uncomment of fixed lag smoother --- gtsam_unstable/gtsam_unstable.h | 250 ++++++++++++++++---------------- 1 file changed, 125 insertions(+), 125 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 39c910826..f1bb4005d 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,132 +505,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -// #include -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +#include +gtsam::GaussianFactorGraph* summarizeGraphSequential( + const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); +gtsam::GaussianFactorGraph* summarizeGraphSequential( + const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); -// #include -// class FixedLagSmootherKeyTimestampMapValue { -// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); -// FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -// }; -// -// class FixedLagSmootherKeyTimestampMap { -// FixedLagSmootherKeyTimestampMap(); -// FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); -// -// // Note: no print function -// -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); -// -// double at(const gtsam::Key& key) const; -// void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -// }; -// -// class FixedLagSmootherResult { -// size_t getIterations() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// #include -// virtual class FixedLagSmoother { -// void print(string s) const; -// bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; -// -// gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; -// double smootherLag() const; -// -// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { -// BatchFixedLagSmoother(); -// BatchFixedLagSmoother(double smootherLag); -// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// -// gtsam::LevenbergMarquardtParams params() const; -// }; -// -// #include -// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { -// IncrementalFixedLagSmoother(); -// IncrementalFixedLagSmoother(double smootherLag); -// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// -// gtsam::ISAM2Params params() const; -// }; -// -// #include -// virtual class ConcurrentFilter { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -// }; -// -// virtual class ConcurrentSmoother { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -// }; -// -// // Synchronize function -// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// -// #include -// class ConcurrentBatchFilterResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { -// ConcurrentBatchFilter(); -// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchFilterResult update(); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// class ConcurrentBatchSmootherResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { -// ConcurrentBatchSmoother(); -// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchSmootherResult update(); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::Values calculateEstimate() const; -// }; +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; + +class FixedLagSmootherKeyTimestampMap { + FixedLagSmootherKeyTimestampMap(); + FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + double at(const gtsam::Key& key) const; + void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +}; + +class FixedLagSmootherResult { + size_t getIterations() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +#include +virtual class FixedLagSmoother { + void print(string s) const; + bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; + + gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; + double smootherLag() const; + + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; //************************************************************************* // slam From 93fd884aa74c5db01b95e7d11bcf3a595c97e4ae Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 04:37:02 -0500 Subject: [PATCH 16/97] Implement and add example --- .../examples/FixedLagSmootherExample.py | 86 +++++++++++++++++++ gtsam_unstable/gtsam_unstable.h | 14 ++- 2 files changed, 92 insertions(+), 8 deletions(-) create mode 100644 cython/gtsam_unstable/examples/FixedLagSmootherExample.py diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py new file mode 100644 index 000000000..5d44dab64 --- /dev/null +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -0,0 +1,86 @@ +""" +Demonstration of the fixed-lag smoothers using a planar robot example +and multiple odometry-like sensors +Author: Frank Dellaert (C++), Jeremy Aguilon (Python) +""" + +import numpy as np + +import gtsam +import gtsam_unstable + +# Create noise models +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) + + +def BatchFixedLagSmootherExample(): + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors + smoother_batch.update(new_factors, new_values, new_timestamps) + + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + + time += delta_time + +if __name__ == '__main__': + BatchFixedLagSmootherExample() + print("Example complete") diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index f1bb4005d..d77895d86 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,15 +505,9 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -gtsam::GaussianFactorGraph* summarizeGraphSequential( - const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -gtsam::GaussianFactorGraph* summarizeGraphSequential( - const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); - #include class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); }; @@ -528,7 +522,7 @@ class FixedLagSmootherKeyTimestampMap { bool empty() const; void clear(); - double at(const gtsam::Key& key) const; + double at(const size_t key) const; void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); }; @@ -558,6 +552,10 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; }; #include From 42ac0e589e233f7f476e0240014df9a1d8ed38af Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 04:55:15 -0500 Subject: [PATCH 17/97] Implement unit test --- .../examples/FixedLagSmootherExample.py | 6 +- cython/gtsam_unstable/examples/__init__.py | 0 cython/gtsam_unstable/tests/__init__.py | 0 .../tests/test_FixedLagSmootherExample.py | 94 +++++++++++++++++++ 4 files changed, 96 insertions(+), 4 deletions(-) create mode 100644 cython/gtsam_unstable/examples/__init__.py create mode 100644 cython/gtsam_unstable/tests/__init__.py create mode 100644 cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 5d44dab64..b9777a07c 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -9,9 +9,6 @@ import numpy as np import gtsam import gtsam_unstable -# Create noise models -PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) def _timestamp_key_value(key, value): return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( @@ -35,8 +32,9 @@ def BatchFixedLagSmootherExample(): # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) X1 = 0 - new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE)) + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) new_timestamps.insert(_timestamp_key_value(X1, 0.0)) diff --git a/cython/gtsam_unstable/examples/__init__.py b/cython/gtsam_unstable/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/__init__.py b/cython/gtsam_unstable/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py new file mode 100644 index 000000000..64895f4d3 --- /dev/null +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -0,0 +1,94 @@ +import unittest +import gtsam +import gtsam_unstable +import numpy as np + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) +class TestFixedLagSmootherExample(unittest.TestCase): + # Simple test that checks for equality between C++ example + # file and the Python implementation + def test_FixedLagSmootherExample(self): + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + i = 0 + + ground_truth = [ + gtsam.Pose2(0.49792, 0.007802, 0.015), + gtsam.Pose2(0.99547, 0.023019, 0.03), + gtsam.Pose2(1.4928, 0.045725, 0.045), + gtsam.Pose2(1.9898, 0.075888, 0.06), + gtsam.Pose2(2.4863, 0.1135, 0.075), + gtsam.Pose2(2.9821, 0.15856, 0.09), + gtsam.Pose2(3.4772, 0.21105, 0.105), + gtsam.Pose2(3.9715, 0.27096, 0.12), + gtsam.Pose2(4.4648, 0.33827, 0.135), + gtsam.Pose2(4.957, 0.41298, 0.15), + gtsam.Pose2(5.4481, 0.49506, 0.165), + gtsam.Pose2(5.9379, 0.5845, 0.18), + ] + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors + smoother_batch.update(new_factors, new_values, new_timestamps) + + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + print("PASS") + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + i += 1 + +if __name__ == "__main__": + unittest.main() From 5e97efa81518f3ecc83d83d43e425edbe951ceed Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 27 Feb 2019 13:04:38 -0500 Subject: [PATCH 18/97] Liner and update Cmakelists --- THANKS | 1 + cython/CMakeLists.txt | 3 +-- cython/gtsam_unstable/__init__.py.in | 1 + .../examples/FixedLagSmootherExample.py | 19 +++++++++++++++++++ .../tests/test_FixedLagSmootherExample.py | 12 +++++++++--- 5 files changed, 31 insertions(+), 5 deletions(-) create mode 100644 cython/gtsam_unstable/__init__.py.in diff --git a/THANKS b/THANKS index f84cfa185..689074b6f 100644 --- a/THANKS +++ b/THANKS @@ -1,5 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: +* Jeremy Aguilon * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index bc21b91d1..b832a76a5 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -26,8 +26,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) endif() # Install the custom-generated __init__.py diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in new file mode 100644 index 000000000..2b0a28321 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py.in @@ -0,0 +1 @@ +from gtsam_unstable import * diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index b9777a07c..5fc3bc98d 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -1,4 +1,11 @@ """ +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 + Demonstration of the fixed-lag smoothers using a planar robot example and multiple odometry-like sensors Author: Frank Dellaert (C++), Jeremy Aguilon (Python) @@ -11,12 +18,21 @@ import gtsam_unstable def _timestamp_key_value(key, value): + """ + + """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value ) def BatchFixedLagSmootherExample(): + """ + Runs a batch fixed smoother on an agent with two odometry + sensors that is simply moving along the x axis in constant + speed. + """ + # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 @@ -41,6 +57,9 @@ def BatchFixedLagSmootherExample(): delta_time = 0.25 time = 0.25 + # Iterates from 0.25s to 3.0s, adding 0.25s each loop + # In each iteration, the agent moves at a constant speed + # and its two odometers measure the change. while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 64895f4d3..8994913a2 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -8,9 +8,16 @@ def _timestamp_key_value(key, value): key, value ) class TestFixedLagSmootherExample(unittest.TestCase): - # Simple test that checks for equality between C++ example - # file and the Python implementation + ''' + Tests the fixed lag smoother wrapper + ''' + def test_FixedLagSmootherExample(self): + ''' + Simple test that checks for equality between C++ example + file and the Python implementation. See + gtsam_unstable/examples/FixedLagSmootherExample.cpp + ''' # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 @@ -81,7 +88,6 @@ class TestFixedLagSmootherExample(unittest.TestCase): estimate = smoother_batch.calculateEstimatePose2(current_key) self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) - print("PASS") new_timestamps.clear() new_values.clear() From 1ca14d516416be6efb460c9695efa2bf773460c8 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 27 Feb 2019 13:08:49 -0500 Subject: [PATCH 19/97] Add comment --- THANKS | 2 +- cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/THANKS b/THANKS index 689074b6f..f2b51f61d 100644 --- a/THANKS +++ b/THANKS @@ -1,6 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: -* Jeremy Aguilon +* Jeremy Aguilon, Facebook * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 8994913a2..e26c450a0 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -58,6 +58,10 @@ class TestFixedLagSmootherExample(unittest.TestCase): gtsam.Pose2(5.4481, 0.49506, 0.165), gtsam.Pose2(5.9379, 0.5845, 0.18), ] + # Iterates from 0.25s to 3.0s, adding 0.25s each loop + # In each iteration, the agent moves at a constant speed + # and its two odometers measure the change. The smoothed + # result is then compared to the ground truth while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time From cca445711cf0daddb6863a75aeea0c7467a86457 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Feb 2019 13:20:42 -0500 Subject: [PATCH 20/97] Updated INSTALL file to use Markdown syntax and applied correct formatting --- INSTALL | 146 ----------------------------------------------------- INSTALL.md | 143 +++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 143 insertions(+), 146 deletions(-) delete mode 100644 INSTALL create mode 100644 INSTALL.md diff --git a/INSTALL b/INSTALL deleted file mode 100644 index d2f0b68df..000000000 --- a/INSTALL +++ /dev/null @@ -1,146 +0,0 @@ -Quickstart - -In the root library folder execute: - -$] mkdir build -$] cd build -$] cmake .. -$] make check (optional, runs unit tests) -$] make install - -Important Installation Notes ----------------------------- - -1) -GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) - - Cmake version 3.0 or higher - - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher - -Optional dependent libraries: - - If TBB is installed and detectable by CMake GTSAM will use it automatically. - Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, - disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB - may be installed from the Ubuntu repositories, and for other platforms it - may be downloaded from https://www.threadingbuildingblocks.org/ - -Tested compilers: - -- GCC 4.2-4.7 -- OSX Clang 2.9-5.0 -- OSX GCC 4.2 -- MSVC 2010, 2012 - -Tested systems: - -- Ubuntu 11.04 - 18.04 -- MacOS 10.6 - 10.9 -- Windows 7, 8, 8.1, 10 - -Known issues: - -- MSVC 2013 is not yet supported because it cannot build the serialization module - of Boost 1.55 (or earlier). - -2) -GTSAM makes extensive use of debug assertions, and we highly recommend you work -in Debug mode while developing (enabled by default). Likewise, it is imperative -that you switch to release mode when running finished code and for timing. GTSAM -will run up to 10x faster in Release mode! See the end of this document for -additional debugging tips. - -3) -GTSAM has Doxygen documentation. To generate, run 'make doc' from your -build directory. - -4) -The instructions below install the library to the default system install path and -build all components. From a terminal, starting in the root library folder, -execute commands as follows for an out-of-source build: - -$] mkdir build -$] cd build -$] cmake .. -$] make check (optional, runs unit tests) -$] make install - -This will build the library and unit tests, run all of the unit tests, -and then install the library itself. - -- CMake Configuration Options and Details - -GTSAM has a number of options that can be configured, which is best done with -one of the following: - - ccmake the curses GUI for cmake - cmake-gui a real GUI for cmake - -Important Options: - -CMAKE_BUILD_TYPE: We support several build configurations for GTSAM (case insensitive) - Debug (default) All error checking options on, no optimization. Use for development. - Release Optimizations turned on, no debug symbols. - Timing Adds ENABLE_TIMING flag to provide statistics on operation - Profiling Standard configuration for use during profiling - RelWithDebInfo Same as Release, but with the -g flag for debug symbols - -CMAKE_INSTALL_PREFIX: The install folder. The default is typically /usr/local/ -To configure to install to your home directory, you could execute: -$] cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME .. - -GTSAM_TOOLBOX_INSTALL_PATH: The Matlab toolbox will be installed in a subdirectory -of this folder, called 'gtsam'. -$] cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox .. - -GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in -subfolders to be linked against convenience libraries rather than the full libgtsam. -Set with the command line as follows: -$] cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON .. - ON (Default) This builds convenience libraries and links tests against them. This - option is suggested for gtsam developers, as it is possible to build - and run tests without first building the rest of the library, and - speeds up compilation for a single test. The downside of this option - is that it will build the entire library again to build the full - libgtsam library, so build/install will be slower. - OFF This will build all of libgtsam before any of the tests, and then - link all of the tests at once. This option is best for users of GTSAM, - as it avoids rebuilding the entirety of gtsam an extra time. - -GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library. -Set with the command line as follows: -$] cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON .. - ON When enabled, libgtsam_unstable will be built and installed with the - same options as libgtsam. In addition, if tests are enabled, the - unit tests will be built as well. The Matlab toolbox will also - be generated if the matlab toolbox is enabled, installing into a - folder called "gtsam_unstable". - OFF (Default) If disabled, no gtsam_unstable code will be included in build or install. - -Check - -"make check" will build and run all of the tests. Note that the tests will only be -built when using the "check" targets, to prevent "make install" from building the tests -unnecessarily. You can also run "make timing" to build all of the timing scripts. -To run check on a particular module only, run "make check.[subfolder]", so to run -just the geometry tests, run "make check.geometry". Individual tests can be run by -appending ".run" to the name of the test, for example, to run testMatrix, run -"make testMatrix.run". - -MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your -shell's PATH environment variable. mex is installed with matlab at -$MATLABROOT/bin/mex - -$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB - -Debugging tips: - -Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks -and safe containers in the standard C++ library and makes problems much easier -to find. - -NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes -it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though. - -NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against -gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of -header-only Eigen. diff --git a/INSTALL.md b/INSTALL.md new file mode 100644 index 000000000..e19363c7e --- /dev/null +++ b/INSTALL.md @@ -0,0 +1,143 @@ +# Quickstart + +In the root library folder execute: + +```sh +$ mkdir build +$ cd build +$ cmake .. +$ make check # (optional, runs unit tests) +$ make install +``` + +## Important Installation Notes + +1. GTSAM requires the following libraries to be installed on your system: + - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) + - Cmake version 3.0 or higher + - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher + + Optional dependent libraries: + - If TBB is installed and detectable by CMake GTSAM will use it automatically. + Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, + disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB + may be installed from the Ubuntu repositories, and for other platforms it + may be downloaded from https://www.threadingbuildingblocks.org/ + + Tested compilers: + + - GCC 4.2-4.7 + - OSX Clang 2.9-5.0 + - OSX GCC 4.2 + - MSVC 2010, 2012 + + Tested systems: + + - Ubuntu 11.04 - 18.04 + - MacOS 10.6 - 10.9 + - Windows 7, 8, 8.1, 10 + + Known issues: + + - MSVC 2013 is not yet supported because it cannot build the serialization module + of Boost 1.55 (or earlier). + +2. GTSAM makes extensive use of debug assertions, and we highly recommend you work +in Debug mode while developing (enabled by default). Likewise, it is imperative +that you switch to release mode when running finished code and for timing. GTSAM +will run up to 10x faster in Release mode! See the end of this document for +additional debugging tips. + +3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your +build directory. + +4. The instructions below install the library to the default system install path and +build all components. From a terminal, starting in the root library folder, +execute commands as follows for an out-of-source build: + + ```sh + $ mkdir build + $ cd build + $ cmake .. + $ make check (optional, runs unit tests) + $ make install + ``` + + This will build the library and unit tests, run all of the unit tests, + and then install the library itself. + +## CMake Configuration Options and Details + +GTSAM has a number of options that can be configured, which is best done with +one of the following: + + - ccmake the curses GUI for cmake + - cmake-gui a real GUI for cmake + +### Important Options: + +#### CMAKE_BUILD_TYPE +We support several build configurations for GTSAM (case insensitive) + +```cmake -DCMAKE_BUILD_TYPE=[Option] ..``` + + - Debug (default) All error checking options on, no optimization. Use for development. + - Release Optimizations turned on, no debug symbols. + - Timing Adds ENABLE_TIMING flag to provide statistics on operation + - Profiling Standard configuration for use during profiling + - RelWithDebInfo Same as Release, but with the -g flag for debug symbols + +#### CMAKE_INSTALL_PREFIX + +The install folder. The default is typically `/usr/local/`. +To configure to install to your home directory, you could execute: + +```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..``` + +#### GTSAM_TOOLBOX_INSTALL_PATH + +The Matlab toolbox will be installed in a subdirectory +of this folder, called 'gtsam'. + +```cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..``` + +#### GTSAM_BUILD_CONVENIENCE_LIBRARIES + +This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. +Set with the command line as follows: + +```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..``` + - ON (Default): This builds convenience libraries and links tests against them. This option is suggested for gtsam developers, as it is possible to build and run tests without first building the rest of the library, and speeds up compilation for a single test. The downside of this option is that it will build the entire library again to build the full libgtsam library, so build/install will be slower. + - OFF: This will build all of libgtsam before any of the tests, and then link all of the tests at once. This option is best for users of GTSAM, as it avoids rebuilding the entirety of gtsam an extra time. + +#### GTSAM_BUILD_UNSTABLE + +Enable build and install for libgtsam_unstable library. +Set with the command line as follows: + +```cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..``` + + ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`. + OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install. + +## Check + +`make check` will build and run all of the tests. Note that the tests will only be +built when using the "check" targets, to prevent `make install` from building the tests +unnecessarily. You can also run `make timing` to build all of the timing scripts. +To run check on a particular module only, run `make check.[subfolder]`, so to run +just the geometry tests, run `make check.geometry`. Individual tests can be run by +appending `.run` to the name of the test, for example, to run testMatrix, run +`make testMatrix.run`. + +MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your shell's PATH environment variable. mex is installed with matlab at `$MATLABROOT/bin/mex` + +$MATLABROOT can be found by executing the command `matlabroot` in MATLAB + +## Debugging tips + +Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find. + +NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though. + +NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen. From 2eee111960d46b5cf90bfd68dabb72210817a4a7 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 27 Feb 2019 13:58:40 -0500 Subject: [PATCH 21/97] Forgotten docstring --- cython/gtsam_unstable/examples/FixedLagSmootherExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 5fc3bc98d..141162044 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -19,7 +19,7 @@ import gtsam_unstable def _timestamp_key_value(key, value): """ - + Creates a key value pair for a FixedLagSmootherKeyTimeStampMap """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value From dae50c9a4a7702065e46e789b3bf4ef275991546 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Feb 2019 14:23:18 -0500 Subject: [PATCH 22/97] updated link in README.md to point to correct INSTALL.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e815d7f4b..7e03c81f2 100644 --- a/README.md +++ b/README.md @@ -71,7 +71,7 @@ Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTS which support (superfast) automatic differentiation, can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home). -See the [`INSTALL`](INSTALL) file for more detailed installation instructions. +See the [`INSTALL`](INSTALL.md) file for more detailed installation instructions. GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. From 27d47c32bb65c22ac6929a23a33afeed1405c16e Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 23:38:59 -0500 Subject: [PATCH 23/97] Update cmaklists --- cython/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index b832a76a5..9acf8174c 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -27,7 +27,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable") endif() + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable From 5670c73158ac2a8c8a3360819d881d3b76889d44 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Tue, 8 Jan 2019 10:28:19 +0000 Subject: [PATCH 24/97] improved cython wrapper python3 support --- cmake/FindCython.cmake | 4 ++-- cython/gtsam/__init__.py.in | 2 +- wrap/Module.cpp | 4 ++++ wrap/tests/expected-cython/geometry.pyx | 2 ++ 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake index 23afb00e6..292d9d51e 100644 --- a/cmake/FindCython.cmake +++ b/cmake/FindCython.cmake @@ -32,7 +32,7 @@ find_package( PythonInterp ) if ( PYTHONINTERP_FOUND ) execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print Cython.__path__" + "import Cython; print(Cython.__path__[0])" RESULT_VARIABLE RESULT OUTPUT_VARIABLE CYTHON_PATH OUTPUT_STRIP_TRAILING_WHITESPACE @@ -51,7 +51,7 @@ endif () # RESULT=0 means ok if ( NOT RESULT ) execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print Cython.__version__" + "import Cython; print(Cython.__version__)" RESULT_VARIABLE RESULT OUTPUT_VARIABLE CYTHON_VAR_OUTPUT ERROR_VARIABLE CYTHON_VAR_OUTPUT diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in index 7d456023f..85451c680 100644 --- a/cython/gtsam/__init__.py.in +++ b/cython/gtsam/__init__.py.in @@ -1,2 +1,2 @@ -from gtsam import * +from .gtsam import * ${GTSAM_UNSTABLE_IMPORT} diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a3b8df630..b2b1dc1dc 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -394,6 +394,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const { /* ************************************************************************* */ void Module::emit_cython_pyx(FileWriter& pyxFile) const { + // directives... + // allow str to automatically coerce to std::string and back (for python3) + pyxFile.oss << "# cython: c_string_type=str, c_string_encoding=ascii\n\n"; + // headers... string pxdHeader = name; pyxFile.oss << "cimport numpy as np\n" diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index 4bd14b130..7c20a500d 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -1,3 +1,5 @@ +# cython: c_string_type=str, c_string_encoding=ascii + cimport numpy as np import numpy as npp cimport geometry From 56ef410276e6dd8fb7ab101b543500300314df6e Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Tue, 8 Jan 2019 10:44:49 +0000 Subject: [PATCH 25/97] adding MKL installation instructions to README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7e03c81f2..64b36878e 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,7 @@ Prerequisites: Optional prerequisites - used automatically if findable by CMake: - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo)) GTSAM 4 Compatibility --------------------- From b63537a47c8f7952a49f8f05fa669de537ada913 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 28 Jan 2019 16:30:29 +0000 Subject: [PATCH 26/97] fixed cython import for gtsam_unstable --- cython/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index bc21b91d1..e53c0b73e 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -19,7 +19,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # wrap gtsam_unstable if(GTSAM_BUILD_UNSTABLE) add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h") - set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *") + set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path From e896ae1c43376b5fc576061ece1dbac79146d236 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 30 Jan 2019 09:41:28 +0000 Subject: [PATCH 27/97] compile cython compatible with the chosen python version --- cmake/GtsamCythonWrap.cmake | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 73e2b63e0..520d72a32 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -35,6 +35,11 @@ function(set_up_required_cython_packages) include_directories(${NUMPY_INCLUDE_DIRS}) endfunction() +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "from __future__ import print_function;import sys;print(sys.version[0], end='')" + OUTPUT_VARIABLE PYTHON_MAJOR_VERSION +) + # Convert pyx to cpp by executing cython # This is the first step to compile cython from the command line # as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html @@ -52,7 +57,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs) add_custom_command( OUTPUT ${generated_cpp} COMMAND - ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp} + ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp} VERBATIM) add_custom_target(${target} ALL DEPENDS ${generated_cpp}) endfunction() From 70470ff59ba9d1c318db506d7c1f023e627e42e5 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 30 Jan 2019 09:49:38 +0000 Subject: [PATCH 28/97] fixed more python 3 related import problems --- wrap/Module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index b2b1dc1dc..a7db9e1f6 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -403,9 +403,9 @@ void Module::emit_cython_pyx(FileWriter& pyxFile) const { pyxFile.oss << "cimport numpy as np\n" "import numpy as npp\n" "cimport " << pxdHeader << "\n" - "from "<< pxdHeader << " cimport shared_ptr\n" - "from "<< pxdHeader << " cimport dynamic_pointer_cast\n" - "from "<< pxdHeader << " cimport make_shared\n"; + "from ."<< pxdHeader << " cimport shared_ptr\n" + "from ."<< pxdHeader << " cimport dynamic_pointer_cast\n" + "from ."<< pxdHeader << " cimport make_shared\n"; pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n" "cdef list process_args(list keywords, tuple args, dict kwargs):\n" From 8df2c0a9a10d295d6cf6d821016e91296fb1ce19 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 30 Jan 2019 10:03:40 +0000 Subject: [PATCH 29/97] updated wrap test expected output --- wrap/tests/expected-cython/geometry.pyx | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index 7c20a500d..cae19d600 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -3,9 +3,9 @@ cimport numpy as np import numpy as npp cimport geometry -from geometry cimport shared_ptr -from geometry cimport dynamic_pointer_cast -from geometry cimport make_shared +from .geometry cimport shared_ptr +from .geometry cimport dynamic_pointer_cast +from .geometry cimport make_shared # C helper function that copies all arguments into a positional list. cdef list process_args(list keywords, tuple args, dict kwargs): cdef str keyword From a79e8654757fa72260956d98967deef7309124a7 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 14:48:24 +0000 Subject: [PATCH 30/97] added note about python interpreter version to README --- cython/README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/cython/README.md b/cython/README.md index 368d2a76d..3a7f65bca 100644 --- a/cython/README.md +++ b/cython/README.md @@ -2,20 +2,22 @@ This is the Cython/Python wrapper around the GTSAM C++ library. INSTALL ======= +- if you want to build the gtsam python library for a specific python version, use the `-DPYTHON_EXECUTABLE:FILEPATH=/path/to/python_interpreter` option when running `cmake` otherwise the interpreter at `$ which python` will be used. + - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam. - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows: ```bash pip install -r /cython/requirements.txt ``` -- For compatiblity with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git), +- For compatibility with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git), named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy. -- Build and install gtsam using cmake with GTSAM_INSTALL_CYTHON_TOOLBOX enabled. -The wrapped module will be installed to GTSAM_CYTHON_INSTALL_PATH, which is -by default: /cython +- Build and install gtsam using cmake with `GTSAM_INSTALL_CYTHON_TOOLBOX` enabled. +The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is +by default: `/cython` -- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH: +- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: ```bash export PYTHONPATH=$PYTHONPATH: ``` From 43ac8ad3438d619764c3b2c010a6d1a6fe90c0c3 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:54:33 +0000 Subject: [PATCH 31/97] made experiment script compatible with python 2 and 3 --- cython/gtsam/tests/experiments.py | 113 ++++++++++++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 cython/gtsam/tests/experiments.py diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py new file mode 100644 index 000000000..db3127a83 --- /dev/null +++ b/cython/gtsam/tests/experiments.py @@ -0,0 +1,113 @@ +""" +This file is not a real python unittest. It contains small experiments +to test the wrapper with gtsam_test, a short version of gtsam.h. +Its name convention is different from other tests so it won't be discovered. +""" +from __future__ import print_function +import gtsam +import numpy as np + +r = gtsam.Rot3() +print(r) +print(r.pitch()) +r2 = gtsam.Rot3() +r3 = r.compose(r2) +print("r3 pitch:", r3.pitch()) + +v = np.array([1, 1, 1]) +print("v = ", v) +r4 = r3.retract(v) +print("r4 pitch:", r4.pitch()) +r4.print_(b'r4: ') +r3.print_(b"r3: ") + +v = r3.localCoordinates(r4) +print("localCoordinates:", v) + +Rmat = np.array([ + [0.990074, -0.0942928, 0.104218], + [0.104218, 0.990074, -0.0942928], + [-0.0942928, 0.104218, 0.990074] + ]) +r5 = gtsam.Rot3(Rmat) +r5.print_(b"r5: ") + +l = gtsam.Rot3.Logmap(r5) +print("l = ", l) + + +noise = gtsam.noiseModel_Gaussian.Covariance(Rmat) +noise.print_(b"noise:") + +D = np.array([1.,2.,3.]) +diag = gtsam.noiseModel_Diagonal.Variances(D) +print("diag:", diag) +diag.print_(b"diag:") +print("diag R:", diag.R()) + +p = gtsam.Point3() +p.print_("p:") +factor = gtsam.BetweenFactorPoint3(1,2,p, noise) +factor.print_(b"factor:") + +vv = gtsam.VectorValues() +vv.print_(b"vv:") +vv.insert(1, np.array([1.,2.,3.])) +vv.insert(2, np.array([3.,4.])) +vv.insert(3, np.array([5.,6.,7.,8.])) +vv.print_(b"vv:") + +vv2 = gtsam.VectorValues(vv) +vv2.insert(4, np.array([4.,2.,1])) +vv2.print_(b"vv2:") +vv.print_(b"vv:") + +vv.insert(4, np.array([1.,2.,4.])) +vv.print_(b"vv:") +vv3 = vv.add(vv2) + +vv3.print_(b"vv3:") + +values = gtsam.Values() +values.insert(1, gtsam.Point3()) +values.insert(2, gtsam.Rot3()) +values.print_(b"values:") + +factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag) +print("Prior factor vector: ", factor) + + + +keys = gtsam.KeyVector() + +keys.push_back(1) +keys.push_back(2) +print('size: ', keys.size()) +print(keys.at(0)) +print(keys.at(1)) + +noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) +noise.print_('noise:') +print('noise print:', noise) +f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) +print('JacobianFactor(7):\n', f) +print("A = ", f.getA()) +print("b = ", f.getb()) + +f = gtsam.JacobianFactor(np.ones(2)) +f.print_('jacoboian b_in:') + + +print("JacobianFactor initalized with b_in:", f) + +diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) +fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag) +print("priorfactorvector: ", fv) + +print("base noise: ", fv.get_noiseModel()) +print("casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())) + +X = gtsam.symbol(65, 19) +print(X) +print(gtsam.symbolChr(X)) +print(gtsam.symbolIndex(X)) From 46eed55448fb753b7afc8d9ef8613f410faea8f0 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 09:17:55 +0000 Subject: [PATCH 32/97] removed MKL installation instructions --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 64b36878e..7e03c81f2 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,7 @@ Prerequisites: Optional prerequisites - used automatically if findable by CMake: - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo)) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) GTSAM 4 Compatibility --------------------- From e251dbaebd95cae3463b62e7dcfbc4c33e8f589b Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 09:27:09 +0000 Subject: [PATCH 33/97] re-adding flags to pass to cmake to use correct python version --- cython/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/README.md b/cython/README.md index 3a7f65bca..896025124 100644 --- a/cython/README.md +++ b/cython/README.md @@ -2,7 +2,7 @@ This is the Cython/Python wrapper around the GTSAM C++ library. INSTALL ======= -- if you want to build the gtsam python library for a specific python version, use the `-DPYTHON_EXECUTABLE:FILEPATH=/path/to/python_interpreter` option when running `cmake` otherwise the interpreter at `$ which python` will be used. +- if you want to build the gtsam python library for python 3, use the `-DPython_ADDITIONAL_VERSIONS=3` option when running `cmake` otherwise the interpreter at `$ which python` will be used. - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam. - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows: From a1fba6a5b1e3f6627414b84201ca3d376cd7bcb4 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 09:28:34 +0000 Subject: [PATCH 34/97] removed experiments.py --- cython/gtsam/tests/experiments.py | 113 ------------------------------ 1 file changed, 113 deletions(-) delete mode 100644 cython/gtsam/tests/experiments.py diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py deleted file mode 100644 index db3127a83..000000000 --- a/cython/gtsam/tests/experiments.py +++ /dev/null @@ -1,113 +0,0 @@ -""" -This file is not a real python unittest. It contains small experiments -to test the wrapper with gtsam_test, a short version of gtsam.h. -Its name convention is different from other tests so it won't be discovered. -""" -from __future__ import print_function -import gtsam -import numpy as np - -r = gtsam.Rot3() -print(r) -print(r.pitch()) -r2 = gtsam.Rot3() -r3 = r.compose(r2) -print("r3 pitch:", r3.pitch()) - -v = np.array([1, 1, 1]) -print("v = ", v) -r4 = r3.retract(v) -print("r4 pitch:", r4.pitch()) -r4.print_(b'r4: ') -r3.print_(b"r3: ") - -v = r3.localCoordinates(r4) -print("localCoordinates:", v) - -Rmat = np.array([ - [0.990074, -0.0942928, 0.104218], - [0.104218, 0.990074, -0.0942928], - [-0.0942928, 0.104218, 0.990074] - ]) -r5 = gtsam.Rot3(Rmat) -r5.print_(b"r5: ") - -l = gtsam.Rot3.Logmap(r5) -print("l = ", l) - - -noise = gtsam.noiseModel_Gaussian.Covariance(Rmat) -noise.print_(b"noise:") - -D = np.array([1.,2.,3.]) -diag = gtsam.noiseModel_Diagonal.Variances(D) -print("diag:", diag) -diag.print_(b"diag:") -print("diag R:", diag.R()) - -p = gtsam.Point3() -p.print_("p:") -factor = gtsam.BetweenFactorPoint3(1,2,p, noise) -factor.print_(b"factor:") - -vv = gtsam.VectorValues() -vv.print_(b"vv:") -vv.insert(1, np.array([1.,2.,3.])) -vv.insert(2, np.array([3.,4.])) -vv.insert(3, np.array([5.,6.,7.,8.])) -vv.print_(b"vv:") - -vv2 = gtsam.VectorValues(vv) -vv2.insert(4, np.array([4.,2.,1])) -vv2.print_(b"vv2:") -vv.print_(b"vv:") - -vv.insert(4, np.array([1.,2.,4.])) -vv.print_(b"vv:") -vv3 = vv.add(vv2) - -vv3.print_(b"vv3:") - -values = gtsam.Values() -values.insert(1, gtsam.Point3()) -values.insert(2, gtsam.Rot3()) -values.print_(b"values:") - -factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag) -print("Prior factor vector: ", factor) - - - -keys = gtsam.KeyVector() - -keys.push_back(1) -keys.push_back(2) -print('size: ', keys.size()) -print(keys.at(0)) -print(keys.at(1)) - -noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) -noise.print_('noise:') -print('noise print:', noise) -f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) -print('JacobianFactor(7):\n', f) -print("A = ", f.getA()) -print("b = ", f.getb()) - -f = gtsam.JacobianFactor(np.ones(2)) -f.print_('jacoboian b_in:') - - -print("JacobianFactor initalized with b_in:", f) - -diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) -fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag) -print("priorfactorvector: ", fv) - -print("base noise: ", fv.get_noiseModel()) -print("casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())) - -X = gtsam.symbol(65, 19) -print(X) -print(gtsam.symbolChr(X)) -print(gtsam.symbolIndex(X)) From 27f87d340e1a24071c728a37c9a00eb5a837f41b Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 09:45:06 +0000 Subject: [PATCH 35/97] caching cmake variables --- cmake/GtsamCythonWrap.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 520d72a32..bc6f230b0 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -3,6 +3,8 @@ # in the current environment are different from the cached! unset(PYTHON_EXECUTABLE CACHE) unset(CYTHON_EXECUTABLE CACHE) +unset(PYTHON_INCLUDE_DIR CACHE) +unset(PYTHON_MAJOR_VERSION CACHE) find_package(Cython 0.25.2 REQUIRED) # User-friendly Cython wrapping and installing function. From 09ac7f7c069a0c5f6d53d98b87a3e36e8c65d5e9 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 09:55:09 +0000 Subject: [PATCH 36/97] removed requirement for python 2.7 in cmake --- cmake/GtsamCythonWrap.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index bc6f230b0..0f4cb827e 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -31,7 +31,7 @@ endfunction() function(set_up_required_cython_packages) # Set up building of cython module - find_package(PythonLibs 2.7 REQUIRED) + find_package(PythonLibs REQUIRED) include_directories(${PYTHON_INCLUDE_DIRS}) find_package(NumPy REQUIRED) include_directories(${NUMPY_INCLUDE_DIRS}) From e9e8ca39900c84b3c2e4c3986ff591cc494700c4 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 14 Feb 2019 09:45:48 +0000 Subject: [PATCH 37/97] added option to specify python version --- CMakeLists.txt | 2 ++ cmake/FindNumPy.cmake | 8 ++++++++ cmake/GtsamCythonWrap.cmake | 21 ++++++++++++++++----- cython/README.md | 2 +- 4 files changed, 27 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c1a421a31..834ce732e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,7 @@ endif() option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_INSTALL_CYTHON_TOOLBOX "Enable/Disable installation of Cython toolbox" OFF) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper or python module for (or Default)") # Check / set dependent variables for MATLAB wrapper if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP) @@ -554,6 +555,7 @@ endif() message(STATUS "Cython toolbox flags ") print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") +message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") message(STATUS "===============================================================") diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake index eafed165e..4f5743aa6 100644 --- a/cmake/FindNumPy.cmake +++ b/cmake/FindNumPy.cmake @@ -40,9 +40,17 @@ # Finding NumPy involves calling the Python interpreter if(NumPy_FIND_REQUIRED) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") find_package(PythonInterp REQUIRED) + else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) + endif() else() + if(GTSAM_PYTHON_VERSION STREQUAL "Default") find_package(PythonInterp) + else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) + endif() endif() if(NOT PYTHONINTERP_FOUND) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 0f4cb827e..f329d31ab 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -5,8 +5,19 @@ unset(PYTHON_EXECUTABLE CACHE) unset(CYTHON_EXECUTABLE CACHE) unset(PYTHON_INCLUDE_DIR CACHE) unset(PYTHON_MAJOR_VERSION CACHE) + +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + find_package(PythonLibs REQUIRED) +else() + find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) +endif() find_package(Cython 0.25.2 REQUIRED) +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "from __future__ import print_function;import sys;print(sys.version[0], end='')" + OUTPUT_VARIABLE PYTHON_MAJOR_VERSION +) + # User-friendly Cython wrapping and installing function. # Builds a Cython module from the provided interface_header. # For example, for the interface header gtsam.h, @@ -31,16 +42,16 @@ endfunction() function(set_up_required_cython_packages) # Set up building of cython module - find_package(PythonLibs REQUIRED) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + find_package(PythonLibs REQUIRED) + else() + find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) + endif() include_directories(${PYTHON_INCLUDE_DIRS}) find_package(NumPy REQUIRED) include_directories(${NUMPY_INCLUDE_DIRS}) endfunction() -execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" - "from __future__ import print_function;import sys;print(sys.version[0], end='')" - OUTPUT_VARIABLE PYTHON_MAJOR_VERSION -) # Convert pyx to cpp by executing cython # This is the first step to compile cython from the command line diff --git a/cython/README.md b/cython/README.md index 896025124..8ba824f8d 100644 --- a/cython/README.md +++ b/cython/README.md @@ -2,7 +2,7 @@ This is the Cython/Python wrapper around the GTSAM C++ library. INSTALL ======= -- if you want to build the gtsam python library for python 3, use the `-DPython_ADDITIONAL_VERSIONS=3` option when running `cmake` otherwise the interpreter at `$ which python` will be used. +- if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used. - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam. - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows: From 9c1dfd244f2bbab5f1a9038d545612768cd527c6 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 14 Feb 2019 10:55:16 +0000 Subject: [PATCH 38/97] fixed a bug where unsetting the cached python version leads to different numpy/cython/libraries being used --- cmake/GtsamCythonWrap.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index f329d31ab..374e00c46 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -7,8 +7,10 @@ unset(PYTHON_INCLUDE_DIR CACHE) unset(PYTHON_MAJOR_VERSION CACHE) if(GTSAM_PYTHON_VERSION STREQUAL "Default") + find_package(PythonInterp REQUIRED) find_package(PythonLibs REQUIRED) else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) endif() find_package(Cython 0.25.2 REQUIRED) From 6bd8e44fc759e7291c41211ee998e12c2be11cdc Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 17:35:50 -0500 Subject: [PATCH 39/97] init --- THANKS | 1 + cmake/GtsamCythonWrap.cmake | 2 +- cython/CMakeLists.txt | 10 ++++++---- cython/gtsam_unstable/__init__.py.in | 1 + 4 files changed, 9 insertions(+), 5 deletions(-) create mode 100644 cython/gtsam_unstable/__init__.py.in diff --git a/THANKS b/THANKS index f84cfa185..f2b51f61d 100644 --- a/THANKS +++ b/THANKS @@ -1,5 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: +* Jeremy Aguilon, Facebook * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 73e2b63e0..1c9868d0d 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -212,7 +212,7 @@ function(install_cython_scripts source_directory dest_directory patterns) FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) + install(DIRECTORY "${source_directory}/" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index bc21b91d1..7a4f518ef 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -22,19 +22,21 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports - "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable") endif() + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" "*.py") # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" "*.py") endif () + diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in new file mode 100644 index 000000000..85f7c3df2 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py.in @@ -0,0 +1 @@ +${GTSAM_UNSTABLE_IMPORT} From 73681b4d2db05127b691ad6199ca5b9852848c67 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 17:42:30 -0500 Subject: [PATCH 40/97] Revert unneeded change --- cmake/GtsamCythonWrap.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 1c9868d0d..73e2b63e0 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -212,7 +212,7 @@ function(install_cython_scripts source_directory dest_directory patterns) FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}/" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() From 409806efc6d3ffeedb0a0a899b6f8c78f6cf52dc Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 18:07:46 -0500 Subject: [PATCH 41/97] Clumsy mistake - line should be inside if statement --- cython/CMakeLists.txt | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 7a4f518ef..b5f1365c7 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -28,15 +28,18 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) ) configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable") - endif() install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" "*.py") + endif() # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" "*.py") + + message("FOO") + message ("${GTSAM_CYTHON_INSTALL_PATH}") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () From f3baa4d5b008adf5c59eb0e92547e822014222bc Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 18:08:22 -0500 Subject: [PATCH 42/97] Forgot to remove print statement --- cython/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index b5f1365c7..8c2a478db 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -37,8 +37,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - message("FOO") - message ("${GTSAM_CYTHON_INSTALL_PATH}") install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () From ed2300dd397307f571a2b7a5494e0610129837ca Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 18:08:48 -0500 Subject: [PATCH 43/97] Remove unwanted lines --- cython/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 8c2a478db..8e5d38eec 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -36,8 +36,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () - From dc80bc07557dffa8ea95cbe87c432e759c9a680b Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 6 Mar 2019 10:00:13 +0000 Subject: [PATCH 44/97] find correct interpreter version before looking for cython --- cmake/FindCython.cmake | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake index 292d9d51e..e5a32c30d 100644 --- a/cmake/FindCython.cmake +++ b/cmake/FindCython.cmake @@ -29,7 +29,12 @@ # Use the Cython executable that lives next to the Python executable # if it is a local installation. -find_package( PythonInterp ) +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + find_package(PythonInterp) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) +endif() + if ( PYTHONINTERP_FOUND ) execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" "import Cython; print(Cython.__path__[0])" From 2a967c74ab091b55acda3497dfab4ada51e2cf82 Mon Sep 17 00:00:00 2001 From: cbeall Date: Thu, 25 Oct 2018 17:16:01 -0700 Subject: [PATCH 45/97] wip - plotting covariances in 2d --- cython/gtsam/examples/OdometryExample.py | 17 +++++++++++++++++ cython/gtsam/examples/Pose2SLAMExample.py | 10 ++++++++++ cython/gtsam/utils/plot.py | 20 +++++++++++++++++--- 3 files changed, 44 insertions(+), 3 deletions(-) diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py index 5d2190d56..e778e3f85 100644 --- a/cython/gtsam/examples/OdometryExample.py +++ b/cython/gtsam/examples/OdometryExample.py @@ -17,6 +17,9 @@ import numpy as np import gtsam +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + # Create noise models ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -50,3 +53,17 @@ params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() print("\nFinal Result:\n{}".format(result)) + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +for i in range(1, 4): + print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + +fig = plt.figure(0) +for i in range(1, 4): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) +plt.axis('equal') +plt.show() + + + diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index b15b90864..680f2209f 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -19,6 +19,9 @@ import numpy as np import gtsam +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + def vector3(x, y, z): """Create 3d double numpy array.""" @@ -85,3 +88,10 @@ print("Final Result:\n{}".format(result)) marginals = gtsam.Marginals(graph, result) for i in range(1, 6): print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + +fig = plt.figure(0) +for i in range(1, 6): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) + +plt.axis('equal') +plt.show() diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 19402a080..3871fa18c 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -2,9 +2,10 @@ import numpy as np import matplotlib.pyplot as plt +from matplotlib import patches -def plot_pose2_on_axes(axes, pose, axis_length=0.1): +def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global @@ -20,13 +21,26 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1): line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], 'g-') + if covariance is not None: + pPp = covariance[0:2, 0:2] + gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) -def plot_pose2(fignum, pose, axis_length=0.1): + w, v = np.linalg.eig(gPp) + + # k = 2.296 + k = 5.0 + + angle = np.arctan2(v[1, 0], v[0, 0]) + e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), + np.rad2deg(angle), fill=False) + axes.add_patch(e1) + +def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): """Plot a 2D pose on given figure with given 'axis_length'.""" # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length) + plot_pose2_on_axes(axes, pose, axis_length, covariance) def plot_point3_on_axes(axes, point, linespec): From 7d2e4d2e64ce16715a110e800c6ab81509ba480b Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 6 Mar 2019 17:47:34 -0500 Subject: [PATCH 46/97] Fix warning message in the unit tests/examples --- .../examples/FixedLagSmootherExample.py | 39 ++++----- .../tests/test_FixedLagSmootherExample.py | 86 +++++++++++-------- .../examples/FixedLagSmootherExample.cpp | 36 ++++---- 3 files changed, 91 insertions(+), 70 deletions(-) diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 141162044..786701e0f 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -19,7 +19,7 @@ import gtsam_unstable def _timestamp_key_value(key, value): """ - Creates a key value pair for a FixedLagSmootherKeyTimeStampMap + """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value @@ -29,8 +29,7 @@ def _timestamp_key_value(key, value): def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry - sensors that is simply moving along the x axis in constant - speed. + sensors that is simply moving to the """ # Define a batch fixed lag smoother, which uses @@ -38,14 +37,12 @@ def BatchFixedLagSmootherExample(): lag = 2.0 smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) - # Create containers to store the factors and linearization points # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() - # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -57,9 +54,6 @@ def BatchFixedLagSmootherExample(): delta_time = 0.25 time = 0.25 - # Iterates from 0.25s to 3.0s, adding 0.25s each loop - # In each iteration, the agent moves at a constant speed - # and its two odometers measure the change. while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time @@ -72,32 +66,37 @@ def BatchFixedLagSmootherExample(): current_pose = gtsam.Pose2(time * 2, 0, 0) new_values.insert(current_key, current_pose) - # Add odometry factors from two different sources with different error stats + # Add odometry factors from two different sources with different error + # stats odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) new_factors.push_back(gtsam.BetweenFactorPose2( previous_key, current_key, odometry_measurement_1, odometry_noise_1 )) odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) new_factors.push_back(gtsam.BetweenFactorPose2( previous_key, current_key, odometry_measurement_2, odometry_noise_2 )) - # Update the smoothers with the new factors - smoother_batch.update(new_factors, new_values, new_timestamps) - - print("Timestamp = " + str(time) + ", Key = " + str(current_key)) - print(smoother_batch.calculateEstimatePose2(current_key)) - - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) time += delta_time + if __name__ == '__main__': BatchFixedLagSmootherExample() print("Example complete") diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index e26c450a0..90c4e436b 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -3,10 +3,13 @@ import gtsam import gtsam_unstable import numpy as np + def _timestamp_key_value(key, value): return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value ) + + class TestFixedLagSmootherExample(unittest.TestCase): ''' Tests the fixed lag smoother wrapper @@ -23,19 +26,20 @@ class TestFixedLagSmootherExample(unittest.TestCase): lag = 2.0 smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) - # Create containers to store the factors and linearization points # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() - # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) - prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) X1 = 0 - new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_factors.push_back( + gtsam.PriorFactorPose2( + X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) new_timestamps.insert(_timestamp_key_value(X1, 0.0)) @@ -45,19 +49,19 @@ class TestFixedLagSmootherExample(unittest.TestCase): i = 0 ground_truth = [ - gtsam.Pose2(0.49792, 0.007802, 0.015), - gtsam.Pose2(0.99547, 0.023019, 0.03), - gtsam.Pose2(1.4928, 0.045725, 0.045), - gtsam.Pose2(1.9898, 0.075888, 0.06), - gtsam.Pose2(2.4863, 0.1135, 0.075), - gtsam.Pose2(2.9821, 0.15856, 0.09), - gtsam.Pose2(3.4772, 0.21105, 0.105), - gtsam.Pose2(3.9715, 0.27096, 0.12), - gtsam.Pose2(4.4648, 0.33827, 0.135), - gtsam.Pose2(4.957, 0.41298, 0.15), - gtsam.Pose2(5.4481, 0.49506, 0.165), - gtsam.Pose2(5.9379, 0.5845, 0.18), + gtsam.Pose2(0.995821, 0.0231012, 0.0300001), + gtsam.Pose2(1.49284, 0.0457247, 0.045), + gtsam.Pose2(1.98981, 0.0758879, 0.06), + gtsam.Pose2(2.48627, 0.113502, 0.075), + gtsam.Pose2(2.98211, 0.158558, 0.09), + gtsam.Pose2(3.47722, 0.211047, 0.105), + gtsam.Pose2(3.97149, 0.270956, 0.12), + gtsam.Pose2(4.4648, 0.338272, 0.135), + gtsam.Pose2(4.95705, 0.41298, 0.15), + gtsam.Pose2(5.44812, 0.495063, 0.165), + gtsam.Pose2(5.9379, 0.584503, 0.18), ] + # Iterates from 0.25s to 3.0s, adding 0.25s each loop # In each iteration, the agent moves at a constant speed # and its two odometers measure the change. The smoothed @@ -70,35 +74,49 @@ class TestFixedLagSmootherExample(unittest.TestCase): new_timestamps.insert(_timestamp_key_value(current_key, time)) # Add a guess for this pose to the new values - # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + # Assume that the robot moves at 2 m/s. Position is time[s] * + # 2[m/s] current_pose = gtsam.Pose2(time * 2, 0, 0) new_values.insert(current_key, current_pose) - # Add odometry factors from two different sources with different error stats + # Add odometry factors from two different sources with different + # error stats odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) - new_factors.push_back(gtsam.BetweenFactorPose2( - previous_key, current_key, odometry_measurement_1, odometry_noise_1 - )) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_1, + odometry_noise_1)) odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) - new_factors.push_back(gtsam.BetweenFactorPose2( - previous_key, current_key, odometry_measurement_2, odometry_noise_2 - )) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_2, + odometry_noise_2)) - # Update the smoothers with the new factors - smoother_batch.update(new_factors, new_values, new_timestamps) + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) - estimate = smoother_batch.calculateEstimatePose2(current_key) - self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + i += 1 - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) time += delta_time - i += 1 + if __name__ == "__main__": unittest.main() diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 931e85c1a..8ccc9cc2c 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,23 +111,27 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors - smootherBatch.update(newFactors, newValues, newTimestamps); - smootherISAM2.update(newFactors, newValues, newTimestamps); - for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations - smootherISAM2.update(); + // Update the smoothers with the new factors. + // In this example, Levenberg-Marquadt needs one iteration + // to pass to accurately estimate. + if (time >= 0.50) { + smootherBatch.update(newFactors, newValues, newTimestamps); + smootherISAM2.update(newFactors, newValues, newTimestamps); + for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations + smootherISAM2.update(); + } + + // Print the optimized current pose + cout << setprecision(5) << "Timestamp = " << time << endl; + smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); + smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); + cout << endl; + + // Clear contains for the next iteration + newTimestamps.clear(); + newValues.clear(); + newFactors.resize(0); } - - // Print the optimized current pose - cout << setprecision(5) << "Timestamp = " << time << endl; - smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); - smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); - cout << endl; - - // Clear contains for the next iteration - newTimestamps.clear(); - newValues.clear(); - newFactors.resize(0); } // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds From 9a3d51792527ff284d8f6bbc4a5a9cf812d78f2b Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 6 Mar 2019 17:49:40 -0500 Subject: [PATCH 47/97] Make documentation on .cpp file more specific --- gtsam_unstable/examples/FixedLagSmootherExample.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 8ccc9cc2c..1376aca40 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,9 +111,9 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors. - // In this example, Levenberg-Marquadt needs one iteration - // to pass to accurately estimate. + // Update the smoothers with the new factors. In this example, batch smoother needs one iteration + // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when + // both are ready for simplicity. if (time >= 0.50) { smootherBatch.update(newFactors, newValues, newTimestamps); smootherISAM2.update(newFactors, newValues, newTimestamps); From 2f232fd4d467980a9dfeaddaf02fccbd398d3825 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 15:58:05 +0000 Subject: [PATCH 48/97] removed redundant call to find_package --- cmake/GtsamCythonWrap.cmake | 5 ----- 1 file changed, 5 deletions(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 374e00c46..6366c1508 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -44,11 +44,6 @@ endfunction() function(set_up_required_cython_packages) # Set up building of cython module - if(GTSAM_PYTHON_VERSION STREQUAL "Default") - find_package(PythonLibs REQUIRED) - else() - find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) - endif() include_directories(${PYTHON_INCLUDE_DIRS}) find_package(NumPy REQUIRED) include_directories(${NUMPY_INCLUDE_DIRS}) From f4bf0d5b0b4319ed99edeec66a18a54b9fe2464a Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Thu, 7 Mar 2019 14:23:11 -0500 Subject: [PATCH 49/97] Update unstable.h file to match upstream --- gtsam_unstable/gtsam_unstable.h | 248 ++++++++++++++++---------------- 1 file changed, 125 insertions(+), 123 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index d77895d86..39c910826 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,130 +505,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); - FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -}; +// #include +// gtsam::GaussianFactorGraph* summarizeGraphSequential( +// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); +// gtsam::GaussianFactorGraph* summarizeGraphSequential( +// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); -class FixedLagSmootherKeyTimestampMap { - FixedLagSmootherKeyTimestampMap(); - FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - double at(const size_t key) const; - void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -}; - -class FixedLagSmootherResult { - size_t getIterations() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -#include -virtual class FixedLagSmoother { - void print(string s) const; - bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; - - gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; - double smootherLag() const; - - gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); - gtsam::Values calculateEstimate() const; -}; - -#include -virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { - BatchFixedLagSmoother(); - BatchFixedLagSmoother(double smootherLag); - BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); - - gtsam::LevenbergMarquardtParams params() const; - template - VALUE calculateEstimate(size_t key) const; -}; - -#include -virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { - IncrementalFixedLagSmoother(); - IncrementalFixedLagSmoother(double smootherLag); - IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); - - gtsam::ISAM2Params params() const; -}; - -#include -virtual class ConcurrentFilter { - void print(string s) const; - bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -}; - -virtual class ConcurrentSmoother { - void print(string s) const; - bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -}; - -// Synchronize function -void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); - -#include -class ConcurrentBatchFilterResult { - size_t getIterations() const; - size_t getLambdas() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { - ConcurrentBatchFilter(); - ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); - - gtsam::NonlinearFactorGraph getFactors() const; - gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::VectorValues getDelta() const; - - gtsam::ConcurrentBatchFilterResult update(); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); - gtsam::Values calculateEstimate() const; -}; - -#include -class ConcurrentBatchSmootherResult { - size_t getIterations() const; - size_t getLambdas() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { - ConcurrentBatchSmoother(); - ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); - - gtsam::NonlinearFactorGraph getFactors() const; - gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::VectorValues getDelta() const; - - gtsam::ConcurrentBatchSmootherResult update(); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::Values calculateEstimate() const; -}; +// #include +// class FixedLagSmootherKeyTimestampMapValue { +// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); +// FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +// }; +// +// class FixedLagSmootherKeyTimestampMap { +// FixedLagSmootherKeyTimestampMap(); +// FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); +// +// // Note: no print function +// +// // common STL methods +// size_t size() const; +// bool empty() const; +// void clear(); +// +// double at(const gtsam::Key& key) const; +// void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +// }; +// +// class FixedLagSmootherResult { +// size_t getIterations() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// #include +// virtual class FixedLagSmoother { +// void print(string s) const; +// bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; +// +// gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; +// double smootherLag() const; +// +// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); +// gtsam::Values calculateEstimate() const; +// }; +// +// #include +// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { +// BatchFixedLagSmoother(); +// BatchFixedLagSmoother(double smootherLag); +// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); +// +// gtsam::LevenbergMarquardtParams params() const; +// }; +// +// #include +// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { +// IncrementalFixedLagSmoother(); +// IncrementalFixedLagSmoother(double smootherLag); +// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); +// +// gtsam::ISAM2Params params() const; +// }; +// +// #include +// virtual class ConcurrentFilter { +// void print(string s) const; +// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +// }; +// +// virtual class ConcurrentSmoother { +// void print(string s) const; +// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +// }; +// +// // Synchronize function +// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); +// +// #include +// class ConcurrentBatchFilterResult { +// size_t getIterations() const; +// size_t getLambdas() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { +// ConcurrentBatchFilter(); +// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); +// +// gtsam::NonlinearFactorGraph getFactors() const; +// gtsam::Values getLinearizationPoint() const; +// gtsam::Ordering getOrdering() const; +// gtsam::VectorValues getDelta() const; +// +// gtsam::ConcurrentBatchFilterResult update(); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); +// gtsam::Values calculateEstimate() const; +// }; +// +// #include +// class ConcurrentBatchSmootherResult { +// size_t getIterations() const; +// size_t getLambdas() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { +// ConcurrentBatchSmoother(); +// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); +// +// gtsam::NonlinearFactorGraph getFactors() const; +// gtsam::Values getLinearizationPoint() const; +// gtsam::Ordering getOrdering() const; +// gtsam::VectorValues getDelta() const; +// +// gtsam::ConcurrentBatchSmootherResult update(); +// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); +// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); +// gtsam::Values calculateEstimate() const; +// }; //************************************************************************* // slam From fe3741e4662c8400e9b41196311d670d881573fa Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Thu, 7 Mar 2019 14:25:05 -0500 Subject: [PATCH 50/97] Fix broken file --- gtsam_unstable/gtsam_unstable.h | 248 ++++++++++++++++---------------- 1 file changed, 123 insertions(+), 125 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 39c910826..d77895d86 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -// #include -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; -// #include -// class FixedLagSmootherKeyTimestampMapValue { -// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); -// FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -// }; -// -// class FixedLagSmootherKeyTimestampMap { -// FixedLagSmootherKeyTimestampMap(); -// FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); -// -// // Note: no print function -// -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); -// -// double at(const gtsam::Key& key) const; -// void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -// }; -// -// class FixedLagSmootherResult { -// size_t getIterations() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// #include -// virtual class FixedLagSmoother { -// void print(string s) const; -// bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; -// -// gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; -// double smootherLag() const; -// -// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { -// BatchFixedLagSmoother(); -// BatchFixedLagSmoother(double smootherLag); -// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// -// gtsam::LevenbergMarquardtParams params() const; -// }; -// -// #include -// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { -// IncrementalFixedLagSmoother(); -// IncrementalFixedLagSmoother(double smootherLag); -// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// -// gtsam::ISAM2Params params() const; -// }; -// -// #include -// virtual class ConcurrentFilter { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -// }; -// -// virtual class ConcurrentSmoother { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -// }; -// -// // Synchronize function -// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// -// #include -// class ConcurrentBatchFilterResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { -// ConcurrentBatchFilter(); -// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchFilterResult update(); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// class ConcurrentBatchSmootherResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { -// ConcurrentBatchSmoother(); -// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchSmootherResult update(); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::Values calculateEstimate() const; -// }; +class FixedLagSmootherKeyTimestampMap { + FixedLagSmootherKeyTimestampMap(); + FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + double at(const size_t key) const; + void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +}; + +class FixedLagSmootherResult { + size_t getIterations() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +#include +virtual class FixedLagSmoother { + void print(string s) const; + bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; + + gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; + double smootherLag() const; + + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; //************************************************************************* // slam From 549fcca2acd92556ac528ee9cdfad19426ee9192 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 9 Mar 2019 13:06:31 -0500 Subject: [PATCH 51/97] Fixed 2 unit tests --- cython/gtsam/tests/test_Scenario.py | 18 ++++++++++++++++- cython/gtsam/tests/test_Values.py | 31 +++++++++++++++++++++-------- 2 files changed, 40 insertions(+), 9 deletions(-) diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index 4cca1400b..d68566b25 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/cython/gtsam/tests/test_Scenario.py @@ -1,5 +1,20 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + import math import unittest + import numpy as np import gtsam @@ -29,7 +44,8 @@ class TestScenario(unittest.TestCase): T30 = scenario.pose(T) np.testing.assert_almost_equal( np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) - self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + self.assertTrue(gtsam.Point3( + 0, 0, 2.0 * R).equals(T30.translation(), 1e-9)) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 08e133840..0bb1e0806 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -1,8 +1,21 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 import unittest + import numpy as np -from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3 -from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias +from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, + Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, + imuBias_ConstantBias) class TestValues(unittest.TestCase): @@ -12,8 +25,8 @@ class TestValues(unittest.TestCase): E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 - values.insert(0, Point2(0,0)) - values.insert(1, Point3(0,0,0)) + values.insert(0, Point2(0, 0)) + values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) @@ -34,18 +47,19 @@ class TestValues(unittest.TestCase): # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. - vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is + # for vectors, the order is not important, but dtype still is + vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! - mat2 = np.array([[1,2,],[3,5]]) + mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) - self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) + self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) + self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) @@ -65,5 +79,6 @@ class TestValues(unittest.TestCase): actualMatrix2 = values.atMatrix(13) self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + if __name__ == "__main__": unittest.main() From 8a81364dafcad5f2f22cc2948b98bce7bb7afa14 Mon Sep 17 00:00:00 2001 From: mbway Date: Sun, 10 Mar 2019 16:25:42 +0000 Subject: [PATCH 52/97] started porting more examples to python --- cython/gtsam/examples/README.md | 57 ++++++++++- cython/gtsam/examples/SFMExample.py | 113 +++++++++++++++++++++ cython/gtsam/examples/SimpleRotation.py | 85 ++++++++++++++++ cython/gtsam/examples/VisualISAMExample.py | 100 ++++++++++++++++++ 4 files changed, 353 insertions(+), 2 deletions(-) create mode 100644 cython/gtsam/examples/SFMExample.py create mode 100644 cython/gtsam/examples/SimpleRotation.py create mode 100644 cython/gtsam/examples/VisualISAMExample.py diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 067929a20..35ec4f66d 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -1,4 +1,57 @@ These examples are almost identical to the old handwritten python wrapper examples. However, there are just some slight name changes, for example -noiseModel.Diagonal becomes noiseModel_Diagonal etc... -Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0)) +`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc... +Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))` + +# Porting Progress + +| C++ Example Name | Ported | +|-------------------------------------------------------|--------| +| CameraResectioning | | +| CreateSFMExampleData | | +| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython | +| ImuFactorExample2 | X | +| ImuFactorsExample | | +| ISAM2Example_SmartFactor | | +| ISAM2_SmartFactorStereo_IMU | | +| LocalizationExample | impossible? | +| METISOrderingExample | | +| OdometryExample | X | +| PlanarSLAMExample | X | +| Pose2SLAMExample | X | +| Pose2SLAMExampleExpressions | | +| Pose2SLAMExample_g2o | X | +| Pose2SLAMExample_graph | | +| Pose2SLAMExample_graphviz | | +| Pose2SLAMExample_lago | lago not exposed through cython | +| Pose2SLAMStressTest | | +| Pose2SLAMwSPCG | | +| Pose3SLAMExample_changeKeys | | +| Pose3SLAMExampleExpressions_BearingRangeWithTransform | | +| Pose3SLAMExample_g2o | X | +| Pose3SLAMExample_initializePose3Chordal | | +| Pose3SLAMExample_initializePose3Gradient | | +| RangeISAMExample_plaza2 | | +| SelfCalibrationExample | | +| SFMExample_bal_COLAMD_METIS | | +| SFMExample_bal | | +| SFMExample | X | +| SFMExampleExpressions_bal | | +| SFMExampleExpressions | | +| SFMExample_SmartFactor | | +| SFMExample_SmartFactorPCG | | +| SimpleRotation | X | +| SolverComparer | | +| StereoVOExample | | +| StereoVOExample_large | | +| TimeTBB | | +| UGM_chain | discrete functionality not exposed | +| UGM_small | discrete functionality not exposed | +| VisualISAM2Example | X | +| VisualISAMExample | X | + +Extra Examples (with no C++ equivalent) +- PlanarManipulatorExample +- SFMData diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py new file mode 100644 index 000000000..3a64e0cdb --- /dev/null +++ b/cython/gtsam/examples/SFMExample.py @@ -0,0 +1,113 @@ +""" +GTSAM Copyright 2010, 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 structure-from-motion problem on a simulated dataset +""" + +import gtsam +import numpy as np +from gtsam.examples import SFMdata +from gtsam.gtsam import Values, NonlinearFactorGraph, PriorFactorPose3, SimpleCamera, \ + GenericProjectionFactorCal3_S2, \ + PriorFactorPoint3, Pose3, Rot3, Point3, DoglegOptimizer, Cal3_S2 + + +# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +# +# Each variable in the system (poses and landmarks) must be identified with a unique key. +# We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +# Here we will use Symbols +# +# In GTSAM, measurement functions are represented as 'factors'. Several common factors +# have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +# Here we will use Projection factors to model the camera's landmark observations. +# Also, we will initialize the robot at some location using a Prior factor. +# +# When the factors are created, we will add them to a Factor Graph. As the factors we are using +# are nonlinear factors, we will need a Nonlinear Factor Graph. +# +# Finally, once all of the factors have been added to our factor graph, we will want to +# solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +# GTSAM includes several nonlinear optimizers to perform this step. Here we will use a +# trust-region method known as Powell's Degleg +# +# The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +# nonlinear functions around an initial linearization point, then solve the linear system +# to update the linearization point. This happens repeatedly until the solver converges +# to a consistent set of variable values. This requires us to specify an initial guess +# for each variable, held in a Values container. + +def symbol(name, index): + return gtsam.symbol(ord(name), index) + + +def main(): + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses(K) + + # Create a factor graph + graph = NonlinearFactorGraph() + + # Add a prior on pose x1. This indirectly specifies where the origin is. + # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + graph.push_back(factor) + + # Simulated measurements from each camera pose, adding them to the factor graph + for i, pose in enumerate(poses): + camera = SimpleCamera(pose, K) + for j, point in enumerate(points): + measurement = camera.project(point) + factor = GenericProjectionFactorCal3_S2( + measurement, measurement_noise, symbol('x', i), symbol('l', j), K) + graph.push_back(factor) + + # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance + # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + graph.push_back(factor) + graph.print_('Factor Graph:\n') + + # Create the data structure to hold the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initial_estimate = Values() + for i, pose in enumerate(poses): + r = Rot3.Rodrigues(-0.1, 0.2, 0.25) + t = Point3(0.05, -0.10, 0.20) + transformed_pose = pose.compose(Pose3(r, t)) + initial_estimate.insert(symbol('x', i), transformed_pose) + for j, point in enumerate(points): + transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15])) + initial_estimate.insert(symbol('l', j), transformed_point) + initial_estimate.print_('Initial Estimates:\n') + + # Optimize the graph and print results + params = gtsam.DoglegParams() + params.setVerbosity('TERMINATION') + optimizer = DoglegOptimizer(graph, initial_estimate, params) + print('Optimizing:') + result = optimizer.optimize() + result.print_('Final results:\n') + print('initial error = {}'.format(graph.error(initial_estimate))) + print('final error = {}'.format(graph.error(result))) + + +if __name__ == '__main__': + main() diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py new file mode 100644 index 000000000..0c3ac467f --- /dev/null +++ b/cython/gtsam/examples/SimpleRotation.py @@ -0,0 +1,85 @@ +""" +GTSAM Copyright 2010, 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 + +This example will perform a relatively trivial optimization on +a single variable with a single factor. +""" + +import gtsam + +import numpy as np + + +def main(): + """ + Step 1: Create a factor to express a unary constraint + + The "prior" in this case is the measurement from a sensor, + with a model of the noise on the measurement. + + The "Key" created here is a label used to associate parts of the + state (stored in "RotValues") with particular factors. They require + an index to allow for lookup, and should be unique. + + In general, creating a factor requires: + - A key or set of keys labeling the variables that are acted upon + - A measurement value + - A measurement model with the correct dimensionality for the factor + """ + prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) + prior.print_('goal angle') + model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) + key = gtsam.symbol(ord('x'), 1) + factor = gtsam.PriorFactorRot2(key, prior, model) + + """ + Step 2: Create a graph container and add the factor to it + + Before optimizing, all factors need to be added to a Graph container, + which provides the necessary top-level functionality for defining a + system of constraints. + + In this case, there is only one factor, but in a practical scenario, + many more factors would be added. + """ + graph = gtsam.NonlinearFactorGraph() + graph.push_back(factor) + graph.print_('full graph') + + """ + Step 3: Create an initial estimate + + An initial estimate of the solution for the system is necessary to + start optimization. This system state is the "Values" instance, + which is similar in structure to a dictionary, in that it maps + keys (the label created in step 1) to specific values. + + The initial estimate provided to optimization will be used as + a linearization point for optimization, so it is important that + all of the variables in the graph have a corresponding value in + this structure. + """ + initial = gtsam.Values() + initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) + initial.print_('initial estimate') + + """ + Step 4: Optimize + + After formulating the problem with a graph of constraints + and an initial estimate, executing optimization is as simple + as calling a general optimization function with the graph and + initial estimate. This will yield a new RotValues structure + with the final state of the optimization. + """ + result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() + result.print_('final result') + + +if __name__ == '__main__': + main() diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py new file mode 100644 index 000000000..23b880bec --- /dev/null +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -0,0 +1,100 @@ +""" +GTSAM Copyright 2010, 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 visualSLAM example for the structure-from-motion problem on a simulated dataset +This version uses iSAM to solve the problem incrementally +""" + +# A structure-from-motion example with landmarks +# - The landmarks form a 10 meter cube +# - The robot rotates around the landmarks, always facing towards the cube + +import gtsam +from gtsam.gtsam import Values, Cal3_S2, NonlinearISAM, NonlinearFactorGraph, SimpleCamera, Pose3, Rot3, Point3, \ + PriorFactorPose3, PriorFactorPoint3, GenericProjectionFactorCal3_S2 + +import numpy as np +from gtsam.examples import SFMdata + + +def symbol(name, index): + return gtsam.symbol(ord(name), index) + + +def main(): + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + # Create the set of ground-truth poses + poses = SFMdata.createPoses(K) + + # Create a NonlinearISAM object which will relinearize and reorder the variables + # every "reorderInterval" updates + isam = NonlinearISAM(reorderInterval=3) + + # Create a Factor Graph and Values to hold the new data + graph = NonlinearFactorGraph() + initial_estimate = Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + camera = SimpleCamera(pose, K) + # Add factors for each landmark observation + for j, point in enumerate(points): + measurement = camera.project(point) + factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K) + graph.push_back(factor) + + # Intentionally initialize the variables off from the ground truth + noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) + initial_xi = pose.compose(noise) + + # Add an initial guess for the current pose + initial_estimate.insert(symbol('x', i), initial_xi) + + # If this is the first iteration, add a prior on the first pose to set the coordinate frame + # and a prior on the first landmark to set the scale + # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + # adding it to iSAM. + if i == 0: + # Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + graph.push_back(factor) + + # Add a prior on landmark l0 + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + graph.push_back(factor) + + # Add initial guesses to all observed landmarks + noise = np.array([-0.25, 0.20, 0.15]) + for j, point in enumerate(points): + # Intentionally initialize the variables off from the ground truth + initial_lj = points[j].vector() + noise + initial_estimate.insert(symbol('l', j), Point3(initial_lj)) + else: + # Update iSAM with the new factors + isam.update(graph, initial_estimate) + current_estimate = isam.estimate() + print('*' * 50) + print('Frame {}:'.format(i)) + current_estimate.print_('Current estimate: ') + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initial_estimate.clear() + + +if __name__ == '__main__': + main() From 54512731e31c90f10b136b4449ffc0abc0217555 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 11:57:05 +0000 Subject: [PATCH 53/97] added setup.py --- cython/CMakeLists.txt | 1 + cython/README.md | 4 +++- cython/setup.py.in | 28 ++++++++++++++++++++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) create mode 100644 cython/setup.py.in diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index e53c0b73e..317fb7e2d 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -33,6 +33,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) + configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") diff --git a/cython/README.md b/cython/README.md index 8ba824f8d..78fc7e594 100644 --- a/cython/README.md +++ b/cython/README.md @@ -17,10 +17,12 @@ named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy. The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is by default: `/cython` -- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: +- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: ```bash export PYTHONPATH=$PYTHONPATH: ``` +- To install system-wide: navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` + - (the same command can be used to install into a virtual environment if it is active) UNIT TESTS ========== diff --git a/cython/setup.py.in b/cython/setup.py.in new file mode 100644 index 000000000..6a2009913 --- /dev/null +++ b/cython/setup.py.in @@ -0,0 +1,28 @@ +try: + from setuptools import setup +except ImportError: + from distutils.core import setup + +setup( + name='gtsam', + + description='Georgia Tech Smoothing And Mapping library', + url='https://bitbucket.org/gtborg/gtsam', + version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ + license='Simplified BSD license', + keywords='slam sam', + long_description=open('${PROJECT_SOURCE_DIR}/README.md', 'r').read(), + # https://pypi.org/pypi?%3Aaction=list_classifiers + classifiers=[ + 'Development Status :: 5 - Production/Stable', + 'Intended Audience :: Education', + 'Intended Audience :: Developers', + 'Intended Audience :: Science/Research', + 'License :: OSI Approved :: BSD License', + 'Programming Language :: Python :: 2', + 'Programming Language :: Python :: 3', + ], + + packages=['gtsam', 'gtsam_eigency'], + install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() +) From f6af989e6776aee45d968f9c7eada5f74e88f63f Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:15:20 +0000 Subject: [PATCH 54/97] setup.py install .so files --- cython/setup.py.in | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 6a2009913..e99bd5de1 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -1,8 +1,15 @@ +import os try: from setuptools import setup except ImportError: from distutils.core import setup + +packages = ['gtsam', 'gtsam_eigency'] +if ${GTSAM_BUILD_UNSTABLE}: + packages += 'gtsam_unstable' + + setup( name='gtsam', @@ -23,6 +30,10 @@ setup( 'Programming Language :: Python :: 3', ], - packages=['gtsam', 'gtsam_eigency'], + packages=packages, + package_data={package: + [f for f in os.listdir(package) if os.path.splitext(f)[1] == '.so'] + for package in packages + }, install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() ) From a62197ec6ecc98ff092bbf43b4b7bfa4dc490999 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:34:09 +0000 Subject: [PATCH 55/97] gtsam_unstable as a separate package --- cython/CMakeLists.txt | 7 ++----- cython/gtsam/__init__.py | 1 + cython/gtsam/__init__.py.in | 2 -- cython/gtsam_unstable/__init__.py | 2 ++ cython/setup.py.in | 10 +++------- 5 files changed, 8 insertions(+), 14 deletions(-) create mode 100644 cython/gtsam/__init__.py delete mode 100644 cython/gtsam/__init__.py.in create mode 100644 cython/gtsam_unstable/__init__.py diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 317fb7e2d..c2d38fd47 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -22,19 +22,16 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports - "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") endif() # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable - configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) - install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") + install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py new file mode 100644 index 000000000..9bda86efe --- /dev/null +++ b/cython/gtsam/__init__.py @@ -0,0 +1 @@ +from .gtsam import * diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in deleted file mode 100644 index 85451c680..000000000 --- a/cython/gtsam/__init__.py.in +++ /dev/null @@ -1,2 +0,0 @@ -from .gtsam import * -${GTSAM_UNSTABLE_IMPORT} diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py new file mode 100644 index 000000000..04ce7f1e0 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py @@ -0,0 +1,2 @@ +from .gtsam_unstable import * + diff --git a/cython/setup.py.in b/cython/setup.py.in index e99bd5de1..e72dbd7c1 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -1,18 +1,14 @@ import os try: - from setuptools import setup + from setuptools import setup, find_packages except ImportError: - from distutils.core import setup + from distutils.core import setup, find_packages -packages = ['gtsam', 'gtsam_eigency'] -if ${GTSAM_BUILD_UNSTABLE}: - packages += 'gtsam_unstable' - +packages = find_packages() setup( name='gtsam', - description='Georgia Tech Smoothing And Mapping library', url='https://bitbucket.org/gtborg/gtsam', version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ From fe3003a688a85cb7cdb80511bffec50e138ec2aa Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:38:25 +0000 Subject: [PATCH 56/97] install gtsam_unstable __init__.py --- cython/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index c2d38fd47..3dedbeec2 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -34,5 +34,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () From c1b048020ee99ee209abe1d35fb0a9aaba3d6ec7 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:55:49 +0000 Subject: [PATCH 57/97] fixed bug with detecting nested python packages in setup.py --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index e72dbd7c1..0a994b18f 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -28,7 +28,7 @@ setup( packages=packages, package_data={package: - [f for f in os.listdir(package) if os.path.splitext(f)[1] == '.so'] + [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] == '.so'] for package in packages }, install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() From 88dac759d7e675662919ad8fc8524d741ca81ca4 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:57:52 +0000 Subject: [PATCH 58/97] setup.py gets installed into correct directory --- cython/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 3dedbeec2..d5e8b8fac 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -31,7 +31,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) - install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/") + install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") From 0d7b52d99fb47a2aa9131e99567ad0e1b8213a01 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 16:03:51 +0000 Subject: [PATCH 59/97] copy __init__.py before compiling c++ --- cython/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index d5e8b8fac..ca5c0ecfb 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -30,6 +30,8 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY) + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY) configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}") # install scripts and tests From d56efcceadb6d9258a6441a0484d6ae85c1ca32f Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 20:32:05 +0000 Subject: [PATCH 60/97] setup.py only installs for the python version it is built for --- cython/setup.py.in | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 0a994b18f..0f133f781 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -22,8 +22,7 @@ setup( 'Intended Audience :: Developers', 'Intended Audience :: Science/Research', 'License :: OSI Approved :: BSD License', - 'Programming Language :: Python :: 2', - 'Programming Language :: Python :: 3', + 'Programming Language :: Python :: ${PYTHON_VERSION_MAJOR}', ], packages=packages, From fe9ede47d1956390894175ce6f26b4df66d16e3a Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 20:32:38 +0000 Subject: [PATCH 61/97] added dlls to package_data --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 0f133f781..3161c36e7 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -27,7 +27,7 @@ setup( packages=packages, package_data={package: - [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] == '.so'] + [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')] for package in packages }, install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() From 135ef5a0d0647e0d9ef14071c5c27a1b88d951bf Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 14:38:46 +0000 Subject: [PATCH 62/97] baking in requirements and README to setup.py rather than reading at install time --- cython/CMakeLists.txt | 3 +++ cython/setup.py.in | 6 ++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index ca5c0ecfb..400e96e13 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -28,6 +28,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) ) endif() + file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS) + file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) + # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY) diff --git a/cython/setup.py.in b/cython/setup.py.in index 3161c36e7..0e22dbf11 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -14,7 +14,7 @@ setup( version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ license='Simplified BSD license', keywords='slam sam', - long_description=open('${PROJECT_SOURCE_DIR}/README.md', 'r').read(), + long_description='''${README_CONTENTS}''', # https://pypi.org/pypi?%3Aaction=list_classifiers classifiers=[ 'Development Status :: 5 - Production/Stable', @@ -30,5 +30,7 @@ setup( [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')] for package in packages }, - install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() + install_requires=[line.strip() for line in ''' +${CYTHON_INSTALL_REQUIREMENTS} +'''.splitlines() if len(line) > 0 and not line.strip().startswith('#')] ) From 290aad3372fb9372cef8f852d9c27cb57a4154b3 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 20 Feb 2019 09:03:34 +0000 Subject: [PATCH 63/97] small change to setup.py --- cython/gtsam_unstable/__init__.py | 1 - cython/setup.py.in | 8 ++++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py index 04ce7f1e0..3195e6da4 100644 --- a/cython/gtsam_unstable/__init__.py +++ b/cython/gtsam_unstable/__init__.py @@ -1,2 +1 @@ from .gtsam_unstable import * - diff --git a/cython/setup.py.in b/cython/setup.py.in index 0e22dbf11..e631d89c0 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -21,8 +21,12 @@ setup( 'Intended Audience :: Education', 'Intended Audience :: Developers', 'Intended Audience :: Science/Research', + 'Operating System :: MacOS', + 'Operating System :: Microsoft :: Windows', + 'Operating System :: POSIX', 'License :: OSI Approved :: BSD License', - 'Programming Language :: Python :: ${PYTHON_VERSION_MAJOR}', + 'Programming Language :: Python :: 2', + 'Programming Language :: Python :: 3', ], packages=packages, @@ -32,5 +36,5 @@ setup( }, install_requires=[line.strip() for line in ''' ${CYTHON_INSTALL_REQUIREMENTS} -'''.splitlines() if len(line) > 0 and not line.strip().startswith('#')] +'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')] ) From 803c14deb3058e12aa6779c1f9cb4fe7e1764e7d Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 6 Mar 2019 10:15:07 +0000 Subject: [PATCH 64/97] removed unnecessary variable from cmake --- cython/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 400e96e13..a351ec52b 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -19,7 +19,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # wrap gtsam_unstable if(GTSAM_BUILD_UNSTABLE) add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h") - set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path From fcfcceef71bc833bfc9000b487333bf20ad6df26 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 09:11:16 +0000 Subject: [PATCH 65/97] added gtsam_unstable import back to gtsam --- cython/gtsam/__init__.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py index 9bda86efe..724572240 100644 --- a/cython/gtsam/__init__.py +++ b/cython/gtsam/__init__.py @@ -1 +1,19 @@ from .gtsam import * + +import gtsam_unstable + + +def deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + warn('importing the unstable item "{}" from gtsam is deprecated. Please import it from gtsam_unstable.'.format(name)) + return item(*args, **kwargs) + return wrapper + +for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = deprecated_wrapper(item, name) + globals()[name] = item + From 7161c0461095b92d407a2a40ce76d295a51d9ec6 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 10:05:02 +0000 Subject: [PATCH 66/97] fixed __init__.py to not crash if gtsam_unstable doesn't exist --- cython/gtsam/__init__.py | 33 ++++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py index 724572240..f27c6fa44 100644 --- a/cython/gtsam/__init__.py +++ b/cython/gtsam/__init__.py @@ -1,19 +1,26 @@ from .gtsam import * -import gtsam_unstable +try: + import gtsam_unstable -def deprecated_wrapper(item, name): - def wrapper(*args, **kwargs): - from warnings import warn - warn('importing the unstable item "{}" from gtsam is deprecated. Please import it from gtsam_unstable.'.format(name)) - return item(*args, **kwargs) - return wrapper + def _deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + + 'Please import it from gtsam_unstable.') + warn(message, category=DeprecationWarning) + return item(*args, **kwargs) + return wrapper -for name in dir(gtsam_unstable): - if not name.startswith('__'): - item = getattr(gtsam_unstable, name) - if callable(item): - item = deprecated_wrapper(item, name) - globals()[name] = item + + for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = _deprecated_wrapper(item, name) + globals()[name] = item + +except ImportError: + pass From 9b80f4b15865346d57a82bef44ccd59e191aaca9 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 10:11:01 +0000 Subject: [PATCH 67/97] not using DeprecationWarning because it is ignored by default --- cython/gtsam/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py index f27c6fa44..d40ee4502 100644 --- a/cython/gtsam/__init__.py +++ b/cython/gtsam/__init__.py @@ -9,7 +9,7 @@ try: from warnings import warn message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + 'Please import it from gtsam_unstable.') - warn(message, category=DeprecationWarning) + warn(message) return item(*args, **kwargs) return wrapper From 91fa7adf0753de4f8ff92a13c39e83bcfc2978eb Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 11 Mar 2019 14:54:12 +0000 Subject: [PATCH 68/97] added more keywords --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index e631d89c0..a4c96f1b8 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -13,7 +13,7 @@ setup( url='https://bitbucket.org/gtborg/gtsam', version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ license='Simplified BSD license', - keywords='slam sam', + keywords='slam sam robotics localization mapping optimization', long_description='''${README_CONTENTS}''', # https://pypi.org/pypi?%3Aaction=list_classifiers classifiers=[ From 03500b004bc5e16e48725a3d3500b0f25bab43dd Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 11 Mar 2019 15:02:11 +0000 Subject: [PATCH 69/97] enforcing the setup script from being run from the installation directory --- cython/setup.py.in | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/cython/setup.py.in b/cython/setup.py.in index a4c96f1b8..0c37cd660 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -1,10 +1,19 @@ import os +import sys try: from setuptools import setup, find_packages except ImportError: from distutils.core import setup, find_packages +script_path = os.path.abspath(os.path.realpath(__file__)) +install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) +if script_path != install_path: + print('setup.py is being run from an unexpected location: "{script_path}"') + print('please run `make install` and run the script from there') + sys.exit(1) + + packages = find_packages() setup( From 5a0e7bb92a38cc3d2e6fe91584ac590dab5fb98c Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 11 Mar 2019 15:05:53 +0000 Subject: [PATCH 70/97] fixed string formatting to work with python 2 and 3 --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 0c37cd660..b7b0b7bc5 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -9,7 +9,7 @@ except ImportError: script_path = os.path.abspath(os.path.realpath(__file__)) install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) if script_path != install_path: - print('setup.py is being run from an unexpected location: "{script_path}"') + print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) print('please run `make install` and run the script from there') sys.exit(1) From 0d924e23dbd2d519dfb3d9b50589caabae20ebe2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Mar 2019 14:49:18 +0800 Subject: [PATCH 71/97] Fix compilation on MKL 2019 --- cmake/FindMKL.cmake | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index cbe46a908..32e183baa 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS ) ENDIF() + IF(NOT MKL_LAPACK_LIBRARY) + FIND_LIBRARY(MKL_LAPACK_LIBRARY + mkl_intel_lp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + # iomp5 IF("${MKL_ARCH_DIR}" STREQUAL "32") IF(UNIX AND NOT APPLE) From ecb8471e860850f8c8f8a665519d3ea6bd6fa16a Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Tue, 12 Mar 2019 09:47:25 +0000 Subject: [PATCH 72/97] updated cython README --- cython/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cython/README.md b/cython/README.md index 78fc7e594..4e76225c7 100644 --- a/cython/README.md +++ b/cython/README.md @@ -21,8 +21,9 @@ by default: `/cython` ```bash export PYTHONPATH=$PYTHONPATH: ``` -- To install system-wide: navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` +- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` - (the same command can be used to install into a virtual environment if it is active) + - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to install gtsam to a subdirectory of the build directory. UNIT TESTS ========== From a4b713454a9f955cafd44bace050957734c4f484 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Tue, 12 Mar 2019 09:49:03 +0000 Subject: [PATCH 73/97] updated cython README --- cython/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/README.md b/cython/README.md index 4e76225c7..a524ac04c 100644 --- a/cython/README.md +++ b/cython/README.md @@ -23,7 +23,7 @@ export PYTHONPATH=$PYTHONPATH: ``` - To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` - (the same command can be used to install into a virtual environment if it is active) - - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to install gtsam to a subdirectory of the build directory. + - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory. UNIT TESTS ========== From e24b402db44850db06730008b73b96471069c3b5 Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Tue, 12 Mar 2019 09:22:09 -0700 Subject: [PATCH 74/97] Turn MKL off by default. Add section on performance to INSTALL.md --- CMakeLists.txt | 10 ++-------- INSTALL.md | 37 +++++++++++++++++++++++++++++++------ 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 834ce732e..d9633b3c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,8 +61,8 @@ option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the defau option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) @@ -563,12 +563,6 @@ message(STATUS "===============================================================" if(GTSAM_WITH_TBB AND NOT TBB_FOUND) message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") endif() -if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) - message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.") -endif() -if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) - message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") -endif() if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) message(WARNING "${GTSAM_PYTHON_WARNINGS}") endif() diff --git a/INSTALL.md b/INSTALL.md index e19363c7e..23953decf 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -23,18 +23,22 @@ $ make install disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB may be installed from the Ubuntu repositories, and for other platforms it may be downloaded from https://www.threadingbuildingblocks.org/ + - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and + `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually + achieved with MKL disabled. We therefore advise you to benchmark your problem + before using MKL. Tested compilers: - - GCC 4.2-4.7 - - OSX Clang 2.9-5.0 - - OSX GCC 4.2 - - MSVC 2010, 2012 + - GCC 4.2-7.3 + - OS X Clang 2.9-10.0 + - OS X GCC 4.2 + - MSVC 2010, 2012, 2017 Tested systems: - - Ubuntu 11.04 - 18.04 - - MacOS 10.6 - 10.9 + - Ubuntu 16.04 - 18.04 + - MacOS 10.6 - 10.14 - Windows 7, 8, 8.1, 10 Known issues: @@ -134,6 +138,27 @@ MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included i $MATLABROOT can be found by executing the command `matlabroot` in MATLAB +## Performance + +Here are some tips to get the best possible performance out of GTSAM. + +1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. +2. Enable TBB. On modern processors with multiple cores, this can easily speed up + optimization by 30-50%. Please note that this may not be true for very small + problems where the overhead of dispatching work to multiple threads outweighs + the benefit. We recommend that you benchmark your problem with/without TBB. +3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of + 25-30% can be expected on modern processors. Note that this affects the portability + of your executable. It may not run when copied to another system with older/different + processor architecture. + Also note that all dependent projects *must* be compiled with the same flag, or + seg-faults and other undefined behavior may result. +4. Enable MKL. Please note that our benchmarks have shown that this helps only + in very limited cases, and actually hurts performance in the usual case. We therefore + recommend that you do *not* enable MKL, unless you have benchmarked it on + your problem and have verified that it improves performance. + + ## Debugging tips Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find. From 40134c3a9ecaf32a8312ef856d383ae92c04af04 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 12 Mar 2019 11:16:32 -0700 Subject: [PATCH 75/97] Restore warnings about MKL, change notice about performance. --- CMakeLists.txt | 6 ++++++ INSTALL.md | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9633b3c3..186dabaf4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -563,6 +563,12 @@ message(STATUS "===============================================================" if(GTSAM_WITH_TBB AND NOT TBB_FOUND) message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") endif() +if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) + message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.") +endif() +if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) + message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") +endif() if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) message(WARNING "${GTSAM_PYTHON_WARNINGS}") endif() diff --git a/INSTALL.md b/INSTALL.md index 23953decf..3437d074d 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -153,7 +153,7 @@ Here are some tips to get the best possible performance out of GTSAM. processor architecture. Also note that all dependent projects *must* be compiled with the same flag, or seg-faults and other undefined behavior may result. -4. Enable MKL. Please note that our benchmarks have shown that this helps only +4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only in very limited cases, and actually hurts performance in the usual case. We therefore recommend that you do *not* enable MKL, unless you have benchmarked it on your problem and have verified that it improves performance. From fe647a9e941bfad17a6a3d22983866fb91e78f39 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 13 Mar 2019 01:32:49 -0400 Subject: [PATCH 76/97] Remove unneeeded file in build chain --- cython/gtsam_unstable/__init__.py.in | 1 - 1 file changed, 1 deletion(-) delete mode 100644 cython/gtsam_unstable/__init__.py.in diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in deleted file mode 100644 index 85f7c3df2..000000000 --- a/cython/gtsam_unstable/__init__.py.in +++ /dev/null @@ -1 +0,0 @@ -${GTSAM_UNSTABLE_IMPORT} From 9982d79d74c20704cc987cd4ca63cb0934dfb8b1 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Mar 2019 09:02:04 +0000 Subject: [PATCH 77/97] added reasoning behind the setup.py unexpected location check --- cython/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cython/README.md b/cython/README.md index a524ac04c..b9ce2f012 100644 --- a/cython/README.md +++ b/cython/README.md @@ -24,6 +24,8 @@ export PYTHONPATH=$PYTHONPATH: - To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` - (the same command can be used to install into a virtual environment if it is active) - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory. + - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`. + Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package. UNIT TESTS ========== From 173191621eee0c3ae0fde428e547335f540fe3ac Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Mar 2019 09:45:56 +0000 Subject: [PATCH 78/97] made it possible to disable the setup.py check --- cython/setup.py.in | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index b7b0b7bc5..d6af28762 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -5,13 +5,13 @@ try: except ImportError: from distutils.core import setup, find_packages - -script_path = os.path.abspath(os.path.realpath(__file__)) -install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) -if script_path != install_path: - print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) - print('please run `make install` and run the script from there') - sys.exit(1) +if 'SETUP_PY_NO_CHECK' not in os.environ: + script_path = os.path.abspath(os.path.realpath(__file__)) + install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) + if script_path != install_path: + print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) + print('please run `make install` and run the script from there') + sys.exit(1) packages = find_packages() From 724a906bee985b3c7b3141fce81dd72141163094 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Mar 2019 22:47:23 -0400 Subject: [PATCH 79/97] Test existing readG2o --- cython/gtsam/tests/test_dataset.py | 33 ++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 cython/gtsam/tests/test_dataset.py diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py new file mode 100644 index 000000000..c634fb3f7 --- /dev/null +++ b/cython/gtsam/tests/test_dataset.py @@ -0,0 +1,33 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import unittest + +import gtsam + + +class TestDataset(unittest.TestCase): + def setUp(self): + pass + + def test_3d_graph(self): + is3D = True + g2o_file = gtsam.findExampleDataFile("pose3example.txt") + graph, initial = gtsam.readG2o(g2o_file, is3D) + self.assertEqual(graph.size(), 6) + self.assertEqual(initial.size(), 5) + + +if __name__ == '__main__': + unittest.main() From 0ca3f9d199e8900f119248a64a71694af27bcf17 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Mar 2019 23:22:37 -0400 Subject: [PATCH 80/97] Use c+11 initializer lists --- gtsam/slam/tests/testDataset.cpp | 91 +++++++++++++++----------------- 1 file changed, 44 insertions(+), 47 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 39c2d3722..4a327b8e1 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,65 +162,62 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -TEST( dataSet, readG2o3D) -{ +TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - Values expectedValues; - Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); - Point3 p0 = Point3(0.000000, 0.000000, 0.000000); - expectedValues.insert(0, Pose3(R0, p0)); + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - Point3 p1 = Point3(1.001367, 0.015390, 0.004948); - expectedValues.insert(1, Pose3(R1, p1)); + // Check values + for (size_t i : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); + } - Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); - Point3 p2 = Point3(1.993500, 0.023275, 0.003793); - expectedValues.insert(2, Pose3(R2, p2)); + // Initialize 6 relative measurements with quaternion/point3 values: + const std::vector measurements = { + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.105373, 0.311512, 0.656877, -0.678505}, + {0.523923, 0.776654, 0.326659}}, + {{0.568551, 0.595795, -0.561677, 0.079353}, + {0.910927, 0.055169, -0.411761}}, + {{0.542221, -0.592077, 0.303380, -0.513226}, + {0.775288, 0.228798, -0.596923}}, + {{0.327419, -0.125250, -0.534379, 0.769122}, + {-0.577841, 0.628016, -0.543592}}, + {{0.083672, 0.104639, 0.627755, 0.766795}, + {-0.623267, 0.086928, 0.773222}}, + }; - Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); - Point3 p3 = Point3(2.004291, 1.024305, 0.018047); - expectedValues.insert(3, Pose3(R3, p3)); + // Specify connectivity: + std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, + {3, 4}, {1, 4}, {3, 0}}; - Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); - Point3 p4 = Point3(0.999908, 1.055073, 0.020212); - expectedValues.insert(4, Pose3(R4, p4)); - - EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished()); + // Create expected factor graph + auto model = noiseModel::Isotropic::Precision(6, 10000); NonlinearFactorGraph expectedGraph; + size_t i = 0; + for (const auto& keys : pairs) { + expectedGraph.emplace_shared>( + keys.first, keys.second, measurements[i++], model); + } - Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); - - Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); - - Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); - - Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); - - Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); - - Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); - - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + // Check if actual graph is correct + EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); } /* ************************************************************************* */ From d8ee79fb8fd747bf2d497bacb9c808cb8d1b5fb6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 00:27:02 -0400 Subject: [PATCH 81/97] Working parseG2o3D --- gtsam/slam/tests/testDataset.cpp | 167 +++++++++++++++++++++++++------ 1 file changed, 135 insertions(+), 32 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4a327b8e1..3e2ed51c7 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,33 +162,96 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ +using BetweenFactors = std::vector::shared_ptr>; +using Poses = std::map; +std::pair parseG2o3D(const std::string&); + +#include +#define LINESIZE 81920 + +/* ************************************************************************* */ +std::pair parseG2o3D(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("load3D: can not find file " + filename); + + BetweenFactors factors; + Poses poses; + + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "VERTEX3") { + Key id; + double x, y, z, roll, pitch, yaw; + ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); + } + if (tag == "VERTEX_SE3:QUAT") { + Key id; + double x, y, z, qx, qy, qz, qw; + ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); + } + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "EDGE3") { + Key id1, id2; + double x, y, z, roll, pitch, yaw; + ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); + } + if (tag == "EDGE_SE3:QUAT") { + Key id1, id2; + double x, y, z, qx, qy, qz, qw; + ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { + double mij; + ls >> mij; + m(i, j) = mij; + m(j, i) = mij; + } + } + Matrix mgtsam(6, 6); + + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal + + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); + } + } + return make_pair(factors, poses); +} + TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - bool is3D = true; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - // Initialize 5 poses with quaternion/point3 values: - const std::vector poses = { - {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, - {{0.854230, 0.190253, 0.283162, -0.392318}, - {1.001367, 0.015390, 0.004948}}, - {{0.421446, -0.351729, -0.597838, 0.584174}, - {1.993500, 0.023275, 0.003793}}, - {{0.067024, 0.331798, -0.200659, 0.919323}, - {2.004291, 1.024305, 0.018047}}, - {{0.765488, -0.035697, -0.462490, 0.445933}, - {0.999908, 1.055073, 0.020212}}, - }; - - // Check values - for (size_t i : {0, 1, 2, 3, 4}) { - EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); - } + auto model = noiseModel::Isotropic::Precision(6, 10000); // Initialize 6 relative measurements with quaternion/point3 values: - const std::vector measurements = { + const std::vector relative_poses = { {{0.854230, 0.190253, 0.283162, -0.392318}, {1.001367, 0.015390, 0.004948}}, {{0.105373, 0.311512, 0.656877, -0.678505}, @@ -203,21 +266,61 @@ TEST(dataSet, readG2o3D) { {-0.623267, 0.086928, 0.773222}}, }; - // Specify connectivity: - std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, - {3, 4}, {1, 4}, {3, 0}}; + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - // Create expected factor graph - auto model = noiseModel::Isotropic::Precision(6, 10000); - NonlinearFactorGraph expectedGraph; + // Specify connectivity: + using KeyPair = pair; + std::vector edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}}; + + // Created expected factor graph size_t i = 0; - for (const auto& keys : pairs) { + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { expectedGraph.emplace_shared>( - keys.first, keys.second, measurements[i++], model); + keys.first, keys.second, relative_poses[i++], model); } - // Check if actual graph is correct + // Call parse version + BetweenFactors actualFactors; + Poses actualPoses; + std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); + + // Check factors + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(expectedGraph[i]), + *actualFactors[i], 1e-5)); + } + + // Check poses + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + + // Call graph version + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = true; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); + + // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + + // Check values + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */ From a47c52cb5ee858fec44b4fde82734aff95e47f64 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 00:58:22 -0400 Subject: [PATCH 82/97] Split parsing and moved to dataset.* --- gtsam/slam/dataset.cpp | 100 ++++++++++++++++-------------- gtsam/slam/dataset.h | 10 ++- gtsam/slam/tests/testDataset.cpp | 101 ++----------------------------- 3 files changed, 67 insertions(+), 144 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 34370d4e2..b7d1b04d6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, /* ************************************************************************* */ GraphAndValues readG2o(const string& g2oFile, const bool is3D, - KernelFunctionType kernelFunctionType) { - // just call load2D - int maxID = 0; - bool addNoise = false; - bool smart = true; - - if(is3D) + KernelFunctionType kernelFunctionType) { + if (is3D) { return load3D(g2oFile); - - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, - NoiseFormatG2O, kernelFunctionType); + } else { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); + } } /* ************************************************************************* */ @@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { - +std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load3D: can not find file " + filename); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + throw invalid_argument("parse3DPoses: can not find file " + filename); + std::map poses; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); } if (tag == "VERTEX_SE3:QUAT") { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); } } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); + return poses; +} +/* ************************************************************************* */ +std::vector::shared_ptr> parse3DFactors(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); + + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - Matrix m = I_6x6; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); SharedNoiseModel model = noiseModel::Gaussian::Information(m); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { double mij; ls >> mij; m(i, j) = mij; m(j, i) = mij; } } - Matrix mgtsam = I_6x6; + Matrix mgtsam(6, 6); - mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation - mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation - mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal - mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); } } + return factors; +} + +/* ************************************************************************* */ +GraphAndValues load3D(const string& filename) { + const auto factors = parse3DFactors(filename); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + for (const auto& factor : factors) { + graph->push_back(factor); + } + + const auto poses = parse3DPoses(filename); + Values::shared_ptr initial(new Values); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7c9b54a6f..9706bace0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -153,9 +153,13 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); -/** - * Load TORO 3D Graph - */ +/// Parse edges in 3D TORO graph file into a set of BetweenFactors. +GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); + +/// Parse vertices in 3D TORO graph file into a map of Pose3s. +GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); + +/// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 3e2ed51c7..0e131af32 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,90 +162,6 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -using BetweenFactors = std::vector::shared_ptr>; -using Poses = std::map; -std::pair parseG2o3D(const std::string&); - -#include -#define LINESIZE 81920 - -/* ************************************************************************* */ -std::pair parseG2o3D(const string& filename) { - ifstream is(filename.c_str()); - if (!is) throw invalid_argument("load3D: can not find file " + filename); - - BetweenFactors factors; - Poses poses; - - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "VERTEX3") { - Key id; - double x, y, z, roll, pitch, yaw; - ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); - } - if (tag == "VERTEX_SE3:QUAT") { - Key id; - double x, y, z, qx, qy, qz, qw; - ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); - } - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "EDGE3") { - Key id1, id2; - double x, y, z, roll, pitch, yaw; - ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Matrix m(6, 6); - for (size_t i = 0; i < 6; i++) - for (size_t j = i; j < 6; j++) ls >> m(i, j); - SharedNoiseModel model = noiseModel::Gaussian::Information(m); - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); - } - if (tag == "EDGE_SE3:QUAT") { - Key id1, id2; - double x, y, z, qx, qy, qz, qw; - ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Matrix m(6, 6); - for (size_t i = 0; i < 6; i++) { - for (size_t j = i; j < 6; j++) { - double mij; - ls >> mij; - m(i, j) = mij; - m(j, i) = mij; - } - } - Matrix mgtsam(6, 6); - - mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation - mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation - mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal - mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal - - SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); - } - } - return make_pair(factors, poses); -} - TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); auto model = noiseModel::Isotropic::Precision(6, 10000); @@ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) { keys.first, keys.second, relative_poses[i++], model); } - // Call parse version - BetweenFactors actualFactors; - Poses actualPoses; - std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); - - // Check factors + // Check factor parsing + const auto actualFactors = parse3DFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(expectedGraph[i]), *actualFactors[i], 1e-5)); } - // Check poses + // Check pose parsing + const auto actualPoses = parse3DPoses(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } - // Call graph version + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); - - // Check values for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); } From 88ac6de4afb1cef6ac6837f1bd33280c9f0cfb5d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 01:25:06 -0400 Subject: [PATCH 83/97] Wrapped parse3DFactors --- cython/gtsam/tests/test_dataset.py | 23 +++++++++++++++++------ gtsam.h | 10 ++++++++++ gtsam/slam/dataset.cpp | 2 +- gtsam/slam/dataset.h | 5 ++++- gtsam/slam/tests/testDataset.cpp | 2 +- 5 files changed, 33 insertions(+), 9 deletions(-) diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py index c634fb3f7..e561be1a8 100644 --- a/cython/gtsam/tests/test_dataset.py +++ b/cython/gtsam/tests/test_dataset.py @@ -8,26 +8,37 @@ See LICENSE for the license information Unit tests for testing dataset access. Author: Frank Dellaert """ -# pylint: disable=invalid-name, E1101 +# pylint: disable=invalid-name, no-name-in-module, no-member from __future__ import print_function import unittest import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s class TestDataset(unittest.TestCase): - def setUp(self): - pass + """Tests for datasets.h wrapper.""" - def test_3d_graph(self): + def setUp(self): + """Get some common paths.""" + self.pose3_example_g2o_file = gtsam.findExampleDataFile( + "pose3example.txt") + + def test_readG2o3D(self): + """Test reading directly into factor graph.""" is3D = True - g2o_file = gtsam.findExampleDataFile("pose3example.txt") - graph, initial = gtsam.readG2o(g2o_file, is3D) + graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D) self.assertEqual(graph.size(), 6) self.assertEqual(initial.size(), 5) + def test_parse3Dfactors(self): + """Test parsing into data structure.""" + factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) + self.assertEqual(factors.size(), 6) + self.assertIsInstance(factors.at(0), BetweenFactorPose3) + if __name__ == '__main__': unittest.main() diff --git a/gtsam.h b/gtsam.h index 48768db40..255a7a449 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2480,6 +2480,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +class BetweenFactorPose3s +{ + size_t size() const; + gtsam::BetweenFactorPose3* at(size_t i) const; +}; + +gtsam::BetweenFactorPose3s parse3DFactors(string filename); +pair load3D(string filename); + pair readG2o(string filename); pair readG2o(string filename, bool is3D); void writeG2o(const gtsam::NonlinearFactorGraph& graph, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b7d1b04d6..c32a049e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -540,7 +540,7 @@ std::map parse3DPoses(const string& filename) { } /* ************************************************************************* */ -std::vector::shared_ptr> parse3DFactors(const string& filename) { +BetweenFactorPose3s parse3DFactors(const string& filename) { ifstream is(filename.c_str()); if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 9706bace0..2106df48d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -34,6 +35,7 @@ #include // for pair #include #include +#include namespace gtsam { @@ -154,7 +156,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); /// Parse edges in 3D TORO graph file into a set of BetweenFactors. -GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); /// Parse vertices in 3D TORO graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 0e131af32..9434280d4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -16,8 +16,8 @@ */ -#include #include +#include #include #include #include From cbb84a64360cce71ced04ba0ad5f8224ef746c53 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 14:14:20 +0000 Subject: [PATCH 84/97] Added information about LieGroup helper class --- GTSAM-Concepts.md | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 366a58a09..a6cfee984 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc For Lie groups, the `exponential map` above is the most obvious mapping: it associates straight lines in the tangent space with geodesics on the manifold -(and it's inverse, the log map). However, there are two cases in which we deviate from this: +(and it's inverse, the log map). However, there are several cases in which we deviate from this: However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. +A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs: + +* operator* : implements group operator +* inverse: implements group inverse +* AdjointMap: maps tangent vectors according to group element +* Expmap/Logmap: exponential map and its inverse +* ChartAtOrigin: struct where you define Retract/Local at origin + +To use, simply derive, but also say `using LieGroup::inverse` so you get an inverse with a derivative. + +Finally, to create the traits automatically you can use `internal::LieGroupTraits` + Vector Space ------------ From dd7fa966e4093d166c4301b64c1bb9cf6a0cb9c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 15 Mar 2019 00:25:52 -0400 Subject: [PATCH 85/97] Added print in base class so all derived have it. Added comment how wrap currently does not handle Base class correctly in case of name clash, apparently. --- gtsam.h | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/gtsam.h b/gtsam.h index 48768db40..ed4bf8425 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1218,6 +1218,13 @@ class VariableIndex { namespace noiseModel { #include virtual class Base { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; }; virtual class Gaussian : gtsam::noiseModel::Base { @@ -1225,7 +1232,6 @@ virtual class Gaussian : gtsam::noiseModel::Base { static gtsam::noiseModel::Gaussian* Covariance(Matrix R); Matrix R() const; bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1236,7 +1242,6 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Variances(Vector variances); static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1263,7 +1268,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1271,7 +1275,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { virtual class Unit : gtsam::noiseModel::Isotropic { static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1279,11 +1282,11 @@ virtual class Unit : gtsam::noiseModel::Isotropic { namespace mEstimator { virtual class Base { + void print(string s) const; }; virtual class Null: gtsam::noiseModel::mEstimator::Base { Null(); - void print(string s) const; static gtsam::noiseModel::mEstimator::Null* Create(); // enabling serialization functionality @@ -1292,7 +1295,6 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { virtual class Fair: gtsam::noiseModel::mEstimator::Base { Fair(double c); - void print(string s) const; static gtsam::noiseModel::mEstimator::Fair* Create(double c); // enabling serialization functionality @@ -1301,7 +1303,6 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { virtual class Huber: gtsam::noiseModel::mEstimator::Base { Huber(double k); - void print(string s) const; static gtsam::noiseModel::mEstimator::Huber* Create(double k); // enabling serialization functionality @@ -1310,7 +1311,6 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { virtual class Tukey: gtsam::noiseModel::mEstimator::Base { Tukey(double k); - void print(string s) const; static gtsam::noiseModel::mEstimator::Tukey* Create(double k); // enabling serialization functionality @@ -1322,7 +1322,6 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { virtual class Robust : gtsam::noiseModel::Base { Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; // enabling serialization functionality void serializable() const; From a7e60a08fe5a5c6345eb8db9206fe0a24b40b231 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 18 Mar 2019 23:15:53 -0400 Subject: [PATCH 86/97] Wrapped more useful noiseModel methods --- gtsam.h | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index ed4bf8425..fb72660d5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1230,9 +1230,19 @@ virtual class Base { virtual class Gaussian : gtsam::noiseModel::Base { static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; + bool equals(gtsam::noiseModel::Base& expected, double tol); + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + // enabling serialization functionality void serializable() const; }; @@ -1243,6 +1253,11 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + // enabling serialization functionality void serializable() const; }; @@ -1269,6 +1284,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + // access to noise model + double sigma() const; + // enabling serialization functionality void serializable() const; }; From 205803a0eabd2c0b1fd4098bb37ee0e6b1b71a30 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 00:11:45 -0400 Subject: [PATCH 87/97] Better optimization parameter wrapping, plus python test --- cython/gtsam/tests/test_NonlinearOptimizer.py | 71 +++++++++++++++++++ gtsam.h | 29 ++++++-- gtsam/nonlinear/LevenbergMarquardtParams.h | 6 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 47 +++--------- 4 files changed, 109 insertions(+), 44 deletions(-) create mode 100644 cython/gtsam/tests/test_NonlinearOptimizer.py diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py new file mode 100644 index 000000000..0d0318f40 --- /dev/null +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -0,0 +1,71 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, + GaussNewtonParams, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + Point2, PriorFactorPoint2, Values) + +KEY1 = 1 +KEY2 = 2 + + +class TestScenario(unittest.TestCase): + def test_optimize(self): + """Do trivial test with three optimizer variants.""" + fg = NonlinearFactorGraph() + model = gtsam.noiseModel_Unit.Create(2) + fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) + + # test error at minimum + xstar = Point2(0, 0) + optimal_values = Values() + optimal_values.insert(KEY1, xstar) + self.assertEqual(0.0, fg.error(optimal_values), 0.0) + + # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + x0 = Point2(3, 3) + initial_values = Values() + initial_values.insert(KEY1, x0) + self.assertEqual(9.0, fg.error(initial_values), 1e-3) + + # optimize parameters + ordering = Ordering() + ordering.push_back(KEY1) + + # Gauss-Newton + gnParams = GaussNewtonParams() + gnParams.setOrdering(ordering) + actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() + self.assertAlmostEqual(0, fg.error(actual1)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setOrdering(ordering) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Dogleg + dlParams = DoglegParams() + dlParams.setOrdering(ordering) + actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() + self.assertAlmostEqual(0, fg.error(actual3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index ed4bf8425..50d59f973 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2001,10 +2001,12 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); bool isMultifrontal() const; bool isSequential() const; @@ -2025,15 +2027,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { LevenbergMarquardtParams(); - double getlambdaInitial() const; + bool getDiagonalDamping() const; double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; string getVerbosityLM() const; - - void setlambdaInitial(double value); + + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); }; #include diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index abb8c3c22..491fbd233 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -122,22 +122,24 @@ public: virtual ~LevenbergMarquardtParams() {} void print(const std::string& str = "") const override; - /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @name Getters/Setters, mainly for wrappers. Use fields above in C++. /// @{ bool getDiagonalDamping() const { return diagonalDamping; } double getlambdaFactor() const { return lambdaFactor; } double getlambdaInitial() const { return lambdaInitial; } double getlambdaLowerBound() const { return lambdaLowerBound; } double getlambdaUpperBound() const { return lambdaUpperBound; } + bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } std::string getLogFile() const { return logFile; } std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } void setlambdaFactor(double value) { lambdaFactor = value; } void setlambdaInitial(double value) { lambdaInitial = value; } void setlambdaLowerBound(double value) { lambdaLowerBound = value; } void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - void setLogFile(const std::string& s) { logFile = s; } void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} }; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 88fc0f850..8e4678590 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -54,47 +54,24 @@ public: } virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { - return maxIterations; - } - double getRelativeErrorTol() const { - return relativeErrorTol; - } - double getAbsoluteErrorTol() const { - return absoluteErrorTol; - } - double getErrorTol() const { - return errorTol; - } - std::string getVerbosity() const { - return verbosityTranslator(verbosity); - } + size_t getMaxIterations() const { return maxIterations; } + double getRelativeErrorTol() const { return relativeErrorTol; } + double getAbsoluteErrorTol() const { return absoluteErrorTol; } + double getErrorTol() const { return errorTol; } + std::string getVerbosity() const { return verbosityTranslator(verbosity); } - void setMaxIterations(int value) { - maxIterations = value; - } - void setRelativeErrorTol(double value) { - relativeErrorTol = value; - } - void setAbsoluteErrorTol(double value) { - absoluteErrorTol = value; - } - void setErrorTol(double value) { - errorTol = value; - } - void setVerbosity(const std::string &src) { + void setMaxIterations(int value) { maxIterations = value; } + void setRelativeErrorTol(double value) { relativeErrorTol = value; } + void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } + void setErrorTol(double value) { errorTol = value; } + void setVerbosity(const std::string& src) { verbosity = verbosityTranslator(src); } static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - // Successive Linearization Parameters - -public: - /** See NonlinearOptimizerParams::linearSolverType */ - enum LinearSolverType { MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_QR, @@ -168,13 +145,9 @@ public: private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; - }; // For backward compatibility: From 5af2256277372cfd5c17016155604e3c45f998e8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 10:30:30 -0400 Subject: [PATCH 88/97] Added missing clone for MATLAB wrapper --- gtsam/nonlinear/LevenbergMarquardtParams.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index 491fbd233..4962ad366 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -142,6 +142,15 @@ public: void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + boost::shared_ptr clone() const { + return boost::shared_ptr(new LevenbergMarquardtParams(*this)); + } + + /// @} }; } From 37eba50932df03dae90cc3c5c94f19275c16dbb8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 15:47:01 -0400 Subject: [PATCH 89/97] Modernized, documented --- gtsam/slam/InitializePose3.cpp | 186 +++++++++-------------- gtsam/slam/InitializePose3.h | 61 +++++--- gtsam/slam/tests/testInitializePose3.cpp | 24 +-- 3 files changed, 129 insertions(+), 142 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3979996da..b781f79f0 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -16,7 +16,7 @@ * @date August, 2014 */ -#include +#include #include #include #include @@ -29,13 +29,9 @@ using namespace std; namespace gtsam { -namespace InitializePose3 { +namespace initialize_pose3 { -static const Matrix I9 = I_9x9; -static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33 = Z_3x3; - -static const Key keyAnchor = symbol('Z', 9999999); +static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { @@ -43,26 +39,29 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - for(const boost::shared_ptr& factor: g) { + for(const auto& factor: g) { Matrix3 Rij; - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) Rij = pose3Between->measured().rotation().matrix(); else - std::cout << "Error in buildLinearOrientationGraph" << std::endl; + cout << "Error in buildLinearOrientationGraph" << endl; - const KeyVector& keys = factor->keys(); + const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I9, key2, M9, zero9, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model); + linearGraph.add( + kAnchorKey, I_9x9, + (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) + .finished(), + model); return linearGraph; } @@ -75,23 +74,15 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - for(const VectorValues::value_type& it: relaxedRot3) { + for(const auto& it: relaxedRot3) { Key key = it.first; - if (key != keyAnchor) { - const Vector& rotVector = it.second; - Matrix3 rotMat; - rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); - rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); - rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + if (key != kAnchorKey) { + Matrix3 R; R << it.second; Matrix U, V; Vector s; - svd(rotMat, U, s, V); + svd(R.transpose(), U, s, V); Matrix3 normalizedRotMat = U * V.transpose(); - // std::cout << "rotMat \n" << rotMat << std::endl; - // std::cout << "U V' \n" << U * V.transpose() << std::endl; - // std::cout << "V \n" << V << std::endl; - if(normalizedRotMat.determinant() < 0) normalizedRotMat = U * ppm * V.transpose(); @@ -103,31 +94,27 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - for(const boost::shared_ptr& factor: graph) { + for(const auto& factor: graph) { // recast to a between on Pose3 - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + const auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) pose3Graph.add(pose3Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr > pose3Prior = - boost::dynamic_pointer_cast >(factor); + const auto pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Graph.emplace_shared >(kAnchorKey, pose3Prior->keys()[0], pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); @@ -142,14 +129,16 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, + const Values& givenGuess, + const size_t maxIter, + const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; - inverseRot.insert(keyAnchor, Rot3()); - for(const Values::ConstKeyValuePair& key_value: givenGuess) { + inverseRot.insert(kAnchorKey, Rot3()); + for(const auto& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -164,9 +153,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - grad.insert(key,Vector3::Zero()); + grad.insert(key,Z_3x1); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); if(currNodeDeg > maxNodeDeg) maxNodeDeg = currNodeDeg; @@ -180,42 +169,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad; // gradient iterations size_t it; - for(it=0; it < maxIter; it++){ + for (it = 0; it < maxIter; it++) { ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node - //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a - // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for (const auto& key_value : inverseRot) { Key key = key_value.key; - //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; - Vector gradKey = Vector3::Zero(); + Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key - for(const size_t& factorId: adjEdgesMap.at(key)){ + for (const size_t& factorId : adjEdgesMap.at(key)) { Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); - if( key == (pose3Graph.at(factorId))->keys()[0] ){ - Key key1 = (pose3Graph.at(factorId))->keys()[1]; + auto factor = pose3Graph.at(factorId); + const auto& keys = factor->keys(); + if (key == keys[0]) { + Key key1 = keys[1]; Rot3 Rj = inverseRot.at(key1); - gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); - //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; - }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ - Key key0 = (pose3Graph.at(factorId))->keys()[0]; + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + } else if (key == keys[1]) { + Key key0 = keys[0]; Rot3 Rj = inverseRot.at(key0); gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); - //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; - }else{ - std::cout << "Error in gradient computation" << std::endl; + } else { + cout << "Error in gradient computation" << endl; } - } // end of i-th gradient computation - grad.at(key) = stepsize * gradKey; + } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -230,14 +214,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; - // Return correct rotations - const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior + const Rot3& Rref = inverseRot.at(kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - if (key != keyAnchor) { + if (key != kAnchorKey) { const Rot3& R = inverseRot.at(key); if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); @@ -249,11 +231,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ +void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; - for(const boost::shared_ptr& factor: pose3Graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + for(const auto& factor: pose3Graph) { + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap.insert(pair(factorId,Rij)); @@ -275,7 +257,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, adjEdgesMap.insert(pair >(key2, edge_id)); } }else{ - std::cout << "Error in computeOrientationsGradient" << std::endl; + cout << "Error in createSymbolicGraph" << endl; } factorId++; } @@ -292,10 +274,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 && th == th){ // nonzero valid rotations + if (th > 1e-5 && th == th) { // nonzero valid rotations logRot = logRot / th; - }else{ - logRot = Vector3::Zero(); + } else { + logRot = Z_3x1; th = 0.0; } @@ -320,24 +302,25 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values initialPose; - for(const Values::ConstKeyValuePair& key_value: initialRot){ + for (const auto& key_value : initialRot) { Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } + // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - initialPose.insert(keyAnchor, Pose3()); - pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); + initialPose.insert(kAnchorKey, Pose3()); + pose3graph.emplace_shared >(kAnchorKey, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; bool singleIter = true; - if(singleIter){ + if (singleIter) { params.maxIterations = 1; - }else{ - std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <(key); estimate.insert(key, pose); } @@ -356,22 +339,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, + bool useGradient) { gttic(InitializePose3_initialize); - - // We "extract" the Pose3 subgraph of the original graph: this - // is done to properly model priors and avoiding operating on a larger graph - NonlinearFactorGraph pose3Graph = buildPose3graph(graph); - - // Get orientations from relative orientation measurements - Values valueRot3 = computeOrientationsChordal(pose3Graph); - - // Compute the full poses (1 GN iteration on full poses) - return computePoses(pose3Graph, valueRot3); -} - -/* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { Values initialValues; // We "extract" the Pose3 subgraph of the original graph: this @@ -380,27 +350,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Get orientations from relative orientation measurements Values orientations; - if(useGradient) + if (useGradient) orientations = computeOrientationsGradient(pose3Graph, givenGuess); else orientations = computeOrientationsChordal(pose3Graph); -// orientations.print("orientations\n"); - // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - - // for(const Values::ConstKeyValuePair& key_value: orientations) { - // Key key = key_value.key; - // if (key != keyAnchor) { - // const Point3& pos = givenGuess.at(key).translation(); - // const Rot3& rot = orientations.at(key); - // Pose3 initializedPoses = Pose3(rot, pos); - // initialValues.insert(key, initializedPoses); - // } - // } - // return initialValues; } -} // end of namespace lago -} // end of namespace gtsam +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph) { + return initialize(graph, Values(), false); +} + +} // namespace initialize_pose3 +} // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index dba5ceac3..d9bea7932 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -20,40 +20,65 @@ #pragma once -#include +#include +#include #include #include -#include -#include +#include namespace gtsam { typedef std::map > KeyVectorMap; -typedef std::map KeyRotMap; +typedef std::map KeyRotMap; -namespace InitializePose3 { +namespace initialize_pose3 { -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); +GTSAM_EXPORT GaussianFactorGraph +buildLinearOrientationGraph(const NonlinearFactorGraph& g); GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); +/** + * Return the orientations of a graph including only BetweenFactors + */ +GTSAM_EXPORT Values +computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); +/** + * Return the orientations of a graph including only BetweenFactors + */ +GTSAM_EXPORT Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); +GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b); +GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, + const double a, const double b); -GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); +/** + * Select the subgraph of betweenFactors and transforms priors into between wrt + * a fictitious node + */ +GTSAM_EXPORT NonlinearFactorGraph +buildPose3graph(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); +GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); +/** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal method), + * and finish up with 1 GN iteration on full poses. + */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, + bool useGradient = false); + +/// Calls initialize above using Chordal method. GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); - -} // end of namespace lago -} // end of namespace gtsam +} // namespace initialize_pose3 +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..ea474c480 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -74,15 +74,15 @@ NonlinearFactorGraph graph() { /* *************************************************************************** */ TEST( InitializePose3, buildPose3graph ) { - NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph()); // pose3graph.print(""); } /* *************************************************************************** */ TEST( InitializePose3, orientations ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); - Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); @@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); KeyVectorMap adjEdgesMap; KeyRotMap factorId2RotMap; - InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); + initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); @@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) { double a = 6.010534238540223; double b = 1.0; - Vector actual = InitializePose3::gradientTron(R1, R2, a, b); + Vector actual = initialize_pose3::gradientTron(R1, R2, a, b); Vector expected = Vector3::Zero(); expected(2) = 1.962658662803917; @@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) { /* *************************************************************************** */ TEST( InitializePose3, iterationGradient ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) { size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); + Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, 0.033572116359134, 0.999436104312325, -0.000621610948719, @@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); + Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); @@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) { givenPoses.insert(x2,simple::pose2); givenPoses.insert(x3,simple::pose3); - Values initial = InitializePose3::initialize(simple::graph(), givenPoses); + Values initial = initialize_pose3::initialize(simple::graph(), givenPoses); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(givenPoses, initial, 1e-6)); @@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses ) noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); inputGraph->add(PriorFactor(0, Pose3(), priorModel)); - Values initial = InitializePose3::initialize(*inputGraph); + Values initial = initialize_pose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! } From 3a371a1cf284139717204b5c0fad71a87247486d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 15:47:51 -0400 Subject: [PATCH 90/97] Wrapped --- gtsam.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam.h b/gtsam.h index 9a06b7bd5..0aca81d02 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2523,6 +2523,23 @@ class BetweenFactorPose3s gtsam::BetweenFactorPose3* at(size_t i) const; }; +#include +namespace initialize_pose3 { +gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); +gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); +gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); +gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, bool useGradient); +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +} // namespace initialize_pose3 + gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); From e2cf42773a67abdb386c12de5ef710325a35ff66 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 17:04:31 -0400 Subject: [PATCH 91/97] Switched to struct with static methods as apparently global methods in namespaces are not wrapped. --- gtsam.h | 33 +++++----- gtsam/slam/InitializePose3.cpp | 32 +++++---- gtsam/slam/InitializePose3.h | 83 ++++++++++++------------ gtsam/slam/tests/testInitializePose3.cpp | 24 +++---- 4 files changed, 88 insertions(+), 84 deletions(-) diff --git a/gtsam.h b/gtsam.h index 0aca81d02..014907037 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2524,21 +2524,24 @@ class BetweenFactorPose3s }; #include -namespace initialize_pose3 { -gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); -gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); -gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); -gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, bool useGradient); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -} // namespace initialize_pose3 +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b781f79f0..667ccef0d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -29,12 +29,11 @@ using namespace std; namespace gtsam { -namespace initialize_pose3 { static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { +GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); @@ -67,7 +66,8 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { /* ************************************************************************* */ // Transform VectorValues into valid Rot3 -Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { +Values InitializePose3::normalizeRelaxedRotations( + const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Z_3x3; // plus plus minus @@ -94,7 +94,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { +NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; @@ -115,7 +115,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { +Values InitializePose3::computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); // regularize measurements and plug everything in a factor graph @@ -129,10 +130,9 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, - const size_t maxIter, - const bool setRefFrame) { +Values InitializePose3::computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -231,7 +231,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, +void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; for(const auto& factor: pose3Graph) { @@ -264,7 +264,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, } /* ************************************************************************* */ -Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { +Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); @@ -286,8 +286,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl } /* ************************************************************************* */ -Values initializeOrientations(const NonlinearFactorGraph& graph) { - +Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) { // We "extract" the Pose3 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose3Graph = buildPose3graph(graph); @@ -297,7 +296,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { +Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { gttic(InitializePose3_computePoses); // put into Values structure @@ -339,7 +338,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, +Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { gttic(InitializePose3_initialize); Values initialValues; @@ -360,9 +359,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values InitializePose3::initialize(const NonlinearFactorGraph& graph) { return initialize(graph, Values(), false); } -} // namespace initialize_pose3 } // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index d9bea7932..ce1b28854 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -31,54 +31,57 @@ namespace gtsam { typedef std::map > KeyVectorMap; typedef std::map KeyRotMap; -namespace initialize_pose3 { +struct GTSAM_EXPORT InitializePose3 { + static GaussianFactorGraph buildLinearOrientationGraph( + const NonlinearFactorGraph& g); -GTSAM_EXPORT GaussianFactorGraph -buildLinearOrientationGraph(const NonlinearFactorGraph& g); + static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph); -/** - * Return the orientations of a graph including only BetweenFactors - */ -GTSAM_EXPORT Values -computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -/** - * Return the orientations of a graph including only BetweenFactors - */ -GTSAM_EXPORT Values computeOrientationsGradient( - const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, - size_t maxIter = 10000, const bool setRefFrame = true); + static void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, - KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); + static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, + const double b); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, - const double a, const double b); + /** + * Select the subgraph of betweenFactors and transforms priors into between + * wrt a fictitious node + */ + static NonlinearFactorGraph buildPose3graph( + const NonlinearFactorGraph& graph); -/** - * Select the subgraph of betweenFactors and transforms priors into between wrt - * a fictitious node - */ -GTSAM_EXPORT NonlinearFactorGraph -buildPose3graph(const NonlinearFactorGraph& graph); + static Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, - Values& initialRot); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements using chordal method. + */ + static Values initializeOrientations(const NonlinearFactorGraph& graph); -/** - * "extract" the Pose3 subgraph of the original graph, get orientations from - * relative orientation measurements (using either gradient or chordal method), - * and finish up with 1 GN iteration on full poses. - */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, - const Values& givenGuess, - bool useGradient = false); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal + * method), and finish up with 1 GN iteration on full poses. + */ + static Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, bool useGradient = false); -/// Calls initialize above using Chordal method. -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); - -} // namespace initialize_pose3 + /// Calls initialize above using Chordal method. + static Values initialize(const NonlinearFactorGraph& graph); +}; } // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index ea474c480..e6f08286a 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -74,15 +74,15 @@ NonlinearFactorGraph graph() { /* *************************************************************************** */ TEST( InitializePose3, buildPose3graph ) { - NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); // pose3graph.print(""); } /* *************************************************************************** */ TEST( InitializePose3, orientations ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); - Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph); + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); @@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); KeyVectorMap adjEdgesMap; KeyRotMap factorId2RotMap; - initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); + InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); @@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) { double a = 6.010534238540223; double b = 1.0; - Vector actual = initialize_pose3::gradientTron(R1, R2, a, b); + Vector actual = InitializePose3::gradientTron(R1, R2, a, b); Vector expected = Vector3::Zero(); expected(2) = 1.962658662803917; @@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) { /* *************************************************************************** */ TEST( InitializePose3, iterationGradient ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) { size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; - Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, 0.033572116359134, 0.999436104312325, -0.000621610948719, @@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; - Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); @@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) { givenPoses.insert(x2,simple::pose2); givenPoses.insert(x3,simple::pose3); - Values initial = initialize_pose3::initialize(simple::graph(), givenPoses); + Values initial = InitializePose3::initialize(simple::graph(), givenPoses); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(givenPoses, initial, 1e-6)); @@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses ) noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); inputGraph->add(PriorFactor(0, Pose3(), priorModel)); - Values initial = initialize_pose3::initialize(*inputGraph); + Values initial = InitializePose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! } From a89e422a8ac4c49a5b476f10c65da7e7b8ab7b29 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 17:04:48 -0400 Subject: [PATCH 92/97] Added python example and test --- ...Pose3SLAMExample_initializePose3Chordal.py | 35 ++++++++ cython/gtsam/tests/test_initialize_pose3.py | 88 +++++++++++++++++++ 2 files changed, 123 insertions(+) create mode 100644 cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py create mode 100644 cython/gtsam/tests/test_initialize_pose3.py diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py new file mode 100644 index 000000000..02c696905 --- /dev/null +++ b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -0,0 +1,35 @@ +""" +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 + +Initialize PoseSLAM with Chordal init +Author: Luca Carlone, Frank Dellaert (python port) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Read graph from file +g2oFile = gtsam.findExampleDataFile("pose3example.txt") + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add prior on the first key. TODO: assumes first key ios z +priorModel = gtsam.noiseModel_Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + +# Initializing Pose3 - chordal relaxation" +initialization = gtsam.InitializePose3.initialize(graph) + +print(initialization) diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py new file mode 100644 index 000000000..ee9195c48 --- /dev/null +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -0,0 +1,88 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for 3D SLAM initialization, using rotation relaxation. +Author: Luca Carlone and Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values + +x0, x1, x2, x3 = 0, 1, 2, 3 + + +class TestValues(unittest.TestCase): + + def setUp(self): + + model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + + # We consider a small graph: + # symbolic FG + # x2 0 1 + # / | \ 1 2 + # / | \ 2 3 + # x3 | x1 2 0 + # \ | / 0 3 + # \ | / + # x0 + # + p0 = Point3(0, 0, 0) + self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) + p1 = Point3(1, 2, 0) + self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) + p2 = Point3(0, 2, 0) + self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) + p3 = Point3(-1, 1, 0) + self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) + + pose0 = Pose3(self.R0, p0) + pose1 = Pose3(self.R1, p1) + pose2 = Pose3(self.R2, p2) + pose3 = Pose3(self.R3, p3) + + g = NonlinearFactorGraph() + g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) + g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) + g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) + g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) + g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) + g.add(gtsam.PriorFactorPose3(x0, pose0, model)) + self.graph = g + + def test_buildPose3graph(self): + pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + def test_orientations(self): + pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) + + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI + self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6)) + self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6)) + self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6)) + self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6)) + + def test_initializePoses(self): + g2oFile = gtsam.findExampleDataFile("pose3example-grid") + is3D = True + inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) + priorModel = gtsam.noiseModel_Unit.Create(6) + inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) + + initial = gtsam.InitializePose3.initialize(inputGraph) + # TODO(frank): very loose !! + self.assertTrue(initial.equals(expectedValues, 0.1)) + + +if __name__ == "__main__": + unittest.main() From 1e944fb86a5420d7e9d60185a0d28a7725654998 Mon Sep 17 00:00:00 2001 From: lucacarlone Date: Tue, 19 Mar 2019 18:44:09 -0400 Subject: [PATCH 93/97] now initialization is aware of rotation noise model --- gtsam/slam/InitializePose3.cpp | 15 ++++++++++----- gtsam/slam/tests/testInitializePose3.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 667ccef0d..2f247811d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -36,16 +36,21 @@ static const Key kAnchorKey = symbol('Z', 9999999); GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); for(const auto& factor: g) { Matrix3 Rij; + double rotationPrecision = 1.0; auto pose3Between = boost::dynamic_pointer_cast >(factor); - if (pose3Between) + if (pose3Between){ Rij = pose3Between->measured().rotation().matrix(); - else + Vector precisions = Vector::Zero(6); + precisions[0] = 1.0; // vector of all zeros except first entry equal to 1 + pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable + rotationPrecision = precisions[0]; // rotations first + }else{ cout << "Error in buildLinearOrientationGraph" << endl; + } const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; @@ -53,14 +58,14 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision)); } // prior on the anchor orientation linearGraph.add( kAnchorKey, I_9x9, (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) .finished(), - model); + noiseModel::Isotropic::Precision(9, 1)); return linearGraph; } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..6fe8b3d7c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -70,6 +70,17 @@ NonlinearFactorGraph graph() { g.add(PriorFactor(x0, pose0, model)); return g; } + +NonlinearFactorGraph graph2() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information + g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin + g.add(PriorFactor(x0, pose0, model)); + return g; +} } /* *************************************************************************** */ @@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsPrecisions ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2()); + + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); From 90e6eb95cf0372d95d166d0384bd77650cc36e43 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Mar 2019 17:35:31 -0400 Subject: [PATCH 94/97] Added GtsamTestCase --- cython/gtsam/utils/test_case.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 cython/gtsam/utils/test_case.py diff --git a/cython/gtsam/utils/test_case.py b/cython/gtsam/utils/test_case.py new file mode 100644 index 000000000..7df1e6ee9 --- /dev/null +++ b/cython/gtsam/utils/test_case.py @@ -0,0 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +TestCase class with GTSAM assert utils. +Author: Frank Dellaert +""" +import unittest + + +class GtsamTestCase(unittest.TestCase): + """TestCase class with GTSAM assert utils.""" + + def gtsamAssertEquals(self, actual, expected, tol=1e-9): + """ AssertEqual function that prints out actual and expected if not equal. + Usage: + self.gtsamAssertEqual(actual,expected) + Keyword Arguments: + tol {float} -- tolerance passed to 'equals', default 1e-9 + """ + equal = actual.equals(expected, tol) + if not equal: + raise self.failureException( + "Values are not equal:\n{}!={}".format(actual, expected)) From c442df38664a719ed7c5a482a82e8bdb3ee1acf8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Mar 2019 17:35:53 -0400 Subject: [PATCH 95/97] Modernized all tests --- cython/gtsam/tests/test_Cal3Unified.py | 26 +++++++++++--- cython/gtsam/tests/test_JacobianFactor.py | 22 +++++++++--- cython/gtsam/tests/test_KalmanFilter.py | 18 ++++++++-- .../gtsam/tests/test_LocalizationExample.py | 20 +++++++++-- cython/gtsam/tests/test_NonlinearOptimizer.py | 3 +- cython/gtsam/tests/test_OdometryExample.py | 20 +++++++++-- cython/gtsam/tests/test_PlanarSLAMExample.py | 22 +++++++++--- cython/gtsam/tests/test_Pose2.py | 15 ++++++-- cython/gtsam/tests/test_Pose2SLAMExample.py | 20 +++++++++-- cython/gtsam/tests/test_Pose3.py | 19 +++++++--- cython/gtsam/tests/test_Pose3SLAMExample.py | 28 +++++++++++---- cython/gtsam/tests/test_PriorFactor.py | 18 ++++++++-- cython/gtsam/tests/test_SFMExample.py | 23 +++++++++--- cython/gtsam/tests/test_Scenario.py | 13 +++---- cython/gtsam/tests/test_SimpleCamera.py | 25 +++++++++---- cython/gtsam/tests/test_StereoVOExample.py | 23 +++++++++--- cython/gtsam/tests/test_Values.py | 36 ++++++++++--------- cython/gtsam/tests/test_VisualISAMExample.py | 23 +++++++++--- cython/gtsam/tests/test_dataset.py | 5 +-- cython/gtsam/tests/test_initialize_pose3.py | 13 +++---- .../tests/test_FixedLagSmootherExample.py | 17 +++++++-- 21 files changed, 316 insertions(+), 93 deletions(-) diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index 3225d2ff9..fbf5f3565 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -1,9 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np +import gtsam +from gtsam.utils.test_case import GtsamTestCase -class TestCal3Unified(unittest.TestCase): + +class TestCal3Unified(GtsamTestCase): def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase): self.assertEqual(K.fx(), 1.) def test_retract(self): - expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) - K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') actual = K.retract(d) - self.assertTrue(actual.equals(expected, 1e-9)) + self.gtsamAssertEquals(actual, expected) np.testing.assert_allclose(d, K.localCoordinates(actual)) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py index bf63c839b..04433492b 100644 --- a/cython/gtsam/tests/test_JacobianFactor.py +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +JacobianFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestJacobianFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestJacobianFactor(GtsamTestCase): def test_eliminate(self): # Recommended way to specify a matrix (see cython/README) @@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase): expectedCG = gtsam.GaussianConditional( x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) # check if the result matches - self.assertTrue(actualCG.equals(expectedCG, 1e-4)) + self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) # the expected linear factor Bl1 = np.array([[4.47214, 0.00], @@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase): expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) # check if the result matches the combined (reduced) factor - self.assertTrue(lf.equals(expectedLF, 1e-4)) + self.gtsamAssertEquals(lf, expectedLF, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py index 56f9e2573..94c41df72 100644 --- a/cython/gtsam/tests/test_KalmanFilter.py +++ b/cython/gtsam/tests/test_KalmanFilter.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestKalmanFilter(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestKalmanFilter(GtsamTestCase): def test_KalmanFilter(self): F = np.eye(2) diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py index c373f162c..6ce65f087 100644 --- a/cython/gtsam/tests/test_LocalizationExample.py +++ b/cython/gtsam/tests/test_LocalizationExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Localization unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestLocalizationExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestLocalizationExample(GtsamTestCase): def test_LocalizationExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase): P = [None] * result.size() for i in range(0, result.size()): pose_i = result.atPose2(i) - self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4)) + self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) P[i] = marginals.marginalCovariance(i) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py index 0d0318f40..efefb218a 100644 --- a/cython/gtsam/tests/test_NonlinearOptimizer.py +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -19,12 +19,13 @@ from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, GaussNewtonParams, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, Point2, PriorFactorPoint2, Values) +from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 KEY2 = 2 -class TestScenario(unittest.TestCase): +class TestScenario(GtsamTestCase): def test_optimize(self): """Do trivial test with three optimizer variants.""" fg = NonlinearFactorGraph() diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py index 1100e8334..c8ea95588 100644 --- a/cython/gtsam/tests/test_OdometryExample.py +++ b/cython/gtsam/tests/test_OdometryExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Odometry unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestOdometryExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestOdometryExample(GtsamTestCase): def test_OdometryExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase): # Check first pose equality pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py index 046a93f35..ae813d35c 100644 --- a/cython/gtsam/tests/test_PlanarSLAMExample.py +++ b/cython/gtsam/tests/test_PlanarSLAMExample.py @@ -1,11 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PlanarSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase - def test_Pose2SLAMExample(self): + +class TestPlanarSLAM(GtsamTestCase): + + def test_PlanarSLAM(self): # Assumptions # - All values are axis aligned # - Robot poses are facing along the X axis (horizontal, to the right in images) @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py index afd0c5773..9652b594a 100644 --- a/cython/gtsam/tests/test_Pose2.py +++ b/cython/gtsam/tests/test_Pose2.py @@ -1,12 +1,23 @@ -"""Pose2 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest import numpy as np +import gtsam from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase -class TestPose2(unittest.TestCase): +class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" def test_adjoint(self): diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py index bcaa7be4f..a79b6b18c 100644 --- a/cython/gtsam/tests/test_Pose2SLAMExample.py +++ b/cython/gtsam/tests/test_Pose2SLAMExample.py @@ -1,9 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2SLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2SLAMExample(GtsamTestCase): def test_Pose2SLAMExample(self): # Assumptions @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index b878a9551..3eb4067c9 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -1,13 +1,24 @@ -"""Pose3 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose3 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math import unittest import numpy as np +import gtsam from gtsam import Point3, Pose3, Rot3 +from gtsam.utils.test_case import GtsamTestCase -class TestPose3(unittest.TestCase): +class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" def test_between(self): @@ -16,14 +27,14 @@ class TestPose3(unittest.TestCase): T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_transform_to(self): """Test transform_to method.""" transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) actual = transform.transform_to(Point3(3, 2, 10)) expected = Point3(2, 1, 10) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_range(self): """Test range method.""" diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py index e33db2145..1e9eaac67 100644 --- a/cython/gtsam/tests/test_Pose3SLAMExample.py +++ b/cython/gtsam/tests/test_Pose3SLAMExample.py @@ -1,10 +1,24 @@ -import unittest -import numpy as np -import gtsam -from math import pi -from gtsam.utils.circlePose3 import * +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved -class TestPose3SLAMExample(unittest.TestCase): +See LICENSE for the license information + +PoseSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.utils.circlePose3 import * + + +class TestPose3SLAMExample(GtsamTestCase): def test_Pose3SLAMExample(self): # Create a hexagon of poses @@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase): result = optimizer.optimizeSafely() pose_1 = result.atPose3(1) - self.assertTrue(pose_1.equals(p1, 1e-4)) + self.gtsamAssertEquals(pose_1, p1, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py index 95ec2ae94..0acf50c2c 100644 --- a/cython/gtsam/tests/test_PriorFactor.py +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PriorFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestPriorFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPriorFactor(GtsamTestCase): def test_PriorFactor(self): values = gtsam.Values() diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py index 606b26a43..e8fa46186 100644 --- a/cython/gtsam/tests/test_SFMExample.py +++ b/cython/gtsam/tests/test_SFMExample.py @@ -1,11 +1,24 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SFM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np + +import gtsam import gtsam.utils.visual_data_generator as generator +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestSFMExample(unittest.TestCase): +class TestSFMExample(GtsamTestCase): def test_SFMExample(self): options = generator.Options() @@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase): # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('p'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index d68566b25..09601fba5 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/cython/gtsam/tests/test_Scenario.py @@ -5,11 +5,9 @@ All Rights Reserved See LICENSE for the license information -Unit tests for IMU testing scenarios. +Scenario unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ -# pylint: disable=invalid-name, E1101 - from __future__ import print_function import math @@ -18,9 +16,12 @@ import unittest import numpy as np import gtsam +from gtsam.utils.test_case import GtsamTestCase + +# pylint: disable=invalid-name, E1101 -class TestScenario(unittest.TestCase): +class TestScenario(GtsamTestCase): def setUp(self): pass @@ -44,8 +45,8 @@ class TestScenario(unittest.TestCase): T30 = scenario.pose(T) np.testing.assert_almost_equal( np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) - self.assertTrue(gtsam.Point3( - 0, 0, 2.0 * R).equals(T30.translation(), 1e-9)) + self.gtsamAssertEquals(gtsam.Point3( + 0, 0, 2.0 * R), T30.translation(), 1e-9) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index 7924a9b1c..efdfec561 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -1,18 +1,31 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SimpleCamera unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math -import numpy as np import unittest -from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera +import numpy as np + +import gtsam +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) -class TestSimpleCamera(unittest.TestCase): +class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) camera = SimpleCamera(pose1, K) - self.assertTrue(camera.calibration().equals(K, 1e-9)) - self.assertTrue(camera.pose().equals(pose1, 1e-9)) + self.gtsamAssertEquals(camera.calibration(), K, 1e-9) + self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction @@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase): z = Point3(0,1,0) wRc = Rot3(x,y,z) expected = Pose3(wRc,Point3(0.4,0.3,0.1)) - self.assertTrue(camera.pose().equals(expected, 1e-9)) + self.gtsamAssertEquals(camera.pose(), expected, 1e-9) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py index dacd4a116..3f5f57522 100644 --- a/cython/gtsam/tests/test_StereoVOExample.py +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -1,10 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Stereo VO unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np +import gtsam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestStereoVOExample(unittest.TestCase): + +class TestStereoVOExample(GtsamTestCase): def test_StereoVOExample(self): ## Assumptions @@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase): ## check equality for the first pose and point pose_x1 = result.atPose3(x1) - self.assertTrue(pose_x1.equals(first_pose,1e-4)) + self.gtsamAssertEquals(pose_x1, first_pose,1e-4) point_l1 = result.atPoint3(l1) - self.assertTrue(point_l1.equals(expected_l1,1e-4)) + self.gtsamAssertEquals(point_l1, expected_l1,1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 0bb1e0806..20634a21c 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -5,7 +5,7 @@ All Rights Reserved See LICENSE for the license information -Unit tests for IMU testing scenarios. +Values unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ # pylint: disable=invalid-name, E1101, E0611 @@ -13,12 +13,14 @@ import unittest import numpy as np +import gtsam from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias_ConstantBias) +from gtsam.utils.test_case import GtsamTestCase -class TestValues(unittest.TestCase): +class TestValues(GtsamTestCase): def test_values(self): values = Values() @@ -58,26 +60,26 @@ class TestValues(unittest.TestCase): mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) - self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) - self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) - self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) - self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) - self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) - self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) - self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) - self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) - self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) - self.assertTrue(values.atimuBias_ConstantBias( - 10).equals(imuBias_ConstantBias(), tol)) + self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol) + self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol) + self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol) + self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol) + self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol) + self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol) + self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol) + self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol) + self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol) + self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol) + self.gtsamAssertEquals(values.atimuBias_ConstantBias( + 10), imuBias_ConstantBias(), tol) # special cases for Vector and Matrix: actualVector = values.atVector(11) - self.assertTrue(np.allclose(vec, actualVector, tol)) + np.testing.assert_allclose(vec, actualVector, tol) actualMatrix = values.atMatrix(12) - self.assertTrue(np.allclose(mat, actualMatrix, tol)) + np.testing.assert_allclose(mat, actualMatrix, tol) actualMatrix2 = values.atMatrix(13) - self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + np.testing.assert_allclose(mat2, actualMatrix2, tol) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py index 39bfa6eb4..99d7e6160 100644 --- a/cython/gtsam/tests/test_VisualISAMExample.py +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -1,10 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest + import numpy as np -from gtsam import symbol + +import gtsam import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestVisualISAMExample(unittest.TestCase): + +class TestVisualISAMExample(GtsamTestCase): def test_VisualISAMExample(self): # Data Options @@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase): for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('l'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py index e561be1a8..60fb9450d 100644 --- a/cython/gtsam/tests/test_dataset.py +++ b/cython/gtsam/tests/test_dataset.py @@ -6,7 +6,7 @@ All Rights Reserved See LICENSE for the license information Unit tests for testing dataset access. -Author: Frank Dellaert +Author: Frank Dellaert & Duy Nguyen Ta (Python) """ # pylint: disable=invalid-name, no-name-in-module, no-member @@ -16,9 +16,10 @@ import unittest import gtsam from gtsam import BetweenFactorPose3, BetweenFactorPose3s +from gtsam.utils.test_case import GtsamTestCase -class TestDataset(unittest.TestCase): +class TestDataset(GtsamTestCase): """Tests for datasets.h wrapper.""" def setUp(self): diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py index ee9195c48..3aa7e3470 100644 --- a/cython/gtsam/tests/test_initialize_pose3.py +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -15,11 +15,12 @@ import numpy as np import gtsam from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values +from gtsam.utils.test_case import GtsamTestCase x0, x1, x2, x3 = 0, 1, 2, 3 -class TestValues(unittest.TestCase): +class TestValues(GtsamTestCase): def setUp(self): @@ -67,10 +68,10 @@ class TestValues(unittest.TestCase): initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) # comparison is up to M_PI, that's why we add some multiples of 2*M_PI - self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6)) - self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6)) - self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6)) - self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6)) + self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6) def test_initializePoses(self): g2oFile = gtsam.findExampleDataFile("pose3example-grid") @@ -81,7 +82,7 @@ class TestValues(unittest.TestCase): initial = gtsam.InitializePose3.initialize(inputGraph) # TODO(frank): very loose !! - self.assertTrue(initial.equals(expectedValues, 0.1)) + self.gtsamAssertEquals(initial, expectedValues, 0.1) if __name__ == "__main__": diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 90c4e436b..8d3af311f 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -1,7 +1,20 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest + +import numpy as np + import gtsam import gtsam_unstable -import numpy as np +from gtsam.utils.test_case import GtsamTestCase def _timestamp_key_value(key, value): @@ -10,7 +23,7 @@ def _timestamp_key_value(key, value): ) -class TestFixedLagSmootherExample(unittest.TestCase): +class TestFixedLagSmootherExample(GtsamTestCase): ''' Tests the fixed lag smoother wrapper ''' From 79880d6a7ced9a838d6a9387a2f027c51c66af61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Mar 2019 17:36:11 -0400 Subject: [PATCH 96/97] Use GtsamTestCase in example --- cython/gtsam/examples/PlanarManipulatorExample.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py index af21e6ca5..73959b6c5 100644 --- a/cython/gtsam/examples/PlanarManipulatorExample.py +++ b/cython/gtsam/examples/PlanarManipulatorExample.py @@ -24,6 +24,7 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 import gtsam import gtsam.utils.plot as gtsam_plot from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase def vector3(x, y, z): @@ -202,7 +203,7 @@ Q1 = np.radians(vector3(-30, -45, -90)) Q2 = np.radians(vector3(-90, 90, 0)) -class TestPose2SLAMExample(unittest.TestCase): +class TestPose2SLAMExample(GtsamTestCase): """Unit tests for functions used below.""" def setUp(self): From 86973559a664b4a9731721754f2916f2db388b48 Mon Sep 17 00:00:00 2001 From: mbway Date: Sat, 23 Mar 2019 11:32:58 +0000 Subject: [PATCH 97/97] addressed comments --- THANKS | 1 + cython/gtsam/examples/SFMExample.py | 65 ++++++++++++---------- cython/gtsam/examples/SimpleRotation.py | 3 +- cython/gtsam/examples/VisualISAMExample.py | 24 +++++--- 4 files changed, 52 insertions(+), 41 deletions(-) diff --git a/THANKS b/THANKS index f2b51f61d..7db7daaf3 100644 --- a/THANKS +++ b/THANKS @@ -27,6 +27,7 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, li * Natesh Srinivasan * Alex Trevor * Stephen Williams, BossaNova +* Matthew Broadway at ETH, Zurich diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index 3a64e0cdb..bf46f09d5 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -8,45 +8,50 @@ See LICENSE for the license information A structure-from-motion problem on a simulated dataset """ +from __future__ import print_function import gtsam import numpy as np from gtsam.examples import SFMdata -from gtsam.gtsam import Values, NonlinearFactorGraph, PriorFactorPose3, SimpleCamera, \ - GenericProjectionFactorCal3_S2, \ - PriorFactorPoint3, Pose3, Rot3, Point3, DoglegOptimizer, Cal3_S2 +from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, + GenericProjectionFactorCal3_S2, NonlinearFactorGraph, + Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, + Rot3, SimpleCamera, Values) -# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -# -# Each variable in the system (poses and landmarks) must be identified with a unique key. -# We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -# Here we will use Symbols -# -# In GTSAM, measurement functions are represented as 'factors'. Several common factors -# have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -# Here we will use Projection factors to model the camera's landmark observations. -# Also, we will initialize the robot at some location using a Prior factor. -# -# When the factors are created, we will add them to a Factor Graph. As the factors we are using -# are nonlinear factors, we will need a Nonlinear Factor Graph. -# -# Finally, once all of the factors have been added to our factor graph, we will want to -# solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -# GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -# trust-region method known as Powell's Degleg -# -# The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -# nonlinear functions around an initial linearization point, then solve the linear system -# to update the linearization point. This happens repeatedly until the solver converges -# to a consistent set of variable values. This requires us to specify an initial guess -# for each variable, held in a Values container. - -def symbol(name, index): +def symbol(name: str, index: int) -> int: + """ helper for creating a symbol without explicitly casting 'name' from str to int """ return gtsam.symbol(ord(name), index) def main(): + """ + Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). + + Each variable in the system (poses and landmarks) must be identified with a unique key. + We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). + Here we will use Symbols + + In GTSAM, measurement functions are represented as 'factors'. Several common factors + have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. + Here we will use Projection factors to model the camera's landmark observations. + Also, we will initialize the robot at some location using a Prior factor. + + When the factors are created, we will add them to a Factor Graph. As the factors we are using + are nonlinear factors, we will need a Nonlinear Factor Graph. + + Finally, once all of the factors have been added to our factor graph, we will want to + solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. + GTSAM includes several nonlinear optimizers to perform this step. Here we will use a + trust-region method known as Powell's Degleg + + The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the + nonlinear functions around an initial linearization point, then solve the linear system + to update the linearization point. This happens repeatedly until the solver converges + to a consistent set of variable values. This requires us to specify an initial guess + for each variable, held in a Values container. + """ + # Define the camera calibration parameters K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) @@ -63,7 +68,7 @@ def main(): graph = NonlinearFactorGraph() # Add a prior on pose x1. This indirectly specifies where the origin is. - # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) graph.push_back(factor) diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py index 0c3ac467f..6e9b1320b 100644 --- a/cython/gtsam/examples/SimpleRotation.py +++ b/cython/gtsam/examples/SimpleRotation.py @@ -10,9 +10,8 @@ This example will perform a relatively trivial optimization on a single variable with a single factor. """ -import gtsam - import numpy as np +import gtsam def main(): diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py index 23b880bec..f4d81eaf7 100644 --- a/cython/gtsam/examples/VisualISAMExample.py +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -10,23 +10,29 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase This version uses iSAM to solve the problem incrementally """ -# A structure-from-motion example with landmarks -# - The landmarks form a 10 meter cube -# - The robot rotates around the landmarks, always facing towards the cube - -import gtsam -from gtsam.gtsam import Values, Cal3_S2, NonlinearISAM, NonlinearFactorGraph, SimpleCamera, Pose3, Rot3, Point3, \ - PriorFactorPose3, PriorFactorPoint3, GenericProjectionFactorCal3_S2 +from __future__ import print_function import numpy as np +import gtsam from gtsam.examples import SFMdata +from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, + NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, + SimpleCamera, Values) -def symbol(name, index): +def symbol(name: str, index: int) -> int: + """ helper for creating a symbol without explicitly casting 'name' from str to int """ return gtsam.symbol(ord(name), index) def main(): + """ + A structure-from-motion example with landmarks + - The landmarks form a 10 meter cube + - The robot rotates around the landmarks, always facing towards the cube + """ + # Define the camera calibration parameters K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) @@ -67,7 +73,7 @@ def main(): # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before # adding it to iSAM. if i == 0: - # Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) graph.push_back(factor)