From 859c157a0fdd475304fb2549ebe81f73341735d3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Sep 2020 21:26:11 -0400 Subject: [PATCH 01/21] Use cmake to identify best Python version if Default requested --- CMakeLists.txt | 17 +++++++++++++++++ cmake/FindNumPy.cmake | 12 ++---------- cmake/GtsamMatlabWrap.cmake | 22 +++++++++------------- 3 files changed, 28 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aa6082cb3..82adbc99f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,11 @@ set (GTSAM_VERSION_PATCH 0) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +set (CMAKE_PROJECT_VERSION ${GTSAM_VERSION_STRING}) +set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) + ############################################################################### # Gather information, perform checks, set defaults @@ -113,6 +118,18 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) endif() if(GTSAM_BUILD_PYTHON) + # Get info about the Python3 interpreter + # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 + find_package(Python3 COMPONENTS Interpreter Development) + + if(NOT ${Python3_FOUND}) + message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.") + endif() + + if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") + set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" CACHE STRING "The version of Python to build the wrappers against." FORCE) + endif() + if(GTSAM_UNSTABLE_BUILD_PYTHON) if (NOT GTSAM_BUILD_UNSTABLE) message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake index 4f5743aa6..d55a760c6 100644 --- a/cmake/FindNumPy.cmake +++ b/cmake/FindNumPy.cmake @@ -40,17 +40,9 @@ # 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() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) else() - if(GTSAM_PYTHON_VERSION STREQUAL "Default") - find_package(PythonInterp) - else() - find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) - endif() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) endif() if(NOT PYTHONINTERP_FOUND) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 111114a7b..4c44d2cb3 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -215,19 +215,15 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex # Set up generation of module source file file(MAKE_DIRECTORY "${generated_files_path}") - 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(PythonInterp + ${GTSAM_PYTHON_VERSION} + EXACT + REQUIRED) + find_package(PythonLibs + ${GTSAM_PYTHON_VERSION} + EXACT + REQUIRED) + set(_ignore gtsam::Point2 gtsam::Point3) From 75e24ef86755aed0bec7aaffd68747db6847ee9d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Sep 2020 21:42:53 -0400 Subject: [PATCH 02/21] print version of TBB found --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aa6082cb3..069b8e37c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -529,9 +529,9 @@ print_build_options_for_target(gtsam) print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") if(GTSAM_USE_TBB) - print_config("Use Intel TBB" "Yes") + print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") elseif(TBB_FOUND) - print_config("Use Intel TBB" "TBB found but GTSAM_WITH_TBB is disabled") + print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled") else() print_config("Use Intel TBB" "TBB not found") endif() From f64ced8791e442fb91b999d642b1095c33de0599 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Sep 2020 21:43:31 -0400 Subject: [PATCH 03/21] Don't print private compile options --- cmake/GtsamPrinting.cmake | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake index 5757f5ee1..c68679667 100644 --- a/cmake/GtsamPrinting.cmake +++ b/cmake/GtsamPrinting.cmake @@ -46,16 +46,16 @@ endfunction() # Prints all the relevant CMake build options for a given target: function(print_build_options_for_target target_name_) print_padded(GTSAM_COMPILE_FEATURES_PUBLIC) - print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE) + # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE) print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC) - print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) + # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC) foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_toupper) - print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) + # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}) - print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) + # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}) endforeach() endfunction() From 25eb4a13fd9dac20efe8decca3855f2c90e77be0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 18 Sep 2020 11:49:03 -0400 Subject: [PATCH 04/21] minor formatting --- CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 82adbc99f..a45c17677 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,7 +127,11 @@ if(GTSAM_BUILD_PYTHON) endif() if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") - set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" CACHE STRING "The version of Python to build the wrappers against." FORCE) + set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" + CACHE + STRING + "The version of Python to build the wrappers against." + FORCE) endif() if(GTSAM_UNSTABLE_BUILD_PYTHON) From 9670b6661db44f3c731b491d8cc2c4c6952239b3 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 19 Sep 2020 16:47:13 -0400 Subject: [PATCH 05/21] Adding logmap API which applied a logarithmic map taking the object to the argument --- gtsam/gtsam.i | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..f53c00043 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -361,6 +361,7 @@ class LieScalar { // Lie group static gtsam::LieScalar Expmap(Vector v); static Vector Logmap(const gtsam::LieScalar& p); + Vector logmap(const gtsam::LieScalar& p); }; #include @@ -390,6 +391,7 @@ class LieVector { // Lie group static gtsam::LieVector Expmap(Vector v); static Vector Logmap(const gtsam::LieVector& p); + Vector logmap(const gtsam::LieVector& p); // enabling serialization functionality void serialize() const; @@ -422,6 +424,7 @@ class LieMatrix { // Lie group static gtsam::LieMatrix Expmap(Vector v); static Vector Logmap(const gtsam::LieMatrix& p); + Vector logmap(const gtsam::LieMatrix& p); // enabling serialization functionality void serialize() const; @@ -506,6 +509,7 @@ class StereoPoint2 { // Lie Group static gtsam::StereoPoint2 Expmap(Vector v); static Vector Logmap(const gtsam::StereoPoint2& p); + Vector logmap(const gtsam::StereoPoint2& p); // Standard Interface Vector vector() const; @@ -567,6 +571,7 @@ class Rot2 { // Lie Group static gtsam::Rot2 Expmap(Vector v); static Vector Logmap(const gtsam::Rot2& p); + Vector logmap(const gtsam::Rot2& p); // Group Action on Point2 gtsam::Point2 rotate(const gtsam::Point2& point) const; @@ -727,6 +732,7 @@ class Rot3 { // Standard Interface static gtsam::Rot3 Expmap(Vector v); static Vector Logmap(const gtsam::Rot3& p); + Vector logmap(const gtsam::Rot3& p); Matrix matrix() const; Matrix transpose() const; gtsam::Point3 column(size_t index) const; @@ -772,6 +778,7 @@ class Pose2 { // Lie Group static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p); static Matrix ExpmapDerivative(Vector v); static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; @@ -825,6 +832,7 @@ class Pose3 { // Lie Group static gtsam::Pose3 Expmap(Vector v); static Vector Logmap(const gtsam::Pose3& pose); + Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; static Matrix adjointMap_(Vector xi); @@ -3143,6 +3151,7 @@ class ConstantBias { // Lie Group static gtsam::imuBias::ConstantBias Expmap(Vector v); static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + Vector logmap(const gtsam::imuBias::ConstantBias& b); // Standard Interface Vector vector() const; From 1d0b9d510e0e84f67082400ee68e44f35dfc0077 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 19 Sep 2020 17:57:39 -0400 Subject: [PATCH 06/21] Removing logmap in case of failure --- gtsam/gtsam.i | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index f53c00043..d8297c43b 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -361,7 +361,6 @@ class LieScalar { // Lie group static gtsam::LieScalar Expmap(Vector v); static Vector Logmap(const gtsam::LieScalar& p); - Vector logmap(const gtsam::LieScalar& p); }; #include @@ -509,7 +508,6 @@ class StereoPoint2 { // Lie Group static gtsam::StereoPoint2 Expmap(Vector v); static Vector Logmap(const gtsam::StereoPoint2& p); - Vector logmap(const gtsam::StereoPoint2& p); // Standard Interface Vector vector() const; @@ -3151,7 +3149,6 @@ class ConstantBias { // Lie Group static gtsam::imuBias::ConstantBias Expmap(Vector v); static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - Vector logmap(const gtsam::imuBias::ConstantBias& b); // Standard Interface Vector vector() const; From 457e52d05db4841cf713fcd0b736a7824591b279 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 09:04:17 -0400 Subject: [PATCH 07/21] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index cc8af7248..60eff197a 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali ## Wrappers -We provide support for [MATLAB](matlab/README.md) and [Python](cython/README.md) wrappers for GTSAM. Please refer to the linked documents for more details. +We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details. ## The Preintegrated IMU Factor From bcaed27c54006010e76488f838ac13c541c82dcf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 09:38:35 -0400 Subject: [PATCH 08/21] improve python wrapper cmake flag instructions --- python/README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/README.md b/python/README.md index b1a3a865f..bb883222c 100644 --- a/python/README.md +++ b/python/README.md @@ -7,7 +7,10 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Requirements - If you want to build the GTSAM python library for a specific python version (eg 3.6), - use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. + use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. For example: + ```bash + cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 + ``` - 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 `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows: From bb22773a810c2d1f341ca2b2f41b7219f3a29114 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 09:39:46 -0400 Subject: [PATCH 09/21] Update README.md --- python/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/README.md b/python/README.md index bb883222c..be69034f9 100644 --- a/python/README.md +++ b/python/README.md @@ -7,7 +7,7 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Requirements - If you want to build the GTSAM python library for a specific python version (eg 3.6), - use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. For example: + use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. For example, if you local Python version is 3.6.10, then run: ```bash cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 ``` From 133b81867c1bf4fcd9d619ffa7bddba024e8a28e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 09:40:42 -0400 Subject: [PATCH 10/21] Update README.md --- python/README.md | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/python/README.md b/python/README.md index be69034f9..be5d316b1 100644 --- a/python/README.md +++ b/python/README.md @@ -7,10 +7,7 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Requirements - If you want to build the GTSAM python library for a specific python version (eg 3.6), - use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. For example, if you local Python version is 3.6.10, then run: - ```bash - cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 - ``` + use the `-DGTSAM_PYTHON_VERSION=3.6` 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 `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows: @@ -21,8 +18,10 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Install -- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `/python`. - +- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `/python`. For example, if you local Python version is 3.6.10, then run: + ```bash + cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 + ``` - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`). - To install, simply run `make python-install` (`ninja python-install`). From fe0312fd63b404dd3f304338ee2e70cf1183a28d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 09:58:03 -0400 Subject: [PATCH 11/21] Update README.md --- python/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/README.md b/python/README.md index be5d316b1..6e2a30d2e 100644 --- a/python/README.md +++ b/python/README.md @@ -7,7 +7,7 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Requirements - If you want to build the GTSAM python library for a specific python version (eg 3.6), - use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. + use the `-DGTSAM_PYTHON_VERSION=3.6` 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 `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows: @@ -18,7 +18,7 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Install -- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `/python`. For example, if you local Python version is 3.6.10, then run: +- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `/python`. For example, if your local Python version is 3.6.10, then you should run: ```bash cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 ``` From 169d42f313bd81a1dedfffd05fa24fe95c34cb90 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:08:18 -0400 Subject: [PATCH 12/21] use cleaner checkmark symbol in readme --- python/gtsam/examples/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index e998e4dcd..d70867dda 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -7,7 +7,7 @@ | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | | easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | | elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | -| ImuFactorExample2 | X | +| ImuFactorExample2 | :heavy_check_mark: | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | @@ -49,4 +49,4 @@ Extra Examples (with no C++ equivalent) - PlanarManipulatorExample -- SFMData \ No newline at end of file +- SFMData From 7478c19aad268ff8b5e677dad2f405b20b9ea285 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:16:20 -0400 Subject: [PATCH 13/21] update example list --- python/gtsam/examples/README.md | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index d70867dda..3a325aa30 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -3,49 +3,58 @@ | C++ Example Name | Ported | |-------------------------------------------------------|--------| | CameraResectioning | | +| CombinedImuFactorsExample | | | CreateSFMExampleData | | +| DiscreteBayesNetExample | | | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | | easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | | elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | -| ImuFactorExample2 | :heavy_check_mark: | +| FisheyeExample | | +| HMMExample | | +| ImuFactorsExample2 | :heavy_check_mark: | | ImuFactorsExample | | +| IMUKittiExampleGPS.cpp | | +| InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | | LocalizationExample | impossible? | | METISOrderingExample | | -| OdometryExample | X | -| PlanarSLAMExample | X | -| Pose2SLAMExample | X | +| OdometryExample | :heavy_check_mark: | +| PlanarSLAMExample | :heavy_check_mark: | +| Pose2SLAMExample | :heavy_check_mark: | | Pose2SLAMExampleExpressions | | -| Pose2SLAMExample_g2o | X | +| Pose2SLAMExample_g2o | :heavy_check_mark: | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | | Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | +| Pose3Localization.cpp | | | Pose3SLAMExample_changeKeys | | | Pose3SLAMExampleExpressions_BearingRangeWithTransform | | -| Pose3SLAMExample_g2o | X | +| Pose3SLAMExample_g2o | :heavy_check_mark: | | Pose3SLAMExample_initializePose3Chordal | | | Pose3SLAMExample_initializePose3Gradient | | | RangeISAMExample_plaza2 | | | SelfCalibrationExample | | +| SFMdata | | | SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | | -| SFMExample | X | +| SFMExample_bal | :heavy_check_mark: | +| SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | | SFMExample_SmartFactor | | | SFMExample_SmartFactorPCG | | -| SimpleRotation | X | +| ShonanAveragingCLI.cpp | | +| SimpleRotation | :heavy_check_mark: | | SolverComparer | | | StereoVOExample | | | StereoVOExample_large | | | TimeTBB | | | UGM_chain | discrete functionality not yet exposed | | UGM_small | discrete functionality not yet exposed | -| VisualISAM2Example | X | -| VisualISAMExample | X | +| VisualISAM2Example | :heavy_check_mark: | +| VisualISAMExample | :heavy_check_mark: | Extra Examples (with no C++ equivalent) - PlanarManipulatorExample From 0e7719ae9893ef04f13adbf8fa797c11d167a293 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:19:33 -0400 Subject: [PATCH 14/21] update python examples list --- python/gtsam/examples/README.md | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 3a325aa30..84a41990f 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -13,7 +13,7 @@ | HMMExample | | | ImuFactorsExample2 | :heavy_check_mark: | | ImuFactorsExample | | -| IMUKittiExampleGPS.cpp | | +| IMUKittiExampleGPS | | | InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | @@ -29,11 +29,11 @@ | Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | -| Pose3Localization.cpp | | +| Pose3Localization | | | Pose3SLAMExample_changeKeys | | | Pose3SLAMExampleExpressions_BearingRangeWithTransform | | | Pose3SLAMExample_g2o | :heavy_check_mark: | -| Pose3SLAMExample_initializePose3Chordal | | +| Pose3SLAMExample_initializePose3Chordal | :heavy_check_mark: | | Pose3SLAMExample_initializePose3Gradient | | | RangeISAMExample_plaza2 | | | SelfCalibrationExample | | @@ -45,7 +45,7 @@ | SFMExampleExpressions | | | SFMExample_SmartFactor | | | SFMExample_SmartFactorPCG | | -| ShonanAveragingCLI.cpp | | +| ShonanAveragingCLI | :heavy_check_mark: | | SimpleRotation | :heavy_check_mark: | | SolverComparer | | | StereoVOExample | | @@ -57,5 +57,8 @@ | VisualISAMExample | :heavy_check_mark: | Extra Examples (with no C++ equivalent) +- DogLegOptimizerExample +- GPSFactorExample - PlanarManipulatorExample +- PreintegrationExample - SFMData From 2af56303fc4f05c9ea841398974715ee3c8fe3f8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:28:16 -0400 Subject: [PATCH 15/21] add p --- python/gtsam/examples/SFMExample_bal.py | 111 ++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 python/gtsam/examples/SFMExample_bal.py diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py new file mode 100644 index 000000000..50e450fea --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,111 @@ +""" + 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 + + Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + Author: John Lambert +""" + +import argparse +import logging + +import gtsam +import matplotlib.pyplot as plt +import numpy as np + +from gtsam import symbol_shorthand +from gtsam import readBal + +L = symbol_shorthand.L +X = symbol_shorthand.X + +import pdb + +#include +#include +#include +#include +#include // for loading BAL datasets ! + + +# using symbol_shorthand::C; +# using symbol_shorthand::P; + +# We will be using a projection factor that ties a SFM_Camera to a 3D point. +# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +# and has a total of 9 free parameters +#typedef GeneralSFMFactor MyFactor; + + +def run(args): + """ Run LM optimization with BAL input data and report resulting error """ + # Find default file, but if an argument is given, try loading a file + if args.input_file: + input_file = args.input_file + else: + input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") + + # # Load the SfM data from file + mydata = readBal(input_file) + logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") + + # # Create a factor graph + graph = gtsam.NonlinearFactorGraph() + + # # We share *one* noiseModel between all projection factors + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Add measurements to the factor graph + j = 0 + pdb.set_trace() + for t_idx in range(mydata.number_tracks()): + track = mydata.track(t_idx) # SfmTrack + for m_idx in range(track.number_measurements()): + # retrieve the SfmMeasurement + # i represents the camera index, and uv is the 2d measurement + i, uv = track.measurement(0) # + #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P + #graph.add + j += 1 + pdb.set_trace() + + # # Add a prior on pose x1. This indirectly specifies where the origin is. + # # and a prior on the position of the first landmark to fix the scale + # graph.addPrior(C(0), mydata.cameras[0], gtsam.noiseModel.Isotropic.Sigma(9, 0.1)) + # graph.addPrior(P(0), mydata.tracks[0].p, gtsam.noiseModel.Isotropic.Sigma(3, 0.1)) + + # # Create initial estimate + initial = gtsam.Values() + # i = 0 + # j = 0; + # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) + # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) + + # Optimize the graph and print results + # Values result; + try: + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("ERROR") + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = lm.optimize() + except Exception as e: + logging.exception("LM Optimization failed") + return + + logging.info(f"final error: {graph.error(result)}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('-i', '--input_file', type=str, default="", + help='Read SFM data from the specified BAL file') + run(parser.parse_args()) + + + + From 0d19859f8259c81bad5cf97b736b7f4d84e6ced1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:28:46 -0400 Subject: [PATCH 16/21] add python equivalent of c++ sfm data calls --- python/gtsam/examples/SFMExample_bal.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 50e450fea..b70b8057b 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -81,8 +81,8 @@ def run(args): # # Create initial estimate initial = gtsam.Values() - # i = 0 - # j = 0; + i = 0 + j = 0 # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) From afa74c4f575a5cd92442823c6acad41ff31efde7 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 14:21:33 -0400 Subject: [PATCH 17/21] cannot retrieve p attribute --- python/gtsam/examples/SFMExample_bal.py | 40 ++++++++++++++----------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index b70b8057b..ab3baa26c 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -4,7 +4,6 @@ All Rights Reserved Authors: Frank Dellaert, et al. (see THANKS for the full author list) - See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file @@ -21,20 +20,12 @@ import numpy as np from gtsam import symbol_shorthand from gtsam import readBal -L = symbol_shorthand.L -X = symbol_shorthand.X +C = symbol_shorthand.C +P = symbol_shorthand.P import pdb -#include -#include -#include #include -#include // for loading BAL datasets ! - - -# using symbol_shorthand::C; -# using symbol_shorthand::P; # We will be using a projection factor that ties a SFM_Camera to a 3D point. # An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration @@ -65,8 +56,8 @@ def run(args): pdb.set_trace() for t_idx in range(mydata.number_tracks()): track = mydata.track(t_idx) # SfmTrack + # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): - # retrieve the SfmMeasurement # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(0) # #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P @@ -76,18 +67,33 @@ def run(args): # # Add a prior on pose x1. This indirectly specifies where the origin is. # # and a prior on the position of the first landmark to fix the scale - # graph.addPrior(C(0), mydata.cameras[0], gtsam.noiseModel.Isotropic.Sigma(9, 0.1)) - # graph.addPrior(P(0), mydata.tracks[0].p, gtsam.noiseModel.Isotropic.Sigma(3, 0.1)) + + + graph.push_back(gtsam.PriorFactorVector( + C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) + # # graph.addPrior(P(0), mydata.track(0).p, + # equivalent of addPrior + graph.push_back(gtsam.PriorFactorVector( + P(0), mydata.track(0)[1], gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) # # Create initial estimate initial = gtsam.Values() + i = 0 + # add each SfmCamera + for cam_idx in range(mydata.number_cameras()): + camera = mydata.camera(0) + initial.insert(C(i), camera) + i += 1 + j = 0 - # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) - # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) + # add each SfmTrack + for t_idx in range(mydata.number_tracks()): + track = mydata.track(0) + initial.insert(P(j), track.p) + j += 1 # Optimize the graph and print results - # Values result; try: params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("ERROR") From 34f670e9d5ba6225b7ea8705d4c7ad310429169f Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 22 Sep 2020 15:44:31 -0400 Subject: [PATCH 18/21] remove BAL for now, and add get3dPoint() for p access --- gtsam/gtsam.i | 1 + gtsam/slam/dataset.h | 3 + python/gtsam/examples/SFMExample_bal.py | 117 ------------------------ 3 files changed, 4 insertions(+), 117 deletions(-) delete mode 100644 python/gtsam/examples/SFMExample_bal.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..78e004390 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2847,6 +2847,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { #include class SfmTrack { + Point3 get3dPoint() const; size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 91ceaa5fd..5bb8063bd 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -233,6 +233,9 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } + Point3 get3dPoint() const { + return p; + } }; /// Define the structure for the camera poses diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py deleted file mode 100644 index ab3baa26c..000000000 --- a/python/gtsam/examples/SFMExample_bal.py +++ /dev/null @@ -1,117 +0,0 @@ -""" - 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 - - Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: John Lambert -""" - -import argparse -import logging - -import gtsam -import matplotlib.pyplot as plt -import numpy as np - -from gtsam import symbol_shorthand -from gtsam import readBal - -C = symbol_shorthand.C -P = symbol_shorthand.P - -import pdb - -#include - -# We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration -# and has a total of 9 free parameters -#typedef GeneralSFMFactor MyFactor; - - -def run(args): - """ Run LM optimization with BAL input data and report resulting error """ - # Find default file, but if an argument is given, try loading a file - if args.input_file: - input_file = args.input_file - else: - input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") - - # # Load the SfM data from file - mydata = readBal(input_file) - logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") - - # # Create a factor graph - graph = gtsam.NonlinearFactorGraph() - - # # We share *one* noiseModel between all projection factors - noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v - - # Add measurements to the factor graph - j = 0 - pdb.set_trace() - for t_idx in range(mydata.number_tracks()): - track = mydata.track(t_idx) # SfmTrack - # retrieve the SfmMeasurement objects - for m_idx in range(track.number_measurements()): - # i represents the camera index, and uv is the 2d measurement - i, uv = track.measurement(0) # - #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P - #graph.add - j += 1 - pdb.set_trace() - - # # Add a prior on pose x1. This indirectly specifies where the origin is. - # # and a prior on the position of the first landmark to fix the scale - - - graph.push_back(gtsam.PriorFactorVector( - C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) - # # graph.addPrior(P(0), mydata.track(0).p, - # equivalent of addPrior - graph.push_back(gtsam.PriorFactorVector( - P(0), mydata.track(0)[1], gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) - - # # Create initial estimate - initial = gtsam.Values() - - i = 0 - # add each SfmCamera - for cam_idx in range(mydata.number_cameras()): - camera = mydata.camera(0) - initial.insert(C(i), camera) - i += 1 - - j = 0 - # add each SfmTrack - for t_idx in range(mydata.number_tracks()): - track = mydata.track(0) - initial.insert(P(j), track.p) - j += 1 - - # Optimize the graph and print results - try: - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("ERROR") - lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = lm.optimize() - except Exception as e: - logging.exception("LM Optimization failed") - return - - logging.info(f"final error: {graph.error(result)}") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument('-i', '--input_file', type=str, default="", - help='Read SFM data from the specified BAL file') - run(parser.parse_args()) - - - - From 8e0b0c1641fa6baf897250ccb33d3959f1d79ae5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 22 Sep 2020 15:49:05 -0400 Subject: [PATCH 19/21] mark SFMExample_bal as still in progress --- python/gtsam/examples/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 84a41990f..e05a0c7e1 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -39,7 +39,7 @@ | SelfCalibrationExample | | | SFMdata | | | SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | :heavy_check_mark: | +| SFMExample_bal | | | SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | From 0afec43612cfe3fabdc0227ea95c9590c46c7ec9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 22 Sep 2020 17:05:42 -0400 Subject: [PATCH 20/21] remove logmap function from LieVector and LieMatrix --- gtsam/gtsam.i | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d8297c43b..d75d29ef2 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -390,7 +390,6 @@ class LieVector { // Lie group static gtsam::LieVector Expmap(Vector v); static Vector Logmap(const gtsam::LieVector& p); - Vector logmap(const gtsam::LieVector& p); // enabling serialization functionality void serialize() const; @@ -423,7 +422,6 @@ class LieMatrix { // Lie group static gtsam::LieMatrix Expmap(Vector v); static Vector Logmap(const gtsam::LieMatrix& p); - Vector logmap(const gtsam::LieMatrix& p); // enabling serialization functionality void serialize() const; From e6057fc4faf5f44f223ca3f0d62018231c5d099a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 22 Sep 2020 23:20:36 -0400 Subject: [PATCH 21/21] rename get3dPoint() to point3() --- gtsam/gtsam.i | 2 +- gtsam/slam/dataset.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 78e004390..1764ecd82 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2847,7 +2847,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { #include class SfmTrack { - Point3 get3dPoint() const; + Point3 point3() const; size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5bb8063bd..a8fddcfe4 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -233,7 +233,7 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } - Point3 get3dPoint() const { + Point3 point3() const { return p; } };