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) {