From b84c6efe51949cb5c248b4c989a0a0239fe6d535 Mon Sep 17 00:00:00 2001 From: Ellon Paiva Mendes Date: Thu, 3 Oct 2019 13:56:26 +0200 Subject: [PATCH 001/136] Bump version to 4.0.2 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0457cb1c..1c9f0639a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 0) -set (GTSAM_VERSION_PATCH 0) +set (GTSAM_VERSION_PATCH 2) 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}") From 0760858aab942c03b438cfbfe89cab326d83cfb4 Mon Sep 17 00:00:00 2001 From: Ellon Paiva Mendes Date: Thu, 3 Oct 2019 17:45:58 +0200 Subject: [PATCH 002/136] Install GTSAMConfigVersion.cmake --- cmake/GtsamMakeConfigFile.cmake | 28 ++++++++++++++++++++++++++-- gtsam_extra.cmake.in | 5 ++--- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/cmake/GtsamMakeConfigFile.cmake b/cmake/GtsamMakeConfigFile.cmake index 74081b58a..2fff888ef 100644 --- a/cmake/GtsamMakeConfigFile.cmake +++ b/cmake/GtsamMakeConfigFile.cmake @@ -20,13 +20,37 @@ function(GtsamMakeConfigFile PACKAGE_NAME) set(EXTRA_FILE "_does_not_exist_") endif() + # GTSAM defines its version variable as GTSAM_VERSION_STRING while other + # projects may define it as _VERSION. Since this file is + # installed on the system as part of GTSAMCMakeTools and may be useful in + # external projects, we need to handle both cases of version variable naming + # here. + if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING) + set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING}) + endif() + + # Version file + include(CMakePackageConfigHelpers) + write_basic_package_version_file( + "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}ConfigVersion.cmake" + VERSION ${${PACKAGE_NAME}_VERSION} + COMPATIBILITY SameMajorVersion + ) + + # Config file file(RELATIVE_PATH CONF_REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/include") file(RELATIVE_PATH CONF_REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/lib") configure_file(${GTSAM_CONFIG_TEMPLATE_PATH}/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY) message(STATUS "Wrote ${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake") - # Install config and exports files (for find scripts) - install(FILES "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" DESTINATION "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}") + # Install config, version and exports files (for find scripts) + install( + FILES + "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" + "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}ConfigVersion.cmake" + DESTINATION + "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" + ) install(EXPORT ${PACKAGE_NAME}-exports DESTINATION ${DEF_INSTALL_CMAKE_DIR}) endfunction() diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index b7990b3cc..8a9a13648 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -1,8 +1,7 @@ # Extra CMake definitions for GTSAM -set (GTSAM_VERSION_MAJOR @GTSAM_VERSION_MAJOR@) -set (GTSAM_VERSION_MINOR @GTSAM_VERSION_MINOR@) -set (GTSAM_VERSION_PATCH @GTSAM_VERSION_PATCH@) +# All version variables are handled by GTSAMConfigVersion.cmake, except these +# two below. We continue to set them here in case someone uses them set (GTSAM_VERSION_NUMERIC @GTSAM_VERSION_NUMERIC@) set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") From 0aaf496b6dae7e7e7785e1886e81b95539855d1e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 11:15:31 +0200 Subject: [PATCH 003/136] Remove obsolete cmake FindXX modules. Exported config files are preferred over modules, and easier to maintain. --- cmake/obsolete/FindGTSAM.cmake | 88 ------------------------- cmake/obsolete/FindGTSAM_UNSTABLE.cmake | 88 ------------------------- 2 files changed, 176 deletions(-) delete mode 100644 cmake/obsolete/FindGTSAM.cmake delete mode 100644 cmake/obsolete/FindGTSAM_UNSTABLE.cmake diff --git a/cmake/obsolete/FindGTSAM.cmake b/cmake/obsolete/FindGTSAM.cmake deleted file mode 100644 index 895eb853b..000000000 --- a/cmake/obsolete/FindGTSAM.cmake +++ /dev/null @@ -1,88 +0,0 @@ -# This is FindGTSAM.cmake -# DEPRECIATED: Use config file approach to pull in targets from gtsam -# CMake module to locate the GTSAM package -# -# The following cache variables may be set before calling this script: -# -# GTSAM_DIR (or GTSAM_ROOT): (Optional) The install prefix OR source tree of gtsam (e.g. /usr/local or src/gtsam) -# GTSAM_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory -# within it (e.g build-debug). Without this defined, this script tries to -# intelligently find the build directory based on the project's build directory name -# or based on the build type (Debug/Release/etc). -# -# The following variables will be defined: -# -# GTSAM_FOUND : TRUE if the package has been successfully found -# GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories -# GTSAM_LIBS : paths to GTSAM's libraries -# -# NOTES on compiling against an uninstalled GTSAM build tree: -# - A GTSAM source tree will be automatically searched for in the directory -# 'gtsam' next to your project directory, after searching -# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. -# - The build directory will be searched first with the same name as your -# project's build directory, e.g. if you build from 'MyProject/build-optimized', -# 'gtsam/build-optimized' will be searched first. Next, a build directory for -# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is -# 'Release', then 'gtsam/build-release' will be searched next. Finally, plain -# 'gtsam/build' will be searched. -# - You can control the gtsam build directory name directly by defining the CMake -# cache variable 'GTSAM_BUILD_NAME', then only 'gtsam/${GTSAM_BUILD_NAME} will -# be searched. -# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_DIR, to find a specific gtsam -# directory. - -# Get path suffixes to help look for gtsam -if(GTSAM_BUILD_NAME) - set(gtsam_build_names "${GTSAM_BUILD_NAME}/gtsam") -else() - # lowercase build type - string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) - # build suffix of this project - get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) - - set(gtsam_build_names "${my_build_name}/gtsam" "build-${build_type_suffix}/gtsam" "build/gtsam") -endif() - -# Use GTSAM_ROOT or GTSAM_DIR equivalently -if(GTSAM_ROOT AND NOT GTSAM_DIR) - set(GTSAM_DIR "${GTSAM_ROOT}") -endif() - -if(GTSAM_DIR) - # Find include dirs - find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h - PATHS "${GTSAM_DIR}/include" "${GTSAM_DIR}" NO_DEFAULT_PATH - DOC "GTSAM include directories") - - # Find libraries - find_library(GTSAM_LIBS NAMES gtsam - HINTS "${GTSAM_DIR}/lib" "${GTSAM_DIR}" NO_DEFAULT_PATH - PATH_SUFFIXES ${gtsam_build_names} - DOC "GTSAM libraries") -else() - # Find include dirs - set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include) - find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h - PATHS ${extra_include_paths} - DOC "GTSAM include directories") - if(NOT GTSAM_INCLUDE_DIR) - message(STATUS "Searched for gtsam headers in default paths plus ${extra_include_paths}") - endif() - - # Find libraries - find_library(GTSAM_LIBS NAMES gtsam - HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib - PATH_SUFFIXES ${gtsam_build_names} - DOC "GTSAM libraries") -endif() - -# handle the QUIETLY and REQUIRED arguments and set GTSAM_FOUND to TRUE -# if all listed variables are TRUE -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(GTSAM DEFAULT_MSG - GTSAM_LIBS GTSAM_INCLUDE_DIR) - - - - diff --git a/cmake/obsolete/FindGTSAM_UNSTABLE.cmake b/cmake/obsolete/FindGTSAM_UNSTABLE.cmake deleted file mode 100644 index 42cc9c8b0..000000000 --- a/cmake/obsolete/FindGTSAM_UNSTABLE.cmake +++ /dev/null @@ -1,88 +0,0 @@ -# This is FindGTSAM_UNSTABLE.cmake -# DEPRECIATED: Use config file approach to pull in targets from gtsam -# CMake module to locate the GTSAM_UNSTABLE package -# -# The following cache variables may be set before calling this script: -# -# GTSAM_UNSTABLE_DIR (or GTSAM_UNSTABLE_ROOT): (Optional) The install prefix OR source tree of gtsam_unstable (e.g. /usr/local or src/gtsam_unstable) -# GTSAM_UNSTABLE_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory -# within it (e.g build-debug). Without this defined, this script tries to -# intelligently find the build directory based on the project's build directory name -# or based on the build type (Debug/Release/etc). -# -# The following variables will be defined: -# -# GTSAM_UNSTABLE_FOUND : TRUE if the package has been successfully found -# GTSAM_UNSTABLE_INCLUDE_DIR : paths to GTSAM_UNSTABLE's INCLUDE directories -# GTSAM_UNSTABLE_LIBS : paths to GTSAM_UNSTABLE's libraries -# -# NOTES on compiling against an uninstalled GTSAM_UNSTABLE build tree: -# - A GTSAM_UNSTABLE source tree will be automatically searched for in the directory -# 'gtsam_unstable' next to your project directory, after searching -# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. -# - The build directory will be searched first with the same name as your -# project's build directory, e.g. if you build from 'MyProject/build-optimized', -# 'gtsam_unstable/build-optimized' will be searched first. Next, a build directory for -# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is -# 'Release', then 'gtsam_unstable/build-release' will be searched next. Finally, plain -# 'gtsam_unstable/build' will be searched. -# - You can control the gtsam build directory name directly by defining the CMake -# cache variable 'GTSAM_UNSTABLE_BUILD_NAME', then only 'gtsam/${GTSAM_UNSTABLE_BUILD_NAME} will -# be searched. -# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_UNSTABLE_DIR, to find a specific gtsam -# directory. - -# Get path suffixes to help look for gtsam_unstable -if(GTSAM_UNSTABLE_BUILD_NAME) - set(gtsam_unstable_build_names "${GTSAM_UNSTABLE_BUILD_NAME}/gtsam_unstable") -else() - # lowercase build type - string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) - # build suffix of this project - get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) - - set(gtsam_unstable_build_names "${my_build_name}/gtsam_unstable" "build-${build_type_suffix}/gtsam_unstable" "build/gtsam_unstable") -endif() - -# Use GTSAM_UNSTABLE_ROOT or GTSAM_UNSTABLE_DIR equivalently -if(GTSAM_UNSTABLE_ROOT AND NOT GTSAM_UNSTABLE_DIR) - set(GTSAM_UNSTABLE_DIR "${GTSAM_UNSTABLE_ROOT}") -endif() - -if(GTSAM_UNSTABLE_DIR) - # Find include dirs - find_path(GTSAM_UNSTABLE_INCLUDE_DIR gtsam_unstable/base/DSF.h - PATHS "${GTSAM_UNSTABLE_DIR}/include" "${GTSAM_UNSTABLE_DIR}" NO_DEFAULT_PATH - DOC "GTSAM_UNSTABLE include directories") - - # Find libraries - find_library(GTSAM_UNSTABLE_LIBS NAMES gtsam_unstable - HINTS "${GTSAM_UNSTABLE_DIR}/lib" "${GTSAM_UNSTABLE_DIR}" NO_DEFAULT_PATH - PATH_SUFFIXES ${gtsam_unstable_build_names} - DOC "GTSAM_UNSTABLE libraries") -else() - # Find include dirs - set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include) - find_path(GTSAM_UNSTABLE_INCLUDE_DIR gtsam_unstable/base/DSF.h - PATHS ${extra_include_paths} - DOC "GTSAM_UNSTABLE include directories") - if(NOT GTSAM_UNSTABLE_INCLUDE_DIR) - message(STATUS "Searched for gtsam_unstable headers in default paths plus ${extra_include_paths}") - endif() - - # Find libraries - find_library(GTSAM_UNSTABLE_LIBS NAMES gtsam_unstable - HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib - PATH_SUFFIXES ${gtsam_unstable_build_names} - DOC "GTSAM_UNSTABLE libraries") -endif() - -# handle the QUIETLY and REQUIRED arguments and set GTSAM_UNSTABLE_FOUND to TRUE -# if all listed variables are TRUE -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(GTSAM_UNSTABLE DEFAULT_MSG - GTSAM_UNSTABLE_LIBS GTSAM_UNSTABLE_INCLUDE_DIR) - - - - From 8425957e3ff52caa606d4c3de8c28b93b91edb75 Mon Sep 17 00:00:00 2001 From: ss Date: Sun, 9 Aug 2020 21:53:35 -0400 Subject: [PATCH 004/136] Finish Sim3 align and transformFrom functions. --- gtsam/geometry/Pose3.h | 3 + gtsam_unstable/geometry/Similarity3.cpp | 95 ++++++++++++++++ gtsam_unstable/geometry/Similarity3.h | 19 ++++ .../geometry/tests/testSimilarity3.cpp | 103 ++++++++++++++++++ 4 files changed, 220 insertions(+) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 159fd2927..57bec4dee 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -356,6 +356,9 @@ inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); } +// Convenience typedef +typedef std::pair Pose3Pair; + // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index d40fb0b59..740960f0f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -85,10 +85,105 @@ Point3 Similarity3::transformFrom(const Point3& p, // return s_ * q; } +Pose3 Similarity3::transformFrom(const Pose3& T) const { + Rot3 R = R_.compose(T.rotation()); + Point3 t = Point3(s_ * (R_.matrix() * T.translation().vector() + t_.vector())); + return Pose3(R, t); +} + Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } +Similarity3 Similarity3::Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); + if (n < 3) throw std::runtime_error("less than 3 pairs"); // we need at least three pairs + + // calculate centroids + Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + for (const Point3Pair& abPair : abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + + // Add to form H matrix + Matrix3 H = Z_3x3; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += da * db.transpose(); + } + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + + // Calculate scale + double x = 0; + double y = 0; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + double s = y / x; + + Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s; + return Similarity3(aRb, aTb, s); +} + +Rot3 Similarity3::rotationAveraging(const std::vector& rotations, double error) { + Rot3 R = rotations[0]; + const size_t n = rotations.size(); + Vector3 r; + r << 0, 0, 0; + while (1) { + for (const Rot3 R_i : rotations) { + r += Rot3::Logmap(R.inverse().compose(R_i)); + } + r /= n; + if (r.norm() < error) return R; + R = R.compose(Rot3::Expmap(r)); + } +} + +Similarity3 Similarity3::Align(const std::vector& abPosePairs) { + const size_t n = abPosePairs.size(); + if (n < 2) throw std::runtime_error("less than 2 pairs"); // we need at least two pairs + + // calculate centroids + Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + vector rotationList; + for (const Pose3Pair& abPair : abPosePairs) { + aCentroid += abPair.first.translation(); + bCentroid += abPair.second.translation(); + rotationList.push_back(abPair.first.rotation().compose(abPair.second.rotation().inverse())); + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + Rot3 aRb = Similarity3::rotationAveraging(rotationList); + + // Calculate scale + double x = 0; + double y = 0; + for (const Pose3Pair& abPair : abPosePairs) { + Point3 da = abPair.first.translation() - aCentroid; + Point3 db = abPair.second.translation() - bCentroid; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + double s = y / x; + + Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s; + return Similarity3(aRb, aTb, s); +} + Matrix4 Similarity3::wedge(const Vector7& xi) { // http://www.ethaneade.org/latex2html/lie/node29.html const auto w = xi.head<3>(); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index bf4937ed4..06b609eee 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -101,9 +102,27 @@ public: OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; + /// Action on a pose T + GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; + /** syntactic sugar for transformFrom */ GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const; + /** + * Create Similarity3 by aligning two point pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); + + /** + * Calculate the average rotation from a list of rotations + */ + GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector& rotations, double error = 1e-10); + + /** + * Create Similarity3 by aligning two pose pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); + /// @} /// @name Lie Group /// @{ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d23346896..770196e20 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -51,6 +51,8 @@ static const Similarity3 T4(R, P, s); static const Similarity3 T5(R, P, 10); static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform +const double degree = M_PI / 180; + //****************************************************************************** TEST(Similarity3, Concepts) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -255,6 +257,107 @@ TEST(Similarity3, GroupAction) { } } +//****************************************************************************** +// Group action on Pose3 +TEST(Similarity3, GroupActionPose3) { + Similarity3 bTa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 expectedTb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expectedTb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + EXPECT(assert_equal(expectedTb1, bTa.transformFrom(Ta1))); + EXPECT(assert_equal(expectedTb2, bTa.transformFrom(Ta2))); +} + +//****************************************************************************** +// Align with Point3 Pairs +TEST(Similarity3, AlignPoint3_1) { + Similarity3 expected(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0); + + Point3 p1 = Point3(0, 0, 0); + Point3 p2 = Point3(3, 0, 0); + Point3 p3 = Point3(3, 0, 4); + + Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); + Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); + Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + +TEST(Similarity3, AlignPoint3_2) { + Similarity3 expected(Rot3(), Point3(10, 10, 0), 1.0); + + Point3 p1 = Point3(0, 0, 0); + Point3 p2 = Point3(20, 10, 0); + Point3 p3 = Point3(10, 20, 0); + + Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); + Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); + Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + +TEST(Similarity3, AlignPoint3_3) { + Similarity3 expected(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0); + + Point3 p1 = Point3(0, 0, 1); + Point3 p2 = Point3(10, 0, 2); + Point3 p3 = Point3(20, -10, 30); + + Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); + Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); + Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +// Rotation Averaging +TEST(Similarity3, RotationAveraging) { + Rot3 expected = Rot3::Rx(90 * degree); + vector rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)}; + Rot3 actual = Similarity3::rotationAveraging(rotations); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +// Align with Pose3 Pairs +TEST(Similarity3, AlignPose3) { + Similarity3 expected(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + Pose3Pair bTa1(make_pair(Tb1, Ta1)); + Pose3Pair bTa2(make_pair(Tb2, Ta2)); + + vector correspondences{bTa1, bTa2}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + //****************************************************************************** // Test very simple prior optimization example TEST(Similarity3, Optimization) { From e6b1599063e5eff2e73b948ad860ac2c8011f23e Mon Sep 17 00:00:00 2001 From: ss Date: Mon, 10 Aug 2020 08:25:21 -0400 Subject: [PATCH 005/136] Fix document. --- gtsam_unstable/geometry/Similarity3.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 06b609eee..e04db4df7 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -102,14 +102,14 @@ public: OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; - /// Action on a pose T + /// Action on a pose T, R = R*T.R, t = s(R*T.t+t) GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; /** syntactic sugar for transformFrom */ GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const; /** - * Create Similarity3 by aligning two point pairs + * Create Similarity3 by aligning at least three point pairs */ GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); @@ -119,7 +119,7 @@ public: GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector& rotations, double error = 1e-10); /** - * Create Similarity3 by aligning two pose pairs + * Create Similarity3 by aligning at least two pose pairs */ GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); From 8dd9ff5c52b7b12d974097e06cf782c676fa3085 Mon Sep 17 00:00:00 2001 From: ss Date: Mon, 10 Aug 2020 08:25:42 -0400 Subject: [PATCH 006/136] Improve code quality. --- gtsam_unstable/geometry/Similarity3.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 740960f0f..87c4d3ae2 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -97,7 +97,7 @@ Point3 Similarity3::operator*(const Point3& p) const { Similarity3 Similarity3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); - if (n < 3) throw std::runtime_error("less than 3 pairs"); // we need at least three pairs + if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs // calculate centroids Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); @@ -111,9 +111,11 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { // Add to form H matrix Matrix3 H = Z_3x3; + vector d_abPairs; for (const Point3Pair& abPair : abPointPairs) { Point3 da = abPair.first - aCentroid; Point3 db = abPair.second - bCentroid; + d_abPairs.push_back(make_pair(da,db)); H += da * db.transpose(); } @@ -123,9 +125,9 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { // Calculate scale double x = 0; double y = 0; - for (const Point3Pair& abPair : abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; + for (const Point3Pair& d_abPair : d_abPairs) { + Point3 da = d_abPair.first; + Point3 db = d_abPair.second; Vector3 Rdb = aRb * db; y += da.transpose() * Rdb; x += Rdb.transpose() * Rdb; @@ -153,9 +155,9 @@ Rot3 Similarity3::rotationAveraging(const std::vector& rotations, double e Similarity3 Similarity3::Align(const std::vector& abPosePairs) { const size_t n = abPosePairs.size(); - if (n < 2) throw std::runtime_error("less than 2 pairs"); // we need at least two pairs + if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs - // calculate centroids + // calculate rotation and centroids Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); vector rotationList; for (const Pose3Pair& abPair : abPosePairs) { From 7cfcbff4dbb0c143501027f55407431780dfea57 Mon Sep 17 00:00:00 2001 From: ss Date: Mon, 10 Aug 2020 08:37:39 -0400 Subject: [PATCH 007/136] Update doc. --- gtsam_unstable/geometry/Similarity3.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 87c4d3ae2..4a330b15b 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -95,6 +95,7 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } +// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 Similarity3 Similarity3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs @@ -138,6 +139,8 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { return Similarity3(aRb, aTb, s); } +// Use the geodesic L2 mean to solve the mean of rotations, +// Refer to: http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf (on page 18) Rot3 Similarity3::rotationAveraging(const std::vector& rotations, double error) { Rot3 R = rotations[0]; const size_t n = rotations.size(); From aa2d0f3dece0b1401b942596c50340362c735141 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Wed, 12 Aug 2020 13:14:18 -0400 Subject: [PATCH 008/136] Change typedef into using. --- gtsam/geometry/Pose3.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 57bec4dee..3efa1b04d 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -357,7 +357,8 @@ inline Matrix wedge(const Vector& xi) { } // Convenience typedef -typedef std::pair Pose3Pair; +// typedef std::pair Pose3Pair; +using Pose3Pair = std::pair; // For MATLAB wrapper typedef std::vector Pose3Vector; From 58ec261cd7ef1495bdaf7e953255f06a8cd65663 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 16 Aug 2020 13:00:27 -0400 Subject: [PATCH 009/136] Fix GTSAM_TYPEDEF_POINTS_TO_VECTORS. --- gtsam_unstable/geometry/Similarity3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 4a330b15b..859db1c4a 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -87,7 +87,7 @@ Point3 Similarity3::transformFrom(const Point3& p, // Pose3 Similarity3::transformFrom(const Pose3& T) const { Rot3 R = R_.compose(T.rotation()); - Point3 t = Point3(s_ * (R_.matrix() * T.translation().vector() + t_.vector())); + Point3 t = Point3(s_ * (R_ * T.translation() + t_)); return Pose3(R, t); } From 6f33d00654b7c5eb8c2fc48acce9f1f6b9de6d3c Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Mon, 17 Aug 2020 17:58:45 -0400 Subject: [PATCH 010/136] Correct variable names and refactor code. --- gtsam_unstable/geometry/Similarity3.cpp | 13 ++-- .../geometry/tests/testSimilarity3.cpp | 64 +++++++++---------- 2 files changed, 36 insertions(+), 41 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 859db1c4a..531a16428 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -106,17 +106,18 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { aCentroid += abPair.first; bCentroid += abPair.second; } - double f = 1.0 / n; + const double f = 1.0 / n; aCentroid *= f; bCentroid *= f; // Add to form H matrix Matrix3 H = Z_3x3; vector d_abPairs; + d_abPairs.reserve(n); for (const Point3Pair& abPair : abPointPairs) { Point3 da = abPair.first - aCentroid; Point3 db = abPair.second - bCentroid; - d_abPairs.push_back(make_pair(da,db)); + d_abPairs.emplace_back(da, db); H += da * db.transpose(); } @@ -135,7 +136,7 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { } double s = y / x; - Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s; + Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; return Similarity3(aRb, aTb, s); } @@ -166,9 +167,9 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { for (const Pose3Pair& abPair : abPosePairs) { aCentroid += abPair.first.translation(); bCentroid += abPair.second.translation(); - rotationList.push_back(abPair.first.rotation().compose(abPair.second.rotation().inverse())); + rotationList.emplace_back(abPair.first.rotation().compose(abPair.second.rotation().inverse())); } - double f = 1.0 / n; + const double f = 1.0 / n; aCentroid *= f; bCentroid *= f; Rot3 aRb = Similarity3::rotationAveraging(rotationList); @@ -185,7 +186,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { } double s = y / x; - Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s; + Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; return Similarity3(aRb, aTb, s); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 770196e20..e6e814af6 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -260,71 +260,65 @@ TEST(Similarity3, GroupAction) { //****************************************************************************** // Group action on Pose3 TEST(Similarity3, GroupActionPose3) { - Similarity3 bTa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); // Create source poses Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); // Create destination poses - Pose3 expectedTb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 expectedTb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 expected_Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expected_Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); - EXPECT(assert_equal(expectedTb1, bTa.transformFrom(Ta1))); - EXPECT(assert_equal(expectedTb2, bTa.transformFrom(Ta2))); + EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); + EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); } //****************************************************************************** // Align with Point3 Pairs TEST(Similarity3, AlignPoint3_1) { - Similarity3 expected(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0); + Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0); - Point3 p1 = Point3(0, 0, 0); - Point3 p2 = Point3(3, 0, 0); - Point3 p3 = Point3(3, 0, 4); + Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4); - Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); - Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); - Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); vector correspondences{ab1, ab2, ab3}; - Similarity3 actual = Similarity3::Align(correspondences); - EXPECT(assert_equal(expected, actual)); + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); } TEST(Similarity3, AlignPoint3_2) { - Similarity3 expected(Rot3(), Point3(10, 10, 0), 1.0); + Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0); - Point3 p1 = Point3(0, 0, 0); - Point3 p2 = Point3(20, 10, 0); - Point3 p3 = Point3(10, 20, 0); + Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0); - Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); - Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); - Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); vector correspondences{ab1, ab2, ab3}; - Similarity3 actual = Similarity3::Align(correspondences); - EXPECT(assert_equal(expected, actual)); + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); } TEST(Similarity3, AlignPoint3_3) { - Similarity3 expected(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0); + Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0); - Point3 p1 = Point3(0, 0, 1); - Point3 p2 = Point3(10, 0, 2); - Point3 p3 = Point3(20, -10, 30); + Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30); - Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); - Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); - Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); vector correspondences{ab1, ab2, ab3}; - Similarity3 actual = Similarity3::Align(correspondences); - EXPECT(assert_equal(expected, actual)); + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); } //****************************************************************************** @@ -339,7 +333,7 @@ TEST(Similarity3, RotationAveraging) { //****************************************************************************** // Align with Pose3 Pairs TEST(Similarity3, AlignPose3) { - Similarity3 expected(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); // Create source poses Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); @@ -354,8 +348,8 @@ TEST(Similarity3, AlignPose3) { vector correspondences{bTa1, bTa2}; - Similarity3 actual = Similarity3::Align(correspondences); - EXPECT(assert_equal(expected, actual)); + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); } //****************************************************************************** From 362c93bb2b58bf62d85c9c29404322ea3af54d6e Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Wed, 19 Aug 2020 16:48:05 -0400 Subject: [PATCH 011/136] Change sim3 variable from T to S. --- gtsam_unstable/geometry/Similarity3.cpp | 4 ++-- gtsam_unstable/geometry/Similarity3.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 531a16428..9ea6ff80c 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -61,8 +61,8 @@ void Similarity3::print(const std::string& s) const { Similarity3 Similarity3::identity() { return Similarity3(); } -Similarity3 Similarity3::operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); +Similarity3 Similarity3::operator*(const Similarity3& S) const { + return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); } Similarity3 Similarity3::inverse() const { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index e04db4df7..a7797fbb4 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -88,7 +88,7 @@ public: GTSAM_UNSTABLE_EXPORT static Similarity3 identity(); /// Composition - GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const; + GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const; /// Return the inverse GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const; From c80cfe068f7536440c5449b2e5bb728b5d7e1831 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Thu, 20 Aug 2020 11:47:46 -0400 Subject: [PATCH 012/136] Modify the print function print out format. --- gtsam_unstable/geometry/Similarity3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 9ea6ff80c..de9792da8 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -54,8 +54,8 @@ bool Similarity3::operator==(const Similarity3& other) const { void Similarity3::print(const std::string& s) const { std::cout << std::endl; std::cout << s; - rotation().print("R:\n"); - std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl; + rotation().print("\nR:\n"); + std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { From e94aae10bf75febf0d384db5b1dcf7137f95c552 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Fri, 21 Aug 2020 13:51:21 -0400 Subject: [PATCH 013/136] Replace rotAveraging with gtsam::FindKarcherMean. --- gtsam_unstable/geometry/Similarity3.cpp | 19 +------------------ gtsam_unstable/geometry/Similarity3.h | 10 ++++------ .../geometry/tests/testSimilarity3.cpp | 9 --------- 3 files changed, 5 insertions(+), 33 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index de9792da8..25a3e1f99 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -140,23 +140,6 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { return Similarity3(aRb, aTb, s); } -// Use the geodesic L2 mean to solve the mean of rotations, -// Refer to: http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf (on page 18) -Rot3 Similarity3::rotationAveraging(const std::vector& rotations, double error) { - Rot3 R = rotations[0]; - const size_t n = rotations.size(); - Vector3 r; - r << 0, 0, 0; - while (1) { - for (const Rot3 R_i : rotations) { - r += Rot3::Logmap(R.inverse().compose(R_i)); - } - r /= n; - if (r.norm() < error) return R; - R = R.compose(Rot3::Expmap(r)); - } -} - Similarity3 Similarity3::Align(const std::vector& abPosePairs) { const size_t n = abPosePairs.size(); if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs @@ -172,7 +155,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { const double f = 1.0 / n; aCentroid *= f; bCentroid *= f; - Rot3 aRb = Similarity3::rotationAveraging(rotationList); + const Rot3 aRb = FindKarcherMean(rotationList); // Calculate scale double x = 0; diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index a7797fbb4..7af87d761 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -23,6 +23,9 @@ #include #include #include +#include +#include + namespace gtsam { @@ -112,12 +115,7 @@ public: * Create Similarity3 by aligning at least three point pairs */ GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); - - /** - * Calculate the average rotation from a list of rotations - */ - GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector& rotations, double error = 1e-10); - + /** * Create Similarity3 by aligning at least two pose pairs */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index e6e814af6..daf2643c3 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -321,15 +321,6 @@ TEST(Similarity3, AlignPoint3_3) { EXPECT(assert_equal(expected_aSb, actual_aSb)); } -//****************************************************************************** -// Rotation Averaging -TEST(Similarity3, RotationAveraging) { - Rot3 expected = Rot3::Rx(90 * degree); - vector rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)}; - Rot3 actual = Similarity3::rotationAveraging(rotations); - EXPECT(assert_equal(expected, actual)); -} - //****************************************************************************** // Align with Pose3 Pairs TEST(Similarity3, AlignPose3) { From f5611fbc49a9ec2fba879cf68590846df44cf4ba Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Fri, 21 Aug 2020 14:54:43 -0400 Subject: [PATCH 014/136] Add Compatibility unittest. --- .../geometry/tests/testSimilarity3.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index daf2643c3..fcd4c58f7 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -274,6 +274,26 @@ TEST(Similarity3, GroupActionPose3) { EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); } +TEST(Similarity3, GroupActionPose3_Compatibility) { + Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0); + Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0); + + // Create poses + Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tc1 = Pose3(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12)); + Pose3 Tc2 = Pose3(Rot3(0, 0, -1, 0, 1, 0, -1, 0, 0), Point3(0, 6, 12)); + + EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1))); + EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2))); + + EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1))); + EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2))); +} + //****************************************************************************** // Align with Point3 Pairs TEST(Similarity3, AlignPoint3_1) { From 9a07a61779b48334bf07e50ff260fac5c5c01464 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Fri, 21 Aug 2020 14:57:29 -0400 Subject: [PATCH 015/136] reformat pose3 declaration. --- .../geometry/tests/testSimilarity3.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index fcd4c58f7..2ae0d8e82 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -263,12 +263,12 @@ TEST(Similarity3, GroupActionPose3) { Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); // Create source poses - Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); // Create destination poses - Pose3 expected_Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 expected_Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expected_Tb2(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); @@ -280,12 +280,12 @@ TEST(Similarity3, GroupActionPose3_Compatibility) { Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0); // Create poses - Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); - Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); - Pose3 Tc1 = Pose3(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12)); - Pose3 Tc2 = Pose3(Rot3(0, 0, -1, 0, 1, 0, -1, 0, 0), Point3(0, 6, 12)); + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12)); + Pose3 Tc2(Rot3(0, 0, -1, 0, 1, 0, -1, 0, 0), Point3(0, 6, 12)); EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1))); EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2))); @@ -347,12 +347,12 @@ TEST(Similarity3, AlignPose3) { Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); // Create source poses - Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); // Create destination poses - Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); Pose3Pair bTa1(make_pair(Tb1, Ta1)); Pose3Pair bTa2(make_pair(Tb2, Ta2)); From e00fa5605a872e306737cbd972d1b7077b6839a4 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Fri, 21 Aug 2020 20:53:52 -0400 Subject: [PATCH 016/136] create a helper function to remove repeat code. --- gtsam_unstable/geometry/Similarity3.cpp | 52 ++++++++++--------------- gtsam_unstable/geometry/Similarity3.h | 5 ++- 2 files changed, 25 insertions(+), 32 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 25a3e1f99..eb2868f7e 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -95,6 +95,22 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } +// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 +Similarity3 Similarity3::GetSim3(const std::vector& abPointPairs, const Point3 aCentroid, const Point3 bCentroid, const Rot3 aRb) { + double x = 0; + double y = 0; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + double s = y / x; + Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; + return Similarity3(aRb, aTb, s); +} + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 Similarity3 Similarity3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); @@ -112,32 +128,16 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { // Add to form H matrix Matrix3 H = Z_3x3; - vector d_abPairs; - d_abPairs.reserve(n); for (const Point3Pair& abPair : abPointPairs) { Point3 da = abPair.first - aCentroid; Point3 db = abPair.second - bCentroid; - d_abPairs.emplace_back(da, db); H += da * db.transpose(); } // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - // Calculate scale - double x = 0; - double y = 0; - for (const Point3Pair& d_abPair : d_abPairs) { - Point3 da = d_abPair.first; - Point3 db = d_abPair.second; - Vector3 Rdb = aRb * db; - y += da.transpose() * Rdb; - x += Rdb.transpose() * Rdb; - } - double s = y / x; - - Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; - return Similarity3(aRb, aTb, s); + return GetSim3(abPointPairs, aCentroid, bCentroid, aRb); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { @@ -147,30 +147,20 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { // calculate rotation and centroids Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); vector rotationList; + vector abPointPairs; + abPointPairs.reserve(n); for (const Pose3Pair& abPair : abPosePairs) { aCentroid += abPair.first.translation(); bCentroid += abPair.second.translation(); rotationList.emplace_back(abPair.first.rotation().compose(abPair.second.rotation().inverse())); + abPointPairs.emplace_back(abPair.first.translation(), abPair.second.translation()); } const double f = 1.0 / n; aCentroid *= f; bCentroid *= f; const Rot3 aRb = FindKarcherMean(rotationList); - // Calculate scale - double x = 0; - double y = 0; - for (const Pose3Pair& abPair : abPosePairs) { - Point3 da = abPair.first.translation() - aCentroid; - Point3 db = abPair.second.translation() - bCentroid; - Vector3 Rdb = aRb * db; - y += da.transpose() * Rdb; - x += Rdb.transpose() * Rdb; - } - double s = y / x; - - Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; - return Similarity3(aRb, aTb, s); + return GetSim3(abPointPairs, aCentroid, bCentroid, aRb); } Matrix4 Similarity3::wedge(const Vector7& xi) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 7af87d761..c77ab2038 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -199,10 +199,13 @@ public: /// @name Helper functions /// @{ - /// Calculate expmap and logmap coefficients. private: + /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); + /// Calculate scale and translation with point pairs, rotation, and centroids. + static Similarity3 GetSim3(const std::vector& abPointPairs, const Point3 aCentroid, const Point3 bCentroid, const Rot3 aRb); + /// @} }; From a9dd3ed3c78e2f3880a218c093476e245c3f082c Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Fri, 21 Aug 2020 21:09:36 -0400 Subject: [PATCH 017/136] Add a comment for transformFrom pose. --- gtsam_unstable/geometry/Similarity3.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index c77ab2038..d09850764 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -105,7 +105,16 @@ public: OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; - /// Action on a pose T, R = R*T.R, t = s(R*T.t+t) + /** + * Action on a pose T. + * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object. + * To retrieve a Pose3, we normalized the scale value into 1. + * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| + * | 0 1/s | = | 0 1 | + * + * This group action satisfies the compatibility condition. + */ GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; /** syntactic sugar for transformFrom */ From 4789cd268282caa1e3e972e9e2ad92b6becb9f6d Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 22 Aug 2020 15:20:20 -0400 Subject: [PATCH 018/136] Modify comments and move header file declaration. --- gtsam_unstable/geometry/Similarity3.cpp | 1 + gtsam_unstable/geometry/Similarity3.h | 5 ++--- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 2 ++ 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index eb2868f7e..0083799ce 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index d09850764..018778d09 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -23,8 +23,6 @@ #include #include #include -#include -#include namespace gtsam { @@ -113,7 +111,8 @@ public: * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| * | 0 1/s | = | 0 1 | * - * This group action satisfies the compatibility condition. + * This group action satisfies the compatibility condition. + * For more details, refer to: https://en.wikipedia.org/wiki/Group_action */ GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 2ae0d8e82..01e33b330 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -274,6 +274,8 @@ TEST(Similarity3, GroupActionPose3) { EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); } +// Test left group action compatibility. +// cSa*Ta = cSb*bSa*Ta TEST(Similarity3, GroupActionPose3_Compatibility) { Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0); From 9fd5c66a24ed58ab5551b353fc63f4d1fc3d068d Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 23 Aug 2020 20:32:16 -0400 Subject: [PATCH 019/136] Add mean function into Point3 class. --- gtsam/geometry/Point3.cpp | 13 +++++++++++++ gtsam/geometry/Point3.h | 10 ++++++++++ gtsam/geometry/tests/testPoint3.cpp | 26 ++++++++++++++++++++++++++ 3 files changed, 49 insertions(+) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 7a46f5988..ce4ceee89 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -75,6 +75,19 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } +Point3Pair mean(const std::vector &abPointPairs) { + const size_t n = abPointPairs.size(); + Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + for (const Point3Pair &abPair : abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + const double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + return make_pair(aCentroid, bCentroid); +} + /* ************************************************************************* */ ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) { os << p.first << " <-> " << p.second; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 19e328022..7ffdd2d03 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { @@ -58,6 +59,15 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = boost::none, OptionalJacobian<1, 3> H_q = boost::none); +/// mean +template +GTSAM_EXPORT Point3 mean(const CONTAINER& points) { + Point3 sum(0, 0, 0); + sum = std::accumulate(points.begin(), points.end(), sum); + return sum / points.size(); +} +GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); + template struct Range; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a7c2ac50c..68518b1ff 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -164,6 +164,32 @@ TEST (Point3, normalize) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +//************************************************************************* +TEST(Point3, mean) { + Point3 expected_a_mean(2, 2, 2), expected_b_mean(-1, 1, 0); + Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); + Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); + std::vector a_points{a1, a2, a3}, b_points{b1, b2, b3}; + Point3 actual_a_mean = mean(a_points); + Point3 actual_b_mean = mean(b_points); + EXPECT(assert_equal(expected_a_mean, actual_a_mean)); + EXPECT(assert_equal(expected_b_mean, actual_b_mean)); +} + +TEST(Point3, mean_pair) { + Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0); + Point3Pair expected_mean = std::make_pair(a_mean, b_mean); + Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); + Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); + Point3Pair ab1(std::make_pair(a1, b1)); + Point3Pair ab2(std::make_pair(a2, b2)); + Point3Pair ab3(std::make_pair(a3, b3)); + std::vector point_pairs{ab1, ab2, ab3}; + Point3Pair actual_mean = mean(point_pairs); + EXPECT(assert_equal(expected_mean.first, actual_mean.first)); + EXPECT(assert_equal(expected_mean.second, actual_mean.second)); +} + //************************************************************************* double norm_proxy(const Point3& point) { return double(point.norm()); From 9890744fab64a37088c092b31027c57e812bf258 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 23 Aug 2020 20:43:27 -0400 Subject: [PATCH 020/136] Create AlignGivenR function and refactor code. --- gtsam_unstable/geometry/Similarity3.cpp | 53 +++++++++++++------------ gtsam_unstable/geometry/Similarity3.h | 4 +- 2 files changed, 30 insertions(+), 27 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 0083799ce..a1220cc80 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -97,18 +97,28 @@ Point3 Similarity3::operator*(const Point3& p) const { } // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::GetSim3(const std::vector& abPointPairs, const Point3 aCentroid, const Point3 bCentroid, const Rot3 aRb) { +Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { + // calculate centroids + const Point3Pair centroids = mean(abPointPairs); + const Point3 aCentroid = centroids.first; + const Point3 bCentroid = centroids.second; + + // calculate scale double x = 0; double y = 0; + Point3 aPoint, bPoint; for (const Point3Pair& abPair : abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; + std::tie(aPoint, bPoint) = abPair; + const Point3 da = aPoint - aCentroid; + const Point3 db = bPoint - bCentroid; Vector3 Rdb = aRb * db; y += da.transpose() * Rdb; x += Rdb.transpose() * Rdb; } - double s = y / x; - Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; + const double s = y / x; + + // calculate translation + const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; return Similarity3(aRb, aTb, s); } @@ -118,27 +128,24 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs // calculate centroids - Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); - for (const Point3Pair& abPair : abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; - } - const double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; + const Point3Pair centroids = mean(abPointPairs); + const Point3 aCentroid = centroids.first; + const Point3 bCentroid = centroids.second; // Add to form H matrix Matrix3 H = Z_3x3; + Point3 aPoint, bPoint; for (const Point3Pair& abPair : abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; + std::tie(aPoint, bPoint) = abPair; + Point3 da = aPoint - aCentroid; + Point3 db = bPoint - bCentroid; H += da * db.transpose(); } // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return GetSim3(abPointPairs, aCentroid, bCentroid, aRb); + return AlignGivenR(abPointPairs, aRb); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { @@ -146,22 +153,18 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs // calculate rotation and centroids - Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); vector rotationList; vector abPointPairs; abPointPairs.reserve(n); + Pose3 wTa, wTb; for (const Pose3Pair& abPair : abPosePairs) { - aCentroid += abPair.first.translation(); - bCentroid += abPair.second.translation(); - rotationList.emplace_back(abPair.first.rotation().compose(abPair.second.rotation().inverse())); - abPointPairs.emplace_back(abPair.first.translation(), abPair.second.translation()); + std::tie(wTa, wTb) = abPair; + rotationList.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + abPointPairs.emplace_back(wTa.translation(), wTb.translation()); } - const double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; const Rot3 aRb = FindKarcherMean(rotationList); - return GetSim3(abPointPairs, aCentroid, bCentroid, aRb); + return AlignGivenR(abPointPairs, aRb); } Matrix4 Similarity3::wedge(const Vector7& xi) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 018778d09..b618705de 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -211,8 +211,8 @@ private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); - /// Calculate scale and translation with point pairs, rotation, and centroids. - static Similarity3 GetSim3(const std::vector& abPointPairs, const Point3 aCentroid, const Point3 bCentroid, const Rot3 aRb); + /// This methods estimates the similarity transform from point pairs, given a known or estimated rotation. + static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); /// @} }; From f5c0830b53e59c4d639a36b25faf72b49240fc4e Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 23 Aug 2020 20:54:08 -0400 Subject: [PATCH 021/136] Change CMakelist file to fix merge conflict. --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f9b21e015..9dce69903 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,8 +9,8 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) -set (GTSAM_VERSION_MINOR 0) -set (GTSAM_VERSION_PATCH 2) +set (GTSAM_VERSION_MINOR 1) +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}") From 19d1ac42b95664530b276744f2ad19da34207793 Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Thu, 3 Sep 2020 15:21:15 +0800 Subject: [PATCH 022/136] correct coefficients of approximated SE3 Q_r --- gtsam/geometry/Pose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0a56e2ef5..ecc0598eb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -230,8 +230,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { } else { Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - + 1./24.*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + - 1./24.*(W*W*V + V*W*W - 3*W*V*W) + + 1./120.*(W*V*W*W + W*W*V*W); } #endif From 7d91540865bb9a7db74d5800bf81c3204105b178 Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Fri, 4 Sep 2020 17:19:58 +0800 Subject: [PATCH 023/136] test computeQforExpmapDerivative --- gtsam/geometry/Pose3.cpp | 4 ++-- gtsam/geometry/Pose3.h | 3 +++ gtsam/geometry/tests/testPose3.cpp | 9 +++++++++ 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0a56e2ef5..e505e8dff 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -198,7 +198,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is similar to formula 102 in Barfoot14tro) */ -static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { +Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); const Matrix3 V = skewSymmetric(v); @@ -220,7 +220,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (std::abs(phi)>1e-5) { + if (std::abs(phi)>nearZeroThreshold) { const double sinPhi = sin(phi), cosPhi = cos(phi); const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 159fd2927..575fbaa5c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -181,6 +181,9 @@ public: static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); }; + static Matrix3 computeQforExpmapDerivative( + const Vector6& xi, double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8586f35a0..55720abca 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -818,6 +818,15 @@ TEST( Pose3, LogmapDerivative) { EXPECT(assert_equal(expectedH, actualH)); } +TEST( Pose3, computeQforExpmapDerivative) { + Vector6 w = Vector6::Random(); + w.head<3>().normalize(); + w.head<3>() = w.head<3>() * 0.09; + Matrix3 Qexact = Pose3::computeQforExpmapDerivative(w); + Matrix3 Qapprox = Pose3::computeQforExpmapDerivative(w, 0.1); + EXPECT(assert_equal(Qapprox, Qexact, 1e-5)); +} + /* ************************************************************************* */ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi) * v; From 8fa76865cba039a50e65fe509eb2059951226f0f Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Tue, 8 Sep 2020 21:31:54 -0400 Subject: [PATCH 024/136] remove commented out code --- gtsam/geometry/Pose3.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 3efa1b04d..ed69981d8 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -357,7 +357,6 @@ inline Matrix wedge(const Vector& xi) { } // Convenience typedef -// typedef std::pair Pose3Pair; using Pose3Pair = std::pair; // For MATLAB wrapper From a1b73b3776d5121f4765a3d44680b629b42f72f2 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Tue, 8 Sep 2020 22:42:09 -0400 Subject: [PATCH 025/136] document and use std::tie --- gtsam/geometry/Point3.h | 2 ++ gtsam_unstable/geometry/Similarity3.cpp | 12 +++++------- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7ffdd2d03..7f58497e9 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -66,6 +66,8 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { sum = std::accumulate(points.begin(), points.end(), sum); return sum / points.size(); } + +/// mean of Point3 pair GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); template diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index a1220cc80..1d127f8cf 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -99,9 +99,8 @@ Point3 Similarity3::operator*(const Point3& p) const { // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { // calculate centroids - const Point3Pair centroids = mean(abPointPairs); - const Point3 aCentroid = centroids.first; - const Point3 bCentroid = centroids.second; + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); // calculate scale double x = 0; @@ -128,9 +127,8 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs // calculate centroids - const Point3Pair centroids = mean(abPointPairs); - const Point3 aCentroid = centroids.first; - const Point3 bCentroid = centroids.second; + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); // Add to form H matrix Matrix3 H = Z_3x3; @@ -152,7 +150,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { const size_t n = abPosePairs.size(); if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs - // calculate rotation and centroids + // calculate rotation vector rotationList; vector abPointPairs; abPointPairs.reserve(n); From 41921c3173eefdaae058871bcc26fec014a9e25e Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Tue, 8 Sep 2020 22:45:32 -0400 Subject: [PATCH 026/136] Refactor mean and mean_pair test case. --- gtsam/geometry/tests/testPoint3.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 68518b1ff..a481a8072 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -166,14 +166,11 @@ TEST (Point3, normalize) { //************************************************************************* TEST(Point3, mean) { - Point3 expected_a_mean(2, 2, 2), expected_b_mean(-1, 1, 0); + Point3 expected_a_mean(2, 2, 2); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); - Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); - std::vector a_points{a1, a2, a3}, b_points{b1, b2, b3}; + std::vector a_points{a1, a2, a3}; Point3 actual_a_mean = mean(a_points); - Point3 actual_b_mean = mean(b_points); EXPECT(assert_equal(expected_a_mean, actual_a_mean)); - EXPECT(assert_equal(expected_b_mean, actual_b_mean)); } TEST(Point3, mean_pair) { @@ -181,10 +178,7 @@ TEST(Point3, mean_pair) { Point3Pair expected_mean = std::make_pair(a_mean, b_mean); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); - Point3Pair ab1(std::make_pair(a1, b1)); - Point3Pair ab2(std::make_pair(a2, b2)); - Point3Pair ab3(std::make_pair(a3, b3)); - std::vector point_pairs{ab1, ab2, ab3}; + std::vector point_pairs{{a1,b1},{a2,b2},{a3,b3}}; Point3Pair actual_mean = mean(point_pairs); EXPECT(assert_equal(expected_mean.first, actual_mean.first)); EXPECT(assert_equal(expected_mean.second, actual_mean.second)); From 9acf8d4ebca32e2f13e7c7deb75e9d449d86cffc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 10 Sep 2020 19:59:18 -0400 Subject: [PATCH 027/136] ISAM2 helper methods and wrapper to evaluate nonlinear error --- gtsam/gtsam.i | 2 ++ gtsam/nonlinear/ISAM2Result.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..8779c946f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2551,6 +2551,8 @@ class ISAM2Result { size_t getVariablesRelinearized() const; size_t getVariablesReeliminated() const; size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; }; class ISAM2 { diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index e45b17e4a..b249af5c5 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -176,6 +176,8 @@ struct ISAM2Result { size_t getVariablesRelinearized() const { return variablesRelinearized; } size_t getVariablesReeliminated() const { return variablesReeliminated; } size_t getCliques() const { return cliques; } + double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); } + double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); } }; } // namespace gtsam From 66c9a63919a8e12b3d8f5593e57ec4ffaa4f7eb0 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Mon, 14 Sep 2020 00:20:50 -0400 Subject: [PATCH 028/136] Fix double computation. --- gtsam_unstable/geometry/Similarity3.cpp | 25 ++++++++++++++++++++++--- gtsam_unstable/geometry/Similarity3.h | 6 +++++- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 1d127f8cf..5b35b9978 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -103,8 +103,7 @@ Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs std::tie(aCentroid, bCentroid) = mean(abPointPairs); // calculate scale - double x = 0; - double y = 0; + double x = 0, y = 0; Point3 aPoint, bPoint; for (const Point3Pair& abPair : abPointPairs) { std::tie(aPoint, bPoint) = abPair; @@ -121,6 +120,24 @@ Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs return Similarity3(aRb, aTb, s); } +// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 +Similarity3 Similarity3::AlignGivenR(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { + // calculate scale + double x = 0, y = 0; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + const double s = y / x; + + // calculate translation + const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; + return Similarity3(aRb, aTb, s); +} + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 Similarity3 Similarity3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); @@ -133,17 +150,19 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { // Add to form H matrix Matrix3 H = Z_3x3; Point3 aPoint, bPoint; + std::vector d_abPointPairs; for (const Point3Pair& abPair : abPointPairs) { std::tie(aPoint, bPoint) = abPair; Point3 da = aPoint - aCentroid; Point3 db = bPoint - bCentroid; + d_abPointPairs.emplace_back(da, db); H += da * db.transpose(); } // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return AlignGivenR(abPointPairs, aRb); + return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index b618705de..9b34f3095 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -211,9 +211,13 @@ private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); - /// This methods estimates the similarity transform from point pairs, given a known or estimated rotation. + /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); + /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. + static Similarity3 AlignGivenR(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); + + /// @} }; From f9749c8affacc8bdaba0d95e66df99b42e7687ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 18 Sep 2020 11:44:59 -0400 Subject: [PATCH 029/136] fix wrapper TODOs for ISAM2 and BearingRange --- gtsam/gtsam.i | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..fedb28226 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -314,6 +314,14 @@ class DSFMapIndexPair { gtsam::IndexPairSetMap sets(); }; +#include +template +class FastList {}; + +#include +template +class FastMap {}; + #include bool linear_independent(Matrix A, Matrix B, double tol); @@ -2567,9 +2575,10 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - // TODO: wrap the full version of update - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys, const gtsam::FastList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys, const gtsam::FastList& extraReelimKeys, bool force_relinearize); gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; @@ -2701,8 +2710,7 @@ 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 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; From 1ed651b1a2bc537d5c7f249f8d30bdd1e42ff8e5 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 18 Sep 2020 23:14:07 -0700 Subject: [PATCH 030/136] wrap MFAS --- gtsam/gtsam.i | 21 +++++++++++++++++++++ gtsam/sfm/MFAS.h | 2 ++ python/CMakeLists.txt | 7 +++++-- python/gtsam/specializations.h | 1 + 4 files changed, 29 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..649d80ae3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3105,6 +3105,27 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include + +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(pair) const; +}; + +class MFAS { + MFAS(const KeyVector*& nodes, + const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); + + KeyPairDoubleMap computeOutlierWeights() const; + KeyVector computeOrdering() const; +}; + #include class TranslationRecovery { TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 929aa5ff0..67a7df219 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -108,4 +108,6 @@ class MFAS { std::map computeOutlierWeights() const; }; +typedef std::map, double> KeyPairDoubleMap; + } // namespace gtsam diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bec02fb64..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -37,7 +37,8 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector - gtsam::BinaryMeasurementsUnit3) + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_py # target ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header @@ -80,7 +81,9 @@ set(ignore gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue - gtsam::BinaryMeasurementsUnit3) + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) + pybind_wrap(gtsam_unstable_py # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3f6b8fa38..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -12,3 +12,4 @@ py::bind_vector py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); +py::bind_map(m_, "KeyPairDoubleMap"); From 6c14605ed0860f648ef73321a434033d98ed0796 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 19 Sep 2020 08:36:49 +0000 Subject: [PATCH 031/136] changing to boost shared_ptr --- gtsam/gtsam.i | 8 ++++---- gtsam/sfm/MFAS.cpp | 4 +++- gtsam/sfm/MFAS.h | 8 +++++--- gtsam/sfm/tests/testMFAS.cpp | 10 +++++++--- 4 files changed, 19 insertions(+), 11 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 649d80ae3..df486e19e 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3114,16 +3114,16 @@ class KeyPairDoubleMap { size_t size() const; bool empty() const; void clear(); - size_t at(pair) const; + size_t at(const pair& keypair) const; }; class MFAS { - MFAS(const KeyVector*& nodes, + MFAS(const gtsam::KeyVector* nodes, const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::Unit3& projectionDirection); - KeyPairDoubleMap computeOutlierWeights() const; - KeyVector computeOrdering() const; + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; }; #include diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 6dccc2dee..0a407785b 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -7,6 +7,8 @@ #include +#include + #include #include #include @@ -111,7 +113,7 @@ void removeNodeFromGraph(const Key node, graph.erase(node); } -MFAS::MFAS(const std::shared_ptr>& nodes, +MFAS::MFAS(const boost::shared_ptr> nodes, const TranslationEdges& relativeTranslations, const Unit3& projectionDirection) : nodes_(nodes) { diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 67a7df219..ca85d3248 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -22,6 +22,8 @@ #include #include +#include + #include #include #include @@ -57,7 +59,7 @@ class MFAS { private: // pointer to nodes in the graph - const std::shared_ptr> nodes_; + const boost::shared_ptr> nodes_; // edges with a direction such that all weights are positive // i.e, edges that originally had negative weights are flipped @@ -74,7 +76,7 @@ class MFAS { * @param nodes: Nodes in the graph * @param edgeWeights: weights of edges in the graph */ - MFAS(const std::shared_ptr> &nodes, + MFAS(const boost::shared_ptr> nodes, const std::map &edgeWeights) : nodes_(nodes), edgeWeights_(edgeWeights) {} @@ -88,7 +90,7 @@ class MFAS { * @param relativeTranslations translation directions between the cameras * @param projectionDirection direction in which edges are to be projected */ - MFAS(const std::shared_ptr> &nodes, + MFAS(const boost::shared_ptr> nodes, const TranslationEdges &relativeTranslations, const Unit3 &projectionDirection); diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 58ea4cc84..53526cce1 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -6,9 +6,13 @@ */ #include -#include + +#include +#include #include +#include + using namespace std; using namespace gtsam; @@ -46,7 +50,7 @@ map getEdgeWeights(const vector &edges, // test the ordering and the outlierWeights function using weights2 - outlier // edge is rejected when projected in a direction that gives weights2 TEST(MFAS, OrderingWeights2) { - MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights2)); + MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights2)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -76,7 +80,7 @@ TEST(MFAS, OrderingWeights2) { // weights1 (outlier edge is accepted when projected in a direction that // produces weights1) TEST(MFAS, OrderingWeights1) { - MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights1)); + MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights1)); vector ordered_nodes = mfas_obj.computeOrdering(); From bf0651b626d1adf34dcd550f0d3f654eab084206 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 20 Sep 2020 11:45:30 -0400 Subject: [PATCH 032/136] Refactor Align with short functions. --- gtsam_unstable/geometry/Similarity3.cpp | 82 ++++++++++++------------- gtsam_unstable/geometry/Similarity3.h | 14 ++++- 2 files changed, 52 insertions(+), 44 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 5b35b9978..d6515e022 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -96,33 +96,19 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { - // calculate centroids - Point3 aCentroid, bCentroid; - std::tie(aCentroid, bCentroid) = mean(abPointPairs); - - // calculate scale - double x = 0, y = 0; +std::vector Similarity3::SubtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) { + std::vector d_abPointPairs; Point3 aPoint, bPoint; for (const Point3Pair& abPair : abPointPairs) { std::tie(aPoint, bPoint) = abPair; - const Point3 da = aPoint - aCentroid; - const Point3 db = bPoint - bCentroid; - Vector3 Rdb = aRb * db; - y += da.transpose() * Rdb; - x += Rdb.transpose() * Rdb; + Point3 da = aPoint - aCentroid; + Point3 db = bPoint - bCentroid; + d_abPointPairs.emplace_back(da, db); } - const double s = y / x; - - // calculate translation - const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; - return Similarity3(aRb, aTb, s); + return d_abPointPairs; } -// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::AlignGivenR(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { - // calculate scale +std::pair Similarity3::GetXY(const std::vector& d_abPointPairs, const Rot3& aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { @@ -131,38 +117,52 @@ Similarity3 Similarity3::AlignGivenR(const std::vector& d_abPointPai y += da.transpose() * Rdb; x += Rdb.transpose() * Rdb; } - const double s = y / x; + return std::make_pair(x, y); +} +Matrix3 Similarity3::GetH(const std::vector& d_abPointPairs) { + Matrix3 H = Z_3x3; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + H += da * db.transpose(); + } + return H; +} + +Similarity3 Similarity3::AlignGivenRandCentroids(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { + double x, y; + std::tie(x, y) = GetXY(d_abPointPairs, aRb); + const double s = y / x; // calculate translation const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; return Similarity3(aRb, aTb, s); } -// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::Align(const std::vector& abPointPairs) { - const size_t n = abPointPairs.size(); - if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs - +Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 // calculate centroids Point3 aCentroid, bCentroid; std::tie(aCentroid, bCentroid) = mean(abPointPairs); + // substract centroids + std::vector d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid); + return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid); +} - // Add to form H matrix - Matrix3 H = Z_3x3; - Point3 aPoint, bPoint; - std::vector d_abPointPairs; - for (const Point3Pair& abPair : abPointPairs) { - std::tie(aPoint, bPoint) = abPair; - Point3 da = aPoint - aCentroid; - Point3 db = bPoint - bCentroid; - d_abPointPairs.emplace_back(da, db); - H += da * db.transpose(); - } - +Similarity3 Similarity3::Align(const std::vector& abPointPairs) { + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 + const size_t n = abPointPairs.size(); + if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs + // calculate centroids + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); + // substract centroids + std::vector d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid); + // form H matrix + Matrix3 H = GetH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - - return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid); + return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 9b34f3095..d596d1193 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -211,12 +211,20 @@ private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); - /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. - static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); + /// Subtract centroids from point pairs. + static std::vector SubtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid); + + /// Form inner products x and y. + static std::pair GetXY(const std::vector& d_abPointPairs, const Rot3& aRb); + + /// Form outer product H. + static Matrix3 GetH(const std::vector& d_abPointPairs); /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. - static Similarity3 AlignGivenR(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); + static Similarity3 AlignGivenRandCentroids(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); + /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. + static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); /// @} }; From 1f5c6b8b4b712250e5e7261ae7ba68273d734741 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 20 Sep 2020 20:33:37 +0000 Subject: [PATCH 033/136] remove unusede ptr member in MFAS --- gtsam/gtsam.i | 4 ++-- gtsam/sfm/MFAS.cpp | 6 ++---- gtsam/sfm/MFAS.h | 24 +++++------------------- gtsam/sfm/tests/testMFAS.cpp | 7 ++----- 4 files changed, 11 insertions(+), 30 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index df486e19e..3a7bebaba 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2972,6 +2972,7 @@ class BinaryMeasurement { size_t key1() const; size_t key2() const; T measured() const; + gtsam::noiseModel::Base* noiseModel() const; }; typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; @@ -3118,8 +3119,7 @@ class KeyPairDoubleMap { }; class MFAS { - MFAS(const gtsam::KeyVector* nodes, - const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::Unit3& projectionDirection); gtsam::KeyPairDoubleMap computeOutlierWeights() const; diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 0a407785b..bc66d0711 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -113,10 +113,8 @@ void removeNodeFromGraph(const Key node, graph.erase(node); } -MFAS::MFAS(const boost::shared_ptr> nodes, - const TranslationEdges& relativeTranslations, - const Unit3& projectionDirection) - : nodes_(nodes) { +MFAS::MFAS(const TranslationEdges& relativeTranslations, + const Unit3& projectionDirection) { // Iterate over edges, obtain weights by projecting // their relativeTranslations along the projection direction for (const auto& measurement : relativeTranslations) { diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index ca85d3248..3b01122a9 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -22,8 +22,6 @@ #include #include -#include - #include #include #include @@ -58,40 +56,28 @@ class MFAS { using TranslationEdges = std::vector>; private: - // pointer to nodes in the graph - const boost::shared_ptr> nodes_; - // edges with a direction such that all weights are positive // i.e, edges that originally had negative weights are flipped std::map edgeWeights_; public: /** - * @brief Construct from the nodes in a graph and weighted directed edges + * @brief Construct from the weighted directed edges * between the nodes. Each node is identified by a Key. - * A shared pointer to the nodes is used as input parameter - * because, MFAS ordering is usually used to compute the ordering of a - * large graph that is already stored in memory. It is unnecessary to make a - * copy of the nodes in this class. - * @param nodes: Nodes in the graph * @param edgeWeights: weights of edges in the graph */ - MFAS(const boost::shared_ptr> nodes, - const std::map &edgeWeights) - : nodes_(nodes), edgeWeights_(edgeWeights) {} + MFAS(const std::map &edgeWeights) + : edgeWeights_(edgeWeights) {} /** * @brief Constructor to be used in the context of translation averaging. * Here, the nodes of the graph are cameras in 3D and the edges have a unit * translation direction between them. The weights of the edges is computed by * projecting them along a projection direction. - * @param nodes cameras in the epipolar graph (each camera is identified by a - * Key) * @param relativeTranslations translation directions between the cameras * @param projectionDirection direction in which edges are to be projected */ - MFAS(const boost::shared_ptr> nodes, - const TranslationEdges &relativeTranslations, + MFAS(const TranslationEdges &relativeTranslations, const Unit3 &projectionDirection); /** @@ -101,7 +87,7 @@ class MFAS { std::vector computeOrdering() const; /** - * @brief Computes the "outlier weights" of the graph. We define the outlier + * @brief Computes the outlier weights of the graph. We define the outlier * weight of a edge to be zero if the edge is an inlier and the magnitude of * its edgeWeight if it is an outlier. This function internally calls * computeOrdering and uses the obtained ordering to identify outlier edges. diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 53526cce1..b2daf0d2e 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -7,8 +7,6 @@ #include -#include -#include #include #include @@ -43,14 +41,13 @@ map getEdgeWeights(const vector &edges, for (size_t i = 0; i < edges.size(); i++) { edgeWeights[edges[i]] = weights[i]; } - cout << "returning edge weights " << edgeWeights.size() << endl; return edgeWeights; } // test the ordering and the outlierWeights function using weights2 - outlier // edge is rejected when projected in a direction that gives weights2 TEST(MFAS, OrderingWeights2) { - MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights2)); + MFAS mfas_obj(getEdgeWeights(edges, weights2)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -80,7 +77,7 @@ TEST(MFAS, OrderingWeights2) { // weights1 (outlier edge is accepted when projected in a direction that // produces weights1) TEST(MFAS, OrderingWeights1) { - MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights1)); + MFAS mfas_obj(getEdgeWeights(edges, weights1)); vector ordered_nodes = mfas_obj.computeOrdering(); From 565467f2ff48dcdd9a53f4e93c664ac7fa51b4b3 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 20 Sep 2020 20:34:10 +0000 Subject: [PATCH 034/136] translation averaging example --- .../examples/TranslationAveragingExample.py | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 python/gtsam/examples/TranslationAveragingExample.py diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py new file mode 100644 index 000000000..09d261d97 --- /dev/null +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -0,0 +1,93 @@ +from collections import Counter +import functools +import operator + +import numpy as np + +import gtsam +from gtsam.examples import SFMdata + +max_1dsfm_projection_directions = 50 +outlier_weight_threshold = 0.1 + +def get_data(): + """"Returns data from SfMData.createPoses(). This contains the global rotations and the unit translations directions.""" + # Using toy dataset in SfMdata for example. + poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + rotations = gtsam.Values() + translation_directions = [] + for i in range(0, len(poses) - 2): + # Add the rotation + rotations.insert(i, poses[i].rotation()) + # Create unit translation measurements with next two poses + for j in range(i+1, i+3): + i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(poses[j].translation() - poses[i].translation())) + translation_directions.append(gtsam.BinaryMeasurementUnit3( + i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + # Add the last two rotations. + rotations.insert(len(poses) - 1, poses[-1].rotation()) + rotations.insert(len(poses) - 2, poses[-2].rotation()) + return (rotations, translation_directions) + + +def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, + rotations: gtsam.Values): + """Estimate poses given normalized translation directions and rotations between nodes. + + Arguments: + measurements - List of translation direction from the first node to the second node in the coordinate frame of the first node. + rotations {Values} -- Estimated rotations + + Returns: + Values -- Estimated poses. + """ + + # Convert the translation directions to global frame using the rotations. + w_measurements = gtsam.BinaryMeasurementsUnit3() + for measurement in measurements: + w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2( + ), gtsam.Unit3(rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) + + # Indices of measurements that are to be used as projection directions. These are randomly chosen. + indices = np.random.choice(len(w_measurements), min( + max_1dsfm_projection_directions, len(w_measurements)), replace=False) + # Sample projection directions from the measurements. + projection_directions = [w_measurements[idx].measured() for idx in indices] + + outlier_weights = [] + # Find the outlier weights for each direction using MFAS. + for direction in projection_directions: + algorithm = gtsam.MFAS(w_measurements, direction) + outlier_weights.append(algorithm.computeOutlierWeights()) + + # Compute average of outlier weights. + avg_outlier_weights = {} + for outlier_weight_dict in outlier_weights: + for k, v in outlier_weight_dict.items(): + if k in avg_outlier_weights: + avg_outlier_weights[k] += v/len(outlier_weights) + else: + avg_outlier_weights[k] = v/len(outlier_weights) + + # Remove measurements that have weight greater than threshold. + inlier_measurements = gtsam.BinaryMeasurementsUnit3() + [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[(m.key1(), m.key2())] < outlier_weight_threshold] + + # Run the optimizer to obtain translations for normalized directions. + translations = gtsam.TranslationRecovery(inlier_measurements).run() + + poses = gtsam.Values() + for key in rotations.keys(): + poses.insert(key, gtsam.Pose3( + rotations.atRot3(key), translations.atPoint3(key))) + return poses + +def main(): + rotations, translation_directions = get_data() + poses = estimate_poses_given_rot(translation_directions, rotations) + print("**** Translation averaging output ****") + print(poses) + print("**************************************") + +if __name__ == '__main__': + main() From 4b06616dfedcd2c2b850cf865c4c8118e81681c3 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 21 Sep 2020 20:40:43 -0700 Subject: [PATCH 035/136] adding documentation for example --- .../examples/TranslationAveragingExample.py | 49 +++++++++++++------ 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 09d261d97..4e3c7467a 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -1,17 +1,27 @@ -from collections import Counter -import functools -import operator +""" +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 + +This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery) +together for estimating global translations from relative translation directions and global rotations. +The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset. + +Author: Akshay Krishnan +Date: September 2020 +""" import numpy as np import gtsam from gtsam.examples import SFMdata -max_1dsfm_projection_directions = 50 -outlier_weight_threshold = 0.1 def get_data(): - """"Returns data from SfMData.createPoses(). This contains the global rotations and the unit translations directions.""" + """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" # Using toy dataset in SfMdata for example. poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) rotations = gtsam.Values() @@ -20,8 +30,9 @@ def get_data(): # Add the rotation rotations.insert(i, poses[i].rotation()) # Create unit translation measurements with next two poses - for j in range(i+1, i+3): - i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(poses[j].translation() - poses[i].translation())) + for j in range(i + 1, i + 3): + i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate( + poses[j].translation() - poses[i].translation())) translation_directions.append(gtsam.BinaryMeasurementUnit3( i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. @@ -35,25 +46,30 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, """Estimate poses given normalized translation directions and rotations between nodes. Arguments: - measurements - List of translation direction from the first node to the second node in the coordinate frame of the first node. + measurements {BinaryMeasurementsUnit3}- List of translation direction from the first node to + the second node in the coordinate frame of the first node. rotations {Values} -- Estimated rotations Returns: Values -- Estimated poses. """ + # Some hyperparameters. + max_1dsfm_projection_directions = 50 + outlier_weight_threshold = 0.1 + # Convert the translation directions to global frame using the rotations. w_measurements = gtsam.BinaryMeasurementsUnit3() for measurement in measurements: - w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2( - ), gtsam.Unit3(rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) + w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2(), gtsam.Unit3( + rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) # Indices of measurements that are to be used as projection directions. These are randomly chosen. indices = np.random.choice(len(w_measurements), min( max_1dsfm_projection_directions, len(w_measurements)), replace=False) # Sample projection directions from the measurements. projection_directions = [w_measurements[idx].measured() for idx in indices] - + outlier_weights = [] # Find the outlier weights for each direction using MFAS. for direction in projection_directions: @@ -65,13 +81,14 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, for outlier_weight_dict in outlier_weights: for k, v in outlier_weight_dict.items(): if k in avg_outlier_weights: - avg_outlier_weights[k] += v/len(outlier_weights) + avg_outlier_weights[k] += v / len(outlier_weights) else: - avg_outlier_weights[k] = v/len(outlier_weights) + avg_outlier_weights[k] = v / len(outlier_weights) # Remove measurements that have weight greater than threshold. inlier_measurements = gtsam.BinaryMeasurementsUnit3() - [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[(m.key1(), m.key2())] < outlier_weight_threshold] + [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[( + m.key1(), m.key2())] < outlier_weight_threshold] # Run the optimizer to obtain translations for normalized directions. translations = gtsam.TranslationRecovery(inlier_measurements).run() @@ -82,6 +99,7 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, rotations.atRot3(key), translations.atPoint3(key))) return poses + def main(): rotations, translation_directions = get_data() poses = estimate_poses_given_rot(translation_directions, rotations) @@ -89,5 +107,6 @@ def main(): print(poses) print("**************************************") + if __name__ == '__main__': main() From ff57bbc2c4fd0a1b7669e414a15738fd42e0aaee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 22 Sep 2020 15:25:05 -0400 Subject: [PATCH 036/136] update numericalDerivative functions to take in optional dimension template parameter and added corresponding tests --- gtsam/base/numericalDerivative.h | 104 +++++++++++-------- gtsam/base/tests/testNumericalDerivative.cpp | 54 ++++++++++ 2 files changed, 116 insertions(+), 42 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 34ff0327e..fc5531cdc 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -109,7 +109,7 @@ typename Eigen::Matrix numericalGradient( * @param delta increment for numerical derivative * Class Y is the output argument * Class X is the input argument - * int N is the dimension of the X input value if variable dimension type but known at test time + * @tparam int N is the dimension of the X input value if variable dimension type but known at test time * @return m*n Jacobian computed via central differencing */ @@ -167,15 +167,16 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const * @param x2 second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ @@ -192,15 +193,16 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons * @param x2 n-dimensional second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); } /** use a raw C++ function pointer */ @@ -219,8 +221,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(cons * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -228,7 +231,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative31( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); } template @@ -247,8 +250,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(cons * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -256,7 +260,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative32( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); } template @@ -275,8 +279,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (* * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -284,7 +289,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative33( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); } template @@ -303,8 +308,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative41( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -312,7 +318,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); } template @@ -330,8 +336,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative42( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -339,7 +346,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); } template @@ -357,8 +364,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative43( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -366,7 +374,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); } template @@ -384,8 +392,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (* * @param x4 n-dimensional fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative44( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -393,7 +402,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); } template @@ -412,8 +421,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative51( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -421,7 +431,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative51( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); } template @@ -440,8 +450,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative52( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -449,7 +460,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative52( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); } template @@ -468,8 +479,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative53( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -477,7 +489,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative53( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); } template @@ -496,8 +508,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative54( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -505,7 +518,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative54( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); } template @@ -524,8 +537,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative55( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -533,7 +547,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative55( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); } template @@ -553,8 +567,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative61( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -562,7 +577,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative61( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); } template @@ -582,8 +597,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative62( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -591,7 +607,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative62( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); } template @@ -608,11 +624,12 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (* * @param x3 third argument value * @param x4 fourth argument value * @param x5 fifth argument value - * @param x6 sixth argument value + * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative63( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -620,7 +637,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative63( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); } template @@ -640,8 +657,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative64( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -649,7 +667,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative64( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); } template @@ -669,8 +687,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative65( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -678,7 +697,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative65( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); } template @@ -698,8 +717,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X6 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative66( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -708,7 +728,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); } template diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index d27e43040..6d6f41fdd 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -143,6 +143,13 @@ Vector6 f6(const double x1, const double x2, const double x3, const double x4, return result; } +Vector g6(const double x1, const double x2, const double x3, const double x4, + const double x5, const double x6) { + Vector result(6); + result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6); + return result; +} + /* ************************************************************************* */ // TEST(testNumericalDerivative, numeriDerivative61) { @@ -153,6 +160,14 @@ TEST(testNumericalDerivative, numeriDerivative61) { double, double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected61, actual61, 1e-5)); + + Matrix expected61Dynamic = Matrix::Zero(6, 1); + expected61Dynamic(0, 0) = cos(x1); + Matrix actual61Dynamic = + numericalDerivative61(g6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected61Dynamic, actual61Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -165,6 +180,13 @@ TEST(testNumericalDerivative, numeriDerivative62) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected62, actual62, 1e-5)); + + Matrix expected62Dynamic = Matrix::Zero(6, 1); + expected62Dynamic(1, 0) = -sin(x2); + Matrix61 actual62Dynamic = numericalDerivative62(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected62Dynamic, actual62Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -177,6 +199,14 @@ TEST(testNumericalDerivative, numeriDerivative63) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected63, actual63, 1e-5)); + + Matrix expected63Dynamic = Matrix::Zero(6, 1); + expected63Dynamic(2, 0) = 2 * x3; + Matrix61 actual63Dynamic = + numericalDerivative63(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected63Dynamic, actual63Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -189,6 +219,14 @@ TEST(testNumericalDerivative, numeriDerivative64) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected64, actual64, 1e-5)); + + Matrix expected64Dynamic = Matrix::Zero(6, 1); + expected64Dynamic(3, 0) = 3 * x4 * x4; + Matrix61 actual64Dynamic = + numericalDerivative64(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected64Dynamic, actual64Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -201,6 +239,14 @@ TEST(testNumericalDerivative, numeriDerivative65) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected65, actual65, 1e-5)); + + Matrix expected65Dynamic = Matrix::Zero(6, 1); + expected65Dynamic(4, 0) = 0.5 / sqrt(x5); + Matrix61 actual65Dynamic = + numericalDerivative65(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected65Dynamic, actual65Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -213,6 +259,14 @@ TEST(testNumericalDerivative, numeriDerivative66) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected66, actual66, 1e-5)); + + Matrix expected66Dynamic = Matrix::Zero(6, 1); + expected66Dynamic(5, 0) = cos(x6) + sin(x6); + Matrix61 actual66Dynamic = + numericalDerivative66(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected66Dynamic, actual66Dynamic, 1e-5)); } /* ************************************************************************* */ From 921dc848a08ba08dc369ef461d9646a9a11f63cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 22 Sep 2020 20:17:09 -0400 Subject: [PATCH 037/136] added calibrate with jacobians for Cal3Bundler --- gtsam/geometry/Cal3Bundler.cpp | 33 ++++++++++++++++++++++ gtsam/geometry/Cal3Bundler.h | 10 +++++++ gtsam/geometry/tests/testPinholeCamera.cpp | 9 ++++++ 3 files changed, 52 insertions(+) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 0d64d0184..56f62e3ac 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -121,6 +121,39 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { return pn; } +/* ************************************************************************* */ +Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp) const { + Point2 pn = calibrate(p, 1e-5); + + // Approximate the jacobians via a single iteration of g. + if (Dcal) { + const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; + const double xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double inv_f = 1 / f_, inv_g = 1 / g; + + *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, + -y * inv_g * rr, -y * inv_g * rr * rr; + } + if (Dp) { + const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; + const double xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double inv_f = 1 / f_, inv_g = 1 / g; + + const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); + const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); + + *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv, + -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv); + } + + return pn; +} + /* ************************************************************************* */ Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { Matrix2 Dp; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4787f565b..0042b710f 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -120,6 +120,16 @@ public: /// Convert a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; + /** + * Convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; + /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index a9b68bdec..ad6f4e921 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) { EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } +/* ************************************************************************* */ +TEST( PinholeCamera, Cal3Bundler) { + Cal3Bundler calibration; + Pose3 wTc; + PinholeCamera camera(wTc, calibration); + Point2 p(50, 100); + camera.backproject(p, 10); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 3fab304191b04b80e7525f980c28b6244ef14bef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 23 Sep 2020 09:59:46 -0400 Subject: [PATCH 038/136] combine the calibrate functions into one --- gtsam/geometry/Cal3Bundler.cpp | 28 ++++++++-------------------- gtsam/geometry/Cal3Bundler.h | 11 +++++------ 2 files changed, 13 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 56f62e3ac..4677dbe45 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -94,7 +94,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp, + const double tol) const { // Copied from Cal3DS2 :-( // but specialized with k1,k2 non-zero only and fx=fy and s=0 const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); @@ -118,32 +120,18 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - return pn; -} - -/* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal, - OptionalJacobian<2, 2> Dp) const { - Point2 pn = calibrate(p, 1e-5); + const double x = invKPi.x(), y = invKPi.y(); + const double xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double inv_f = 1 / f_, inv_g = 1 / g; // Approximate the jacobians via a single iteration of g. if (Dcal) { - const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; - const double xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double inv_f = 1 / f_, inv_g = 1 / g; - *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, -y * inv_g * rr, -y * inv_g * rr * rr; } if (Dp) { - const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; - const double xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double inv_f = 1 / f_, inv_g = 1 / g; - const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0042b710f..0636399a2 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -117,18 +117,17 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; - /// Convert a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; - /** - * Convert image coordinates uv to intrinsic coordinates xy + * Convert a pixel coordinate to ideal coordinate xy * @param p point in image coordinates * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @param tol optional tolerance threshold value for iterative minimization * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none, + const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; From 611974c63ad7469973fa847138979a09e9a102f7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 23 Sep 2020 10:45:57 -0400 Subject: [PATCH 039/136] put the jacobians at the end so that the calibrate function can be wrapped --- gtsam/geometry/Cal3Bundler.cpp | 6 +++--- gtsam/geometry/Cal3Bundler.h | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 4677dbe45..837ce35e0 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -94,9 +94,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, - OptionalJacobian<2, 2> Dp, - const double tol) const { +Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, + OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp) const { // Copied from Cal3DS2 :-( // but specialized with k1,k2 non-zero only and fx=fy and s=0 const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0636399a2..35398303f 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -120,14 +120,14 @@ public: /** * Convert a pixel coordinate to ideal coordinate xy * @param p point in image coordinates + * @param tol optional tolerance threshold value for iterative minimization * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @param tol optional tolerance threshold value for iterative minimization * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none, - const double tol = 1e-5) const; + Point2 calibrate(const Point2& pi, const double tol = 1e-5, + OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; From b499006b12dbab9b1d7b1af366a74c1c554f2437 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 23 Sep 2020 13:48:40 -0400 Subject: [PATCH 040/136] make tolerance as a constructor param --- gtsam/geometry/Cal3Bundler.cpp | 21 +++++++++++---------- gtsam/geometry/Cal3Bundler.h | 8 ++++++-- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 837ce35e0..470c97c75 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -25,13 +25,13 @@ namespace gtsam { /* ************************************************************************* */ Cal3Bundler::Cal3Bundler() : - f_(1), k1_(0), k2_(0), u0_(0), v0_(0) { + f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) { } /* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : - f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { -} +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0, + double tol) + : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {} /* ************************************************************************* */ Matrix3 Cal3Bundler::K() const { @@ -94,23 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, +Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // Copied from Cal3DS2 :-( // but specialized with k1,k2 non-zero only and fx=fy and s=0 - const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); + double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_; + const Point2 invKPi(x, y); // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; + Point2 pn(x, y); // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (distance2(uncalibrate(pn), pi) <= tol) + if (distance2(uncalibrate(pn), pi) <= tol_) break; - const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; + const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); pn = invKPi / g; @@ -120,7 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - const double x = invKPi.x(), y = invKPi.y(); + x = invKPi.x(), y = invKPi.y(); const double xx = x * x, yy = y * y; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 35398303f..96765f899 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -33,6 +33,7 @@ private: double f_; ///< focal length double k1_, k2_; ///< radial distortion double u0_, v0_; ///< image center, not a parameter to be optimized but a constant + double tol_; ///< tolerance value when calibrating public: @@ -51,8 +52,10 @@ public: * @param k2 second radial distortion coefficient (quartic) * @param u0 optional image center (default 0), considered a constant * @param v0 optional image center (default 0), considered a constant + * @param tol optional calibration tolerance value */ - Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); + Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0, + double tol = 1e-5); virtual ~Cal3Bundler() {} @@ -125,7 +128,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, const double tol = 1e-5, + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; @@ -173,6 +176,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(tol_); } /// @} From cb78b281a57f125ccbdd5383aa454425ed3dbbfe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 24 Sep 2020 00:09:39 -0400 Subject: [PATCH 041/136] update calibrate in wrapper --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d8297c43b..369ea983c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1053,7 +1053,7 @@ class Cal3_S2Stereo { class Cal3Bundler { // Standard Constructors Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable void print(string s) const; @@ -1066,7 +1066,7 @@ class Cal3Bundler { Vector localCoordinates(const gtsam::Cal3Bundler& c) const; // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; // Standard Interface From 74c4a60e83d1d6d6e4d4fd14303477af7c6031a7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 24 Sep 2020 17:34:30 -0400 Subject: [PATCH 042/136] small fixes to ensure marginals are computed correctly --- python/gtsam/examples/ImuFactorExample.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index eec7c5ebd..bb707a8f5 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -104,7 +104,7 @@ class ImuFactorExample(PreintegrationExample): if verbose: print(factor) - print(pim.predict(actual_state_i, self.actualBias)) + print(pim.predict(initial_state_i, self.actualBias)) pim.resetIntegration() @@ -125,7 +125,7 @@ class ImuFactorExample(PreintegrationExample): i += 1 # add priors on end - # self.addPrior(num_poses - 1, graph) + self.addPrior(num_poses - 1, graph) initial.print_("Initial values:") From 09f09fa44fc04381b66f695deb760a96a4cce720 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 24 Sep 2020 19:01:52 -0400 Subject: [PATCH 043/136] update wrapper to also work for Matlab --- gtsam/gtsam.i | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index fedb28226..5f031dd1d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -314,14 +314,6 @@ class DSFMapIndexPair { gtsam::IndexPairSetMap sets(); }; -#include -template -class FastList {}; - -#include -template -class FastMap {}; - #include bool linear_independent(Matrix A, Matrix B, double tol); @@ -2575,10 +2567,9 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys, const gtsam::FastList& extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys, const gtsam::FastList& extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; @@ -2710,7 +2701,8 @@ class BearingRange { BearingRange(const BEARING& b, const RANGE& r); BEARING bearing() const; RANGE range() const; - static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); + //TODO need to update wrap to allow self referencing of class + // 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; From fbb26eea07ccc05c5496ea3ef019c6b024f4e626 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 24 Sep 2020 22:32:04 -0700 Subject: [PATCH 044/136] naming and other changes - review1 --- .../examples/TranslationAveragingExample.py | 78 +++++++++++-------- 1 file changed, 44 insertions(+), 34 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 4e3c7467a..c3dcf2ad2 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -14,22 +14,28 @@ Author: Akshay Krishnan Date: September 2020 """ +from collections import defaultdict +from typing import Tuple, List + import numpy as np import gtsam from gtsam.examples import SFMdata -def get_data(): +def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" # Using toy dataset in SfMdata for example. poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + # Rotations of the cameras in the world frame - wRc. rotations = gtsam.Values() + # Normalized translation directions for pairs of cameras - from first camera to second, + # in the coordinate frame of the first camera. translation_directions = [] for i in range(0, len(poses) - 2): - # Add the rotation + # Add the rotation. rotations.insert(i, poses[i].rotation()) - # Create unit translation measurements with next two poses + # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate( poses[j].translation() - poses[i].translation())) @@ -41,68 +47,72 @@ def get_data(): return (rotations, translation_directions) -def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, - rotations: gtsam.Values): - """Estimate poses given normalized translation directions and rotations between nodes. +def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, + rotations: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations normalized translation directions between cameras. - Arguments: - measurements {BinaryMeasurementsUnit3}- List of translation direction from the first node to - the second node in the coordinate frame of the first node. - rotations {Values} -- Estimated rotations + Args: + relative_translations -- List of normalized translation directions between camera pairs, each direction + is from the first camera to the second, in the frame of the first camera. + rotations -- Rotations of the cameras in the world frame. Returns: Values -- Estimated poses. """ - # Some hyperparameters. - max_1dsfm_projection_directions = 50 + # Some hyperparameters, values used from 1dsfm. + max_1dsfm_projection_directions = 48 outlier_weight_threshold = 0.1 # Convert the translation directions to global frame using the rotations. - w_measurements = gtsam.BinaryMeasurementsUnit3() - for measurement in measurements: - w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2(), gtsam.Unit3( - rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) + w_relative_translations = gtsam.BinaryMeasurementsUnit3() + for relative_translation in relative_translations: + w_relative_translation = gtsam.Unit3(rotations.atRot3(relative_translation.key1()) + .rotate(relative_translation.measured().point3())) + w_relative_translations.append(gtsam.BinaryMeasurementUnit3(relative_translation.key1(), + relative_translation.key2(), + w_relative_translation, + relative_translation.noiseModel())) # Indices of measurements that are to be used as projection directions. These are randomly chosen. - indices = np.random.choice(len(w_measurements), min( - max_1dsfm_projection_directions, len(w_measurements)), replace=False) + sampled_indices = np.random.choice(len(w_relative_translations), min( + max_1dsfm_projection_directions, len(w_relative_translations)), replace=False) # Sample projection directions from the measurements. - projection_directions = [w_measurements[idx].measured() for idx in indices] + projection_directions = [ + w_relative_translations[idx].measured() for idx in sampled_indices] outlier_weights = [] # Find the outlier weights for each direction using MFAS. for direction in projection_directions: - algorithm = gtsam.MFAS(w_measurements, direction) + algorithm = gtsam.MFAS(w_relative_translations, direction) outlier_weights.append(algorithm.computeOutlierWeights()) - # Compute average of outlier weights. - avg_outlier_weights = {} + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, + # where weights are proportional to the probability of the edge being an outlier. + avg_outlier_weights = defaultdict(lambda: 0.0) for outlier_weight_dict in outlier_weights: - for k, v in outlier_weight_dict.items(): - if k in avg_outlier_weights: - avg_outlier_weights[k] += v / len(outlier_weights) - else: - avg_outlier_weights[k] = v / len(outlier_weights) + for keypair, weight in outlier_weight_dict.items(): + avg_outlier_weights[keypair] += weight / len(outlier_weights) - # Remove measurements that have weight greater than threshold. - inlier_measurements = gtsam.BinaryMeasurementsUnit3() - [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[( - m.key1(), m.key2())] < outlier_weight_threshold] + # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. + inlier_w_relative_translations = gtsam.BinaryMeasurementsUnit3() + [inlier_w_relative_translations.append(Z) for Z in w_relative_translations + if avg_outlier_weights[(Z.key1(), Z.key2())] < outlier_weight_threshold] # Run the optimizer to obtain translations for normalized directions. - translations = gtsam.TranslationRecovery(inlier_measurements).run() + w_translations = gtsam.TranslationRecovery( + inlier_w_relative_translations).run() poses = gtsam.Values() for key in rotations.keys(): poses.insert(key, gtsam.Pose3( - rotations.atRot3(key), translations.atPoint3(key))) + rotations.atRot3(key), w_translations.atPoint3(key))) return poses def main(): rotations, translation_directions = get_data() - poses = estimate_poses_given_rot(translation_directions, rotations) + poses = estimate_poses(translation_directions, rotations) print("**** Translation averaging output ****") print(poses) print("**************************************") From 52fc9cf4ba86582d96d4aec3b7a17fece655bd9f Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Fri, 25 Sep 2020 19:47:07 +0800 Subject: [PATCH 045/136] test Qr with old codebase fails --- gtsam/geometry/Pose3.cpp | 18 +++++------------- gtsam/geometry/Pose3.h | 11 ++++++++++- gtsam/geometry/tests/testPose3.cpp | 20 +++++++++++--------- 3 files changed, 26 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2e6a2136e..12f628731 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -190,15 +190,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -/** - * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix - * J(xi) = [J_(w) Z_3x3; - * Q J_(w)] - * where J_(w) is the SO3 Expmap derivative. - * (see Chirikjian11book2, pg 44, eq 10.95. - * The closed-form formula is similar to formula 102 in Barfoot14tro) - */ -Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); const Matrix3 V = skewSymmetric(v); @@ -230,8 +222,8 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr } else { Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - - 1./24.*(W*W*V + V*W*W - 3*W*V*W) - + 1./120.*(W*V*W*W + W*W*V*W); + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); } #endif @@ -242,7 +234,7 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; J << Jw, Z_3x3, Q, Jw; return J; @@ -253,7 +245,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { const Vector6 xi = Logmap(pose); const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q = ComputeQforExpmapDerivative(xi); const Matrix3 Q2 = -Jw*Q*Jw; Matrix6 J; J << Jw, Z_3x3, Q2, Jw; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 575fbaa5c..2f0802cab 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -181,7 +181,16 @@ public: static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); }; - static Matrix3 computeQforExpmapDerivative( + /** + * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix + * J_r(xi) = [J_(w) Z_3x3; + * Q_r J_(w)] + * where J_(w) is the SO3 Expmap right derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is identical to formula 102 in Barfoot14tro where + * Q_l of the SE3 Expmap left derivative matrix is given. + */ + static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); using LieGroup::inverse; // version with derivative diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 55720abca..9639ffbcf 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) { } } +TEST( Pose3, ExpmapDerivativeQr) { + Vector6 w = Vector6::Random(); + w.head<3>().normalize(); + w.head<3>() = w.head<3>() * 0.9e-2; + Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); + EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); +} + /* ************************************************************************* */ TEST( Pose3, LogmapDerivative) { Matrix6 actualH; @@ -818,15 +829,6 @@ TEST( Pose3, LogmapDerivative) { EXPECT(assert_equal(expectedH, actualH)); } -TEST( Pose3, computeQforExpmapDerivative) { - Vector6 w = Vector6::Random(); - w.head<3>().normalize(); - w.head<3>() = w.head<3>() * 0.09; - Matrix3 Qexact = Pose3::computeQforExpmapDerivative(w); - Matrix3 Qapprox = Pose3::computeQforExpmapDerivative(w, 0.1); - EXPECT(assert_equal(Qapprox, Qexact, 1e-5)); -} - /* ************************************************************************* */ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi) * v; From 2550bf76a493ac5148297c09f5adc45f5cf872b9 Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Fri, 25 Sep 2020 20:26:24 +0800 Subject: [PATCH 046/136] correct Qr coefficients in approximation --- gtsam/geometry/Pose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 12f628731..ea822b796 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -222,8 +222,8 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr } else { Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - + 1./24.*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + - 1./24.*(W*W*V + V*W*W - 3*W*V*W) + + 1./120.*(W*V*W*W + W*W*V*W); } #endif From f79a1fb2b32de40fd07f5a9c11f96b2a721f6e41 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Sep 2020 20:59:12 -0400 Subject: [PATCH 047/136] Squashed 'wrap/' changes from 314b121fd..5e1373486 5e1373486 Merge pull request #7 from varunagrawal/fix/matlab-wrapper-templates db647232d support for This with multiple templates git-subtree-dir: wrap git-subtree-split: 5e1373486d5d9ba0827aab20ca3ea1083e24b3da --- matlab_wrapper.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/matlab_wrapper.py b/matlab_wrapper.py index 23f56c8b0..1b6b75a49 100755 --- a/matlab_wrapper.py +++ b/matlab_wrapper.py @@ -210,18 +210,26 @@ class MatlabWrapper(object): else: formatted_type_name += name - if len(type_name.instantiations) == 1: - if separator == "::": # C++ - formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0], - include_namespace=include_namespace, - constructor=constructor, method=method)) - else: + if separator == "::": # C++ + templates = [] + for idx in range(len(type_name.instantiations)): + template = '{}'.format(cls._format_type_name(type_name.instantiations[idx], + include_namespace=include_namespace, + constructor=constructor, method=method)) + templates.append(template) + + if len(templates) > 0: # If there are no templates + formatted_type_name += '<{}>'.format(','.join(templates)) + + else: + for idx in range(len(type_name.instantiations)): formatted_type_name += '{}'.format(cls._format_type_name( - type_name.instantiations[0], + type_name.instantiations[idx], separator=separator, include_namespace=False, constructor=constructor, method=method )) + return formatted_type_name @classmethod From 463b634328650d9653705b3b176208868484e1c3 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 11:58:10 -0400 Subject: [PATCH 048/136] Move private func to .cpp. --- gtsam_unstable/geometry/Similarity3.cpp | 122 +++++++++++++----------- gtsam_unstable/geometry/Similarity3.h | 15 --- 2 files changed, 65 insertions(+), 72 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index d6515e022..eed2d8b97 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,6 +23,67 @@ namespace gtsam { +namespace{ + /// Subtract centroids from point pairs. + static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) { + std::vector d_abPointPairs; + Point3 aPoint, bPoint; + for (const Point3Pair& abPair : abPointPairs) { + std::tie(aPoint, bPoint) = abPair; + Point3 da = aPoint - aCentroid; + Point3 db = bPoint - bCentroid; + d_abPointPairs.emplace_back(da, db); + } + return d_abPointPairs; + } + + /// Form inner products x and y. + static std::pair getXY(const std::vector& d_abPointPairs, const Rot3& aRb) { + double x = 0, y = 0; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + return std::make_pair(x, y); + } + + /// Form outer product H. + static Matrix3 calculateH(const std::vector& d_abPointPairs) { + Matrix3 H = Z_3x3; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + H += da * db.transpose(); + } + return H; + } + + /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. + static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { + double x, y; + std::tie(x, y) = getXY(d_abPointPairs, aRb); + const double s = y / x; + // calculate translation + const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; + return Similarity3(aRb, aTb, s); + } + + /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. + static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 + // calculate centroids + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); + // substract centroids + std::vector d_abPointPairs = subtractCentroids(abPointPairs, aCentroid, bCentroid); + return align(d_abPointPairs, aRb, aCentroid, bCentroid); + } +} + + Similarity3::Similarity3() : t_(0,0,0), s_(1) { } @@ -96,59 +157,6 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -std::vector Similarity3::SubtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) { - std::vector d_abPointPairs; - Point3 aPoint, bPoint; - for (const Point3Pair& abPair : abPointPairs) { - std::tie(aPoint, bPoint) = abPair; - Point3 da = aPoint - aCentroid; - Point3 db = bPoint - bCentroid; - d_abPointPairs.emplace_back(da, db); - } - return d_abPointPairs; -} - -std::pair Similarity3::GetXY(const std::vector& d_abPointPairs, const Rot3& aRb) { - double x = 0, y = 0; - Point3 da, db; - for (const Point3Pair& d_abPair : d_abPointPairs) { - std::tie(da, db) = d_abPair; - Vector3 Rdb = aRb * db; - y += da.transpose() * Rdb; - x += Rdb.transpose() * Rdb; - } - return std::make_pair(x, y); -} - -Matrix3 Similarity3::GetH(const std::vector& d_abPointPairs) { - Matrix3 H = Z_3x3; - Point3 da, db; - for (const Point3Pair& d_abPair : d_abPointPairs) { - std::tie(da, db) = d_abPair; - H += da * db.transpose(); - } - return H; -} - -Similarity3 Similarity3::AlignGivenRandCentroids(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { - double x, y; - std::tie(x, y) = GetXY(d_abPointPairs, aRb); - const double s = y / x; - // calculate translation - const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; - return Similarity3(aRb, aTb, s); -} - -Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { - // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 - // calculate centroids - Point3 aCentroid, bCentroid; - std::tie(aCentroid, bCentroid) = mean(abPointPairs); - // substract centroids - std::vector d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid); - return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid); -} - Similarity3 Similarity3::Align(const std::vector& abPointPairs) { // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 const size_t n = abPointPairs.size(); @@ -157,12 +165,12 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { Point3 aCentroid, bCentroid; std::tie(aCentroid, bCentroid) = mean(abPointPairs); // substract centroids - std::vector d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid); + std::vector d_abPointPairs = subtractCentroids(abPointPairs, aCentroid, bCentroid); // form H matrix - Matrix3 H = GetH(d_abPointPairs); + Matrix3 H = calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid); + return align(d_abPointPairs, aRb, aCentroid, bCentroid); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { @@ -181,7 +189,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { } const Rot3 aRb = FindKarcherMean(rotationList); - return AlignGivenR(abPointPairs, aRb); + return alignGivenR(abPointPairs, aRb); } Matrix4 Similarity3::wedge(const Vector7& xi) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index d596d1193..b82862ddb 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -211,21 +211,6 @@ private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); - /// Subtract centroids from point pairs. - static std::vector SubtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid); - - /// Form inner products x and y. - static std::pair GetXY(const std::vector& d_abPointPairs, const Rot3& aRb); - - /// Form outer product H. - static Matrix3 GetH(const std::vector& d_abPointPairs); - - /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. - static Similarity3 AlignGivenRandCentroids(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); - - /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. - static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); - /// @} }; From ffd0d5e6b9ae252da544af5440a3435a9b72edb5 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 12:03:10 -0400 Subject: [PATCH 049/136] Change getXY to calculateScale. --- gtsam_unstable/geometry/Similarity3.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index eed2d8b97..e78a3925e 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -37,8 +37,8 @@ namespace{ return d_abPointPairs; } - /// Form inner products x and y. - static std::pair getXY(const std::vector& d_abPointPairs, const Rot3& aRb) { + /// Form inner products x and y and calculate scale. + static const double calculateScale(const std::vector& d_abPointPairs, const Rot3& aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { @@ -47,7 +47,8 @@ namespace{ y += da.transpose() * Rdb; x += Rdb.transpose() * Rdb; } - return std::make_pair(x, y); + const double s = y / x; + return s; } /// Form outer product H. @@ -63,9 +64,7 @@ namespace{ /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { - double x, y; - std::tie(x, y) = getXY(d_abPointPairs, aRb); - const double s = y / x; + const double s = calculateScale(d_abPointPairs, aRb); // calculate translation const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; return Similarity3(aRb, aTb, s); From 933565c045322cd25c34dd08431ee547933666ad Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 12:05:17 -0400 Subject: [PATCH 050/136] Emphasize Rdb is a vector. --- gtsam_unstable/geometry/Similarity3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index e78a3925e..52b33d5a6 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -43,9 +43,9 @@ namespace{ Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { std::tie(da, db) = d_abPair; - Vector3 Rdb = aRb * db; - y += da.transpose() * Rdb; - x += Rdb.transpose() * Rdb; + const Vector3 da_prime = aRb * db; + y += da.transpose() * da_prime; + x += da_prime.transpose() * da_prime; } const double s = y / x; return s; From e12d3ba197206e2b44ea133d32b84b24ed279b63 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 12:13:40 -0400 Subject: [PATCH 051/136] Change input into centroids. --- gtsam_unstable/geometry/Similarity3.cpp | 27 ++++++++++--------------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 52b33d5a6..6fc720e75 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -25,13 +25,11 @@ namespace gtsam { namespace{ /// Subtract centroids from point pairs. - static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) { + static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3Pair& centroids) { std::vector d_abPointPairs; - Point3 aPoint, bPoint; for (const Point3Pair& abPair : abPointPairs) { - std::tie(aPoint, bPoint) = abPair; - Point3 da = aPoint - aCentroid; - Point3 db = bPoint - bCentroid; + Point3 da = abPair.first - centroids.first; + Point3 db = abPair.second - centroids.second; d_abPointPairs.emplace_back(da, db); } return d_abPointPairs; @@ -63,22 +61,20 @@ namespace{ } /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. - static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { + static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { const double s = calculateScale(d_abPointPairs, aRb); // calculate translation - const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; + const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; return Similarity3(aRb, aTb, s); } /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 - // calculate centroids - Point3 aCentroid, bCentroid; - std::tie(aCentroid, bCentroid) = mean(abPointPairs); + auto centroids = mean(abPointPairs); // substract centroids - std::vector d_abPointPairs = subtractCentroids(abPointPairs, aCentroid, bCentroid); - return align(d_abPointPairs, aRb, aCentroid, bCentroid); + std::vector d_abPointPairs = subtractCentroids(abPointPairs, centroids); + return align(d_abPointPairs, aRb, centroids); } } @@ -161,15 +157,14 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs // calculate centroids - Point3 aCentroid, bCentroid; - std::tie(aCentroid, bCentroid) = mean(abPointPairs); + auto centroids = mean(abPointPairs); // substract centroids - std::vector d_abPointPairs = subtractCentroids(abPointPairs, aCentroid, bCentroid); + std::vector d_abPointPairs = subtractCentroids(abPointPairs, centroids); // form H matrix Matrix3 H = calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return align(d_abPointPairs, aRb, aCentroid, bCentroid); + return align(d_abPointPairs, aRb, centroids); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { From 8236d69fa17fc1d5164d032f820e030126ebcc6c Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 14:09:37 -0400 Subject: [PATCH 052/136] Refactor code to increase speed. --- gtsam_unstable/geometry/Similarity3.cpp | 116 +++++++++++------------- 1 file changed, 54 insertions(+), 62 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 6fc720e75..b2d7dc080 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,61 +23,56 @@ namespace gtsam { -namespace{ - /// Subtract centroids from point pairs. - static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3Pair& centroids) { - std::vector d_abPointPairs; - for (const Point3Pair& abPair : abPointPairs) { - Point3 da = abPair.first - centroids.first; - Point3 db = abPair.second - centroids.second; - d_abPointPairs.emplace_back(da, db); - } - return d_abPointPairs; - } - - /// Form inner products x and y and calculate scale. - static const double calculateScale(const std::vector& d_abPointPairs, const Rot3& aRb) { - double x = 0, y = 0; - Point3 da, db; - for (const Point3Pair& d_abPair : d_abPointPairs) { - std::tie(da, db) = d_abPair; - const Vector3 da_prime = aRb * db; - y += da.transpose() * da_prime; - x += da_prime.transpose() * da_prime; - } - const double s = y / x; - return s; - } - - /// Form outer product H. - static Matrix3 calculateH(const std::vector& d_abPointPairs) { - Matrix3 H = Z_3x3; - Point3 da, db; - for (const Point3Pair& d_abPair : d_abPointPairs) { - std::tie(da, db) = d_abPair; - H += da * db.transpose(); - } - return H; - } - - /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. - static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { - const double s = calculateScale(d_abPointPairs, aRb); - // calculate translation - const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; - return Similarity3(aRb, aTb, s); - } - - /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. - static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { - // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 - auto centroids = mean(abPointPairs); - // substract centroids - std::vector d_abPointPairs = subtractCentroids(abPointPairs, centroids); - return align(d_abPointPairs, aRb, centroids); +namespace { +/// Subtract centroids from point pairs. +static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3Pair& centroids) { + std::vector d_abPointPairs; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - centroids.first; + Point3 db = abPair.second - centroids.second; + d_abPointPairs.emplace_back(da, db); } + return d_abPointPairs; } +/// Form inner products x and y and calculate scale. +static const double calculateScale(const std::vector& d_abPointPairs, const Rot3& aRb) { + double x = 0, y = 0; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + const Vector3 da_prime = aRb * db; + y += da.transpose() * da_prime; + x += da_prime.transpose() * da_prime; + } + const double s = y / x; + return s; +} + +/// Form outer product H. +static Matrix3 calculateH(const std::vector& d_abPointPairs) { + Matrix3 H = Z_3x3; + for (const Point3Pair& d_abPair : d_abPointPairs) { + H += d_abPair.first * d_abPair.second.transpose(); + } + return H; +} + +/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. +static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { + const double s = calculateScale(d_abPointPairs, aRb); + const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; + return Similarity3(aRb, aTb, s); +} + +/// This method estimates the similarity transform from point pairs, given a known or estimated rotation. +// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 +static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { + auto centroids = mean(abPointPairs); + auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + return align(d_abPointPairs, aRb, centroids); +} +} // namespace Similarity3::Similarity3() : t_(0,0,0), s_(1) { @@ -154,13 +149,9 @@ Point3 Similarity3::operator*(const Point3& p) const { Similarity3 Similarity3::Align(const std::vector& abPointPairs) { // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 - const size_t n = abPointPairs.size(); - if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs - // calculate centroids + if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); auto centroids = mean(abPointPairs); - // substract centroids - std::vector d_abPointPairs = subtractCentroids(abPointPairs, centroids); - // form H matrix + auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); Matrix3 H = calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); @@ -169,19 +160,20 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { Similarity3 Similarity3::Align(const std::vector& abPosePairs) { const size_t n = abPosePairs.size(); - if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs + if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // calculate rotation - vector rotationList; + vector rotations; vector abPointPairs; + rotations.reserve(n); abPointPairs.reserve(n); Pose3 wTa, wTb; for (const Pose3Pair& abPair : abPosePairs) { std::tie(wTa, wTb) = abPair; - rotationList.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); } - const Rot3 aRb = FindKarcherMean(rotationList); + const Rot3 aRb = FindKarcherMean(rotations); return alignGivenR(abPointPairs, aRb); } From 5defa4c278aa00bbb46a355916538d44fdbbae8d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 26 Sep 2020 14:43:39 -0400 Subject: [PATCH 053/136] wrapped BearingRange::Measure method and removed deprecated classes --- gtsam/gtsam.i | 96 +-------------------------------------------------- 1 file changed, 1 insertion(+), 95 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 6492b70b1..d9aa23893 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -334,99 +334,6 @@ virtual class GenericValue : gtsam::Value { void serializable() const; }; -#include -class LieScalar { - // Standard constructors - LieScalar(); - LieScalar(double d); - - // Standard interface - double value() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieScalar& expected, double tol) const; - - // Group - static gtsam::LieScalar identity(); - gtsam::LieScalar inverse() const; - gtsam::LieScalar compose(const gtsam::LieScalar& p) const; - gtsam::LieScalar between(const gtsam::LieScalar& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieScalar retract(Vector v) const; - Vector localCoordinates(const gtsam::LieScalar& t2) const; - - // Lie group - static gtsam::LieScalar Expmap(Vector v); - static Vector Logmap(const gtsam::LieScalar& p); -}; - -#include -class LieVector { - // Standard constructors - LieVector(); - LieVector(Vector v); - - // Standard interface - Vector vector() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieVector& expected, double tol) const; - - // Group - static gtsam::LieVector identity(); - gtsam::LieVector inverse() const; - gtsam::LieVector compose(const gtsam::LieVector& p) const; - gtsam::LieVector between(const gtsam::LieVector& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieVector retract(Vector v) const; - Vector localCoordinates(const gtsam::LieVector& t2) const; - - // Lie group - static gtsam::LieVector Expmap(Vector v); - static Vector Logmap(const gtsam::LieVector& p); - - // enabling serialization functionality - void serialize() const; -}; - -#include -class LieMatrix { - // Standard constructors - LieMatrix(); - LieMatrix(Matrix v); - - // Standard interface - Matrix matrix() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieMatrix& expected, double tol) const; - - // Group - static gtsam::LieMatrix identity(); - gtsam::LieMatrix inverse() const; - gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; - gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::LieMatrix & t2) const; - - // Lie group - static gtsam::LieMatrix Expmap(Vector v); - static Vector Logmap(const gtsam::LieMatrix& p); - - // enabling serialization functionality - void serialize() const; -}; - //************************************************************************* // geometry //************************************************************************* @@ -2705,8 +2612,7 @@ class BearingRange { BearingRange(const BEARING& b, const RANGE& r); BEARING bearing() const; RANGE range() const; - //TODO need to update wrap to allow self referencing of class - // static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); + static This 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; From 9bdcd2caa73548307ca867ef10bae78e0d1e406a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 26 Sep 2020 18:01:26 -0400 Subject: [PATCH 054/136] remove all LieScalar/LieVector/LieMatrix references from wrapper --- gtsam_unstable/gtsam_unstable.i | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 05b30bb0b..e18d32b59 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -5,8 +5,6 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; class gtsam::Vector6; -class gtsam::LieScalar; -class gtsam::LieVector; class gtsam::Point2; class gtsam::Point2Vector; class gtsam::Rot2; @@ -476,14 +474,12 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; -#include - #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { /** Standard constructor */ VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt); - Vector evaluateError(const gtsam::LieScalar& x1, const gtsam::LieScalar& x2, const gtsam::LieScalar& v) const; + Vector evaluateError(const double& x1, const double& x2, const double& v) const; }; #include @@ -491,7 +487,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt); - Vector evaluateError(const gtsam::LieScalar& qk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& v) const; + Vector evaluateError(const double& qk1, const double& qk, const double& v) const; }; #include @@ -499,21 +495,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g); - Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const; + Vector evaluateError(const double& vk1, const double& vk, const double& q) const; }; virtual class PendulumFactorPk : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); - Vector evaluateError(const gtsam::LieScalar& pk, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; + Vector evaluateError(const double& pk, const double& qk, const double& qk1) const; }; virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); - Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; + Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const; }; #include From ff9dc99ddfdca78cff874e26d6cb3b05e393ba9d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 26 Sep 2020 18:01:42 -0400 Subject: [PATCH 055/136] MATLAB readme update --- matlab/README.md | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/matlab/README.md b/matlab/README.md index 86e7d9fe0..39053364d 100644 --- a/matlab/README.md +++ b/matlab/README.md @@ -4,16 +4,16 @@ http://borg.cc.gatech.edu/projects/gtsam This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake. -The interface to the toolbox is generated automatically by the wrap -tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. +The interface to the toolbox is generated automatically by the wrap tool which directly parses C++ header files. +The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. +The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. ## Ubuntu -If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths: - - /usr/local/MATLAB/[version]/sys/os/[system]/ - /usr/local/MATLAB/[version]/bin/[system]/ +If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths: + /usr/local/MATLAB/[version]/sys/os/[system]/ + /usr/local/MATLAB/[version]/bin/[system]/ ## Adding the toolbox to your MATLAB path @@ -37,25 +37,28 @@ export LD_LIBRARY_PATH=/gtsam:$LD_LIBRARY_PATH ### Linker issues -If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB +If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get any of the following error messages in MATLAB + ``` Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64': Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' ... ``` + run following shell line + ```sh export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 ``` -before you run MATLAB from the same shell. + +before you run MATLAB from the same shell. This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a. - ## Trying out the examples -The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: +The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: ```matlab >> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox @@ -65,7 +68,7 @@ The examples are located in the 'gtsam_examples' subfolder. You may either run ## Running the unit tests -The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example: +The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example: ```matlab >> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox @@ -86,4 +89,4 @@ Tests complete! ## Writing your own code -Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started. +Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started. From 94bb0fb6df5575ec3b0c1bcc086f887c43a8564a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 26 Sep 2020 18:02:07 -0400 Subject: [PATCH 056/136] remove Point3 serialization test since Point3 is now just a vector --- matlab/gtsam_tests/testSerialization.m | 4 ---- 1 file changed, 4 deletions(-) diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index f8b21b7ad..e55822c1c 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -28,10 +28,6 @@ serialized_pose1 = pose1.string_serialize(); pose1ds = Pose2.string_deserialize(serialized_pose1); CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9)); -serialized_landmark1 = landmark1.string_serialize(); -landmark1ds = Point2.string_deserialize(serialized_landmark1); -CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9)); - %% Create and serialize Values values = Values; values.insert(i1, pose1); From 95724be4ae170bfa2860d854a61405d293109948 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 27 Sep 2020 20:36:50 -0400 Subject: [PATCH 057/136] Fix quaternions test failure. --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 01e33b330..b985eb374 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -264,11 +264,11 @@ TEST(Similarity3, GroupActionPose3) { // Create source poses Pose3 Ta1(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); // Create destination poses Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 expected_Tb2(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); @@ -283,11 +283,11 @@ TEST(Similarity3, GroupActionPose3_Compatibility) { // Create poses Pose3 Ta1(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 Tb2(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12)); - Pose3 Tc2(Rot3(0, 0, -1, 0, 1, 0, -1, 0, 0), Point3(0, 6, 12)); + Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12)); EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1))); EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2))); @@ -350,11 +350,11 @@ TEST(Similarity3, AlignPose3) { // Create source poses Pose3 Ta1(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); // Create destination poses Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 Tb2(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); Pose3Pair bTa1(make_pair(Tb1, Ta1)); Pose3Pair bTa2(make_pair(Tb2, Ta2)); From 98404ad27e583ef72fca03aa2f46f9f89fc31254 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 27 Sep 2020 18:55:14 -0700 Subject: [PATCH 058/136] updating defaultdict init --- python/gtsam/examples/TranslationAveragingExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index c3dcf2ad2..0f1314645 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -89,7 +89,7 @@ def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, # where weights are proportional to the probability of the edge being an outlier. - avg_outlier_weights = defaultdict(lambda: 0.0) + avg_outlier_weights = defaultdict(float) for outlier_weight_dict in outlier_weights: for keypair, weight in outlier_weight_dict.items(): avg_outlier_weights[keypair] += weight / len(outlier_weights) From 49ba9a79907da70d11a4316ae1af8fc3f5b323a5 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Mon, 28 Sep 2020 20:52:31 -0400 Subject: [PATCH 059/136] Throw an exception when n=0 . --- gtsam/geometry/Point3.cpp | 1 + gtsam/geometry/Point3.h | 1 + 2 files changed, 2 insertions(+) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce4ceee89..783741262 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -77,6 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, Point3Pair mean(const std::vector &abPointPairs) { const size_t n = abPointPairs.size(); + if (n == 0) throw std::invalid_argument("input should have at least 1 pair of points"); Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); for (const Point3Pair &abPair : abPointPairs) { aCentroid += abPair.first; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7f58497e9..b400470ea 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -62,6 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, /// mean template GTSAM_EXPORT Point3 mean(const CONTAINER& points) { + if (points.size() == 0) throw std::invalid_argument("input should have at least 1 point"); Point3 sum(0, 0, 0); sum = std::accumulate(points.begin(), points.end(), sum); return sum / points.size(); From 3743202a0daa2f1c356ef8ee9f47fd04593d4482 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 01:50:04 -0400 Subject: [PATCH 060/136] use implicit function theorem to compute jacobians of Cal3Bundler::calibrate --- gtsam/geometry/Cal3Bundler.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 470c97c75..dad2a78d1 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -121,23 +121,23 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - x = invKPi.x(), y = invKPi.y(); - const double xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double inv_f = 1 / f_, inv_g = 1 / g; + // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate + // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians + // df/pi = -I (pn and pi are independent args) + // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + Matrix H_uncal_K, H_uncal_pn; - // Approximate the jacobians via a single iteration of g. - if (Dcal) { - *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, - -y * inv_g * rr, -y * inv_g * rr * rr; + if (Dcal || Dp) { + // Compute uncalibrate Jacobians + uncalibrate(pn, H_uncal_K, H_uncal_pn); + } + + if (Dcal) { + *Dcal = -H_uncal_pn.inverse() * H_uncal_K; } if (Dp) { - const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); - const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); - - *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv, - -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv); + *Dp = H_uncal_pn.inverse(); } return pn; From c52c217592d9195b16b232354ef78e5d3f398dc8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 01:50:14 -0400 Subject: [PATCH 061/136] added test for Cal3Bundler::calibrate Jacobians1 --- gtsam/geometry/tests/testCal3Bundler.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 8de049fa4..448600266 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( traits::Equals(pn, pn_hat, 1e-5)); + CHECK(traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } + /* ************************************************************************* */ TEST( Cal3Bundler, Duncalibrate) { @@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate) Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical1,Dcal,1e-7)); CHECK(assert_equal(numerical2,Dp,1e-7)); - CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7)); - CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7)); - Matrix Dcombined(2,5); - Dcombined << Dp, Dcal; - CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, Dcalibrate) +{ + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Point2 actual = K.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical1,Dcal,1e-5)); + CHECK(assert_equal(numerical2,Dp,1e-5)); } /* ************************************************************************* */ From 8634d3f42ea2202d34a16f4d295935f03a28873a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 01:50:21 -0400 Subject: [PATCH 062/136] make Cal3Bundler wrapper constructor backwards compatible --- gtsam/gtsam.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 369ea983c..5be275133 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1053,6 +1053,7 @@ class Cal3_S2Stereo { class Cal3Bundler { // Standard Constructors Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable From 7dab718861880fcdec4417ceb5133843f9e58a52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 10:49:44 -0400 Subject: [PATCH 063/136] fixed sized matrices and minor improvements --- gtsam/geometry/Cal3Bundler.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index dad2a78d1..e986bc820 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -126,20 +126,17 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, // df/pi = -I (pn and pi are independent args) // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K - Matrix H_uncal_K, H_uncal_pn; + Matrix23 H_uncal_K; + Matrix22 H_uncal_pn; if (Dcal || Dp) { // Compute uncalibrate Jacobians - uncalibrate(pn, H_uncal_K, H_uncal_pn); - } + uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); - if (Dcal) { - *Dcal = -H_uncal_pn.inverse() * H_uncal_K; - } - if (Dp) { - *Dp = H_uncal_pn.inverse(); - } + if (Dp) *Dp = H_uncal_pn.inverse(); + if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K; + } return pn; } From 31ad107053b148bec48b931b08afb1e16df273c2 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Tue, 29 Sep 2020 12:04:42 -0400 Subject: [PATCH 064/136] Modify error message to be more descriptive. --- gtsam/geometry/Point3.cpp | 2 +- gtsam/geometry/Point3.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 783741262..9ff220e63 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -77,7 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, Point3Pair mean(const std::vector &abPointPairs) { const size_t n = abPointPairs.size(); - if (n == 0) throw std::invalid_argument("input should have at least 1 pair of points"); + if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty"); Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); for (const Point3Pair &abPair : abPointPairs) { aCentroid += abPair.first; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b400470ea..510a5fa80 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -62,7 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, /// mean template GTSAM_EXPORT Point3 mean(const CONTAINER& points) { - if (points.size() == 0) throw std::invalid_argument("input should have at least 1 point"); + if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty"); Point3 sum(0, 0, 0); sum = std::accumulate(points.begin(), points.end(), sum); return sum / points.size(); From 0ae5a92d424d52ba4f2b378da37a9f9f54e76b8a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 12:57:55 -0400 Subject: [PATCH 065/136] compute inverse only once --- gtsam/geometry/Cal3Bundler.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index e986bc820..b198643b0 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -127,16 +127,19 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K Matrix23 H_uncal_K; - Matrix22 H_uncal_pn; + Matrix22 H_uncal_pn, H_uncal_pn_inv; if (Dcal || Dp) { // Compute uncalibrate Jacobians uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); - if (Dp) *Dp = H_uncal_pn.inverse(); - if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K; + H_uncal_pn_inv = H_uncal_pn.inverse(); + + if (Dp) *Dp = H_uncal_pn_inv; + if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; } + return pn; } From bb08c62693c47ef6b6478d648a1d93c19e2a7c69 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 15:51:47 -0400 Subject: [PATCH 066/136] wrap PinholeCameraCal3Bundler --- gtsam/gtsam.i | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5be275133..dc7c2add2 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1201,7 +1201,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -//typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; #include class StereoCamera { @@ -2862,8 +2862,7 @@ class SfmTrack { class SfmData { size_t number_cameras() const; size_t number_tracks() const; - //TODO(Varun) Need to fix issue #237 first before this can work - // gtsam::PinholeCamera camera(size_t idx) const; + gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; }; From c0fb3a271be928dd48b21267ab764087c4b69a97 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Sep 2020 15:38:08 -0400 Subject: [PATCH 067/136] Small formatting changes and removal of test header --- gtsam_unstable/linear/ActiveSetSolver-inl.h | 22 +++++++++------------ gtsam_unstable/linear/ActiveSetSolver.h | 4 ++-- gtsam_unstable/linear/LPInitSolver.h | 3 +-- 3 files changed, 12 insertions(+), 17 deletions(-) diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h index 602012090..12374ac76 100644 --- a/gtsam_unstable/linear/ActiveSetSolver-inl.h +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor( // to compute the least-square approximation of dual variables return boost::make_shared(Aterms, b); } else { - return boost::make_shared(); + return nullptr; } } @@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor( * if lambda = 0 you are on the constraint * if lambda > 0 you are violating the constraint. */ -Template GaussianFactorGraph::shared_ptr This::buildDualGraph( +Template GaussianFactorGraph This::buildDualGraph( const InequalityFactorGraph& workingSet, const VectorValues& delta) const { - GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + GaussianFactorGraph dualGraph; for (Key key : constrainedKeys_) { // Each constrained key becomes a factor in the dual graph - JacobianFactor::shared_ptr dualFactor = - createDualFactor(key, workingSet, delta); - if (!dualFactor->empty()) dualGraph->push_back(dualFactor); + auto dualFactor = createDualFactor(key, workingSet, delta); + if (dualFactor) dualGraph.push_back(dualFactor); } return dualGraph; } @@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet, Template typename This::State This::iterate( const typename This::State& state) const { // Algorithm 16.3 from Nocedal06book. - // Solve with the current working set eqn 16.39, but instead of solving for p - // solve for x - GaussianFactorGraph workingGraph = - buildWorkingGraph(state.workingSet, state.values); + // Solve with the current working set eqn 16.39, but solve for x not p + auto workingGraph = buildWorkingGraph(state.workingSet, state.values); VectorValues newValues = workingGraph.optimize(); // If we CAN'T move further // if p_k = 0 is the original condition, modified by Duy to say that the state // update is zero. if (newValues.equals(state.values, 1e-7)) { // Compute lambda from the dual graph - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, - newValues); - VectorValues duals = dualGraph->optimize(); + auto dualGraph = buildDualGraph(state.workingSet, newValues); + VectorValues duals = dualGraph.optimize(); int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); // If all inequality constraints are satisfied: We have the solution!! if (leavingFactor < 0) { diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 8c3c5a7e5..318912cf3 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -154,8 +154,8 @@ protected: public: /// Just for testing... /// Builds a dual graph from the current working set. - GaussianFactorGraph::shared_ptr buildDualGraph( - const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet, + const VectorValues &delta) const; /** * Build a working graph of cost, equality and active inequality constraints diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 4eb672fbc..14e5fb000 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -21,7 +21,6 @@ #include #include -#include namespace gtsam { /** @@ -83,7 +82,7 @@ private: const InequalityFactorGraph& inequalities) const; // friend class for unit-testing private methods - FRIEND_TEST(LPInitSolver, initialization); + friend class LPInitSolverInitializationTest; }; } From c51264ac985954d26470bdc56d5805433f31ee3f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Sep 2020 15:38:25 -0400 Subject: [PATCH 068/136] New method "add" as in GaussianFactorGraph --- gtsam_unstable/linear/EqualityFactorGraph.h | 5 +++++ gtsam_unstable/linear/InequalityFactorGraph.h | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h index 43befdbe0..fb3f4c076 100644 --- a/gtsam_unstable/linear/EqualityFactorGraph.h +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -31,6 +31,11 @@ class EqualityFactorGraph: public FactorGraph { public: typedef boost::shared_ptr shared_ptr; + /// Add a linear inequality, forwards arguments to LinearInequality. + template void add(Args &&... args) { + emplace_shared(std::forward(args)...); + } + /// Compute error of a guess. double error(const VectorValues& x) const { double total_error = 0.; diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index c87645697..d042b0436 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -47,6 +47,11 @@ public: return Base::equals(other, tol); } + /// Add a linear inequality, forwards arguments to LinearInequality. + template void add(Args &&... args) { + emplace_shared(std::forward(args)...); + } + /** * Compute error of a guess. * Infinity error if it violates an inequality; zero otherwise. */ From 6b739b17be03292c4d477432b1b2ff6d399fd4c2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Sep 2020 15:38:46 -0400 Subject: [PATCH 069/136] Re-formatting and using "add"/"auto" where we can. --- gtsam_unstable/linear/tests/testLPSolver.cpp | 138 ++++----- gtsam_unstable/linear/tests/testQPSolver.cpp | 307 +++++++++---------- 2 files changed, 199 insertions(+), 246 deletions(-) diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index a105a39f0..de9cd032a 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -16,20 +16,20 @@ * @author Duy-Nguyen Ta */ -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include -#include #include +#include using namespace std; using namespace gtsam; @@ -47,37 +47,27 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1); */ LP simpleLP1() { LP lp; - lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) - lp.inequalities.push_back( - LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0 - lp.inequalities.push_back( - LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0 - lp.inequalities.push_back( - LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4 - lp.inequalities.push_back( - LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12 - lp.inequalities.push_back( - LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1 + lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) + lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0 + lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0 + lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4 + lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12 + lp.inequalities.add(1, Vector2(-1, 1), 1, 5); // -x1 + x2 <= 1 return lp; } /* ************************************************************************* */ namespace gtsam { -TEST(LPInitSolver, infinite_loop_single_var) { - LP initchecker; - initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(1, 0, -1), 20, 4)); // x - alpha <= 20 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(0, -1, -1), 0, 5)); // -y - alpha <= 0 - LPSolver solver(initchecker); +TEST(LPInitSolver, InfiniteLoopSingleVar) { + LP lp; + lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha + lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2 + lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6 + lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0 + lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20 + lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0 + LPSolver solver(lp); VectorValues starter; starter.insert(1, Vector3(0, 0, 2)); VectorValues results, duals; @@ -87,25 +77,23 @@ TEST(LPInitSolver, infinite_loop_single_var) { CHECK(assert_equal(results, expected, 1e-7)); } -TEST(LPInitSolver, infinite_loop_multi_var) { - LP initchecker; +TEST(LPInitSolver, InfiniteLoopMultiVar) { + LP lp; Key X = symbol('X', 1); Key Y = symbol('Y', 1); Key Z = symbol('Z', 1); - initchecker.cost = LinearCost(Z, kOne); // min alpha - initchecker.inequalities.push_back( - LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, - 1)); //-2x-y-alpha <= -2 - initchecker.inequalities.push_back( - LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, - 2)); // -x+2y-alpha <= 6 - initchecker.inequalities.push_back(LinearInequality( - X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0 - initchecker.inequalities.push_back(LinearInequality( - X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20 - initchecker.inequalities.push_back(LinearInequality( - Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0 - LPSolver solver(initchecker); + lp.cost = LinearCost(Z, kOne); // min alpha + lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, + 1); //-2x-y-alpha <= -2 + lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, + 2); // -x+2y-alpha <= 6 + lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0, + 3); // -x - alpha <= 0 + lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20, + 4); // x - alpha <= 20 + lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0, + 5); // -y - alpha <= 0 + LPSolver solver(lp); VectorValues starter; starter.insert(X, kZero); starter.insert(Y, kZero); @@ -119,7 +107,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) { CHECK(assert_equal(results, expected, 1e-7)); } -TEST(LPInitSolver, initialization) { +TEST(LPInitSolver, Initialization) { LP lp = simpleLP1(); LPInitSolver initSolver(lp); @@ -138,19 +126,19 @@ TEST(LPInitSolver, initialization) { LP::shared_ptr initLP = initSolver.buildInitialLP(yKey); LP expectedInitLP; expectedInitLP.cost = LinearCost(yKey, kOne); - expectedInitLP.inequalities.push_back(LinearInequality( - 1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 - expectedInitLP.inequalities.push_back(LinearInequality( - 1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 - expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4, - 3)); // x1 + 2*x2 - y <= 4 - expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12, - 4)); // 4x1 + 2x2 - y <= 12 - expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1, - 5)); // -x1 + x2 - y <= 1 + expectedInitLP.inequalities.add(1, Vector2(-1, 0), 2, Vector::Constant(1, -1), + 0, 1); // -x1 - y <= 0 + expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1), + 0, 2); // -x2 - y <= 0 + expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1), + 4, + 3); // x1 + 2*x2 - y <= 4 + expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1), + 12, + 4); // 4x1 + 2x2 - y <= 12 + expectedInitLP.inequalities.add(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), + 1, + 5); // -x1 + x2 - y <= 1 CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); LPSolver lpSolveInit(*initLP); VectorValues xy0(x0); @@ -164,7 +152,7 @@ TEST(LPInitSolver, initialization) { VectorValues x = initSolver.solve(); CHECK(lp.isFeasible(x)); } -} +} // namespace gtsam /* ************************************************************************* */ /** @@ -173,28 +161,24 @@ TEST(LPInitSolver, initialization) { * x - y = 5 * x + 2y = 6 */ -TEST(LPSolver, overConstrainedLinearSystem) { +TEST(LPSolver, OverConstrainedLinearSystem) { GaussianFactorGraph graph; Matrix A1 = Vector3(1, 1, 1); Matrix A2 = Vector3(1, -1, 2); Vector b = Vector3(1, 5, 6); - JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); - graph.push_back(factor); + graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system - CHECK(factor.error(x) != 0.0); + CHECK(graph[0]->error(x) != 0.0); } TEST(LPSolver, overConstrainedLinearSystem2) { GaussianFactorGraph graph; - graph.emplace_shared(1, I_1x1, 2, I_1x1, kOne, - noiseModel::Constrained::All(1)); - graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, - noiseModel::Constrained::All(1)); - graph.emplace_shared(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, - noiseModel::Constrained::All(1)); + graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1)); + graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1)); + graph.add(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, noiseModel::Constrained::All(1)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system @@ -202,7 +186,7 @@ TEST(LPSolver, overConstrainedLinearSystem2) { } /* ************************************************************************* */ -TEST(LPSolver, simpleTest1) { +TEST(LPSolver, SimpleTest1) { LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues init; @@ -222,7 +206,7 @@ TEST(LPSolver, simpleTest1) { } /* ************************************************************************* */ -TEST(LPSolver, testWithoutInitialValues) { +TEST(LPSolver, TestWithoutInitialValues) { LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues result, duals, expectedResult; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 2292c63d7..d3497d2a3 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -17,11 +17,11 @@ * @author Ivan Dario Jimenez */ +#include #include #include -#include #include -#include +#include using namespace std; using namespace gtsam; @@ -40,15 +40,15 @@ QP createTestCase() { // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation // Should be 0.5x'Gx + gx + f : Nocedal 449 - qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, - Z_1x1, 10.0)); + qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, + 2.0 * I_1x1, Z_1x1, 10.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 + qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, + 0); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.add(X(1), -I_1x1, 0, 1); // -x1 <= 0 + qp.inequalities.add(X(2), -I_1x1, 0, 2); // -x2 <= 0 + qp.inequalities.add(X(1), I_1x1, 1.5, 3); // x1 <= 3/2 return qp; } @@ -94,16 +94,15 @@ QP createEqualityConstrainedTest() { // Note the Hessian encodes: // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 - qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, - 0.0)); + qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, + 2.0 * I_1x1, Z_1x1, 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector Matrix A1 = I_1x1; Matrix A2 = I_1x1; Vector b = -kOne; - qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); + qp.equalities.add(X(1), A1, X(2), A2, b, 0); return qp; } @@ -118,9 +117,8 @@ TEST(QPSolver, dual) { QPSolver solver(qp); - GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( - qp.inequalities, initialValues); - VectorValues dual = dualGraph->optimize(); + auto dualGraph = solver.buildDualGraph(qp.inequalities, initialValues); + VectorValues dual = dualGraph.optimize(); VectorValues expectedDual; expectedDual.insert(0, (Vector(1) << 2.0).finished()); CHECK(assert_equal(expectedDual, dual, 1e-10)); @@ -135,19 +133,19 @@ TEST(QPSolver, indentifyActiveConstraints) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - InequalityFactorGraph workingSet = solver.identifyActiveConstraints( - qp.inequalities, currentSolution); + auto workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); CHECK(!workingSet.at(0)->active()); // inactive - CHECK(workingSet.at(1)->active());// active - CHECK(workingSet.at(2)->active());// active - CHECK(!workingSet.at(3)->active());// inactive + CHECK(workingSet.at(1)->active()); // active + CHECK(workingSet.at(2)->active()); // active + CHECK(!workingSet.at(3)->active()); // inactive VectorValues solution = solver.buildWorkingGraph(workingSet).optimize(); - VectorValues expectedSolution; - expectedSolution.insert(X(1), kZero); - expectedSolution.insert(X(2), kZero); - CHECK(assert_equal(expectedSolution, solution, 1e-100)); + VectorValues expected; + expected.insert(X(1), kZero); + expected.insert(X(2), kZero); + CHECK(assert_equal(expected, solution, 1e-100)); } /* ************************************************************************* */ @@ -159,24 +157,24 @@ TEST(QPSolver, iterate) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - std::vector expectedSolutions(4), expectedDuals(4); - expectedSolutions[0].insert(X(1), kZero); - expectedSolutions[0].insert(X(2), kZero); + std::vector expecteds(4), expectedDuals(4); + expecteds[0].insert(X(1), kZero); + expecteds[0].insert(X(2), kZero); expectedDuals[0].insert(1, (Vector(1) << 3).finished()); expectedDuals[0].insert(2, kZero); - expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[1].insert(X(2), kZero); + expecteds[1].insert(X(1), (Vector(1) << 1.5).finished()); + expecteds[1].insert(X(2), kZero); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); - expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished()); + expecteds[2].insert(X(1), (Vector(1) << 1.5).finished()); + expecteds[2].insert(X(2), (Vector(1) << 0.75).finished()); - expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); + expecteds[3].insert(X(1), (Vector(1) << 1.5).finished()); + expecteds[3].insert(X(2), (Vector(1) << 0.5).finished()); - InequalityFactorGraph workingSet = solver.identifyActiveConstraints( - qp.inequalities, currentSolution); + auto workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); QPSolver::State state(currentSolution, VectorValues(), workingSet, false, 100); @@ -188,12 +186,12 @@ TEST(QPSolver, iterate) { // Forst10book do not follow exactly what we implemented from Nocedal06book. // Specifically, we do not re-identify active constraints and // do not recompute dual variables after every step!!! -// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); -// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); + // CHECK(assert_equal(expecteds[it], state.values, 1e-10)); + // CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); it++; } - CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); + CHECK(assert_equal(expecteds[3], state.values, 1e-10)); } /* ************************************************************************* */ @@ -204,182 +202,161 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { VectorValues initialValues; initialValues.insert(X(1), Z_1x1); initialValues.insert(X(2), Z_1x1); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-100)); + VectorValues solution = solver.optimize(initialValues).first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 1.5).finished()); + expected.insert(X(2), (Vector(1) << 0.5).finished()); + CHECK(assert_equal(expected, solution, 1e-100)); } pair testParser(QPSParser parser) { QP exampleqp = parser.Parse(); - QP expectedqp; + QP expected; Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 - expectedqp.cost.push_back( - HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, - 2.0 * kOne, 8.0)); - // 2x + y >= 2 - // -x + 2y <= 6 - expectedqp.inequalities.push_back( - LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0)); - expectedqp.inequalities.push_back( - LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1)); - // x<= 20 - expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4)); - //x >= 0 - expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2)); - // y > = 0 - expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3)); - return std::make_pair(expectedqp, exampleqp); -} -; + expected.cost.push_back(HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, + -1.5 * kOne, 10.0 * I_1x1, 2.0 * kOne, + 8.0)); + + expected.inequalities.add(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0); // 2x + y >= 2 + expected.inequalities.add(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1); // -x + 2y <= 6 + expected.inequalities.add(X1, I_1x1, 20, 4); // x<= 20 + expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0 + expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0 + return {expected, exampleqp}; +}; TEST(QPSolver, ParserSyntaticTest) { - auto expectedActual = testParser(QPSParser("QPExample.QPS")); - CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, + auto result = testParser(QPSParser("QPExample.QPS")); + CHECK(assert_equal(result.first.cost, result.second.cost, 1e-7)); + CHECK(assert_equal(result.first.inequalities, result.second.inequalities, 1e-7)); - CHECK(assert_equal(expectedActual.first.inequalities, - expectedActual.second.inequalities, 1e-7)); - CHECK(assert_equal(expectedActual.first.equalities, - expectedActual.second.equalities, 1e-7)); + CHECK(assert_equal(result.first.equalities, result.second.equalities, 1e-7)); } TEST(QPSolver, ParserSemanticTest) { - auto expected_actual = testParser(QPSParser("QPExample.QPS")); - VectorValues actualSolution, expectedSolution; - boost::tie(expectedSolution, boost::tuples::ignore) = - QPSolver(expected_actual.first).optimize(); - boost::tie(actualSolution, boost::tuples::ignore) = - QPSolver(expected_actual.second).optimize(); - CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); + auto result = testParser(QPSParser("QPExample.QPS")); + VectorValues expected = QPSolver(result.first).optimize().first; + VectorValues actual = QPSolver(result.second).optimize().first; + CHECK(assert_equal(actual, expected, 1e-7)); } -TEST(QPSolver, QPExampleTest){ +TEST(QPSolver, QPExampleTest) { QP problem = QPSParser("QPExample.QPS").Parse(); - VectorValues actualSolution; auto solver = QPSolver(problem); - boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize(); - VectorValues expectedSolution; - expectedSolution.insert(Symbol('X',1),0.7625*I_1x1); - expectedSolution.insert(Symbol('X',2),0.4750*I_1x1); - double error_expected = problem.cost.error(expectedSolution); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(expectedSolution, actualSolution, 1e-7)) + VectorValues actual = solver.optimize().first; + VectorValues expected; + expected.insert(Symbol('X', 1), 0.7625 * I_1x1); + expected.insert(Symbol('X', 2), 0.4750 * I_1x1); + double error_expected = problem.cost.error(expected); + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(expected, actual, 1e-7)) CHECK(assert_equal(error_expected, error_actual)) } TEST(QPSolver, HS21) { QP problem = QPSParser("HS21.QPS").Parse(); - VectorValues actualSolution; - VectorValues expectedSolution; - expectedSolution.insert(Symbol('X',1), 2.0*I_1x1); - expectedSolution.insert(Symbol('X',2), 0.0*I_1x1); - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); + VectorValues expected; + expected.insert(Symbol('X', 1), 2.0 * I_1x1); + expected.insert(Symbol('X', 2), 0.0 * I_1x1); + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); CHECK(assert_equal(-99.9599999, error_actual, 1e-7)) - CHECK(assert_equal(expectedSolution, actualSolution)) + CHECK(assert_equal(expected, actual)) } TEST(QPSolver, HS35) { QP problem = QPSParser("HS35.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(1.11111111e-01, error_actual, 1e-7)) } TEST(QPSolver, HS35MOD) { QP problem = QPSParser("HS35MOD.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(2.50000001e-01, error_actual, 1e-7)) } TEST(QPSolver, HS51) { QP problem = QPSParser("HS51.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(8.88178420e-16, error_actual, 1e-7)) } TEST(QPSolver, HS52) { QP problem = QPSParser("HS52.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(5.32664756,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(5.32664756, error_actual, 1e-7)) } -TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest +TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of + // tolerance than the rest QP problem = QPSParser("HS268.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(5.73107049e-07, error_actual, 1e-6)) } TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix QP problem = QPSParser("QPTEST.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(0.437187500e01,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(0.437187500e01, error_actual, 1e-7)) } /* ************************************************************************* */ -// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html +// Create Matlab's test graph as in +// http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { QP qp; // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2 // Note the Hessian encodes: - // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + + // 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 - qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, - 6 * I_1x1, 1000.0)); + qp.cost.push_back(HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, + 2.0 * I_1x1, 6 * I_1x1, 1000.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 - qp.inequalities.push_back( - LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 - qp.inequalities.push_back( - LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0 + qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, 0); // x1 + x2 <= 2 + qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1); //-x1 + 2*x2 <=2 + qp.inequalities.add(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2); // 2*x1 + x2 <=3 + qp.inequalities.add(X(1), -I_1x1, 0, 3); // -x1 <= 0 + qp.inequalities.add(X(2), -I_1x1, 0, 4); // -x2 <= 0 return qp; } -///* ************************************************************************* */ +///* ************************************************************************* +///*/ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), Z_1x1); initialValues.insert(X(2), Z_1x1); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); - expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-7)); + VectorValues solution = solver.optimize(initialValues).first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expected, solution, 1e-7)); } -///* ************************************************************************* */ +///* ************************************************************************* +///*/ TEST(QPSolver, optimizeMatlabExNoinitials) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); - expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-7)); + VectorValues solution = solver.optimize().first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expected, solution, 1e-7)); } /* ************************************************************************* */ @@ -387,18 +364,15 @@ TEST(QPSolver, optimizeMatlabExNoinitials) { QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1)); - qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1)); + qp.cost.add(X(1), I_1x1, I_1x1); + qp.cost.add(X(2), I_1x1, 2.5 * I_1x1); // Inequality constraints - qp.inequalities.push_back( - LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0)); - qp.inequalities.push_back( - LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1)); - qp.inequalities.push_back( - LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2)); - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3)); - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4)); + qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0); + qp.inequalities.add(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1); + qp.inequalities.add(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2); + qp.inequalities.add(X(1), -I_1x1, 0.0, 3); + qp.inequalities.add(X(2), -I_1x1, 0.0, 4); return qp; } @@ -410,21 +384,19 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { initialValues.insert(X(1), (Vector(1) << 2.0).finished()); initialValues.insert(X(2), Z_1x1); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 1.4).finished()); - expectedSolution.insert(X(2), (Vector(1) << 1.7).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-7)); + VectorValues solution = solver.optimize(initialValues).first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 1.4).finished()); + expected.insert(X(2), (Vector(1) << 1.7).finished()); + CHECK(assert_equal(expected, solution, 1e-7)); } /* ************************************************************************* */ TEST(QPSolver, failedSubproblem) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); + qp.cost.add(X(1), I_2x2, Z_2x1); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); - qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); + qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); @@ -433,8 +405,7 @@ TEST(QPSolver, failedSubproblem) { initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); QPSolver solver(qp); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues solution = solver.optimize(initialValues).first; CHECK(assert_equal(expected, solution, 1e-7)); } @@ -442,10 +413,9 @@ TEST(QPSolver, failedSubproblem) { /* ************************************************************************* */ TEST(QPSolver, infeasibleInitial) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2))); + qp.cost.add(X(1), I_2x2, Vector::Zero(2)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0)); - qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); + qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); @@ -464,4 +434,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From dc1057f31405401d27abc2a040b041286795a395 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Sep 2020 15:41:43 -0400 Subject: [PATCH 070/136] Fixed spelling mistake --- gtsam_unstable/linear/tests/testQPSolver.cpp | 22 ++++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index d3497d2a3..285f19b3f 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -157,21 +157,21 @@ TEST(QPSolver, iterate) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - std::vector expecteds(4), expectedDuals(4); - expecteds[0].insert(X(1), kZero); - expecteds[0].insert(X(2), kZero); + std::vector expected(4), expectedDuals(4); + expected[0].insert(X(1), kZero); + expected[0].insert(X(2), kZero); expectedDuals[0].insert(1, (Vector(1) << 3).finished()); expectedDuals[0].insert(2, kZero); - expecteds[1].insert(X(1), (Vector(1) << 1.5).finished()); - expecteds[1].insert(X(2), kZero); + expected[1].insert(X(1), (Vector(1) << 1.5).finished()); + expected[1].insert(X(2), kZero); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); - expecteds[2].insert(X(1), (Vector(1) << 1.5).finished()); - expecteds[2].insert(X(2), (Vector(1) << 0.75).finished()); + expected[2].insert(X(1), (Vector(1) << 1.5).finished()); + expected[2].insert(X(2), (Vector(1) << 0.75).finished()); - expecteds[3].insert(X(1), (Vector(1) << 1.5).finished()); - expecteds[3].insert(X(2), (Vector(1) << 0.5).finished()); + expected[3].insert(X(1), (Vector(1) << 1.5).finished()); + expected[3].insert(X(2), (Vector(1) << 0.5).finished()); auto workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); @@ -186,12 +186,12 @@ TEST(QPSolver, iterate) { // Forst10book do not follow exactly what we implemented from Nocedal06book. // Specifically, we do not re-identify active constraints and // do not recompute dual variables after every step!!! - // CHECK(assert_equal(expecteds[it], state.values, 1e-10)); + // CHECK(assert_equal(expected[it], state.values, 1e-10)); // CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); it++; } - CHECK(assert_equal(expecteds[3], state.values, 1e-10)); + CHECK(assert_equal(expected[3], state.values, 1e-10)); } /* ************************************************************************* */ From 634682738e085d1a3c7d4ab2efdf9716971e1327 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 30 Sep 2020 23:25:20 -0700 Subject: [PATCH 071/136] renaming variables --- .../examples/TranslationAveragingExample.py | 116 +++++++++--------- 1 file changed, 60 insertions(+), 56 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 0f1314645..a374dc630 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -22,99 +22,103 @@ import numpy as np import gtsam from gtsam.examples import SFMdata +# Hyperparameters for 1dsfm, values used from Kyle Wilson's code. +MAX_1DSFM_PROJECTION_DIRECTIONS = 48 +OUTLIER_WEIGHT_THRESHOLD = 0.1 + def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: - """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" + """"Returns global rotations and unit translation directions between 8 cameras + that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata + and the unit translations directions between some camera pairs are computed from their + global translations. """ # Using toy dataset in SfMdata for example. - poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) - # Rotations of the cameras in the world frame - wRc. - rotations = gtsam.Values() - # Normalized translation directions for pairs of cameras - from first camera to second, - # in the coordinate frame of the first camera. - translation_directions = [] - for i in range(0, len(poses) - 2): + wTc = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + # Rotations of the cameras in the world frame. + wRc_values = gtsam.Values() + # Normalized translation directions from camera i to camera j + # in the coordinate frame of camera i. + i_iZj_list = [] + for i in range(0, len(wTc) - 2): # Add the rotation. - rotations.insert(i, poses[i].rotation()) + wRc_values.insert(i, wTc[i].rotation()) # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): - i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate( - poses[j].translation() - poses[i].translation())) - translation_directions.append(gtsam.BinaryMeasurementUnit3( - i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + i_iZj = gtsam.Unit3(wTc[i].rotation().unrotate( + wTc[j].translation() - wTc[i].translation())) + i_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. - rotations.insert(len(poses) - 1, poses[-1].rotation()) - rotations.insert(len(poses) - 2, poses[-2].rotation()) - return (rotations, translation_directions) + wRc_values.insert(len(wTc) - 1, wTc[-1].rotation()) + wRc_values.insert(len(wTc) - 2, wTc[-2].rotation()) + return (wRc_values, i_iZj_list) -def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, - rotations: gtsam.Values) -> gtsam.Values: - """Estimate poses given rotations normalized translation directions between cameras. +def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, + wRc_values: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations and normalized translation directions between cameras. Args: - relative_translations -- List of normalized translation directions between camera pairs, each direction - is from the first camera to the second, in the frame of the first camera. - rotations -- Rotations of the cameras in the world frame. + iZj_list -- List of normalized translation direction measurements between camera pairs, + Z here refers to measurements. The measurements are of camera j with reference + to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit + vector to j in i's frame and is not a transformation. + wRc_values -- Rotations of the cameras in the world frame. Returns: Values -- Estimated poses. """ - # Some hyperparameters, values used from 1dsfm. - max_1dsfm_projection_directions = 48 - outlier_weight_threshold = 0.1 + # Convert the translation direction measurements to world frame using the rotations. + w_iZj_list = gtsam.BinaryMeasurementsUnit3() + for i_iZj in i_iZj_list: + w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) + .rotate(i_iZj.measured().point3())) + w_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) - # Convert the translation directions to global frame using the rotations. - w_relative_translations = gtsam.BinaryMeasurementsUnit3() - for relative_translation in relative_translations: - w_relative_translation = gtsam.Unit3(rotations.atRot3(relative_translation.key1()) - .rotate(relative_translation.measured().point3())) - w_relative_translations.append(gtsam.BinaryMeasurementUnit3(relative_translation.key1(), - relative_translation.key2(), - w_relative_translation, - relative_translation.noiseModel())) - - # Indices of measurements that are to be used as projection directions. These are randomly chosen. - sampled_indices = np.random.choice(len(w_relative_translations), min( - max_1dsfm_projection_directions, len(w_relative_translations)), replace=False) + # Indices of measurements that are to be used as projection directions. + # These are randomly chosen. + sampled_indices = np.random.choice(len(w_iZj_list), min( + MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)), replace=False) # Sample projection directions from the measurements. - projection_directions = [ - w_relative_translations[idx].measured() for idx in sampled_indices] + projection_directions = [w_iZj_list[idx].measured() + for idx in sampled_indices] outlier_weights = [] # Find the outlier weights for each direction using MFAS. for direction in projection_directions: - algorithm = gtsam.MFAS(w_relative_translations, direction) + algorithm = gtsam.MFAS(w_iZj_list, direction) outlier_weights.append(algorithm.computeOutlierWeights()) - # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, - # where weights are proportional to the probability of the edge being an outlier. + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys + # (camera IDs) to a weight, where weights are proportional to the probability of the edge + # being an outlier. avg_outlier_weights = defaultdict(float) for outlier_weight_dict in outlier_weights: for keypair, weight in outlier_weight_dict.items(): avg_outlier_weights[keypair] += weight / len(outlier_weights) # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. - inlier_w_relative_translations = gtsam.BinaryMeasurementsUnit3() - [inlier_w_relative_translations.append(Z) for Z in w_relative_translations - if avg_outlier_weights[(Z.key1(), Z.key2())] < outlier_weight_threshold] + w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() + [w_iZj_inliers.append(Z) for Z in w_iZj_list + if avg_outlier_weights[(Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] # Run the optimizer to obtain translations for normalized directions. - w_translations = gtsam.TranslationRecovery( - inlier_w_relative_translations).run() + wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() - poses = gtsam.Values() - for key in rotations.keys(): - poses.insert(key, gtsam.Pose3( - rotations.atRot3(key), w_translations.atPoint3(key))) - return poses + wTc_values = gtsam.Values() + for key in wRc_values.keys(): + wTc_values.insert(key, gtsam.Pose3( + wRc_values.atRot3(key), wtc_values.atPoint3(key))) + return wTc_values def main(): - rotations, translation_directions = get_data() - poses = estimate_poses(translation_directions, rotations) + wRc_values, w_iZj_list = get_data() + wTc_values = estimate_poses(w_iZj_list, wRc_values) print("**** Translation averaging output ****") - print(poses) + print(wTc_values) print("**************************************") From b9174ae0f4e4a6835151da8d1751beb2db5174ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Oct 2020 11:24:26 -0400 Subject: [PATCH 072/136] Changed name to avoid template confusion in VC 2016 --- gtsam/geometry/Point3.cpp | 12 +++++------- gtsam/geometry/Point3.h | 4 ++-- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce4ceee89..8665f7716 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -75,17 +75,15 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } -Point3Pair mean(const std::vector &abPointPairs) { +Point3Pair means(const std::vector &abPointPairs) { const size_t n = abPointPairs.size(); - Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + Point3 aSum(0, 0, 0), bSum(0, 0, 0); for (const Point3Pair &abPair : abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; + aSum += abPair.first; + bSum += abPair.second; } const double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; - return make_pair(aCentroid, bCentroid); + return {aSum * f, bSum * f}; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7f58497e9..33b5836f8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -67,8 +67,8 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { return sum / points.size(); } -/// mean of Point3 pair -GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); +/// mean of Point3 pair +GTSAM_EXPORT Point3Pair means(const std::vector &abPointPairs); template struct Range; From eb4f5288e9b31dd749d8173dd287eac2f1a94a71 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Oct 2020 12:01:33 -0400 Subject: [PATCH 073/136] Clean up code and tests and use "means" --- gtsam/geometry/Pose3.cpp | 65 ++++++++++++------------- gtsam/geometry/tests/testPoint3.cpp | 16 +++--- gtsam_unstable/geometry/Similarity3.cpp | 49 +++++++++++-------- 3 files changed, 69 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ea822b796..22849d4f5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -24,10 +24,11 @@ #include #include -using namespace std; - namespace gtsam { +using std::vector; +using Point3Pairs = vector; + /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); @@ -212,18 +213,20 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (std::abs(phi)>nearZeroThreshold) { - const double sinPhi = sin(phi), cosPhi = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + const Matrix3 WVW = W * V * W; + if (std::abs(phi) > nearZeroThreshold) { + const double s = sin(phi), c = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, + phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); - } - else { - Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - - 1./24.*(W*W*V + V*W*W - 3*W*V*W) - + 1./120.*(W*V*W*W + W*W*V*W); + Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + + (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * + (WVW * W + W * WVW); + } else { + Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - + 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + + 1. / 120. * (WVW * W + W * WVW); } #endif @@ -381,39 +384,33 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, } /* ************************************************************************* */ -boost::optional Pose3::Align(const std::vector& abPointPairs) { +boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { const size_t n = abPointPairs.size(); - if (n < 3) - return boost::none; // we need at least three pairs + if (n < 3) { + return boost::none; // we need at least three pairs + } // calculate centroids - Point3 aCentroid(0,0,0), bCentroid(0,0,0); - for(const Point3Pair& abPair: abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; - } - double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; + const auto centroids = means(abPointPairs); // Add to form H matrix Matrix3 H = Z_3x3; - for(const Point3Pair& abPair: abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; + for (const Point3Pair &abPair : abPointPairs) { + const Point3 da = abPair.first - centroids.first; + const Point3 db = abPair.second - centroids.second; H += da * db.transpose(); - } + } // ClosestTo finds rotation matrix closest to H in Frobenius sense - Rot3 aRb = Rot3::ClosestTo(H); - Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + const Rot3 aRb = Rot3::ClosestTo(H); + const Point3 aTb = centroids.first - aRb * centroids.second; return Pose3(aRb, aTb); } -boost::optional align(const vector& baPointPairs) { - vector abPointPairs; - for (const Point3Pair& baPair: baPointPairs) { - abPointPairs.push_back(make_pair(baPair.second, baPair.first)); +boost::optional align(const Point3Pairs &baPointPairs) { + Point3Pairs abPointPairs; + for (const Point3Pair &baPair : baPointPairs) { + abPointPairs.emplace_back(baPair.second, baPair.first); } return Pose3::Align(abPointPairs); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a481a8072..a655011a0 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -166,22 +166,22 @@ TEST (Point3, normalize) { //************************************************************************* TEST(Point3, mean) { - Point3 expected_a_mean(2, 2, 2); + Point3 expected(2, 2, 2); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); std::vector a_points{a1, a2, a3}; - Point3 actual_a_mean = mean(a_points); - EXPECT(assert_equal(expected_a_mean, actual_a_mean)); + Point3 actual = mean(a_points); + EXPECT(assert_equal(expected, actual)); } TEST(Point3, mean_pair) { Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0); - Point3Pair expected_mean = std::make_pair(a_mean, b_mean); + Point3Pair expected = std::make_pair(a_mean, b_mean); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); - std::vector point_pairs{{a1,b1},{a2,b2},{a3,b3}}; - Point3Pair actual_mean = mean(point_pairs); - EXPECT(assert_equal(expected_mean.first, actual_mean.first)); - EXPECT(assert_equal(expected_mean.second, actual_mean.second)); + std::vector point_pairs{{a1, b1}, {a2, b2}, {a3, b3}}; + Point3Pair actual = means(point_pairs); + EXPECT(assert_equal(expected.first, actual.first)); + EXPECT(assert_equal(expected.second, actual.second)); } //************************************************************************* diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index b2d7dc080..819c51fee 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,10 +23,14 @@ namespace gtsam { +using std::vector; +using PointPairs = vector; + namespace { /// Subtract centroids from point pairs. -static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3Pair& centroids) { - std::vector d_abPointPairs; +static PointPairs subtractCentroids(const PointPairs &abPointPairs, + const Point3Pair ¢roids) { + PointPairs d_abPointPairs; for (const Point3Pair& abPair : abPointPairs) { Point3 da = abPair.first - centroids.first; Point3 db = abPair.second - centroids.second; @@ -36,7 +40,8 @@ static std::vector subtractCentroids(const std::vector& } /// Form inner products x and y and calculate scale. -static const double calculateScale(const std::vector& d_abPointPairs, const Rot3& aRb) { +static const double calculateScale(const PointPairs &d_abPointPairs, + const Rot3 &aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { @@ -50,7 +55,7 @@ static const double calculateScale(const std::vector& d_abPointPairs } /// Form outer product H. -static Matrix3 calculateH(const std::vector& d_abPointPairs) { +static Matrix3 calculateH(const PointPairs &d_abPointPairs) { Matrix3 H = Z_3x3; for (const Point3Pair& d_abPair : d_abPointPairs) { H += d_abPair.first * d_abPair.second.transpose(); @@ -59,7 +64,8 @@ static Matrix3 calculateH(const std::vector& d_abPointPairs) { } /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. -static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { +static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb, + const Point3Pair ¢roids) { const double s = calculateScale(d_abPointPairs, aRb); const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; return Similarity3(aRb, aTb, s); @@ -67,8 +73,9 @@ static Similarity3 align(const std::vector& d_abPointPairs, const Ro /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { - auto centroids = mean(abPointPairs); +static Similarity3 alignGivenR(const PointPairs &abPointPairs, + const Rot3 &aRb) { + auto centroids = means(abPointPairs); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); return align(d_abPointPairs, aRb, centroids); } @@ -147,10 +154,12 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -Similarity3 Similarity3::Align(const std::vector& abPointPairs) { - // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 - if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); - auto centroids = mean(abPointPairs); +Similarity3 Similarity3::Align(const PointPairs &abPointPairs) { + // Refer to Chapter 3 of + // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + if (abPointPairs.size() < 3) + throw std::runtime_error("input should have at least 3 pairs of points"); + auto centroids = means(abPointPairs); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); Matrix3 H = calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense @@ -158,17 +167,18 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { return align(d_abPointPairs, aRb, centroids); } -Similarity3 Similarity3::Align(const std::vector& abPosePairs) { +Similarity3 Similarity3::Align(const vector &abPosePairs) { const size_t n = abPosePairs.size(); - if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); + if (n < 2) + throw std::runtime_error("input should have at least 2 pairs of poses"); // calculate rotation vector rotations; - vector abPointPairs; + PointPairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); Pose3 wTa, wTb; - for (const Pose3Pair& abPair : abPosePairs) { + for (const Pose3Pair &abPair : abPosePairs) { std::tie(wTa, wTb) = abPair; rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); @@ -178,7 +188,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { return alignGivenR(abPointPairs, aRb); } -Matrix4 Similarity3::wedge(const Vector7& xi) { +Matrix4 Similarity3::wedge(const Vector7 &xi) { // http://www.ethaneade.org/latex2html/lie/node29.html const auto w = xi.head<3>(); const auto u = xi.segment<3>(3); @@ -217,12 +227,13 @@ Matrix3 Similarity3::GetV(Vector3 w, double lambda) { W = 1.0 / 24.0 - theta2 / 720.0; } const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda; + const double expMinLambda = exp(-lambda); double A, alpha = 0.0, beta, mu; if (lambda2 > 1e-9) { - A = (1.0 - exp(-lambda)) / lambda; + A = (1.0 - expMinLambda) / lambda; alpha = 1.0 / (1.0 + theta2 / lambda2); - beta = (exp(-lambda) - 1 + lambda) / lambda2; - mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3; + beta = (expMinLambda - 1 + lambda) / lambda2; + mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3; } else { A = 1.0 - lambda / 2.0 + lambda2 / 6.0; beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0; From b30448733c66ec8194fa49e376834b1f4eb711d3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Oct 2020 19:56:35 -0400 Subject: [PATCH 074/136] remove all Cython references --- .github/scripts/python.sh | 56 +++------------- .github/workflows/build-python.yml | 2 - .github/workflows/trigger-python.yml | 6 +- .gitignore | 6 -- INSTALL.md | 4 +- cmake/CMakeLists.txt | 1 - cmake/FindCython.cmake | 81 ----------------------- docker/ubuntu-gtsam-python/Dockerfile | 8 +-- docker/ubuntu-gtsam/Dockerfile | 1 - gtsam/gtsam.i | 8 +-- gtsam/navigation/AHRSFactor.h | 2 +- gtsam/navigation/CombinedImuFactor.h | 2 +- gtsam/navigation/ImuFactor.h | 2 +- gtsam/nonlinear/Marginals.h | 4 +- gtsam_extra.cmake.in | 5 -- python/gtsam/tests/test_JacobianFactor.py | 2 +- 16 files changed, 27 insertions(+), 163 deletions(-) delete mode 100644 cmake/FindCython.cmake diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 6948cc385..a71e14c97 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -43,11 +43,6 @@ if [ -z ${PYTHON_VERSION+x} ]; then exit 127 fi -if [ -z ${WRAPPER+x} ]; then - echo "Please provide the wrapper to build!" - exit 126 -fi - PYTHON="python${PYTHON_VERSION}" if [[ $(uname) == "Darwin" ]]; then @@ -61,25 +56,11 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb -case $WRAPPER in -"cython") - BUILD_CYTHON="ON" - BUILD_PYBIND="OFF" - TYPEDEF_POINTS_TO_VECTORS="OFF" - sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/cython/requirements.txt - ;; -"pybind") - BUILD_CYTHON="OFF" - BUILD_PYBIND="ON" - TYPEDEF_POINTS_TO_VECTORS="ON" +BUILD_PYBIND="ON" +TYPEDEF_POINTS_TO_VECTORS="ON" - sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt - ;; -*) - exit 126 - ;; -esac +sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build @@ -90,7 +71,6 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DGTSAM_INSTALL_CYTHON_TOOLBOX=${BUILD_CYTHON} \ -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \ -DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ @@ -98,30 +78,10 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \ -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install -make -j$(nproc) install & +make -j$(nproc) install -while ps -p $! > /dev/null -do - sleep 60 - now=$(date +%s) - printf "%d seconds have elapsed\n" $(( (now - start) )) -done -case $WRAPPER in -"cython") - cd $GITHUB_WORKSPACE/build/cython - $PYTHON setup.py install --user --prefix= - cd $GITHUB_WORKSPACE/build/cython/gtsam/tests - $PYTHON -m unittest discover - ;; -"pybind") - cd $GITHUB_WORKSPACE/build/python - $PYTHON setup.py install --user --prefix= - cd $GITHUB_WORKSPACE/python/gtsam/tests - $PYTHON -m unittest discover - ;; -*) - echo "THIS SHOULD NEVER HAPPEN!" - exit 125 - ;; -esac \ No newline at end of file +cd $GITHUB_WORKSPACE/build/python +$PYTHON setup.py install --user --prefix= +cd $GITHUB_WORKSPACE/python/gtsam/tests +$PYTHON -m unittest discover diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index dc03ec6c9..b8d6bc311 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -12,7 +12,6 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} - WRAPPER: ${{ matrix.wrapper }} strategy: fail-fast: false matrix: @@ -28,7 +27,6 @@ jobs: build_type: [Debug, Release] python_version: [3] - wrapper: [pybind] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 diff --git a/.github/workflows/trigger-python.yml b/.github/workflows/trigger-python.yml index 94527e732..1e8981d99 100644 --- a/.github/workflows/trigger-python.yml +++ b/.github/workflows/trigger-python.yml @@ -1,11 +1,11 @@ -# This triggers Cython builds on `gtsam-manylinux-build` +# This triggers Python builds on `gtsam-manylinux-build` name: Trigger Python Builds on: push: branches: - develop jobs: - triggerCython: + triggerPython: runs-on: ubuntu-latest steps: - name: Repository Dispatch @@ -13,5 +13,5 @@ jobs: with: token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }} repository: borglab/gtsam-manylinux-build - event-type: cython-wrapper + event-type: python-wrapper client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' diff --git a/.gitignore b/.gitignore index c2d6ce60f..cde059767 100644 --- a/.gitignore +++ b/.gitignore @@ -9,12 +9,6 @@ *.txt.user *.txt.user.6d59f0c *.pydevproject -cython/venv -cython/gtsam.cpp -cython/gtsam.cpython-35m-darwin.so -cython/gtsam.pyx -cython/gtsam.so -cython/gtsam_wrapper.pxd .vscode .env /.vs/ diff --git a/INSTALL.md b/INSTALL.md index cf66766a1..3dbc3a850 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -173,7 +173,7 @@ NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against g Intel has a guide for installing MKL on Linux through APT repositories at . After following the instructions, add the following to your `~/.bashrc` (and afterwards, open a new terminal before compiling GTSAM): -`LD_PRELOAD` need only be set if you are building the cython wrapper to use GTSAM from python. +`LD_PRELOAD` need only be set if you are building the python wrapper to use GTSAM from python. ```sh source /opt/intel/mkl/bin/mklvars.sh intel64 export LD_PRELOAD="$LD_PRELOAD:/opt/intel/mkl/lib/intel64/libmkl_core.so:/opt/intel/mkl/lib/intel64/libmkl_sequential.so" @@ -190,6 +190,6 @@ Failing to specify `LD_PRELOAD` may lead to errors such as: `ImportError: /opt/intel/mkl/lib/intel64/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv` or `Intel MKL FATAL ERROR: Cannot load libmkl_avx2.so or libmkl_def.so.` -when importing GTSAM using the cython wrapper in python. +when importing GTSAM using the python wrapper. diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 9d9ecd48b..451ca38a4 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -19,7 +19,6 @@ install(FILES GtsamMatlabWrap.cmake GtsamTesting.cmake GtsamPrinting.cmake - FindCython.cmake FindNumPy.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake deleted file mode 100644 index e5a32c30d..000000000 --- a/cmake/FindCython.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# Modifed from: https://github.com/nest/nest-simulator/blob/master/cmake/FindCython.cmake -# -# Find the Cython compiler. -# -# This code sets the following variables: -# -# CYTHON_FOUND -# CYTHON_PATH -# CYTHON_EXECUTABLE -# CYTHON_VERSION -# -# See also UseCython.cmake - -#============================================================================= -# Copyright 2011 Kitware, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -#============================================================================= - -# Use the Cython executable that lives next to the Python executable -# if it is a local installation. -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])" - RESULT_VARIABLE RESULT - OUTPUT_VARIABLE CYTHON_PATH - OUTPUT_STRIP_TRAILING_WHITESPACE - ) -endif () - -# RESULT=0 means ok -if ( NOT RESULT ) - get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) - find_program( CYTHON_EXECUTABLE - NAMES cython cython.bat cython3 - HINTS ${_python_path} - ) -endif () - -# RESULT=0 means ok -if ( NOT RESULT ) - execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print(Cython.__version__)" - RESULT_VARIABLE RESULT - OUTPUT_VARIABLE CYTHON_VAR_OUTPUT - ERROR_VARIABLE CYTHON_VAR_OUTPUT - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - if ( RESULT EQUAL 0 ) - string( REGEX REPLACE ".* ([0-9]+\\.[0-9]+(\\.[0-9]+)?).*" "\\1" - CYTHON_VERSION "${CYTHON_VAR_OUTPUT}" ) - endif () -endif () - -include( FindPackageHandleStandardArgs ) -find_package_handle_standard_args( Cython - FOUND_VAR - CYTHON_FOUND - REQUIRED_VARS - CYTHON_PATH - CYTHON_EXECUTABLE - VERSION_VAR - CYTHON_VERSION - ) - diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile index c733ceb19..ce5d8fdca 100644 --- a/docker/ubuntu-gtsam-python/Dockerfile +++ b/docker/ubuntu-gtsam-python/Dockerfile @@ -7,9 +7,9 @@ FROM dellaert/ubuntu-gtsam:bionic RUN apt-get install -y python3-pip python3-dev # Install python wrapper requirements -RUN python3 -m pip install -U -r /usr/src/gtsam/cython/requirements.txt +RUN python3 -m pip install -U -r /usr/src/gtsam/python/requirements.txt -# Run cmake again, now with cython toolbox on +# Run cmake again, now with python toolbox on WORKDIR /usr/src/gtsam/build RUN cmake \ -DCMAKE_BUILD_TYPE=Release \ @@ -17,7 +17,7 @@ RUN cmake \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_TIMING_ALWAYS=OFF \ -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \ + -DGTSAM_BUILD_PYTHON=ON \ -DGTSAM_PYTHON_VERSION=3\ .. @@ -25,7 +25,7 @@ RUN cmake \ RUN make -j4 install && make clean # Needed to run python wrapper: -RUN echo 'export PYTHONPATH=/usr/local/cython/:$PYTHONPATH' >> /root/.bashrc +RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc # Run bash CMD ["bash"] diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile index 187c76314..f2b476f15 100644 --- a/docker/ubuntu-gtsam/Dockerfile +++ b/docker/ubuntu-gtsam/Dockerfile @@ -23,7 +23,6 @@ RUN cmake \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_TIMING_ALWAYS=OFF \ -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_INSTALL_CYTHON_TOOLBOX=OFF \ .. # Build diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index a172df315..e4652f741 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -93,9 +93,9 @@ * - Add "void serializable()" to a class if you only want the class to be serialized as a * part of a container (such as noisemodel). This version does not require a publicly * accessible default constructor. - * Forward declarations and class definitions for Cython: - * - Need to specify the base class (both this forward class and base class are declared in an external cython header) - * This is so Cython can generate proper inheritance. + * Forward declarations and class definitions for Pybind: + * - Need to specify the base class (both this forward class and base class are declared in an external Pybind header) + * This is so Pybind can generate proper inheritance. * Example when wrapping a gtsam-based project: * // forward declarations * virtual class gtsam::NonlinearFactor @@ -104,7 +104,7 @@ * #include * virtual class MyFactor : gtsam::NoiseModelFactor {...}; * - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class - * - This will cause an ambiguity problem in Cython pxd header file + * - This will cause an ambiguity problem in Pybind header file */ /** diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index ec1a07f65..34626fcf6 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -42,7 +42,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation public: - /// Default constructor, only for serialization and Cython wrapper + /// Default constructor, only for serialization and wrappers PreintegratedAhrsMeasurements() {} /** diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 387353136..efca25bff 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -145,7 +145,7 @@ public: /// @name Constructors /// @{ - /// Default constructor only for serialization and Cython wrapper + /// Default constructor only for serialization and wrappers PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 51df3f24a..cd9c591f1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -80,7 +80,7 @@ protected: public: - /// Default constructor for serialization and Cython wrapper + /// Default constructor for serialization and wrappers PreintegratedImuMeasurements() { preintMeasCov_.setZero(); } diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 4e201cc38..9935bafdd 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -48,7 +48,7 @@ protected: public: - /// Default constructor only for Cython wrapper + /// Default constructor only for wrappers Marginals(){} /** Construct a marginals class from a nonlinear factor graph. @@ -156,7 +156,7 @@ protected: FastMap indices_; public: - /// Default constructor only for Cython wrapper + /// Default constructor only for wrappers JointMarginal() {} /** Access a block, corresponding to a pair of variables, of the joint diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 01ac00b37..44ba36bd6 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -7,8 +7,3 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") set (GTSAM_USE_TBB @GTSAM_USE_TBB@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) - -if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") - list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@") - list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") -endif() diff --git a/python/gtsam/tests/test_JacobianFactor.py b/python/gtsam/tests/test_JacobianFactor.py index 6e049ed47..79f512f60 100644 --- a/python/gtsam/tests/test_JacobianFactor.py +++ b/python/gtsam/tests/test_JacobianFactor.py @@ -19,7 +19,7 @@ from gtsam.utils.test_case import GtsamTestCase class TestJacobianFactor(GtsamTestCase): def test_eliminate(self): - # Recommended way to specify a matrix (see cython/README) + # Recommended way to specify a matrix (see python/README) Ax2 = np.array( [[-5., 0.], [+0., -5.], From cef937e09d8505068ad503ab7a6248a194190536 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Oct 2020 20:21:14 -0400 Subject: [PATCH 075/136] Update Point3.h Fix doxygen comment --- gtsam/geometry/Point3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b425af8a4..57188fc5e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { return sum / points.size(); } -/// mean of Point3 pair +/// Calculate the two means of a set of Point3 pairs GTSAM_EXPORT Point3Pair means(const std::vector &abPointPairs); template From 04c12c364fbf93ce5264b4f78049a8590e6d8cdf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 1 Oct 2020 23:40:54 -0400 Subject: [PATCH 076/136] add --- python/gtsam/examples/SFMExample_bal.py | 117 ++++++++++++++++++++++++ 1 file changed, 117 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..8efc8ec19 --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,117 @@ +""" + 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 matplotlib.pyplot as plt +import numpy as np + +import gtsam +from gtsam import ( + PinholeCameraCal3Bundler, + readBal, + symbol_shorthand +) + +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; + +PinholeCameraCal3Bundler +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") + + pdb.set_trace() + # # 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(m_idx) + #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.push_back(gtsam.PriorFactorVector( + P(0), mydata.track(0).point3(), 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(cam_idx) + initial.insert(C(i), camera) + i += 1 + + j = 0 + # add each SfmTrack + for t_idx in range(mydata.number_tracks()): + track = mydata.track(t_idx) + initial.insert(P(j), track.point3()) + 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 a490017669578711140d48f5aaa22b77ce7efc9d Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 1 Oct 2020 22:19:17 -0700 Subject: [PATCH 077/136] outlier rejection in separate fn and other readability changes --- .../examples/TranslationAveragingExample.py | 104 ++++++++++-------- 1 file changed, 61 insertions(+), 43 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index a374dc630..d843f8702 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -15,7 +15,7 @@ Date: September 2020 """ from collections import defaultdict -from typing import Tuple, List +from typing import List, Tuple import numpy as np @@ -28,59 +28,48 @@ OUTLIER_WEIGHT_THRESHOLD = 0.1 def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: - """"Returns global rotations and unit translation directions between 8 cameras - that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata - and the unit translations directions between some camera pairs are computed from their + """"Returns global rotations and unit translation directions between 8 cameras + that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata + and the unit translations directions between some camera pairs are computed from their global translations. """ # Using toy dataset in SfMdata for example. - wTc = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j # in the coordinate frame of camera i. i_iZj_list = [] - for i in range(0, len(wTc) - 2): + for i in range(0, len(wTc_list) - 2): # Add the rotation. - wRc_values.insert(i, wTc[i].rotation()) + wRi = wTc_list[i].rotation() + wRc_values.insert(i, wRi) # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): - i_iZj = gtsam.Unit3(wTc[i].rotation().unrotate( - wTc[j].translation() - wTc[i].translation())) + # Compute the translation from pose i to pose j, in the world reference frame. + w_itj = wTc_list[j].translation() - wTc_list[i].translation() + # Obtain the translation in the camera i's reference frame. + i_itj = wRi.unrotate(w_itj) + # Compute the normalized unit translation direction. + i_iZj = gtsam.Unit3(i_itj) i_iZj_list.append(gtsam.BinaryMeasurementUnit3( i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. - wRc_values.insert(len(wTc) - 1, wTc[-1].rotation()) - wRc_values.insert(len(wTc) - 2, wTc[-2].rotation()) - return (wRc_values, i_iZj_list) + wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation()) + wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation()) + return wRc_values, i_iZj_list -def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, - wRc_values: gtsam.Values) -> gtsam.Values: - """Estimate poses given rotations and normalized translation directions between cameras. +def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: + """Removes outliers from a list of Unit3 measurements that are the + translation directions from camera i to camera j in the world frame.""" - Args: - iZj_list -- List of normalized translation direction measurements between camera pairs, - Z here refers to measurements. The measurements are of camera j with reference - to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit - vector to j in i's frame and is not a transformation. - wRc_values -- Rotations of the cameras in the world frame. + # Indices of measurements that are to be used as projection directions. + # These are randomly chosen. All sampled directions must be unique. + num_directions_to_sample = min( + MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)) + sampled_indices = np.random.choice( + len(w_iZj_list), num_directions_to_sample, replace=False) - Returns: - Values -- Estimated poses. - """ - - # Convert the translation direction measurements to world frame using the rotations. - w_iZj_list = gtsam.BinaryMeasurementsUnit3() - for i_iZj in i_iZj_list: - w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) - .rotate(i_iZj.measured().point3())) - w_iZj_list.append(gtsam.BinaryMeasurementUnit3( - i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) - - # Indices of measurements that are to be used as projection directions. - # These are randomly chosen. - sampled_indices = np.random.choice(len(w_iZj_list), min( - MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)), replace=False) # Sample projection directions from the measurements. projection_directions = [w_iZj_list[idx].measured() for idx in sampled_indices] @@ -91,8 +80,8 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, algorithm = gtsam.MFAS(w_iZj_list, direction) outlier_weights.append(algorithm.computeOutlierWeights()) - # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys - # (camera IDs) to a weight, where weights are proportional to the probability of the edge + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys + # (camera IDs) to a weight, where weights are proportional to the probability of the edge # being an outlier. avg_outlier_weights = defaultdict(float) for outlier_weight_dict in outlier_weights: @@ -101,8 +90,37 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() - [w_iZj_inliers.append(Z) for Z in w_iZj_list - if avg_outlier_weights[(Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] + [w_iZj_inliers.append(Z) for Z in w_iZj_list if avg_outlier_weights[( + Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] + + return w_iZj_inliers + + +def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, + wRc_values: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations and normalized translation directions between cameras. + + Args: + i_iZj_list: List of normalized translation direction measurements between camera pairs, + Z here refers to measurements. The measurements are of camera j with reference + to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit + vector to j in i's frame and is not a transformation. + wRc_values: Rotations of the cameras in the world frame. + + Returns: + Values: Estimated poses. + """ + + # Convert the translation direction measurements to world frame using the rotations. + w_iZj_list = gtsam.BinaryMeasurementsUnit3() + for i_iZj in i_iZj_list: + w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) + .rotate(i_iZj.measured().point3())) + w_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) + + # Remove the outliers in the unit translation directions. + w_iZj_inliers = prune_to_inliers(w_iZj_list) # Run the optimizer to obtain translations for normalized directions. wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() @@ -115,8 +133,8 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, def main(): - wRc_values, w_iZj_list = get_data() - wTc_values = estimate_poses(w_iZj_list, wRc_values) + wRc_values, i_iZj_list = get_data() + wTc_values = estimate_poses(i_iZj_list, wRc_values) print("**** Translation averaging output ****") print(wTc_values) print("**************************************") From 695f75bc8d37cd820108a399c7f992472b4d0b20 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 2 Oct 2020 07:56:41 -0700 Subject: [PATCH 078/136] readability changes --- python/gtsam/examples/TranslationAveragingExample.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index d843f8702..683008749 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -32,8 +32,8 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. """ - # Using toy dataset in SfMdata for example. - wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 + wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j @@ -88,10 +88,10 @@ def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryM for keypair, weight in outlier_weight_dict.items(): avg_outlier_weights[keypair] += weight / len(outlier_weights) - # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. + # Remove w_iZj that have weight greater than threshold, these are outliers. w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() - [w_iZj_inliers.append(Z) for Z in w_iZj_list if avg_outlier_weights[( - Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] + [w_iZj_inliers.append(Z) for w_iZj in w_iZj_list if avg_outlier_weights[( + w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] return w_iZj_inliers @@ -108,7 +108,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, wRc_values: Rotations of the cameras in the world frame. Returns: - Values: Estimated poses. + gtsam.Values: Estimated poses. """ # Convert the translation direction measurements to world frame using the rotations. From f11ce11678b8cac8341d9fb3f4dda4dd7931e084 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 2 Oct 2020 08:03:28 -0700 Subject: [PATCH 079/136] fixing one variable that was not renamed --- python/gtsam/examples/TranslationAveragingExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 683008749..7e8c96b15 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -90,7 +90,7 @@ def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryM # Remove w_iZj that have weight greater than threshold, these are outliers. w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() - [w_iZj_inliers.append(Z) for w_iZj in w_iZj_list if avg_outlier_weights[( + [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[( w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] return w_iZj_inliers From eb9ca8cd92608200ebf72908decf81a298fc1af8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Oct 2020 14:05:13 -0400 Subject: [PATCH 080/136] find python if using Default --- CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eedc42c9e..8b546ebc2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,15 +118,15 @@ 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") + # 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() + set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" CACHE STRING From a8ea6f2bd26f68ed940138be840fa5bbd24bb985 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 2 Oct 2020 16:12:10 -0400 Subject: [PATCH 081/136] Fixed include error --- gtsam_unstable/linear/tests/testLPSolver.cpp | 12 +++++++----- gtsam_unstable/linear/tests/testQPSolver.cpp | 8 +++++--- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index de9cd032a..53c8c7618 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -16,9 +16,9 @@ * @author Duy-Nguyen Ta */ -#include -#include -#include +#include +#include + #include #include #include @@ -28,8 +28,10 @@ #include #include -#include -#include +#include + +#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 285f19b3f..67a0c971e 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -17,12 +17,14 @@ * @author Ivan Dario Jimenez */ -#include -#include -#include #include #include +#include +#include + +#include + using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; From 03ca9053421dcb51c63065a03a5cc99081e8a529 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 2 Oct 2020 23:44:55 -0700 Subject: [PATCH 082/136] removing shared ptr, iostream, renaming --- gtsam/sfm/MFAS.cpp | 2 -- gtsam/sfm/tests/testMFAS.cpp | 2 -- python/gtsam/examples/TranslationAveragingExample.py | 4 ++-- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index bc66d0711..4cd983ecd 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -7,8 +7,6 @@ #include -#include - #include #include #include diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index b2daf0d2e..362027d5d 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -9,8 +9,6 @@ #include -#include - using namespace std; using namespace gtsam; diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 7e8c96b15..054b61126 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -59,7 +59,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: return wRc_values, i_iZj_list -def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: +def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: """Removes outliers from a list of Unit3 measurements that are the translation directions from camera i to camera j in the world frame.""" @@ -120,7 +120,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) # Remove the outliers in the unit translation directions. - w_iZj_inliers = prune_to_inliers(w_iZj_list) + w_iZj_inliers = filter_outliers(w_iZj_list) # Run the optimizer to obtain translations for normalized directions. wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() From 06ac62724927abc947aec6e87017f8b2bd5df39a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Oct 2020 19:55:08 -0400 Subject: [PATCH 083/136] added normalize function to orthogonalize the rotation after composition --- gtsam/geometry/Rot3.h | 7 +++++++ gtsam/geometry/Rot3M.cpp | 28 +++++++++++++++++++++++++++- gtsam/geometry/Rot3Q.cpp | 6 +++++- gtsam/geometry/tests/testRot3.cpp | 20 ++++++++++++++++++++ 4 files changed, 59 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index de9d2b420..db5367c8f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -430,6 +430,13 @@ namespace gtsam { */ Matrix3 transpose() const; + /** + * Normalize rotation so that its determinant is 1. + * This means either re-orthogonalizing the Matrix representation or + * normalizing the quaternion representation. + */ + Rot3 normalize(const Rot3& R) const; + /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 500941a16..ffc468dfc 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -108,9 +108,35 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, ); } +/* ************************************************************************* */ +Rot3 Rot3::normalize(const Rot3& R) const { + /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 + /// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view + + /// Essentially, this computes the orthogonalization error, distributes the + /// error to the x and y rows, and then performs a Taylor expansion to + /// orthogonalize. + + Matrix3 rot = R.matrix(), rot_new; + + if (std::fabs(rot.determinant()-1) < 1e-12) return R; + + Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0); + double error = x.dot(y); + + Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x; + Vector3 z_ort = x_ort.cross(y_ort); + + rot_new.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; + rot_new.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; + rot_new.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; + + return Rot3(rot_new); +} + /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(rot_*R2.rot_); + return normalize(Rot3(rot_*R2.rot_)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 6e1871c64..d4400b0dc 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -86,9 +86,13 @@ namespace gtsam { gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } + /* ************************************************************************* */ + Rot3 Rot3::normalize(const Rot3& R) const { + return Rot3(R.quaternion_.normalized()); + } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); + return normalize(Rot3(quaternion_ * R2.quaternion_)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a7c6f5a77..e86029026 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -910,6 +910,26 @@ TEST(Rot3, yaw_derivative) { CHECK(assert_equal(num, calc)); } +/* ************************************************************************* */ +TEST(Rot3, determinant) { + size_t degree = 1; + Rot3 R_w0; // Zero rotation + Rot3 R_w1 = Rot3::Ry(degree * M_PI / 180); + + Rot3 R_01, R_w2; + double actual, expected = 1.0; + + for (size_t i = 2; i < 360; ++i) { + R_01 = R_w0.between(R_w1); + R_w2 = R_w1 * R_01; + R_w0 = R_w1; + R_w1 = R_w2; + actual = R_w2.matrix().determinant(); + + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7); + } +} + /* ************************************************************************* */ int main() { TestResult tr; From 2e1cc3ca3517eca5431a07405453f776b28c9aeb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Oct 2020 13:25:30 -0400 Subject: [PATCH 084/136] normalized needs to be called explicitly --- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/Rot3M.cpp | 18 ++++++++++-------- gtsam/geometry/Rot3Q.cpp | 6 +++--- gtsam/geometry/tests/testRot3.cpp | 2 +- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index db5367c8f..b1e46308d 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -435,7 +435,7 @@ namespace gtsam { * This means either re-orthogonalizing the Matrix representation or * normalizing the quaternion representation. */ - Rot3 normalize(const Rot3& R) const; + Rot3 normalized() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index ffc468dfc..c372d403b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -109,7 +109,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, } /* ************************************************************************* */ -Rot3 Rot3::normalize(const Rot3& R) const { +Rot3 Rot3::normalized() const { /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 /// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view @@ -117,9 +117,11 @@ Rot3 Rot3::normalize(const Rot3& R) const { /// error to the x and y rows, and then performs a Taylor expansion to /// orthogonalize. - Matrix3 rot = R.matrix(), rot_new; + Matrix3 rot = rot_.matrix(), rot_orth; - if (std::fabs(rot.determinant()-1) < 1e-12) return R; + // Check if determinant is already 1. + // If yes, then return the current Rot3. + if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_); Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0); double error = x.dot(y); @@ -127,16 +129,16 @@ Rot3 Rot3::normalize(const Rot3& R) const { Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x; Vector3 z_ort = x_ort.cross(y_ort); - rot_new.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; - rot_new.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; - rot_new.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; + rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; + rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; + rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; - return Rot3(rot_new); + return Rot3(rot_orth); } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return normalize(Rot3(rot_*R2.rot_)); + return Rot3(rot_*R2.rot_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d4400b0dc..523255d87 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -87,12 +87,12 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::normalize(const Rot3& R) const { - return Rot3(R.quaternion_.normalized()); + Rot3 Rot3::normalized() const { + return Rot3(quaternion_.normalized()); } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return normalize(Rot3(quaternion_ * R2.quaternion_)); + return Rot3(quaternion_ * R2.quaternion_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index e86029026..7b792f8bd 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -923,7 +923,7 @@ TEST(Rot3, determinant) { R_01 = R_w0.between(R_w1); R_w2 = R_w1 * R_01; R_w0 = R_w1; - R_w1 = R_w2; + R_w1 = R_w2.normalized(); actual = R_w2.matrix().determinant(); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7); From 08636189fb57069ed0ec308b25a768f6cf0caf1d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 5 Oct 2020 14:35:27 -0400 Subject: [PATCH 085/136] add WIP PR --- gtsam/gtsam.i | 5 ++++- python/gtsam/examples/SFMExample_bal.py | 26 ++++++++++++++----------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index e5f323d72..1a239541d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1107,6 +1107,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +//typedef gtsam::PinholeCamera SfmCamera; #include class StereoCamera { @@ -2528,7 +2529,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2673,6 +2674,8 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; + template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 8efc8ec19..258ec8917 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -18,7 +18,9 @@ import numpy as np import gtsam from gtsam import ( + #GeneralSFMFactorCal3Bundler, PinholeCameraCal3Bundler, + PriorFactorSfmCamera, readBal, symbol_shorthand ) @@ -28,14 +30,10 @@ 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 +# An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration # and has a total of 9 free parameters -#typedef GeneralSFMFactor MyFactor; -PinholeCameraCal3Bundler 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 @@ -64,17 +62,23 @@ def run(args): for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) - #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P - #graph.add + # note use of shorthand symbols C and P + #graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) 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.push_back(gtsam.PriorFactorVector( - P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) + graph.push_back( + gtsam.PriorFactorSfmCamera( + C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) + ) + ) + graph.push_back( + gtsam.PriorFactorPoint3( + P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + ) + ) # # Create initial estimate initial = gtsam.Values() From 5fb7229fa67fd14b671611dc5b8daf21f5179342 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Oct 2020 22:28:27 -0400 Subject: [PATCH 086/136] Moved normalize next to ClosestTo and add more docs --- gtsam/geometry/Rot3.h | 29 +++++++++++++++++++++-------- gtsam/geometry/Rot3M.cpp | 1 - 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b1e46308d..2334312f6 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -262,9 +262,29 @@ namespace gtsam { static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // const Unit3& a_q, const Unit3& b_q); - /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + /** + * Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + * + * Uses Full SVD to compute the orthogonal matrix, thus is highly accurate and robust. + * + * N. J. Higham. Matrix nearness problems and applications. + * In M. J. C. Gover and S. Barnett, editors, Applications of Matrix Theory, pages 1–27. + * Oxford University Press, 1989. + */ static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); } + /** + * Normalize rotation so that its determinant is 1. + * This means either re-orthogonalizing the Matrix representation or + * normalizing the quaternion representation. + * + * This method is akin to `ClosestTo` but uses a computationally cheaper + * algorithm. + * + * Ref: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view + */ + Rot3 normalized() const; + /// @} /// @name Testable /// @{ @@ -430,13 +450,6 @@ namespace gtsam { */ Matrix3 transpose() const; - /** - * Normalize rotation so that its determinant is 1. - * This means either re-orthogonalizing the Matrix representation or - * normalizing the quaternion representation. - */ - Rot3 normalized() const; - /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index c372d403b..02e5b771f 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -111,7 +111,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, /* ************************************************************************* */ Rot3 Rot3::normalized() const { /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 - /// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view /// Essentially, this computes the orthogonalization error, distributes the /// error to the x and y rows, and then performs a Taylor expansion to From e9e87526c41699a544fb07ae0beedca87658643e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 6 Oct 2020 18:10:06 +0200 Subject: [PATCH 087/136] refactor cmake scripts into smaller files --- CMakeLists.txt | 551 +------------------------ cmake/GtsamBuildTypes.cmake | 14 + cmake/handle_allocators.cmake | 34 ++ cmake/handle_boost.cmake | 56 +++ cmake/handle_ccache.cmake | 14 + cmake/handle_cpack.cmake | 28 ++ cmake/handle_eigen.cmake | 75 ++++ cmake/handle_final_checks.cmake | 10 + cmake/handle_general_options.cmake | 46 +++ cmake/handle_global_build_flags.cmake | 52 +++ cmake/handle_mkl.cmake | 17 + cmake/handle_openmp.cmake | 11 + cmake/handle_perftools.cmake | 4 + cmake/handle_print_configuration.cmake | 104 +++++ cmake/handle_python.cmake | 26 ++ cmake/handle_tbb.cmake | 24 ++ cmake/handle_uninstall.cmake | 10 + matlab/CMakeLists.txt | 7 +- 18 files changed, 545 insertions(+), 538 deletions(-) create mode 100644 cmake/handle_allocators.cmake create mode 100644 cmake/handle_boost.cmake create mode 100644 cmake/handle_ccache.cmake create mode 100644 cmake/handle_cpack.cmake create mode 100644 cmake/handle_eigen.cmake create mode 100644 cmake/handle_final_checks.cmake create mode 100644 cmake/handle_general_options.cmake create mode 100644 cmake/handle_global_build_flags.cmake create mode 100644 cmake/handle_mkl.cmake create mode 100644 cmake/handle_openmp.cmake create mode 100644 cmake/handle_perftools.cmake create mode 100644 cmake/handle_print_configuration.cmake create mode 100644 cmake/handle_python.cmake create mode 100644 cmake/handle_tbb.cmake create mode 100644 cmake/handle_uninstall.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b546ebc2..831ee00f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,17 +22,10 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) ############################################################################### # Gather information, perform checks, set defaults -# Set the default install path to home -#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library") - set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include(GtsamMakeConfigFile) include(GNUInstallDirs) -# Record the root dir for gtsam - needed during external builds, e.g., ROS -set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) -message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") - # Load build type flags and default to Debug mode include(GtsamBuildTypes) @@ -45,399 +38,21 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() -# See whether gtsam_unstable is available (it will be present only if we're using a git checkout) -if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") - set(GTSAM_UNSTABLE_AVAILABLE 1) -else() - set(GTSAM_UNSTABLE_AVAILABLE 0) -endif() +include(cmake/handle_boost.cmake) # Boost +include(cmake/handle_ccache.cmake) # ccache +include(cmake/handle_cpack.cmake) # CPack +include(cmake/handle_eigen.cmake) # Eigen3 +include(cmake/handle_general_options.cmake) # CMake build options +include(cmake/handle_mkl.cmake) # MKL +include(cmake/handle_openmp.cmake) # OpenMP +include(cmake/handle_perftools.cmake) # Google perftools +include(cmake/handle_python.cmake) # Python options and commands +include(cmake/handle_tbb.cmake) # TBB +include(cmake/handle_uninstall.cmake) # for "make uninstall" -# ---------------------------------------------------------------------------- -# Uninstall target, for "make uninstall" -# ---------------------------------------------------------------------------- -configure_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" - IMMEDIATE @ONLY) +include(cmake/handle_allocators.cmake) # Must be after tbb, pertools -add_custom_target(uninstall - "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") - - -############################################################################### -# Set up options - -# Configurable Options -if(GTSAM_UNSTABLE_AVAILABLE) - option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) - option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) - option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) -endif() -option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) -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." ON) -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" 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 with pybind11" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) -option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) -option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) -if(NOT MSVC AND NOT XCODE_VERSION) - option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) -endif() - -if(NOT MSVC AND NOT XCODE_VERSION) - # Set the build type to upper case for downstream use - string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER) - - # Set the GTSAM_BUILD_TAG variable. - # If build type is Release, set to blank (""), else set to the build type. - if(${CMAKE_BUILD_TYPE_UPPER} STREQUAL "RELEASE") - set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory - else() - set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}") - endif() -endif() - -# Options relating to MATLAB wrapper -# TODO: Check for matlab mex binary before handling building of binaries -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") - -# Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) -endif() - -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") -endif() - -if(GTSAM_BUILD_PYTHON) - if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") - # 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() - - 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.") - set(GTSAM_UNSTABLE_BUILD_PYTHON OFF) - endif() - endif() - - set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python") -endif() - -# Flags for choosing default packaging tools -set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") -set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") - -if (CMAKE_GENERATOR STREQUAL "Ninja" AND - ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR - (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5))) - # Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support. - # Rationale in https://github.com/ninja-build/ninja/issues/814 - add_compile_options(-fdiagnostics-color=always) -endif() - -############################################################################### -# Find boost - -# To change the path for boost, you will need to set: -# BOOST_ROOT: path to install prefix for boost -# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT - -if(MSVC) - # By default, boost only builds static libraries on windows - set(Boost_USE_STATIC_LIBS ON) # only find static libs - # If we ever reset above on windows and, ... - # 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_cache(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_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) - endif() -endif() - -# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() -# or explicit instantiation will generate build errors. -# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 -# -if(MSVC AND BUILD_SHARED_LIBS) - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) -endif() - -# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.58) -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 - NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") -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 - Boost::system - Boost::filesystem - Boost::thread - Boost::date_time - Boost::regex -) -if (GTSAM_DISABLE_NEW_TIMERS) - message("WARNING: GTSAM timing instrumentation manually disabled") - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) -else() - if(Boost_TIMER_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.") - endif() -endif() - -############################################################################### -# Find TBB -find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) - -# Set up variables if we're using TBB -if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) - set(TBB_GREATER_EQUAL_2020 1) - else() - set(TBB_GREATER_EQUAL_2020 0) - endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) -else() - set(GTSAM_USE_TBB 0) # This will go into config.h -endif() - -############################################################################### -# Prohibit Timing build mode in combination with TBB -if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) - message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") -endif() - - -############################################################################### -# Find Google perftools -find_package(GooglePerfTools) - -############################################################################### -# Support ccache, if installed -if(NOT MSVC AND NOT XCODE_VERSION) - find_program(CCACHE_FOUND ccache) - if(CCACHE_FOUND) - if(GTSAM_BUILD_WITH_CCACHE) - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) - else() - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "") - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "") - endif() - endif(CCACHE_FOUND) -endif() - -############################################################################### -# Find MKL -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 - list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) - - # --no-as-needed is required with gcc according to the MKL link advisor - if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") - endif() -else() - set(GTSAM_USE_EIGEN_MKL 0) - set(EIGEN_USE_MKL_ALL 0) -endif() - -############################################################################### -# Find OpenMP (if we're also using MKL) -find_package(OpenMP) # do this here to generate correct message if disabled - -if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) - if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) - set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h - list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS}) - endif() -endif() - - -############################################################################### -# Option for using system Eigen or GTSAM-bundled Eigen -### These patches only affect usage of MKL. If you want to enable MKL, you *must* -### use our patched version of Eigen -### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) -### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) -option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) - -# Switch for using system Eigen or GTSAM-bundled Eigen -if(GTSAM_USE_SYSTEM_EIGEN) - find_package(Eigen3 REQUIRED) - - # Use generic Eigen include paths e.g. - 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 - if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) - message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") - endif() - - # Check for Eigen version which doesn't work with MKL - # See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details. - if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4)) - 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 - if(EIGEN3_INCLUDE_DIR) - set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) - endif() - - # 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_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() - -# Detect Eigen version: -set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h") -if (EXISTS ${EIGEN_VER_H}) - file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION) - - # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc... - - string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}") - string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}") - - string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}") - string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}") - - string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}") - string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}") - - set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}") - - message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}") -else() - message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`") -endif () - -if (MSVC) - if (BUILD_SHARED_LIBS) - # mute eigen static assert to avoid errors in shared lib - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) - endif() - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen -endif() - -if (APPLE AND BUILD_SHARED_LIBS) - # Set the default install directory on macOS - set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib") -endif() - -############################################################################### -# Global compile options - -# Build list of possible allocators -set(possible_allocators "") -if(GTSAM_USE_TBB) - list(APPEND possible_allocators TBB) - set(preferred_allocator TBB) -else() - list(APPEND possible_allocators BoostPool STL) - set(preferred_allocator STL) -endif() -if(GOOGLE_PERFTOOLS_FOUND) - list(APPEND possible_allocators tcmalloc) -endif() - -# Check if current allocator choice is valid and set cache option -list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid) -if(allocator_valid EQUAL -1) - set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE) -else() - set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator") -endif() -set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators}) -mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR) - -# Define compile flags depending on allocator -if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool") - set(GTSAM_ALLOCATOR_BOOSTPOOL 1) -elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL") - set(GTSAM_ALLOCATOR_STL 1) -elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB") - set(GTSAM_ALLOCATOR_TBB 1) -elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") - set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator - list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") -endif() - -if(MSVC) - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - list_append_cache(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_cache(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_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) - endif() -endif() - -if(GTSAM_ENABLE_CONSISTENCY_CHECKS) - # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) -endif() +include(cmake/handle_global_build_flags.cmake) # Build flags ############################################################################### # Add components @@ -477,7 +92,6 @@ endif() GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) - # Check for doxygen availability - optional dependency find_package(Doxygen) @@ -489,146 +103,11 @@ endif() # CMake Tools add_subdirectory(cmake) - -############################################################################### -# Set up CPack -set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") -set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") -set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") -set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") -set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") -set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) -set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) -set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) -set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") -#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory -#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error -set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") -set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") -set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") -set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") -#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs - -# Deb-package specific cpack -set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") - - -############################################################################### # Print configuration variables -message(STATUS "===============================================================") -message(STATUS "================ Configuration Options ======================") -print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}") -print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}") -print_config("CMake version" "${CMAKE_VERSION}") -print_config("CMake generator" "${CMAKE_GENERATOR}") -print_config("CMake build tool" "${CMAKE_BUILD_TOOL}") -message(STATUS "Build flags ") -print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests") -print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'") -print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") -if (DOXYGEN_FOUND) - print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs") -endif() -print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries") -print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name") -if(GTSAM_UNSTABLE_AVAILABLE) - print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") - print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ") - print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable") -endif() - -if(NOT MSVC AND NOT XCODE_VERSION) - print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") - print_config("Build type" "${CMAKE_BUILD_TYPE}") - print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") - print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") -endif() - -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 (Version: ${TBB_VERSION})") -elseif(TBB_FOUND) - 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() -if(GTSAM_USE_EIGEN_MKL) - print_config("Eigen will use MKL" "Yes") -elseif(MKL_FOUND) - print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled") -else() - print_config("Eigen will use MKL" "MKL not found") -endif() -if(GTSAM_USE_EIGEN_MKL_OPENMP) - print_config("Eigen will use MKL and OpenMP" "Yes") -elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) - print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") -elseif(OPENMP_FOUND AND NOT MKL_FOUND) - print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found") -elseif(OPENMP_FOUND) - print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") -else() - print_config("Eigen will use MKL and OpenMP" "OpenMP not found") -endif() -print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}") - -if(GTSAM_THROW_CHEIRALITY_EXCEPTION) - print_config("Cheirality exceptions enabled" "YES") -else() - print_config("Cheirality exceptions enabled" "NO") -endif() - -if(NOT MSVC AND NOT XCODE_VERSION) - if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) - print_config("Build with ccache" "Yes") - elseif(CCACHE_FOUND) - print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") - else() - print_config("Build with ccache" "No") - endif() -endif() - -message(STATUS "Packaging flags") -print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}") -print_config("CPack Generator" "${CPACK_GENERATOR}") - -message(STATUS "GTSAM flags ") -print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") -print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") -print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") -print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") - -message(STATUS "MATLAB toolbox flags") -print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") -if (${GTSAM_INSTALL_MATLAB_TOOLBOX}) - print_config("MATLAB root" "${MATLAB_ROOT}") - print_config("MEX binary" "${MEX_COMMAND}") -endif() -message(STATUS "Python toolbox flags ") -print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ") -if(GTSAM_BUILD_PYTHON) - print_config("Python version" ${GTSAM_PYTHON_VERSION}) -endif() - -message(STATUS "===============================================================") +include(cmake/handle_print_configuration.cmake) # Print warnings at the end -if(GTSAM_WITH_TBB AND NOT TBB_FOUND) - message(WARNING "TBB 4.4 or newer 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() +include(cmake/handle_final_checks.cmake) # Include CPack *after* all flags include(CPack) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 53dacd3f5..840d37427 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -263,3 +263,17 @@ function(gtsam_apply_build_flags target_name_) target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) endfunction(gtsam_apply_build_flags) + + +if(NOT MSVC AND NOT XCODE_VERSION) + # Set the build type to upper case for downstream use + string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER) + + # Set the GTSAM_BUILD_TAG variable. + # If build type is Release, set to blank (""), else set to the build type. + if(${CMAKE_BUILD_TYPE_UPPER} STREQUAL "RELEASE") + set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory + else() + set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}") + endif() +endif() diff --git a/cmake/handle_allocators.cmake b/cmake/handle_allocators.cmake new file mode 100644 index 000000000..63411b17b --- /dev/null +++ b/cmake/handle_allocators.cmake @@ -0,0 +1,34 @@ +# Build list of possible allocators +set(possible_allocators "") +if(GTSAM_USE_TBB) + list(APPEND possible_allocators TBB) + set(preferred_allocator TBB) +else() + list(APPEND possible_allocators BoostPool STL) + set(preferred_allocator STL) +endif() +if(GOOGLE_PERFTOOLS_FOUND) + list(APPEND possible_allocators tcmalloc) +endif() + +# Check if current allocator choice is valid and set cache option +list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid) +if(allocator_valid EQUAL -1) + set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE) +else() + set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator") +endif() +set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators}) +mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR) + +# Define compile flags depending on allocator +if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool") + set(GTSAM_ALLOCATOR_BOOSTPOOL 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL") + set(GTSAM_ALLOCATOR_STL 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB") + set(GTSAM_ALLOCATOR_TBB 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") + set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator + list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") +endif() diff --git a/cmake/handle_boost.cmake b/cmake/handle_boost.cmake new file mode 100644 index 000000000..e73c2237d --- /dev/null +++ b/cmake/handle_boost.cmake @@ -0,0 +1,56 @@ +############################################################################### +# Find boost + +# To change the path for boost, you will need to set: +# BOOST_ROOT: path to install prefix for boost +# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT + +if(MSVC) + # By default, boost only builds static libraries on windows + set(Boost_USE_STATIC_LIBS ON) # only find static libs + # If we ever reset above on windows and, ... + # 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_cache(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_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) + endif() +endif() + + +# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. +set(BOOST_FIND_MINIMUM_VERSION 1.58) +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 + NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) + message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") +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 + Boost::system + Boost::filesystem + Boost::thread + Boost::date_time + Boost::regex +) +if (GTSAM_DISABLE_NEW_TIMERS) + message("WARNING: GTSAM timing instrumentation manually disabled") + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) +else() + if(Boost_TIMER_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.") + endif() +endif() diff --git a/cmake/handle_ccache.cmake b/cmake/handle_ccache.cmake new file mode 100644 index 000000000..9eabb1905 --- /dev/null +++ b/cmake/handle_ccache.cmake @@ -0,0 +1,14 @@ +############################################################################### +# Support ccache, if installed +if(NOT MSVC AND NOT XCODE_VERSION) + find_program(CCACHE_FOUND ccache) + if(CCACHE_FOUND) + if(GTSAM_BUILD_WITH_CCACHE) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + else() + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "") + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "") + endif() + endif(CCACHE_FOUND) +endif() diff --git a/cmake/handle_cpack.cmake b/cmake/handle_cpack.cmake new file mode 100644 index 000000000..1c32433a4 --- /dev/null +++ b/cmake/handle_cpack.cmake @@ -0,0 +1,28 @@ +#JLBC: is all this actually used by someone? could it be removed? + +# Flags for choosing default packaging tools +set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") +set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") + +############################################################################### +# Set up CPack +set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") +set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") +set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") +set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) +set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") +#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory +#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error +set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") +set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs + +# Deb-package specific cpack +set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") diff --git a/cmake/handle_eigen.cmake b/cmake/handle_eigen.cmake new file mode 100644 index 000000000..690da6971 --- /dev/null +++ b/cmake/handle_eigen.cmake @@ -0,0 +1,75 @@ +############################################################################### +# Option for using system Eigen or GTSAM-bundled Eigen +### These patches only affect usage of MKL. If you want to enable MKL, you *must* +### use our patched version of Eigen +### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) +### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) +option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) + +# Switch for using system Eigen or GTSAM-bundled Eigen +if(GTSAM_USE_SYSTEM_EIGEN) + find_package(Eigen3 REQUIRED) + + # Use generic Eigen include paths e.g. + 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 + if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) + message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") + endif() + + # Check for Eigen version which doesn't work with MKL + # See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details. + if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4)) + 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 + if(EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) + endif() + + # 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_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() + +# Detect Eigen version: +set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h") +if (EXISTS ${EIGEN_VER_H}) + file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION) + + # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc... + + string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}") + string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}") + + string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}") + string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}") + + string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}") + string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}") + + set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}") + + message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}") +else() + message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`") +endif () + +if (MSVC) + if (BUILD_SHARED_LIBS) + # mute eigen static assert to avoid errors in shared lib + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) + endif() + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen +endif() diff --git a/cmake/handle_final_checks.cmake b/cmake/handle_final_checks.cmake new file mode 100644 index 000000000..f91fc7fdb --- /dev/null +++ b/cmake/handle_final_checks.cmake @@ -0,0 +1,10 @@ +# Print warnings at the end +if(GTSAM_WITH_TBB AND NOT TBB_FOUND) + message(WARNING "TBB 4.4 or newer 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() diff --git a/cmake/handle_general_options.cmake b/cmake/handle_general_options.cmake new file mode 100644 index 000000000..27d73bd86 --- /dev/null +++ b/cmake/handle_general_options.cmake @@ -0,0 +1,46 @@ +############################################################################### +# Set up options + +# See whether gtsam_unstable is available (it will be present only if we're using a git checkout) +if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") + set(GTSAM_UNSTABLE_AVAILABLE 1) +else() + set(GTSAM_UNSTABLE_AVAILABLE 0) +endif() + +# Configurable Options +if(GTSAM_UNSTABLE_AVAILABLE) + option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) + option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) + option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) +endif() +option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +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." ON) +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" 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 with pybind11" OFF) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +if(NOT MSVC AND NOT XCODE_VERSION) + option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) +endif() + +# Options relating to MATLAB wrapper +# TODO: Check for matlab mex binary before handling building of binaries +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") + +# Check / set dependent variables for MATLAB wrapper +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) +endif() + +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") +endif() diff --git a/cmake/handle_global_build_flags.cmake b/cmake/handle_global_build_flags.cmake new file mode 100644 index 000000000..f33e12b94 --- /dev/null +++ b/cmake/handle_global_build_flags.cmake @@ -0,0 +1,52 @@ +# JLBC: These should ideally be ported to "modern cmake" via target properties. +# + +if (CMAKE_GENERATOR STREQUAL "Ninja" AND + ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR + (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5))) + # Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support. + # Rationale in https://github.com/ninja-build/ninja/issues/814 + add_compile_options(-fdiagnostics-color=always) +endif() + + +# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() +# or explicit instantiation will generate build errors. +# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 +# +if(MSVC AND BUILD_SHARED_LIBS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) +endif() + +if (APPLE AND BUILD_SHARED_LIBS) + # Set the default install directory on macOS + set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib") +endif() + +############################################################################### +# Global compile options + +if(MSVC) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings + list_append_cache(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_cache(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_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + endif() +endif() + +if(GTSAM_ENABLE_CONSISTENCY_CHECKS) + # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) +endif() diff --git a/cmake/handle_mkl.cmake b/cmake/handle_mkl.cmake new file mode 100644 index 000000000..5d7ec365b --- /dev/null +++ b/cmake/handle_mkl.cmake @@ -0,0 +1,17 @@ +############################################################################### +# Find MKL +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 + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + + # --no-as-needed is required with gcc according to the MKL link advisor + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") + endif() +else() + set(GTSAM_USE_EIGEN_MKL 0) + set(EIGEN_USE_MKL_ALL 0) +endif() diff --git a/cmake/handle_openmp.cmake b/cmake/handle_openmp.cmake new file mode 100644 index 000000000..4f27aa633 --- /dev/null +++ b/cmake/handle_openmp.cmake @@ -0,0 +1,11 @@ + +############################################################################### +# Find OpenMP (if we're also using MKL) +find_package(OpenMP) # do this here to generate correct message if disabled + +if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) + if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) + set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS}) + endif() +endif() diff --git a/cmake/handle_perftools.cmake b/cmake/handle_perftools.cmake new file mode 100644 index 000000000..499caf80a --- /dev/null +++ b/cmake/handle_perftools.cmake @@ -0,0 +1,4 @@ + +############################################################################### +# Find Google perftools +find_package(GooglePerfTools) diff --git a/cmake/handle_print_configuration.cmake b/cmake/handle_print_configuration.cmake new file mode 100644 index 000000000..4ffd00e54 --- /dev/null +++ b/cmake/handle_print_configuration.cmake @@ -0,0 +1,104 @@ +############################################################################### +# Print configuration variables +message(STATUS "===============================================================") +message(STATUS "================ Configuration Options ======================") +print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}") +print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}") +print_config("CMake version" "${CMAKE_VERSION}") +print_config("CMake generator" "${CMAKE_GENERATOR}") +print_config("CMake build tool" "${CMAKE_BUILD_TOOL}") +message(STATUS "Build flags ") +print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests") +print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'") +print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") +if (DOXYGEN_FOUND) + print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs") +endif() +print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries") +print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name") +if(GTSAM_UNSTABLE_AVAILABLE) + print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") + print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ") + print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable") +endif() + +if(NOT MSVC AND NOT XCODE_VERSION) + print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") + print_config("Build type" "${CMAKE_BUILD_TYPE}") + print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") + print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") +endif() + +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 (Version: ${TBB_VERSION})") +elseif(TBB_FOUND) + 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() +if(GTSAM_USE_EIGEN_MKL) + print_config("Eigen will use MKL" "Yes") +elseif(MKL_FOUND) + print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled") +else() + print_config("Eigen will use MKL" "MKL not found") +endif() +if(GTSAM_USE_EIGEN_MKL_OPENMP) + print_config("Eigen will use MKL and OpenMP" "Yes") +elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) + print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") +elseif(OPENMP_FOUND AND NOT MKL_FOUND) + print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found") +elseif(OPENMP_FOUND) + print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") +else() + print_config("Eigen will use MKL and OpenMP" "OpenMP not found") +endif() +print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}") + +if(GTSAM_THROW_CHEIRALITY_EXCEPTION) + print_config("Cheirality exceptions enabled" "YES") +else() + print_config("Cheirality exceptions enabled" "NO") +endif() + +if(NOT MSVC AND NOT XCODE_VERSION) + if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) + print_config("Build with ccache" "Yes") + elseif(CCACHE_FOUND) + print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") + else() + print_config("Build with ccache" "No") + endif() +endif() + +message(STATUS "Packaging flags") +print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}") +print_config("CPack Generator" "${CPACK_GENERATOR}") + +message(STATUS "GTSAM flags ") +print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") +print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") +print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") + +message(STATUS "MATLAB toolbox flags") +print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") +if (${GTSAM_INSTALL_MATLAB_TOOLBOX}) + print_config("MATLAB root" "${MATLAB_ROOT}") + print_config("MEX binary" "${MEX_COMMAND}") +endif() +message(STATUS "Python toolbox flags ") +print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ") +if(GTSAM_BUILD_PYTHON) + print_config("Python version" ${GTSAM_PYTHON_VERSION}) +endif() + +message(STATUS "===============================================================") diff --git a/cmake/handle_python.cmake b/cmake/handle_python.cmake new file mode 100644 index 000000000..ac7401906 --- /dev/null +++ b/cmake/handle_python.cmake @@ -0,0 +1,26 @@ +if(GTSAM_BUILD_PYTHON) + if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") + # 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() + + 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.") + set(GTSAM_UNSTABLE_BUILD_PYTHON OFF) + endif() + endif() + + set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python") +endif() diff --git a/cmake/handle_tbb.cmake b/cmake/handle_tbb.cmake new file mode 100644 index 000000000..cedee55ea --- /dev/null +++ b/cmake/handle_tbb.cmake @@ -0,0 +1,24 @@ +############################################################################### +# Find TBB +find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) + +# Set up variables if we're using TBB +if(TBB_FOUND AND GTSAM_WITH_TBB) + set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) +else() + set(GTSAM_USE_TBB 0) # This will go into config.h +endif() + +############################################################################### +# Prohibit Timing build mode in combination with TBB +if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") +endif() diff --git a/cmake/handle_uninstall.cmake b/cmake/handle_uninstall.cmake new file mode 100644 index 000000000..1859b0273 --- /dev/null +++ b/cmake/handle_uninstall.cmake @@ -0,0 +1,10 @@ +# ---------------------------------------------------------------------------- +# Uninstall target, for "make uninstall" +# ---------------------------------------------------------------------------- +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" + IMMEDIATE @ONLY) + +add_custom_target(uninstall + "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 9abd4e31d..52d56a2b5 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -2,6 +2,10 @@ include(GtsamMatlabWrap) +# Record the root dir for gtsam - needed during external builds, e.g., ROS +set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) +message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + # Tests #message(STATUS "Installing Matlab Toolbox") install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") @@ -21,7 +25,7 @@ install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox. file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") -set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) +set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) @@ -38,4 +42,3 @@ if(GTSAM_BUILD_TYPE_POSTFIXES) else() install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) endif() - From 8b2b7476e1d304ac7a09d5315ee13674fe7b5f40 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 6 Oct 2020 22:58:21 +0200 Subject: [PATCH 088/136] Remove obsolete comments --- cmake/handle_eigen.cmake | 6 ------ 1 file changed, 6 deletions(-) diff --git a/cmake/handle_eigen.cmake b/cmake/handle_eigen.cmake index 690da6971..4aaf4f2ef 100644 --- a/cmake/handle_eigen.cmake +++ b/cmake/handle_eigen.cmake @@ -1,11 +1,5 @@ ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen -### These patches only affect usage of MKL. If you want to enable MKL, you *must* -### use our patched version of Eigen -### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) -### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) -option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) From b1c2e0174b5cdc20470354b308ff6c2cd2a883d3 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 6 Oct 2020 22:58:42 +0200 Subject: [PATCH 089/136] Use system eigen3 only if first quietly found. --- cmake/handle_eigen.cmake | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/cmake/handle_eigen.cmake b/cmake/handle_eigen.cmake index 4aaf4f2ef..69111303d 100644 --- a/cmake/handle_eigen.cmake +++ b/cmake/handle_eigen.cmake @@ -1,6 +1,22 @@ ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen +# Default: Use system Eigen if it's present: +find_package(Eigen3 QUIET) +if (Eigen3_FOUND) + set(SYS_EIGEN3_DEFAULT_ ON) +else() + set(SYS_EIGEN3_DEFAULT_ OFF) +endif() +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${SYS_EIGEN3_DEFAULT_}) +unset(SYS_EIGEN3_DEFAULT_) + +if(NOT GTSAM_USE_SYSTEM_EIGEN) + # This option only makes sense if using the embedded copy of Eigen, it is + # used to decide whether to *install* the "unsupported" module: + option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) +endif() + # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) From f9e7c7d942618c346d96704a7f0b0880e526b0a8 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 7 Oct 2020 01:42:57 -0400 Subject: [PATCH 090/136] Squashed 'wrap/' changes from 5e1373486..2192b194e 2192b194e Merge pull request #8 from borglab/fix/serialization 3a3461a35 Fix test ce3d5c35d Fix serialization git-subtree-dir: wrap git-subtree-split: 2192b194edc35142e529adcf50ed5e6803d48975 --- pybind_wrapper.py | 4 ++-- tests/expected-python/geometry_pybind.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pybind_wrapper.py b/pybind_wrapper.py index 3624d06df..8022a2f77 100755 --- a/pybind_wrapper.py +++ b/pybind_wrapper.py @@ -74,8 +74,8 @@ class PybindWrapper(object): ) .def("deserialize", []({class_inst} self, string serialized){{ - return gtsam::deserialize(serialized, self); - }}) + gtsam::deserialize(serialized, *self); + }}, py::arg("serialized")) '''.format(class_inst=cpp_class + '*')) is_method = isinstance(method, instantiator.InstantiatedMethod) diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp index 6e18f83d7..2cf104d34 100644 --- a/tests/expected-python/geometry_pybind.cpp +++ b/tests/expected-python/geometry_pybind.cpp @@ -45,8 +45,8 @@ PYBIND11_MODULE(geometry_py, m_) { ) .def("deserialize", [](gtsam::Point2* self, string serialized){ - return gtsam::deserialize(serialized, self); - }) + gtsam::deserialize(serialized, *self); + }, py::arg("serialized")) ; py::class_>(m_gtsam, "Point3") @@ -59,8 +59,8 @@ PYBIND11_MODULE(geometry_py, m_) { ) .def("deserialize", [](gtsam::Point3* self, string serialized){ - return gtsam::deserialize(serialized, self); - }) + gtsam::deserialize(serialized, *self); + }, py::arg("serialized")) .def_static("staticFunction",[](){return gtsam::Point3::staticFunction();}) .def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z")); From 82d6b8b66b8a667bde4058b3c8d0a149fcaa1414 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 7 Oct 2020 02:29:11 -0400 Subject: [PATCH 091/136] Resurrect serialization tests --- gtsam/geometry/tests/testSerializationGeometry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index f7ff881eb..aa111c3ae 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -57,7 +57,7 @@ static StereoCamera cam2(pose3, cal4ptr); static StereoPoint2 spt(1.0, 2.0, 3.0); /* ************************************************************************* */ -TEST_DISABLED (Serialization, text_geometry) { +TEST (Serialization, text_geometry) { EXPECT(equalsObj(Point2(1.0, 2.0))); EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); @@ -82,7 +82,7 @@ TEST_DISABLED (Serialization, text_geometry) { } /* ************************************************************************* */ -TEST_DISABLED (Serialization, xml_geometry) { +TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Point2(1.0, 2.0))); EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); @@ -106,7 +106,7 @@ TEST_DISABLED (Serialization, xml_geometry) { } /* ************************************************************************* */ -TEST_DISABLED (Serialization, binary_geometry) { +TEST (Serialization, binary_geometry) { EXPECT(equalsBinary(Point2(1.0, 2.0))); EXPECT(equalsBinary(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary(Rot2::fromDegrees(30.0))); From 114f069f234445bf0f2904c56be58615f3137da3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 7 Oct 2020 02:29:29 -0400 Subject: [PATCH 092/136] Add unit test for python serdes --- python/gtsam/tests/test_Pose3.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 138f1ff13..e07b904a9 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -65,6 +65,14 @@ class TestPose3(GtsamTestCase): actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_serialization(self): + """Test if serialization is working normally""" + expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + actual = Pose3() + serialized = expected.serialize() + actual.deserialize(serialized) + self.gtsamAssertEquals(expected, actual, 1e-10) + if __name__ == "__main__": unittest.main() From 16418e2fa6493d8a8983ea2b1aaa2aca86ad2bab Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 7 Oct 2020 02:29:41 -0400 Subject: [PATCH 093/136] Squashed 'wrap/' changes from 2192b194e..dfa624e77 dfa624e77 Merge pull request #9 from borglab/fix/serialization 7849665a7 Fix serialization git-subtree-dir: wrap git-subtree-split: dfa624e77e24ce3391d23c614d732fc81b4e6193 --- pybind_wrapper.py | 2 +- tests/expected-python/geometry_pybind.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pybind_wrapper.py b/pybind_wrapper.py index 8022a2f77..326d9be52 100755 --- a/pybind_wrapper.py +++ b/pybind_wrapper.py @@ -69,7 +69,7 @@ class PybindWrapper(object): return textwrap.dedent(''' .def("serialize", []({class_inst} self){{ - return gtsam::serialize(self); + return gtsam::serialize(*self); }} ) .def("deserialize", diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp index 2cf104d34..3eee55bf4 100644 --- a/tests/expected-python/geometry_pybind.cpp +++ b/tests/expected-python/geometry_pybind.cpp @@ -40,7 +40,7 @@ PYBIND11_MODULE(geometry_py, m_) { .def("vectorConfusion",[](gtsam::Point2* self){return self->vectorConfusion();}) .def("serialize", [](gtsam::Point2* self){ - return gtsam::serialize(self); + return gtsam::serialize(*self); } ) .def("deserialize", @@ -54,7 +54,7 @@ PYBIND11_MODULE(geometry_py, m_) { .def("norm",[](gtsam::Point3* self){return self->norm();}) .def("serialize", [](gtsam::Point3* self){ - return gtsam::serialize(self); + return gtsam::serialize(*self); } ) .def("deserialize", From 8cb22624e0520315f402cf2255857301634a9041 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Oct 2020 17:02:39 +0200 Subject: [PATCH 094/136] Use camel case for cmake files --- CMakeLists.txt | 30 +++++++++---------- ...llocators.cmake => HandleAllocators.cmake} | 0 .../{handle_boost.cmake => HandleBoost.cmake} | 0 ...handle_ccache.cmake => HandleCCache.cmake} | 0 .../{handle_cpack.cmake => HandleCPack.cmake} | 0 .../{handle_eigen.cmake => HandleEigen.cmake} | 0 ...l_checks.cmake => HandleFinalChecks.cmake} | 0 ...tions.cmake => HandleGeneralOptions.cmake} | 0 ...ags.cmake => HandleGlobalBuildFlags.cmake} | 0 cmake/{handle_mkl.cmake => HandleMKL.cmake} | 0 ...handle_openmp.cmake => HandleOpenMP.cmake} | 0 ..._perftools.cmake => HandlePerfTools.cmake} | 0 ...n.cmake => HandlePrintConfiguration.cmake} | 0 ...handle_python.cmake => HandlePython.cmake} | 0 cmake/{handle_tbb.cmake => HandleTBB.cmake} | 0 ..._uninstall.cmake => HandleUninstall.cmake} | 0 16 files changed, 15 insertions(+), 15 deletions(-) rename cmake/{handle_allocators.cmake => HandleAllocators.cmake} (100%) rename cmake/{handle_boost.cmake => HandleBoost.cmake} (100%) rename cmake/{handle_ccache.cmake => HandleCCache.cmake} (100%) rename cmake/{handle_cpack.cmake => HandleCPack.cmake} (100%) rename cmake/{handle_eigen.cmake => HandleEigen.cmake} (100%) rename cmake/{handle_final_checks.cmake => HandleFinalChecks.cmake} (100%) rename cmake/{handle_general_options.cmake => HandleGeneralOptions.cmake} (100%) rename cmake/{handle_global_build_flags.cmake => HandleGlobalBuildFlags.cmake} (100%) rename cmake/{handle_mkl.cmake => HandleMKL.cmake} (100%) rename cmake/{handle_openmp.cmake => HandleOpenMP.cmake} (100%) rename cmake/{handle_perftools.cmake => HandlePerfTools.cmake} (100%) rename cmake/{handle_print_configuration.cmake => HandlePrintConfiguration.cmake} (100%) rename cmake/{handle_python.cmake => HandlePython.cmake} (100%) rename cmake/{handle_tbb.cmake => HandleTBB.cmake} (100%) rename cmake/{handle_uninstall.cmake => HandleUninstall.cmake} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 831ee00f3..35c487fd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,21 +38,21 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() -include(cmake/handle_boost.cmake) # Boost -include(cmake/handle_ccache.cmake) # ccache -include(cmake/handle_cpack.cmake) # CPack -include(cmake/handle_eigen.cmake) # Eigen3 -include(cmake/handle_general_options.cmake) # CMake build options -include(cmake/handle_mkl.cmake) # MKL -include(cmake/handle_openmp.cmake) # OpenMP -include(cmake/handle_perftools.cmake) # Google perftools -include(cmake/handle_python.cmake) # Python options and commands -include(cmake/handle_tbb.cmake) # TBB -include(cmake/handle_uninstall.cmake) # for "make uninstall" +include(cmake/HandleBoost.cmake) # Boost +include(cmake/HandleCCache.cmake) # ccache +include(cmake/HandleCPack.cmake) # CPack +include(cmake/HandleEigen.cmake) # Eigen3 +include(cmake/HandleGeneralOptions.cmake) # CMake build options +include(cmake/HandleMKL.cmake) # MKL +include(cmake/HandleOpenMP.cmake) # OpenMP +include(cmake/HandlePerfTools.cmake) # Google perftools +include(cmake/HandlePython.cmake) # Python options and commands +include(cmake/HandleTBB.cmake) # TBB +include(cmake/HandleUninstall.cmake) # for "make uninstall" -include(cmake/handle_allocators.cmake) # Must be after tbb, pertools +include(cmake/HandleAllocators.cmake) # Must be after tbb, pertools -include(cmake/handle_global_build_flags.cmake) # Build flags +include(cmake/HandleGlobalBuildFlags.cmake) # Build flags ############################################################################### # Add components @@ -104,10 +104,10 @@ endif() add_subdirectory(cmake) # Print configuration variables -include(cmake/handle_print_configuration.cmake) +include(cmake/HandlePrintConfiguration.cmake) # Print warnings at the end -include(cmake/handle_final_checks.cmake) +include(cmake/HandleFinalChecks.cmake) # Include CPack *after* all flags include(CPack) diff --git a/cmake/handle_allocators.cmake b/cmake/HandleAllocators.cmake similarity index 100% rename from cmake/handle_allocators.cmake rename to cmake/HandleAllocators.cmake diff --git a/cmake/handle_boost.cmake b/cmake/HandleBoost.cmake similarity index 100% rename from cmake/handle_boost.cmake rename to cmake/HandleBoost.cmake diff --git a/cmake/handle_ccache.cmake b/cmake/HandleCCache.cmake similarity index 100% rename from cmake/handle_ccache.cmake rename to cmake/HandleCCache.cmake diff --git a/cmake/handle_cpack.cmake b/cmake/HandleCPack.cmake similarity index 100% rename from cmake/handle_cpack.cmake rename to cmake/HandleCPack.cmake diff --git a/cmake/handle_eigen.cmake b/cmake/HandleEigen.cmake similarity index 100% rename from cmake/handle_eigen.cmake rename to cmake/HandleEigen.cmake diff --git a/cmake/handle_final_checks.cmake b/cmake/HandleFinalChecks.cmake similarity index 100% rename from cmake/handle_final_checks.cmake rename to cmake/HandleFinalChecks.cmake diff --git a/cmake/handle_general_options.cmake b/cmake/HandleGeneralOptions.cmake similarity index 100% rename from cmake/handle_general_options.cmake rename to cmake/HandleGeneralOptions.cmake diff --git a/cmake/handle_global_build_flags.cmake b/cmake/HandleGlobalBuildFlags.cmake similarity index 100% rename from cmake/handle_global_build_flags.cmake rename to cmake/HandleGlobalBuildFlags.cmake diff --git a/cmake/handle_mkl.cmake b/cmake/HandleMKL.cmake similarity index 100% rename from cmake/handle_mkl.cmake rename to cmake/HandleMKL.cmake diff --git a/cmake/handle_openmp.cmake b/cmake/HandleOpenMP.cmake similarity index 100% rename from cmake/handle_openmp.cmake rename to cmake/HandleOpenMP.cmake diff --git a/cmake/handle_perftools.cmake b/cmake/HandlePerfTools.cmake similarity index 100% rename from cmake/handle_perftools.cmake rename to cmake/HandlePerfTools.cmake diff --git a/cmake/handle_print_configuration.cmake b/cmake/HandlePrintConfiguration.cmake similarity index 100% rename from cmake/handle_print_configuration.cmake rename to cmake/HandlePrintConfiguration.cmake diff --git a/cmake/handle_python.cmake b/cmake/HandlePython.cmake similarity index 100% rename from cmake/handle_python.cmake rename to cmake/HandlePython.cmake diff --git a/cmake/handle_tbb.cmake b/cmake/HandleTBB.cmake similarity index 100% rename from cmake/handle_tbb.cmake rename to cmake/HandleTBB.cmake diff --git a/cmake/handle_uninstall.cmake b/cmake/HandleUninstall.cmake similarity index 100% rename from cmake/handle_uninstall.cmake rename to cmake/HandleUninstall.cmake From 69b2cacbe7d408e92de011a82116007bcf03f590 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Oct 2020 17:03:20 +0200 Subject: [PATCH 095/136] Revert use system Eigen if found --- cmake/HandleEigen.cmake | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake index 69111303d..fda441907 100644 --- a/cmake/HandleEigen.cmake +++ b/cmake/HandleEigen.cmake @@ -1,15 +1,7 @@ ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen -# Default: Use system Eigen if it's present: -find_package(Eigen3 QUIET) -if (Eigen3_FOUND) - set(SYS_EIGEN3_DEFAULT_ ON) -else() - set(SYS_EIGEN3_DEFAULT_ OFF) -endif() -option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${SYS_EIGEN3_DEFAULT_}) -unset(SYS_EIGEN3_DEFAULT_) +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) if(NOT GTSAM_USE_SYSTEM_EIGEN) # This option only makes sense if using the embedded copy of Eigen, it is From 93825d0bc72a7d1764e1c9a5f3d8431132c8e63b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 8 Oct 2020 07:23:35 +0000 Subject: [PATCH 096/136] Wrapping SfmCamera to be used with GeneralSFMFactor --- gtsam/gtsam.i | 16 ++++++++-------- python/gtsam/examples/SFMExample_bal.py | 9 ++++----- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 1a239541d..6665a532e 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1106,8 +1106,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -//typedef gtsam::PinholeCamera SfmCamera; +typedef gtsam::PinholeCamera SfmCamera; #include class StereoCamera { @@ -2069,7 +2068,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template + template void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2163,7 +2162,7 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - // void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void insert(size_t j, const gtsam::SfmCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); @@ -2181,13 +2180,13 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - // void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void update(size_t j, const gtsam::SfmCamera& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double @@ -2491,7 +2490,8 @@ class ISAM2 { template + gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::SfmCamera, + Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -2674,7 +2674,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorSfmCamera; template diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 258ec8917..a5cbcf05f 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -18,8 +18,8 @@ import numpy as np import gtsam from gtsam import ( - #GeneralSFMFactorCal3Bundler, - PinholeCameraCal3Bundler, + GeneralSFMFactorSfmCamera, + SfmCamera, PriorFactorSfmCamera, readBal, symbol_shorthand @@ -31,7 +31,7 @@ P = symbol_shorthand.P import pdb # We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration +# An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration # and has a total of 9 free parameters def run(args): @@ -63,7 +63,7 @@ def run(args): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P - #graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) + graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) j += 1 pdb.set_trace() @@ -118,4 +118,3 @@ if __name__ == "__main__": - From c97af55c63534646f2bd026379d40a6155c2dcd6 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Oct 2020 23:33:53 -0400 Subject: [PATCH 097/136] remove breakpoints --- python/gtsam/examples/SFMExample_bal.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index a5cbcf05f..169cf9f3c 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,11 +7,12 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: John Lambert + Author: Frank Dellaert (Python: Akshay Krishan, John Lambert) """ import argparse import logging +import sys import matplotlib.pyplot as plt import numpy as np @@ -28,7 +29,8 @@ from gtsam import ( C = symbol_shorthand.C P = symbol_shorthand.P -import pdb + +logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) # We will be using a projection factor that ties a SFM_Camera to a 3D point. # An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration @@ -42,7 +44,6 @@ def run(args): else: input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") - pdb.set_trace() # # Load the SfM data from file mydata = readBal(input_file) logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") @@ -55,7 +56,6 @@ def run(args): # 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 @@ -65,7 +65,6 @@ def run(args): # note use of shorthand symbols C and P graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) 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 From ac9077ff67f5ca36790203ac417982e9e88a52e4 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 10 Oct 2020 08:57:09 +0000 Subject: [PATCH 098/136] Renaming SFMCamera to PinholeCameraCal3Bundler --- gtsam/gtsam.i | 17 ++++++++--------- python/gtsam/examples/SFMExample_bal.py | 16 ++++++---------- 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 6665a532e..2a513322d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1106,7 +1106,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera SfmCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; #include class StereoCamera { @@ -2068,7 +2068,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template + template, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2162,7 +2162,7 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::SfmCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); @@ -2180,13 +2180,13 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::SfmCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> T at(size_t j); /// version for double @@ -2490,7 +2490,7 @@ class ISAM2 { template , Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2529,7 +2529,7 @@ class NonlinearISAM { #include #include -template +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2674,8 +2674,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorSfmCamera; - +typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 169cf9f3c..a027dac89 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -19,9 +19,9 @@ import numpy as np import gtsam from gtsam import ( - GeneralSFMFactorSfmCamera, - SfmCamera, - PriorFactorSfmCamera, + GeneralSFMFactorCal3Bundler, + PinholeCameraCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, readBal, symbol_shorthand ) @@ -32,10 +32,6 @@ P = symbol_shorthand.P logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) -# We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration -# and has a total of 9 free parameters - 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 @@ -63,13 +59,13 @@ def run(args): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P - graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) + graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) j += 1 # # 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.PriorFactorSfmCamera( + gtsam.PriorFactorPinholeCameraCal3Bundler( C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) ) ) @@ -83,7 +79,7 @@ def run(args): initial = gtsam.Values() i = 0 - # add each SfmCamera + # add each PinholeCameraCal3Bundler for cam_idx in range(mydata.number_cameras()): camera = mydata.camera(cam_idx) initial.insert(C(i), camera) From d4c801bb6bff04f924edccc847f97359b0906217 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 10 Oct 2020 12:39:05 -0400 Subject: [PATCH 099/136] Fix LLVM repo keys --- .github/workflows/build-linux.yml | 2 ++ .github/workflows/build-python.yml | 2 ++ .github/workflows/build-special.yml | 2 ++ 3 files changed, 6 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index afe328c3b..7553675c8 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -50,6 +50,8 @@ jobs: run: | # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 + gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index b8d6bc311..d4796e2bb 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -63,6 +63,8 @@ jobs: run: | # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 + gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 648365f24..c314acb16 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -56,6 +56,8 @@ jobs: run: | # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 + gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update From bda6222da40e537772657958172cb01da11704b6 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 11 Oct 2020 16:46:10 -0400 Subject: [PATCH 100/136] python wrapper for sfmdata --- gtsam/gtsam.i | 17 +++++++++++++++-- gtsam/slam/dataset.cpp | 12 ++++++------ gtsam/slam/dataset.h | 30 +++++++++++++++++++++++++++--- python/CMakeLists.txt | 2 ++ python/gtsam/preamble.h | 2 ++ python/gtsam/specializations.h | 2 ++ 6 files changed, 54 insertions(+), 11 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 880b1d4c7..73ce4f203 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,21 +2759,34 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +class SfmMeasurement{}; +class SiftIndex{ }; +class SfmMeasurements{}; + class SfmTrack { + SfmTrack(); Point3 point3() const; size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; + gtsam::SfmMeasurement measurement(size_t idx) const; + gtsam::SiftIndex siftIndex(size_t idx) const; + // gtsam::Measurements add_measurement(pair m); + void add_measurement(pair m); + SfmMeasurements& measurements(); }; class SfmData { + SfmData(); size_t number_cameras() const; size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; + // std::vector add_track(gtsam::SfmTrack t); + void add_track(gtsam::SfmTrack t); + void delete_track(size_t idx); }; gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 270dbeb95..9b610b231 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1052,13 +1052,13 @@ bool readBundler(const string &filename, SfmData &data) { size_t nvisible = 0; is >> nvisible; - track.measurements.reserve(nvisible); + track.Measurements.reserve(nvisible); track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.Measurements.emplace_back(cam_idx, Point2(u, -v)); track.siftIndices.emplace_back(cam_idx, point_idx); } @@ -1089,7 +1089,7 @@ bool readBAL(const string &filename, SfmData &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + data.tracks[j].Measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -1163,7 +1163,7 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j - size_t i = track.measurements[k].first; // camera id + size_t i = track.Measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); @@ -1173,9 +1173,9 @@ bool writeBAL(const string &filename, SfmData &data) { << endl; } - double pixelBALx = track.measurements[k].second.x() - + double pixelBALx = track.Measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - + double pixelBALy = -(track.Measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); os << i /*camera id*/ << " " << j /*point id*/ << " " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a8fddcfe4..5a206929c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -210,6 +210,7 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; +typedef std::vector SfmMeasurements; /// SfmTrack typedef std::pair SiftIndex; @@ -219,15 +220,16 @@ struct SfmTrack { SfmTrack(): p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point - std::vector measurements; ///< The 2D image projections (id,(u,v)) + std::vector Measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; + /// Total number of measurements in this track size_t number_measurements() const { - return measurements.size(); + return Measurements.size(); } /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { - return measurements[idx]; + return Measurements[idx]; } /// Get the SIFT feature index corresponding to the measurement at `idx` SiftIndex siftIndex(size_t idx) const { @@ -236,13 +238,27 @@ struct SfmTrack { Point3 point3() const { return p; } + void add_measurement(pair m) { + Measurements.push_back(m); + } + + SfmMeasurements& measurements() { + return Measurements; + } + + void clear() { + Measurements.clear(); + } + }; + /// Define the structure for the camera poses typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { + std::vector Measurements; std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { @@ -260,6 +276,14 @@ struct SfmData { SfmTrack track(size_t idx) const { return tracks[idx]; } + + void add_track(SfmTrack t) { + tracks.push_back(t); + } + /// Delete track at `idx` + void delete_track(size_t idx){ + tracks[idx].clear(); + } }; /** diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 00b537340..322968694 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,6 +36,8 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector + gtsam::SfmMeasurement + gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 6166f615e..8525ed3a3 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index cacad874c..03e2b606c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,3 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_vector >(m_, "Measurement"); +py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file From 22e64ac82d6dee47ba3640b442d3e0c85d4997ee Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 12 Oct 2020 11:54:24 -0400 Subject: [PATCH 101/136] Add comments --- .github/workflows/build-linux.yml | 3 ++- .github/workflows/build-python.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 7553675c8..2195ad05c 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -48,8 +48,9 @@ jobs: - name: Install (Linux) if: runner.os == 'Linux' run: | - # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. + # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index d4796e2bb..e348e3125 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -61,8 +61,9 @@ jobs: - name: Install (Linux) if: runner.os == 'Linux' run: | - # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. + # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" From 8a7ce130ad99bf0054c8786d2f516ebb99a91545 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 14:32:21 -0400 Subject: [PATCH 102/136] Fix warning on clang --- cmake/GtsamBuildTypes.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 840d37427..d13e3aa12 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -109,7 +109,7 @@ else() ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR (CMAKE_CXX_COMPILER_ID MATCHES "GNU") ) - set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + # set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON From 25d801bd15eeda8244b36ae384eb8c5d318966db Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 14 Oct 2020 15:22:23 -0400 Subject: [PATCH 103/136] use argparse defaults --- python/gtsam/examples/SFMExample_bal.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index a027dac89..eda143011 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -34,11 +34,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) 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") + input_file = gtsam.findExampleDataFile(args.input_file) # # Load the SfM data from file mydata = readBal(input_file) @@ -62,13 +58,13 @@ def run(args): graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) j += 1 - # # 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 + # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( gtsam.PriorFactorPinholeCameraCal3Bundler( C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) ) ) + # Also add a prior on the position of the first landmark to fix the scale graph.push_back( gtsam.PriorFactorPoint3( P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) @@ -107,7 +103,7 @@ def run(args): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('-i', '--input_file', type=str, default="", + parser.add_argument('-i', '--input_file', type=str, default="dubrovnik-3-7-pre", help='Read SFM data from the specified BAL file') run(parser.parse_args()) From c9d719cb1f2992c10d43afa48ebb5c2206e142f0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 14 Oct 2020 16:03:14 -0400 Subject: [PATCH 104/136] make a note about how the eror drops --- python/gtsam/examples/SFMExample_bal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index eda143011..b5701669e 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -97,7 +97,7 @@ def run(args): except Exception as e: logging.exception("LM Optimization failed") return - + # Error drops from 2764.22 to 0.046 logging.info(f"final error: {graph.error(result)}") From d4bdaf80800487e96c56f59d748833598f1cecff Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 14 Oct 2020 16:42:54 -0400 Subject: [PATCH 105/136] Add comments --- .github/workflows/build-linux.yml | 1 + .github/workflows/build-python.yml | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 2195ad05c..7aa818d04 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -50,6 +50,7 @@ jobs: run: | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # pool.sks-keyservers.net is the SKS GPG global keyserver pool # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index e348e3125..5b9f7418b 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -63,6 +63,7 @@ jobs: run: | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # pool.sks-keyservers.net is the SKS GPG global keyserver pool # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - From 2b8e9f44fada685d7b73fd47d84c15b35b449785 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 14 Oct 2020 16:44:28 -0400 Subject: [PATCH 106/136] Add comments --- .github/workflows/build-linux.yml | 1 + .github/workflows/build-python.yml | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 7aa818d04..d90aad13c 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -52,6 +52,7 @@ jobs: if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then # pool.sks-keyservers.net is the SKS GPG global keyserver pool # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository + # This key is not in the keystore by default for Ubuntu so we need to add it. gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 5b9f7418b..724f56dda 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -65,6 +65,7 @@ jobs: if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then # pool.sks-keyservers.net is the SKS GPG global keyserver pool # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository + # This key is not in the keystore by default for Ubuntu so we need to add it. gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" From b4bbad32df891a1f1c8178d2e5af38a3e8b85983 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 14 Oct 2020 21:33:13 -0400 Subject: [PATCH 107/136] fix typo --- python/gtsam/examples/SFMExample_bal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index b5701669e..77d79ca24 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,7 +7,7 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishan, John Lambert) + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) """ import argparse From 4c6b488344b701f0313be108a80a54b571d129e0 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 14 Oct 2020 21:44:07 -0400 Subject: [PATCH 108/136] Fix inteminent sks keyserver failure --- .github/workflows/build-linux.yml | 5 +++-- .github/workflows/build-python.yml | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index d90aad13c..d0b16d2ee 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -53,8 +53,9 @@ jobs: # pool.sks-keyservers.net is the SKS GPG global keyserver pool # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. - gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 - gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - + LLVM_KEY=15CF4D18AF4F7421 + gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 724f56dda..1c4953165 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -61,13 +61,13 @@ jobs: - name: Install (Linux) if: runner.os == 'Linux' run: | - # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then # pool.sks-keyservers.net is the SKS GPG global keyserver pool # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. - gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 - gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - + LLVM_KEY=15CF4D18AF4F7421 + gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update From 91e0bb1170ed2f007345878233957f92212e82c6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 15 Oct 2020 21:14:48 -0400 Subject: [PATCH 109/136] Add notes --- .github/workflows/build-linux.yml | 3 ++- .github/workflows/build-python.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index d0b16d2ee..87a0430f8 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -50,7 +50,8 @@ jobs: run: | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then - # pool.sks-keyservers.net is the SKS GPG global keyserver pool + # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool + # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. LLVM_KEY=15CF4D18AF4F7421 diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 1c4953165..fbec491f2 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -62,7 +62,8 @@ jobs: if: runner.os == 'Linux' run: | if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then - # pool.sks-keyservers.net is the SKS GPG global keyserver pool + # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool + # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. LLVM_KEY=15CF4D18AF4F7421 From b1c53000f7a1466100086c230529b327f742017d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 17 Oct 2020 12:47:57 -0400 Subject: [PATCH 110/136] clean up comments --- python/gtsam/examples/SFMExample_bal.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77d79ca24..3ce465f6f 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -36,14 +36,14 @@ def run(args): """ Run LM optimization with BAL input data and report resulting error """ input_file = gtsam.findExampleDataFile(args.input_file) - # # Load the SfM data from file + # 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 + # Create a factor graph graph = gtsam.NonlinearFactorGraph() - # # We share *one* noiseModel between all projection factors + # 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 @@ -71,7 +71,7 @@ def run(args): ) ) - # # Create initial estimate + # Create initial estimate initial = gtsam.Values() i = 0 @@ -97,7 +97,7 @@ def run(args): except Exception as e: logging.exception("LM Optimization failed") return - # Error drops from 2764.22 to 0.046 + # Error drops from ~2764.22 to ~0.046 logging.info(f"final error: {graph.error(result)}") @@ -107,5 +107,3 @@ if __name__ == "__main__": help='Read SFM data from the specified BAL file') run(parser.parse_args()) - - From ed387e38173d837cf8c421eab649483b5e560231 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 11:17:10 -0400 Subject: [PATCH 111/136] unittested features in SfmData --- gtsam/gtsam.i | 7 +-- gtsam/slam/dataset.h | 15 +++--- python/CMakeLists.txt | 1 + python/gtsam/preamble.h | 3 +- python/gtsam/specializations.h | 3 +- python/gtsam/tests/test_SfmData.py | 79 ++++++++++++++++++++++++++++++ 6 files changed, 96 insertions(+), 12 deletions(-) create mode 100644 python/gtsam/tests/test_SfmData.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 73ce4f203..b3792fbec 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,17 +2759,19 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +// Dummy classes, for MATLAB wrappers class SfmMeasurement{}; class SiftIndex{ }; class SfmMeasurements{}; +class SfmCamera{}; class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; + void setP(gtsam::Point3& p_); gtsam::SfmMeasurement measurement(size_t idx) const; gtsam::SiftIndex siftIndex(size_t idx) const; - // gtsam::Measurements add_measurement(pair m); void add_measurement(pair m); SfmMeasurements& measurements(); }; @@ -2780,9 +2782,8 @@ class SfmData { size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; - // std::vector add_track(gtsam::SfmTrack t); void add_track(gtsam::SfmTrack t); - void delete_track(size_t idx); + void add_camera(gtsam::SfmCamera cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5a206929c..2e4c715be 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -227,6 +227,11 @@ struct SfmTrack { size_t number_measurements() const { return Measurements.size(); } + /// Set 3D point + void setP(Point3& p_){ + p = p_; + } + /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return Measurements[idx]; @@ -246,10 +251,6 @@ struct SfmTrack { return Measurements; } - void clear() { - Measurements.clear(); - } - }; @@ -280,9 +281,9 @@ struct SfmData { void add_track(SfmTrack t) { tracks.push_back(t); } - /// Delete track at `idx` - void delete_track(size_t idx){ - tracks[idx].clear(); + + void add_camera(SfmCamera cam){ + cameras.push_back(cam); } }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 322968694..7d12b9800 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -37,6 +37,7 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::SfmMeasurement + gtsam::SfmCamera gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 8525ed3a3..e6ed64689 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,4 +11,5 @@ PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 03e2b606c..dcaaa4c76 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -14,4 +14,5 @@ py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector >(m_, "Measurement"); -py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file +py::bind_vector >(m_, "SiftIndexVector"); +py::bind_vector >(m_, "cameras"); \ No newline at end of file diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py new file mode 100644 index 000000000..4664d746e --- /dev/null +++ b/python/gtsam/tests/test_SfmData.py @@ -0,0 +1,79 @@ +""" +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 (Python: Sushmita Warrier) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import numpy as np + +import gtsam +#from gtsam import SfmCamera +from gtsam.utils.test_case import GtsamTestCase + + +class TestSfmData(GtsamTestCase): + """Tests for SfmData and SfmTrack modules.""" + + def setUp(self): + """Initialize SfmData and SfmTrack""" + self.data = gtsam.SfmData() + self.tracks = gtsam.SfmTrack() + + def test_tracks(self): + """Test functions in SfmTrack""" + # measurement is of format (camera_idx, imgPoint) + # create camera indices for two cameras + i1, i2 = np.random.randint(5), np.random.randint(5) + # create imgPoint for cameras i1 and i2 + uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + m_i1 = (i1, uv_i1) + m_i2 = (i2, uv_i2) + # add measurements to the track + self.tracks.add_measurement(m_i1) + self.tracks.add_measurement(m_i2) + # Number of measurements in the track is 2 + self.assertEqual(self.tracks.number_measurements(), 2) + # camera_idx in the first measurement of the track corresponds to i1 + self.assertEqual(self.tracks.measurement(0)[0], i1) + # Set arbitrary 3D point corresponding to the track + self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2)) + np.testing.assert_array_almost_equal( + gtsam.Point3(2.5,3.3,1.2), + self.tracks.point3() + ) + + + def test_data(self): + """Test functions in SfmData""" + #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) + # Create new track with 3 measurements + track2 = gtsam.SfmTrack() + i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5) + uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) + # add measurements to the track + track2.add_measurement(m_i1) + track2.add_measurement(m_i2) + track2.add_measurement(m_i3) + self.data.add_track(self.tracks) + self.data.add_track(track2) + # Number of tracks in SfmData is 2 + self.assertEqual(self.data.number_tracks(), 2) + # camera idx of first measurement of second track corresponds to i1 + self.assertEqual(self.data.track(1).measurement(0)[0], i1) + +if __name__ == '__main__': + unittest.main() From 1f09f4aab680f49da367d2ceb3639356bd20746f Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 19:05:09 -0400 Subject: [PATCH 112/136] python wrapped SfmData and SfmTrack --- gtsam/slam/SmartFactorBase.h | 4 ++-- gtsam/slam/tests/testDataset.cpp | 16 ++++++++-------- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 8 ++++---- gtsam/slam/tests/testSmartProjectionFactor.cpp | 4 ++-- tests/testGeneralSFMFactorB.cpp | 2 +- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9f2b9c3d..5afc159bd 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -144,8 +144,8 @@ protected: template void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { - this->measured_.push_back(trackToAdd.measurements[k].second); - this->keys_.push_back(trackToAdd.measurements[k].first); + this->measured_.push_back(trackToAdd.Measurements[k].second); + this->keys_.push_back(trackToAdd.Measurements[k].first); } } diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index aad9124c5..ff5cf8b46 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -166,9 +166,9 @@ TEST( dataSet, Balbianello) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,1)); } @@ -476,9 +476,9 @@ TEST( dataSet, readBAL_Dubrovnik) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,12)); } @@ -557,8 +557,8 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); - EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); + EXPECT_LONGS_EQUAL(expectedTrack.Measurements[k].first,actualTrack.Measurements[k].first); + EXPECT(assert_equal(expectedTrack.Measurements[k].second,actualTrack.Measurements[k].second)); } } } @@ -600,9 +600,9 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..6bece036f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -46,10 +46,10 @@ EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; + return data.tracks[i].Measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; + return data.tracks[i].Measurements[1].second; } Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); @@ -367,10 +367,10 @@ EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; + return data.tracks[i].Measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; + return data.tracks[i].Measurements[1].second; } boost::shared_ptr // diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1fd06cc9f..1f3a5d1cb 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -283,14 +283,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfmTrack track1; for (size_t i = 0; i < 3; ++i) { - track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); + track1.Measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfmTrack track2; for (size_t i = 0; i < 3; ++i) { - track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); + track2.Measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 05b4c7f66..fafe87d13 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].measurements) + for (const SfmMeasurement& m: db.tracks[j].Measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } From b0cca7eeddbc280aa1c4eaa4c8588e44905159f1 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 19:05:42 -0400 Subject: [PATCH 113/136] python wrapper sfmtrack reflected in other files --- examples/CreateSFMExampleData.cpp | 2 +- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6..602d396d9 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -51,7 +51,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.Measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 3768ee2a3..fd9ef5648 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for (const SfmMeasurement& m : track.measurements) { + for (const SfmMeasurement& m : track.Measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index ffb5b195b..4572d8424 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,7 +55,7 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.measurements) { + for(const SfmMeasurement& m: track.Measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b79a9fa28..d378d89c3 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for (const SfmTrack& track : mydata.tracks) { - for (const SfmMeasurement& m : track.measurements) { + for (const SfmMeasurement& m : track.Measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared( From 0d88438a2a47d2af2bc7914b662f8623ad8294dd Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 19 Oct 2020 09:08:54 -0400 Subject: [PATCH 114/136] renamed myData to scene_data, and explained BAL parameterization --- python/gtsam/examples/SFMExample_bal.py | 33 ++++++++++++++++--------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77d79ca24..34696700c 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -37,8 +37,8 @@ def run(args): input_file = gtsam.findExampleDataFile(args.input_file) # # Load the SfM data from file - mydata = readBal(input_file) - logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") + scene_data = readBal(input_file) + logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") # # Create a factor graph graph = gtsam.NonlinearFactorGraph() @@ -48,8 +48,8 @@ def run(args): # Add measurements to the factor graph j = 0 - for t_idx in range(mydata.number_tracks()): - track = mydata.track(t_idx) # SfmTrack + for t_idx in range(scene_data.number_tracks()): + track = scene_data.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 @@ -61,13 +61,13 @@ def run(args): # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( gtsam.PriorFactorPinholeCameraCal3Bundler( - C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) + C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) ) ) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( gtsam.PriorFactorPoint3( - P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) ) ) @@ -76,15 +76,15 @@ def run(args): i = 0 # add each PinholeCameraCal3Bundler - for cam_idx in range(mydata.number_cameras()): - camera = mydata.camera(cam_idx) + for cam_idx in range(scene_data.number_cameras()): + camera = scene_data.camera(cam_idx) initial.insert(C(i), camera) i += 1 j = 0 # add each SfmTrack - for t_idx in range(mydata.number_tracks()): - track = mydata.track(t_idx) + for t_idx in range(scene_data.number_tracks()): + track = scene_data.track(t_idx) initial.insert(P(j), track.point3()) j += 1 @@ -103,8 +103,17 @@ def run(args): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('-i', '--input_file', type=str, default="dubrovnik-3-7-pre", - help='Read SFM data from the specified BAL file') + parser.add_argument( + '-i', + '--input_file', + type=str, + default="dubrovnik-3-7-pre", + help='Read SFM data from the specified BAL file' + 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' + 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' + 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' + 'and (x,y,z) 3d point initializations.' + ) run(parser.parse_args()) From 5be4571d5d06b18a64b2f18bcb80a0c188f6b477 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 19 Oct 2020 14:32:36 -0400 Subject: [PATCH 115/136] update list of C++ examples that have been ported to Python --- 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 e05a0c7e1..2a2c9f2ef 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 | | +| SFMExample_bal | :heavy_check_mark: | | SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | From 045780a151513a1723efce05fc95cdec28e14673 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Wed, 21 Oct 2020 23:43:17 -0400 Subject: [PATCH 116/136] changed Measurements to measurements --- examples/CreateSFMExampleData.cpp | 2 +- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 +- gtsam/gtsam.i | 25 +++++++++++++------ gtsam/slam/SmartFactorBase.h | 4 +-- gtsam/slam/dataset.cpp | 12 ++++----- gtsam/slam/dataset.h | 18 ++++++------- gtsam/slam/tests/testDataset.cpp | 16 ++++++------ .../slam/tests/testEssentialMatrixFactor.cpp | 8 +++--- .../slam/tests/testSmartProjectionFactor.cpp | 4 +-- python/gtsam/preamble.h | 3 +-- python/gtsam/specializations.h | 3 +-- tests/testGeneralSFMFactorB.cpp | 2 +- 14 files changed, 55 insertions(+), 48 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 602d396d9..aa0bde8f6 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -51,7 +51,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.Measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index fd9ef5648..3768ee2a3 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for (const SfmMeasurement& m : track.Measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 4572d8424..ffb5b195b 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,7 +55,7 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.Measurements) { + for(const SfmMeasurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index d378d89c3..b79a9fa28 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for (const SfmTrack& track : mydata.tracks) { - for (const SfmMeasurement& m : track.Measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared( diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b3792fbec..5334a233d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,11 +2759,20 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -// Dummy classes, for MATLAB wrappers -class SfmMeasurement{}; -class SiftIndex{ }; -class SfmMeasurements{}; -class SfmCamera{}; +class SfmMeasurement{ + SfmMeasurement(); + size_t i() const; + Point2 j() const; +}; +class SiftIndex{ + SiftIndex(); + size_t i() const; + size_t j() const; + }; +class SfmMeasurements{ + SfmMeasurements(); + +}; class SfmTrack { SfmTrack(); @@ -2772,7 +2781,7 @@ class SfmTrack { void setP(gtsam::Point3& p_); gtsam::SfmMeasurement measurement(size_t idx) const; gtsam::SiftIndex siftIndex(size_t idx) const; - void add_measurement(pair m); + void add_measurement(const pair& m); SfmMeasurements& measurements(); }; @@ -2782,8 +2791,8 @@ class SfmData { size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; - void add_track(gtsam::SfmTrack t); - void add_camera(gtsam::SfmCamera cam); + void add_track(const gtsam::SfmTrack& t) ; + void add_camera(const gtsam::SfmCamer& cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5afc159bd..d9f2b9c3d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -144,8 +144,8 @@ protected: template void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { - this->measured_.push_back(trackToAdd.Measurements[k].second); - this->keys_.push_back(trackToAdd.Measurements[k].first); + this->measured_.push_back(trackToAdd.measurements[k].second); + this->keys_.push_back(trackToAdd.measurements[k].first); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9b610b231..270dbeb95 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1052,13 +1052,13 @@ bool readBundler(const string &filename, SfmData &data) { size_t nvisible = 0; is >> nvisible; - track.Measurements.reserve(nvisible); + track.measurements.reserve(nvisible); track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.Measurements.emplace_back(cam_idx, Point2(u, -v)); + track.measurements.emplace_back(cam_idx, Point2(u, -v)); track.siftIndices.emplace_back(cam_idx, point_idx); } @@ -1089,7 +1089,7 @@ bool readBAL(const string &filename, SfmData &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].Measurements.emplace_back(i, Point2(u, -v)); + data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -1163,7 +1163,7 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j - size_t i = track.Measurements[k].first; // camera id + size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); @@ -1173,9 +1173,9 @@ bool writeBAL(const string &filename, SfmData &data) { << endl; } - double pixelBALx = track.Measurements[k].second.x() - + double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = -(track.Measurements[k].second.y() - + double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); os << i /*camera id*/ << " " << j /*point id*/ << " " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2e4c715be..062d8728c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -220,12 +220,12 @@ struct SfmTrack { SfmTrack(): p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point - std::vector Measurements; ///< The 2D image projections (id,(u,v)) + std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; /// Total number of measurements in this track size_t number_measurements() const { - return Measurements.size(); + return measurements.size(); } /// Set 3D point void setP(Point3& p_){ @@ -234,7 +234,7 @@ struct SfmTrack { /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { - return Measurements[idx]; + return measurements[idx]; } /// Get the SIFT feature index corresponding to the measurement at `idx` SiftIndex siftIndex(size_t idx) const { @@ -243,12 +243,12 @@ struct SfmTrack { Point3 point3() const { return p; } - void add_measurement(pair m) { - Measurements.push_back(m); + void add_measurement(pair& m) const{ + measurements.push_back(m); } SfmMeasurements& measurements() { - return Measurements; + return measurements; } }; @@ -259,7 +259,7 @@ typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { - std::vector Measurements; + std::vector measurements; /// cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { @@ -278,11 +278,11 @@ struct SfmData { return tracks[idx]; } - void add_track(SfmTrack t) { + void add_track(SfmTrack& t) const { tracks.push_back(t); } - void add_camera(SfmCamera cam){ + void add_camera(SfmCamera& cam) const{ cameras.push_back(cam); } }; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index ff5cf8b46..aad9124c5 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -166,9 +166,9 @@ TEST( dataSet, Balbianello) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,1)); } @@ -476,9 +476,9 @@ TEST( dataSet, readBAL_Dubrovnik) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); } @@ -557,8 +557,8 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.Measurements[k].first,actualTrack.Measurements[k].first); - EXPECT(assert_equal(expectedTrack.Measurements[k].second,actualTrack.Measurements[k].second)); + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); + EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } } } @@ -600,9 +600,9 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6bece036f..95db611d7 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -46,10 +46,10 @@ EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].Measurements[0].second; + return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].Measurements[1].second; + return data.tracks[i].measurements[1].second; } Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); @@ -367,10 +367,10 @@ EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].Measurements[0].second; + return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].Measurements[1].second; + return data.tracks[i].measurements[1].second; } boost::shared_ptr // diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1f3a5d1cb..1fd06cc9f 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -283,14 +283,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfmTrack track1; for (size_t i = 0; i < 3; ++i) { - track1.Measurements.emplace_back(i + 1, measurements_cam1.at(i)); + track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfmTrack track2; for (size_t i = 0; i < 3; ++i) { - track2.Measurements.emplace_back(i + 1, measurements_cam2.at(i)); + track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index e6ed64689..8525ed3a3 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,5 +11,4 @@ PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index dcaaa4c76..03e2b606c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -14,5 +14,4 @@ py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector >(m_, "Measurement"); -py::bind_vector >(m_, "SiftIndexVector"); -py::bind_vector >(m_, "cameras"); \ No newline at end of file +py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index fafe87d13..05b4c7f66 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].Measurements) + for (const SfmMeasurement& m: db.tracks[j].measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } From a68b0798f9f63b49fd57243804fd49f7d8272a31 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Wed, 21 Oct 2020 23:44:02 -0400 Subject: [PATCH 117/136] wrapped sfmtrack --- gtsam/gtsam.i | 21 +++------------------ gtsam/slam/dataset.h | 11 ++++------- python/gtsam/preamble.h | 4 ++-- python/gtsam/specializations.h | 4 ++-- 4 files changed, 11 insertions(+), 29 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5334a233d..5307ef45d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,30 +2759,15 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -class SfmMeasurement{ - SfmMeasurement(); - size_t i() const; - Point2 j() const; -}; -class SiftIndex{ - SiftIndex(); - size_t i() const; - size_t j() const; - }; -class SfmMeasurements{ - SfmMeasurements(); - -}; class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; void setP(gtsam::Point3& p_); - gtsam::SfmMeasurement measurement(size_t idx) const; - gtsam::SiftIndex siftIndex(size_t idx) const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; void add_measurement(const pair& m); - SfmMeasurements& measurements(); }; class SfmData { @@ -2792,7 +2777,7 @@ class SfmData { gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamer& cam); + void add_camera(const gtsam::SfmCamera& cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 062d8728c..1a01d8a38 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -228,7 +228,7 @@ struct SfmTrack { return measurements.size(); } /// Set 3D point - void setP(Point3& p_){ + void setP(const Point3& p_){ p = p_; } @@ -243,13 +243,10 @@ struct SfmTrack { Point3 point3() const { return p; } - void add_measurement(pair& m) const{ + void add_measurement(const pair& m) { measurements.push_back(m); } - SfmMeasurements& measurements() { - return measurements; - } }; @@ -278,11 +275,11 @@ struct SfmData { return tracks[idx]; } - void add_track(SfmTrack& t) const { + void add_track(const SfmTrack& t) { tracks.push_back(t); } - void add_camera(SfmCamera& cam) const{ + void add_camera(const SfmCamera& cam){ cameras.push_back(cam); } }; diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 8525ed3a3..a8dfc5787 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,5 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +//PYBIND11_MAKE_OPAQUE(std::vector); +//PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 03e2b606c..2fd353b5f 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector >(m_, "Measurement"); -py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file +//py::bind_vector >(m_, "Measurement"); +//py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file From 6362b5648adebb3a480aaed4d317cf66835bce3f Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 22 Oct 2020 08:41:50 -0400 Subject: [PATCH 118/136] removed measurements from sfmdata --- gtsam/slam/dataset.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 1a01d8a38..c9c451660 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -256,7 +256,6 @@ typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { - std::vector measurements; /// cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { From 7aab3796b0f5cee7b6cd97ff9fbd40df1bea5f3d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 23 Oct 2020 12:50:38 +0200 Subject: [PATCH 119/136] Add alternativeName() --- gtsam/inference/Symbol.h | 6 ++++++ gtsam/inference/tests/testKey.cpp | 19 +++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 5be195db3..000a026c2 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -162,6 +162,12 @@ inline Key W(std::uint64_t j) { return Symbol('w', j); } inline Key X(std::uint64_t j) { return Symbol('x', j); } inline Key Y(std::uint64_t j) { return Symbol('y', j); } inline Key Z(std::uint64_t j) { return Symbol('z', j); } + +/** Generates symbol shorthands with alternative names different than the + * one-letter predefined ones. */ +inline std::function alternativeName(const char c) { + return [c](std::uint64_t j) { return gtsam::Symbol(c, j); }; +} } /// traits diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index a60258581..f34b77c14 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) { EXPECT(assert_equal(original, actual)) } +/* ************************************************************************* */ +TEST(Key, SymbolAlternativeNames) { + const auto x1 = gtsam::symbol_shorthand::X(1); + const auto v1 = gtsam::symbol_shorthand::V(1); + const auto a1 = gtsam::symbol_shorthand::A(1); + + const auto Z = gtsam::symbol_shorthand::alternativeName('x'); + const auto DZ = gtsam::symbol_shorthand::alternativeName('v'); + const auto DDZ = gtsam::symbol_shorthand::alternativeName('a'); + + const auto z1 = Z(1); + const auto dz1 = DZ(1); + const auto ddz1 = DDZ(1); + + EXPECT(assert_equal(x1, z1)); + EXPECT(assert_equal(v1, dz1)); + EXPECT(assert_equal(a1, ddz1)); +} + /* ************************************************************************* */ template Key KeyTestValue(); From e3a28767edb7f86c39e0369a1910c488fa5aca8c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 24 Oct 2020 08:41:00 +0200 Subject: [PATCH 120/136] replaced lambda with class plus functor --- gtsam/inference/Symbol.h | 10 ++++++---- gtsam/inference/tests/testKey.cpp | 9 ++++----- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 000a026c2..6dbb306db 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -165,13 +165,15 @@ inline Key Z(std::uint64_t j) { return Symbol('z', j); } /** Generates symbol shorthands with alternative names different than the * one-letter predefined ones. */ -inline std::function alternativeName(const char c) { - return [c](std::uint64_t j) { return gtsam::Symbol(c, j); }; -} +class SymbolGenerator { + const char c_; +public: + SymbolGenerator(const char c) : c_(c) {} + Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } +}; } /// traits template<> struct traits : public Testable {}; } // \ namespace gtsam - diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index f34b77c14..124ba7652 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -41,14 +41,14 @@ TEST(Key, KeySymbolConversion) { } /* ************************************************************************* */ -TEST(Key, SymbolAlternativeNames) { +TEST(Key, SymbolGenerator) { const auto x1 = gtsam::symbol_shorthand::X(1); const auto v1 = gtsam::symbol_shorthand::V(1); const auto a1 = gtsam::symbol_shorthand::A(1); - const auto Z = gtsam::symbol_shorthand::alternativeName('x'); - const auto DZ = gtsam::symbol_shorthand::alternativeName('v'); - const auto DDZ = gtsam::symbol_shorthand::alternativeName('a'); + const auto Z = gtsam::symbol_shorthand::SymbolGenerator('x'); + const auto DZ = gtsam::symbol_shorthand::SymbolGenerator('v'); + const auto DDZ = gtsam::symbol_shorthand::SymbolGenerator('a'); const auto z1 = Z(1); const auto dz1 = DZ(1); @@ -125,4 +125,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 672635aad46ab69c4e7aaba8a223b7732359a0f8 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 24 Oct 2020 08:44:31 +0200 Subject: [PATCH 121/136] less verbose name path for SymbolGenerator --- gtsam/inference/Symbol.h | 2 +- gtsam/inference/tests/testKey.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6dbb306db..469082f16 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -162,6 +162,7 @@ inline Key W(std::uint64_t j) { return Symbol('w', j); } inline Key X(std::uint64_t j) { return Symbol('x', j); } inline Key Y(std::uint64_t j) { return Symbol('y', j); } inline Key Z(std::uint64_t j) { return Symbol('z', j); } +} /** Generates symbol shorthands with alternative names different than the * one-letter predefined ones. */ @@ -171,7 +172,6 @@ public: SymbolGenerator(const char c) : c_(c) {} Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } }; -} /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 124ba7652..64674c36f 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -46,9 +46,9 @@ TEST(Key, SymbolGenerator) { const auto v1 = gtsam::symbol_shorthand::V(1); const auto a1 = gtsam::symbol_shorthand::A(1); - const auto Z = gtsam::symbol_shorthand::SymbolGenerator('x'); - const auto DZ = gtsam::symbol_shorthand::SymbolGenerator('v'); - const auto DDZ = gtsam::symbol_shorthand::SymbolGenerator('a'); + const auto Z = gtsam::SymbolGenerator('x'); + const auto DZ = gtsam::SymbolGenerator('v'); + const auto DDZ = gtsam::SymbolGenerator('a'); const auto z1 = Z(1); const auto dz1 = DZ(1); From 38010860e449f91634deab3bf8613498b9a3aca8 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 24 Oct 2020 15:46:47 -0400 Subject: [PATCH 122/136] changed setP method name removed commented code --- gtsam/gtsam.i | 2 +- gtsam/slam/dataset.h | 14 ++++++-------- python/CMakeLists.txt | 3 --- python/gtsam/preamble.h | 4 +--- python/gtsam/specializations.h | 2 -- python/gtsam/tests/test_SfmData.py | 22 ++++++++++++---------- 6 files changed, 20 insertions(+), 27 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5307ef45d..d6ae409f8 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2764,7 +2764,7 @@ class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; - void setP(gtsam::Point3& p_); + void set_point3(gtsam::Point3& p_); pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void add_measurement(const pair& m); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index c9c451660..85b32ff5a 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -210,9 +210,8 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; -typedef std::vector SfmMeasurements; -/// SfmTrack +/// Sift index for SfmTrack typedef std::pair SiftIndex; /// Define the structure for the 3D points @@ -228,10 +227,9 @@ struct SfmTrack { return measurements.size(); } /// Set 3D point - void setP(const Point3& p_){ + void set_point3(const Point3& p_){ p = p_; } - /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return measurements[idx]; @@ -240,14 +238,14 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } + /// Get 3D point Point3 point3() const { return p; } + /// Add measurement to track void add_measurement(const pair& m) { measurements.push_back(m); } - - }; @@ -273,11 +271,11 @@ struct SfmData { SfmTrack track(size_t idx) const { return tracks[idx]; } - + /// Add a track to SfmData void add_track(const SfmTrack& t) { tracks.push_back(t); } - + /// Add a camera to SfmData void add_camera(const SfmCamera& cam){ cameras.push_back(cam); } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7d12b9800..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,9 +36,6 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector - gtsam::SfmMeasurement - gtsam::SfmCamera - gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index a8dfc5787..e31b5e6d8 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,6 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); -//PYBIND11_MAKE_OPAQUE(std::vector); -//PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 2fd353b5f..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,3 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -//py::bind_vector >(m_, "Measurement"); -//py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 4664d746e..6263f19a0 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -32,11 +32,12 @@ class TestSfmData(GtsamTestCase): def test_tracks(self): """Test functions in SfmTrack""" # measurement is of format (camera_idx, imgPoint) - # create camera indices for two cameras - i1, i2 = np.random.randint(5), np.random.randint(5) - # create imgPoint for cameras i1 and i2 - uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + # create arbitrary camera indices for two cameras + i1, i2 = 4,5 + # create arbitrary image measurements for cameras i1 and i2 + uv_i1 = gtsam.Point2(12.6, 82) + # translating point uv_i1 along X-axis + uv_i2 = gtsam.Point2(24.88, 82) m_i1 = (i1, uv_i1) m_i2 = (i2, uv_i2) # add measurements to the track @@ -47,7 +48,7 @@ class TestSfmData(GtsamTestCase): # camera_idx in the first measurement of the track corresponds to i1 self.assertEqual(self.tracks.measurement(0)[0], i1) # Set arbitrary 3D point corresponding to the track - self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2)) + self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2)) np.testing.assert_array_almost_equal( gtsam.Point3(2.5,3.3,1.2), self.tracks.point3() @@ -59,10 +60,11 @@ class TestSfmData(GtsamTestCase): #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) # Create new track with 3 measurements track2 = gtsam.SfmTrack() - i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5) - uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + i1, i2, i3 = 3,5,6 + uv_i1 = gtsam.Point2(21.23, 45.64) + # translating along X-axis + uv_i2 = gtsam.Point2(45.7, 45.64) + uv_i3 = gtsam.Point2(68.35, 45.64) m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) # add measurements to the track track2.add_measurement(m_i1) From a7b71cf203b88fdae9244c6faab3e1b12a96f6d2 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 24 Oct 2020 19:06:35 -0400 Subject: [PATCH 123/136] remved commented code --- python/gtsam/tests/test_SfmData.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 6263f19a0..aa93a69bb 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -17,7 +17,6 @@ import unittest import numpy as np import gtsam -#from gtsam import SfmCamera from gtsam.utils.test_case import GtsamTestCase @@ -57,7 +56,6 @@ class TestSfmData(GtsamTestCase): def test_data(self): """Test functions in SfmData""" - #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) # Create new track with 3 measurements track2 = gtsam.SfmTrack() i1, i2, i3 = 3,5,6 From ee0eefbc86a20725a95ac4c8f97b3ae060a8da5e Mon Sep 17 00:00:00 2001 From: Sushmita Date: Tue, 27 Oct 2020 21:52:31 -0400 Subject: [PATCH 124/136] added new constructor and changed to emplace --- gtsam/gtsam.i | 6 +++--- gtsam/slam/dataset.h | 13 +++++-------- python/gtsam/tests/test_SfmData.py | 24 +++++++++++------------- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d6ae409f8..dcacf14d9 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2762,12 +2762,12 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { class SfmTrack { SfmTrack(); - Point3 point3() const; + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; size_t number_measurements() const; - void set_point3(gtsam::Point3& p_); pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; - void add_measurement(const pair& m); + void add_measurement(size_t idx, const gtsam::Point2& m); }; class SfmData { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 85b32ff5a..93bd2b2ee 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -217,6 +217,7 @@ typedef std::pair SiftIndex; /// Define the structure for the 3D points struct SfmTrack { SfmTrack(): p(0,0,0) {} + SfmTrack(const gtsam::Point3& pt) : p(pt) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) @@ -226,10 +227,6 @@ struct SfmTrack { size_t number_measurements() const { return measurements.size(); } - /// Set 3D point - void set_point3(const Point3& p_){ - p = p_; - } /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return measurements[idx]; @@ -239,12 +236,12 @@ struct SfmTrack { return siftIndices[idx]; } /// Get 3D point - Point3 point3() const { + const Point3& point3() const { return p; } - /// Add measurement to track - void add_measurement(const pair& m) { - measurements.push_back(m); + /// Add measurement (camera_idx, Point2) to track + void add_measurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); } }; diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index aa93a69bb..51564fb6f 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -26,7 +26,8 @@ class TestSfmData(GtsamTestCase): def setUp(self): """Initialize SfmData and SfmTrack""" self.data = gtsam.SfmData() - self.tracks = gtsam.SfmTrack() + # initialize SfmTrack with 3D point + self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2)) def test_tracks(self): """Test functions in SfmTrack""" @@ -37,17 +38,14 @@ class TestSfmData(GtsamTestCase): uv_i1 = gtsam.Point2(12.6, 82) # translating point uv_i1 along X-axis uv_i2 = gtsam.Point2(24.88, 82) - m_i1 = (i1, uv_i1) - m_i2 = (i2, uv_i2) # add measurements to the track - self.tracks.add_measurement(m_i1) - self.tracks.add_measurement(m_i2) + self.tracks.add_measurement(i1, uv_i1) + self.tracks.add_measurement(i2, uv_i2) # Number of measurements in the track is 2 self.assertEqual(self.tracks.number_measurements(), 2) # camera_idx in the first measurement of the track corresponds to i1 - self.assertEqual(self.tracks.measurement(0)[0], i1) - # Set arbitrary 3D point corresponding to the track - self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2)) + cam_idx, img_measurement = self.tracks.measurement(0) + self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( gtsam.Point3(2.5,3.3,1.2), self.tracks.point3() @@ -63,17 +61,17 @@ class TestSfmData(GtsamTestCase): # translating along X-axis uv_i2 = gtsam.Point2(45.7, 45.64) uv_i3 = gtsam.Point2(68.35, 45.64) - m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) # add measurements to the track - track2.add_measurement(m_i1) - track2.add_measurement(m_i2) - track2.add_measurement(m_i3) + track2.add_measurement(i1, uv_i1) + track2.add_measurement(i2, uv_i2) + track2.add_measurement(i3, uv_i3) self.data.add_track(self.tracks) self.data.add_track(track2) # Number of tracks in SfmData is 2 self.assertEqual(self.data.number_tracks(), 2) # camera idx of first measurement of second track corresponds to i1 - self.assertEqual(self.data.track(1).measurement(0)[0], i1) + cam_idx, img_measurement = self.data.track(1).measurement(0) + self.assertEqual(cam_idx, i1) if __name__ == '__main__': unittest.main() From 1da968afd3d9122ed8d97dec96a2b4b8183a6cfe Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 28 Oct 2020 07:43:16 +0100 Subject: [PATCH 125/136] Automatic detection of correct suggest-override flag --- cmake/GtsamBuildTypes.cmake | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index d13e3aa12..3155161be 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -1,3 +1,5 @@ +include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag() + # Set cmake policy to recognize the AppleClang compiler # independently from the Clang compiler. if(POLICY CMP0025) @@ -105,11 +107,14 @@ if(MSVC) else() # Common to all configurations, next for each configuration: - if ( - ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR - (CMAKE_CXX_COMPILER_ID MATCHES "GNU") - ) - # set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + if (NOT MSVC) + check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE) + check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE) + if (COMPILER_HAS_WSUGGEST_OVERRIDE) + set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + elseif(COMPILER_HAS_WMISSING_OVERRIDE) + set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday + endif() endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON From 405771397ba3938f094f7d023e75ccbb9a8e3ea3 Mon Sep 17 00:00:00 2001 From: Tim McGrath Date: Sat, 31 Oct 2020 13:40:05 -0400 Subject: [PATCH 126/136] adding additional Unit3 support in the wrapper: PriorFactorUnit3, Values::insert/update/at(Unit3) --- gtsam/gtsam.i | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 575a216d6..b578d134d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2157,6 +2157,7 @@ class Values { void insert(size_t j, const gtsam::SOn& P); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); @@ -2175,6 +2176,7 @@ class Values { void update(size_t j, const gtsam::SOn& P); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); @@ -2186,7 +2188,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> T at(size_t j); /// version for double @@ -2529,7 +2531,7 @@ class NonlinearISAM { #include #include -template}> +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; From 65a6d06bf170206779ab82b301a0b95354ee07f0 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 1 Nov 2020 21:29:38 -0500 Subject: [PATCH 127/136] sfmtrack constructor changed to accept point --- python/gtsam/preamble.h | 2 +- python/gtsam/tests/test_SfmData.py | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index e31b5e6d8..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,4 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 51564fb6f..9c965ddc5 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -27,7 +27,7 @@ class TestSfmData(GtsamTestCase): """Initialize SfmData and SfmTrack""" self.data = gtsam.SfmData() # initialize SfmTrack with 3D point - self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2)) + self.tracks = gtsam.SfmTrack() def test_tracks(self): """Test functions in SfmTrack""" @@ -47,7 +47,7 @@ class TestSfmData(GtsamTestCase): cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( - gtsam.Point3(2.5,3.3,1.2), + gtsam.Point3(0.,0.,0.), self.tracks.point3() ) @@ -55,13 +55,15 @@ class TestSfmData(GtsamTestCase): def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements - track2 = gtsam.SfmTrack() i1, i2, i3 = 3,5,6 uv_i1 = gtsam.Point2(21.23, 45.64) # translating along X-axis uv_i2 = gtsam.Point2(45.7, 45.64) uv_i3 = gtsam.Point2(68.35, 45.64) - # add measurements to the track + # add measurements and arbitrary point to the track + measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] + pt = gtsam.Point3(1.0, 6.0, 2.0) + track2 = gtsam.SfmTrack(pt) track2.add_measurement(i1, uv_i1) track2.add_measurement(i2, uv_i2) track2.add_measurement(i3, uv_i3) From 63ca1c1f5eec82cc04737611469f3ad69b486f16 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 10:46:36 -0500 Subject: [PATCH 128/136] Attempt to fix spooky boost in Homebrew --- .github/workflows/build-macos.yml | 4 ++-- .github/workflows/build-python.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 363cd690f..b2b71ea7d 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,7 +35,7 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew install cmake ninja boost@1.60 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" @@ -48,4 +48,4 @@ jobs: - name: Build and Test (macOS) if: runner.os == 'macOS' run: | - bash .github/scripts/unix.sh -t \ No newline at end of file + bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index fbec491f2..6c90b8bf7 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,7 +87,7 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew install cmake ninja boost@1.60 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" @@ -109,4 +109,4 @@ jobs: - name: Build (macOS) if: runner.os == 'macOS' run: | - bash .github/scripts/python.sh \ No newline at end of file + bash .github/scripts/python.sh From d8089c71322ad1c75f92f0b2de4d2fb286ac27a7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:01:43 -0500 Subject: [PATCH 129/136] Use my tap --- .github/workflows/build-macos.yml | 3 ++- .github/workflows/build-python.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index b2b71ea7d..fd6fb8d0b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,7 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost@1.60 + brew tap ProfFan/robotics + brew install cmake ninja boost@1.68 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 6c90b8bf7..5f79ae90d 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,7 +87,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost@1.60 + brew tap ProfFan/robotics + brew install cmake ninja boost@1.68 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 1ee5650fc774157f824267741852a0b2ec43f919 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:12:52 -0500 Subject: [PATCH 130/136] Use my tap --- .github/workflows/build-macos.yml | 4 ++-- .github/workflows/build-python.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index fd6fb8d0b..9dca41b84 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,8 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/robotics - brew install cmake ninja boost@1.68 + brew tap ProfFan/my + brew install cmake ninja boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 5f79ae90d..ef7c30a62 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,8 +87,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/robotics - brew install cmake ninja boost@1.68 + brew tap ProfFan/my + brew install cmake ninja boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From dddc97c487c2abe91254ac19a4cc759278d7a349 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:20:48 -0500 Subject: [PATCH 131/136] Use explict tap name --- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-python.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 9dca41b84..c9ccfbfc3 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -36,7 +36,7 @@ jobs: if: runner.os == 'macOS' run: | brew tap ProfFan/my - brew install cmake ninja boost@1.59 + brew install cmake ninja ProfFan/my/boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index ef7c30a62..f75701215 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -88,7 +88,7 @@ jobs: if: runner.os == 'macOS' run: | brew tap ProfFan/my - brew install cmake ninja boost@1.59 + brew install cmake ninja ProfFan/my/boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 315380db03f25eec0d05ad6eabeecef0b541e920 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:28:49 -0500 Subject: [PATCH 132/136] Last resort: use the explict 1.73 formula --- .github/workflows/build-macos.yml | 4 ++-- .github/workflows/build-python.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index c9ccfbfc3..212924cc7 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,8 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/my - brew install cmake ninja ProfFan/my/boost@1.59 + brew install cmake ninja + brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f75701215..f6930a8c4 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,8 +87,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/my - brew install cmake ninja ProfFan/my/boost@1.59 + brew install cmake ninja + brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 325b868e8b55f186567de398d85c61b2400cfa80 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 14:42:21 -0500 Subject: [PATCH 133/136] Use my tap --- .github/workflows/build-macos.yml | 3 ++- .github/workflows/build-python.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 212924cc7..3ce98055a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,8 +35,9 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | + brew tap ProfFan/robotics brew install cmake ninja - brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb + brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f6930a8c4..8255fb8ab 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,8 +87,9 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | + brew tap ProfFan/robotics brew install cmake ninja - brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb + brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 6c6cb965d88318623653454aac28b572aeef776b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Nov 2020 12:12:10 -0500 Subject: [PATCH 134/136] Consistent interface for pixel center (#579) * defined u0 and v0 in all camera models for consistent interface * deprecate u0/v0 and use px/py everywhere * Use deprecation macro * fix various issues --- gtsam/geometry/Cal3Bundler.h | 12 ++++++++++++ gtsam/gtsam.i | 14 ++++++++++++-- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/slam/dataset.cpp | 4 ++-- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 96765f899..2e3fab002 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -98,6 +98,17 @@ public: return k2_; } + /// image center in x + inline double px() const { + return u0_; + } + + /// image center in y + inline double py() const { + return v0_; + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /// get parameter u0 inline double u0() const { return u0_; @@ -107,6 +118,7 @@ public: inline double v0() const { return v0_; } +#endif /** diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 575a216d6..8ed93868f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -597,6 +597,7 @@ class Rot3 { Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); static gtsam::Rot3 Rx(double t); static gtsam::Rot3 Ry(double t); @@ -980,8 +981,8 @@ class Cal3Bundler { double fy() const; double k1() const; double k2() const; - double u0() const; - double v0() const; + double px() const; + double py() const; Vector vector() const; Vector k() const; Matrix K() const; @@ -2761,7 +2762,16 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include + class SfmTrack { + SfmTrack(); + + double r; + double g; + double b; + // TODO Need to close wrap#10 to allow this to work. + // std::vector> measurements; + Point3 point3() const; size_t number_measurements() const; pair measurement(size_t idx) const; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 2f4f21286..1423b473e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler { double v0 = 0) : Cal3Bundler(f, k1, k2, u0, v0) {} Cal3Bundler0 retract(const Vector& d) const { - return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py()); } Vector3 localCoordinates(const Cal3Bundler0& T2) const { return T2.vector() - vector(); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 270dbeb95..74e074c9e 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id - double u0 = data.cameras[i].calibration().u0(); - double v0 = data.cameras[i].calibration().v0(); + double u0 = data.cameras[i].calibration().px(); + double v0 = data.cameras[i].calibration().py(); if (u0 != 0 || v0 != 0) { cout << "writeBAL has not been tested for calibration with nonzero " From acd6fff5623e33ddd2a086ec6074c737097693c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 7 Nov 2020 17:28:50 -0500 Subject: [PATCH 135/136] minor typo fixes --- gtsam/geometry/Rot3.h | 2 +- wrap/cmake/PybindWrap.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 2334312f6..abd74e063 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -526,7 +526,7 @@ namespace gtsam { /** * @brief Spherical Linear intERPolation between *this and other - * @param s a value between 0 and 1 + * @param t a value between 0 and 1 * @param other final point of iterpolation geodesic on manifold */ Rot3 slerp(double t, const Rot3& other) const; diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index 85f956d50..ff69b5aed 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -146,7 +146,7 @@ function(install_python_scripts else() set(build_type_tag "") endif() - # Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if + # Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) From c5c54da588f4cb8150437e417c3141c64e714cd8 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 8 Nov 2020 11:07:50 +0100 Subject: [PATCH 136/136] Avoid redundant calls to error() --- gtsam/nonlinear/NonlinearOptimizer.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 9a9c487b6..fd9961742 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -88,20 +88,24 @@ void NonlinearOptimizer::defaultOptimize() { } // Iterative loop + double newError = currentError; // used to avoid repeated calls to error() do { // Do next iteration - currentError = error(); // TODO(frank): don't do this twice at first !? Computed above! + currentError = newError; iterate(); tictoc_finishedIteration(); + // Update newError for either printouts or conditional-end checks: + newError = error(); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) values().print("newValues"); if (params.verbosity >= NonlinearOptimizerParams::ERROR) - cout << "newError: " << error() << endl; + cout << "newError: " << newError << endl; } while (iterations() < params.maxIterations && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, - currentError, error(), params.verbosity) && std::isfinite(currentError)); + currentError, newError, params.verbosity) && std::isfinite(currentError)); // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {