From c2455082dbb342567181d9126b37ed41214e7a52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 13:34:40 -0500 Subject: [PATCH 1/9] Force EXPMAP option for both if either POSE3 or ROT3 is set --- cmake/HandleGeneralOptions.cmake | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 27d73bd86..4bc74804c 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -31,6 +31,13 @@ if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() +# Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. +if(GTSAM_POSE3_EXPMAP) + set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) +elseif(GTSAM_ROT3_EXPMAP) + set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) +endif() + # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) From ddc0d136002c6f3c060691dc4dbdeddf48535bef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 13:34:53 -0500 Subject: [PATCH 2/9] uncomment tests --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9639ffbcf..8fafcb25f 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -906,9 +906,9 @@ TEST(Pose3 , ChartDerivatives) { Pose3 id; if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { CHECK_CHART_DERIVATIVES(id,id); -// CHECK_CHART_DERIVATIVES(id,T2); -// CHECK_CHART_DERIVATIVES(T2,id); -// CHECK_CHART_DERIVATIVES(T2,T3); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } } From b5284e4b3fb6933c4443212ba695d1c151961e74 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 22:46:16 -0500 Subject: [PATCH 3/9] Improved CayleyChart Local --- gtsam/geometry/Rot3M.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 02e5b771f..eb1bd6461 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,17 +176,13 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a = A(0, 0), b = A(0, 1), c = A(0, 2); - const double d = A(1, 0), e = A(1, 1), f = A(1, 2); - const double g = A(2, 0), h = A(2, 1), i = A(2, 2); - const double di = d * i, ce = c * e, cd = c * d, fg = f * g; - const double M = 1 + e - f * h + i + e * i; - const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); + const Matrix3 P = A + I_3x3; + // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. + if (P.determinant() == 0.0) { + throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); + } + Matrix3 Pinv = (A + I_3x3).inverse(); + return SO3::Vee(Pinv * (A - I_3x3)) * 2; } /* ************************************************************************* */ From 098e7045110d64db1da2424c847c23a62ba056ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 22:46:39 -0500 Subject: [PATCH 4/9] Similarity3 test only for Rot3 Expmap --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index b985eb374..937761f02 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) { vector correspondences{bTa1, bTa2}; + // Cayley transform cannot accommodate 180 degree rotations, + // hence we only test for Expmap +#ifdef GTSAM_ROT3_EXPMAP Similarity3 actual_aSb = Similarity3::Align(correspondences); EXPECT(assert_equal(expected_aSb, actual_aSb)); +#endif } //****************************************************************************** From 6bf410c0d8a9180c715d1a87789c0e20ab41bd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 10:57:40 -0500 Subject: [PATCH 5/9] Revert "Improved CayleyChart Local" This reverts commit b5284e4b3fb6933c4443212ba695d1c151961e74. --- gtsam/geometry/Rot3M.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index eb1bd6461..02e5b771f 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,13 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - const Matrix3 P = A + I_3x3; - // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. - if (P.determinant() == 0.0) { - throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); - } - Matrix3 Pinv = (A + I_3x3).inverse(); - return SO3::Vee(Pinv * (A - I_3x3)) * 2; + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */ From 14ef7db636bfd1fc5a16f3dff41fcc7bb0df4a62 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:14:54 -0500 Subject: [PATCH 6/9] Use older and faster Cayley transform but add det check and docs --- gtsam/geometry/Rot3M.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 02e5b771f..67b774a74 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: + + // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. + if ((A + I_3x3).determinant() == 0.0) { + throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); + } + + // Mathematica closed form optimization. + // The following are the essential computations for the following algorithm + // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 + // 2. Compute the Cayley transform C = P^{-1} * (A-I) + // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z. const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2); const double g = A(2, 0), h = A(2, 1), i = A(2, 2); From a94b6dacaf9309dfc125a9b44f5eb71911d0da83 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:15:50 -0500 Subject: [PATCH 7/9] Print message when either Pose3 or Rot3 expmap is ON --- cmake/HandleGeneralOptions.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 4bc74804c..85a529e49 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -33,8 +33,10 @@ endif() # Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. if(GTSAM_POSE3_EXPMAP) + message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well") set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) elseif(GTSAM_ROT3_EXPMAP) + message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well") set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) endif() From 4e3ac1b8398a66e1824947d7a1a25ae21821d362 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:16:50 -0500 Subject: [PATCH 8/9] CI path for Cayley transform --- .github/scripts/unix.sh | 2 ++ .github/workflows/build-special.yml | 14 ++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index f85e67dc1..55a8ac372 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -66,6 +66,8 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ + -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ + -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 715f25733..532f917c1 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -24,6 +24,7 @@ jobs: ubuntu-gcc-deprecated, ubuntu-gcc-quaternions, ubuntu-gcc-tbb, + ubuntu-cayleymap, ] build_type: [Debug, Release] @@ -47,6 +48,12 @@ jobs: version: "9" flag: tbb + - name: ubuntu-cayleymap + os: ubuntu-18.04 + compiler: gcc + version: "9" + flag: cayley + steps: - name: Checkout uses: actions/checkout@master @@ -109,6 +116,13 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Use Cayley Transform for Rot3 + if: matrix.flag == 'cayley' + run: | + echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV + echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV + echo "GTSAM Uses Cayley map for Rot3" + - name: Build & Test run: | bash .github/scripts/unix.sh -t From 9e421c407bac3307ed8d552f01352fa7a3a23211 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 18:17:16 -0500 Subject: [PATCH 9/9] small doc fix --- gtsam/geometry/Rot3M.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 67b774a74..07cc7302a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -185,7 +185,7 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { // Mathematica closed form optimization. // The following are the essential computations for the following algorithm // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 - // 2. Compute the Cayley transform C = P^{-1} * (A-I) + // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I) // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z. const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2);