From b84c6efe51949cb5c248b4c989a0a0239fe6d535 Mon Sep 17 00:00:00 2001 From: Ellon Paiva Mendes Date: Thu, 3 Oct 2019 13:56:26 +0200 Subject: [PATCH 01/32] 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 02/32] 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 03/32] 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 04/32] 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 05/32] 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 06/32] 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 07/32] 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 08/32] 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 09/32] 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 10/32] 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 11/32] 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 12/32] 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 13/32] 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 14/32] 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 15/32] 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 16/32] 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 17/32] 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 18/32] 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 19/32] 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 20/32] 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 21/32] 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 8fa76865cba039a50e65fe509eb2059951226f0f Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Tue, 8 Sep 2020 21:31:54 -0400 Subject: [PATCH 22/32] 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 23/32] 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 24/32] 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 66c9a63919a8e12b3d8f5593e57ec4ffaa4f7eb0 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Mon, 14 Sep 2020 00:20:50 -0400 Subject: [PATCH 25/32] 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 bf0651b626d1adf34dcd550f0d3f654eab084206 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 20 Sep 2020 11:45:30 -0400 Subject: [PATCH 26/32] 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 463b634328650d9653705b3b176208868484e1c3 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 11:58:10 -0400 Subject: [PATCH 27/32] 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 28/32] 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 29/32] 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 30/32] 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 31/32] 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 95724be4ae170bfa2860d854a61405d293109948 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 27 Sep 2020 20:36:50 -0400 Subject: [PATCH 32/32] 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));