From 8f17fbcc8e06f9bf1b79129a28ebf1f2f5961a51 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 13:11:48 -0400 Subject: [PATCH 01/12] improved printing for Point3 and Unit3 --- gtsam/geometry/Point3.cpp | 5 ++++- gtsam/geometry/Unit3.cpp | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 8aa339a89..c9452efff 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -29,7 +29,10 @@ bool Point3::equals(const Point3 &q, double tol) const { } void Point3::print(const string& s) const { - cout << s << *this << endl; + if (s.size() != 0) { + cout << s << " "; + } + cout << *this << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 3d46b18b8..a2a7e6851 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -154,7 +154,10 @@ std::ostream& operator<<(std::ostream& os, const Unit3& pair) { /* ************************************************************************* */ void Unit3::print(const std::string& s) const { - cout << s << ":" << p_ << endl; + if(s.size() != 0) { + cout << s << ":"; + } + cout << p_ << endl; } /* ************************************************************************* */ From e987cb53a0e81daf8f5eebc20c86b5933b9d6c9a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 14:00:27 -0400 Subject: [PATCH 02/12] Lots of improvements to Rot3 - Ensure axis in AxisAngle constructor is a unit vector. Added test. - Improved rotation inverse with support for quaternions. - Use Eigen::Transpose for transpose return type. - Roll/Pitch/Yaw is more efficient. - Fix/Remove old TODOs. --- gtsam/geometry/Rot3.cpp | 1 - gtsam/geometry/Rot3.h | 42 ++++++++++++++----------------- gtsam/geometry/Rot3M.cpp | 3 +-- gtsam/geometry/tests/testRot3.cpp | 4 +++ 4 files changed, 24 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..24bdd6f17 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -36,7 +36,6 @@ void Rot3::print(const std::string& s) const { /* ************************************************************************* */ Rot3 Rot3::Random(std::mt19937& rng) { - // TODO allow any engine without including all of boost :-( Unit3 axis = Unit3::Random(rng); uniform_real_distribution randomAngle(-M_PI, M_PI); double angle = randomAngle(rng); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4..474c88047 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -194,22 +194,24 @@ namespace gtsam { /** * Convert from axis/angle representation - * @param axisw is the rotation axis, unit length - * @param angle rotation angle + * @param axis is the rotation axis, unit length + * @param angle rotation angle * @return incremental rotation */ - static Rot3 AxisAngle(const Point3& axis, double angle) { + static Rot3 AxisAngle(const Vector3& axis, double angle) { + // Convert to unit vector. + Vector3 unitAxis = Unit3(axis).unitVector(); #ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, unitAxis)); #else - return Rot3(SO3::AxisAngle(axis,angle)); + return Rot3(SO3::AxisAngle(unitAxis,angle)); #endif } /** * Convert from axis/angle representation - * @param axis is the rotation axis - * @param angle rotation angle + * @param axis is the rotation axis + * @param angle rotation angle * @return incremental rotation */ static Rot3 AxisAngle(const Unit3& axis, double angle) { @@ -268,9 +270,13 @@ namespace gtsam { /// Syntatic sugar for composing two rotations Rot3 operator*(const Rot3& R2) const; - /// inverse of a rotation, TODO should be different for M/Q + /// inverse of a rotation Rot3 inverse() const { - return Rot3(Matrix3(transpose())); +#ifdef GTSAM_USE_QUATERNIONS + return quaternion_.inverse(); +#else + return Rot3(transpose()); +#endif } /** @@ -405,8 +411,7 @@ namespace gtsam { /** * Return 3*3 transpose (inverse) rotation matrix */ - Matrix3 transpose() const; - // TODO: const Eigen::Transpose transpose() const; + const Eigen::Transpose transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -435,27 +440,18 @@ namespace gtsam { /** * Accessor to get to component of angle representations - * NOTE: these are not efficient to get to multiple separate parts, - * you should instead use xyz() or ypr() - * TODO: make this more efficient */ - inline double roll() const { return ypr()(2); } + inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations - * NOTE: these are not efficient to get to multiple separate parts, - * you should instead use xyz() or ypr() - * TODO: make this more efficient */ - inline double pitch() const { return ypr()(1); } + inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations - * NOTE: these are not efficient to get to multiple separate parts, - * you should instead use xyz() or ypr() - * TODO: make this more efficient */ - inline double yaw() const { return ypr()(0); } + inline double yaw() const { return xyz()(2); } /// @} /// @name Advanced Interface diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d38d33bf1..85d9923fb 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,8 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -// TODO const Eigen::Transpose Rot3::transpose() const { -Matrix3 Rot3::transpose() const { +const Eigen::Transpose Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c95b85f21..d9d08a48e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -115,6 +115,10 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual,1e-5)); Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); CHECK(assert_equal(expected,actual2,1e-5)); + + axis = Vector3(0, 50, 0); + Rot3 actual3 = Rot3::AxisAngle(axis, angle); + CHECK(assert_equal(expected,actual3,1e-5)); } /* ************************************************************************* */ From cd88c795ae6543ac3f9c9648872bb8b450e09ada Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 15:10:13 -0400 Subject: [PATCH 03/12] more efficient and explicit inverse() --- gtsam/geometry/Rot3.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 474c88047..3e0e33609 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -198,7 +198,7 @@ namespace gtsam { * @param angle rotation angle * @return incremental rotation */ - static Rot3 AxisAngle(const Vector3& axis, double angle) { + static Rot3 AxisAngle(const Point3& axis, double angle) { // Convert to unit vector. Vector3 unitAxis = Unit3(axis).unitVector(); #ifdef GTSAM_USE_QUATERNIONS @@ -273,9 +273,9 @@ namespace gtsam { /// inverse of a rotation Rot3 inverse() const { #ifdef GTSAM_USE_QUATERNIONS - return quaternion_.inverse(); + return Rot3(quaternion_.inverse()); #else - return Rot3(transpose()); + return Rot3(rot_.matrix().transpose()); #endif } From 510d5c500c78544079a5f21c567fd85dfa4d5bb5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 15:10:22 -0400 Subject: [PATCH 04/12] Revert "improved printing for Point3 and Unit3" This reverts commit 8f17fbcc8e06f9bf1b79129a28ebf1f2f5961a51. --- gtsam/geometry/Point3.cpp | 5 +---- gtsam/geometry/Unit3.cpp | 5 +---- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index c9452efff..8aa339a89 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -29,10 +29,7 @@ bool Point3::equals(const Point3 &q, double tol) const { } void Point3::print(const string& s) const { - if (s.size() != 0) { - cout << s << " "; - } - cout << *this << endl; + cout << s << *this << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a2a7e6851..3d46b18b8 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -154,10 +154,7 @@ std::ostream& operator<<(std::ostream& os, const Unit3& pair) { /* ************************************************************************* */ void Unit3::print(const std::string& s) const { - if(s.size() != 0) { - cout << s << ":"; - } - cout << p_ << endl; + cout << s << ":" << p_ << endl; } /* ************************************************************************* */ From 359bc161a67798a79e60ccff593107aef2952aba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 10:44:21 -0400 Subject: [PATCH 05/12] updated transpose signature for Rot3Q --- gtsam/geometry/Rot3Q.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 8af9a7144..377cc4a5f 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,11 +79,9 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: Could we do this? It works in Rot3M but not here, probably because - // here we create an intermediate value by calling matrix() - // const Eigen::Transpose Rot3::transpose() const { - Matrix3 Rot3::transpose() const { - return matrix().transpose(); + const Eigen::Transpose Rot3::transpose() const { + // `.eval()` to avoid aliasing effect due to transpose (allows compilation). + return matrix().eval().transpose(); } /* ************************************************************************* */ From 2a07cfed6604d4b9a7f43e71a7a600a0756b6fd0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 18:18:51 -0400 Subject: [PATCH 06/12] improved rotation transpose with quaternions --- gtsam/geometry/Rot3Q.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 377cc4a5f..64e8324b8 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -19,8 +19,8 @@ #ifdef GTSAM_USE_QUATERNIONS -#include #include +#include #include using namespace std; @@ -80,8 +80,8 @@ namespace gtsam { /* ************************************************************************* */ const Eigen::Transpose Rot3::transpose() const { - // `.eval()` to avoid aliasing effect due to transpose (allows compilation). - return matrix().eval().transpose(); + // `eval` for immediate evaluation (allows compilation). + return Rot3(matrix()).matrix().eval().transpose(); } /* ************************************************************************* */ From bde27615c506abf1b0343265af2e711514d866bc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 18:19:40 -0400 Subject: [PATCH 07/12] test in testAHRSFactor is already fixed --- gtsam/navigation/tests/testAHRSFactor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index d32553b94..dd216ce34 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -15,6 +15,7 @@ * @author Krunal Chande * @author Luca Carlone * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) { // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H3e, H3a, 1e-5)); - // FIXME!! DOes not work. Different matrix dimensions. + // 1e-5 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ From 1d5239dd251e91e4a12907d5ce4bc0d1ddd96703 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 18:20:18 -0400 Subject: [PATCH 08/12] test for stability of RQ due to atan2 as well as fix --- gtsam/geometry/Rot3.cpp | 18 +++++++++++++++--- gtsam/geometry/tests/testRot3.cpp | 26 +++++++++++++++++++++++++- 2 files changed, 40 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 24bdd6f17..96c714166 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -197,15 +197,27 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { /* ************************************************************************* */ pair RQ(const Matrix3& A) { - double x = -atan2(-A(2, 1), A(2, 2)); + // atan2 has curious/unstable behavior near 0. + // If either x or y is close to zero, the results can vary depending on + // what the sign of either x or y is. + // E.g. for x = 1e-15, y = -0.08, atan2(x, y) != atan2(-x, y), + // even though arithmetically, they are both atan2(0, y). + // Suppressing small numbers to 0.0 doesn't work since the floating point + // representation still causes the issue due to a delta difference. + // The only fix is to convert to an int so we are guaranteed the value is 0. + // This lambda function checks if a number is close to 0. + // If yes, then return the integer representation, else do nothing. + auto suppress = [](auto x) { return abs(x) > 1e-15 ? x : int(x); }; + + double x = -atan2(-suppress(A(2, 1)), suppress(A(2, 2))); Rot3 Qx = Rot3::Rx(-x); Matrix3 B = A * Qx.matrix(); - double y = -atan2(B(2, 0), B(2, 2)); + double y = -atan2(suppress(B(2, 0)), suppress(B(2, 2))); Rot3 Qy = Rot3::Ry(-y); Matrix3 C = B * Qy.matrix(); - double z = -atan2(-C(1, 0), C(1, 1)); + double z = -atan2(-suppress(C(1, 0)), suppress(C(1, 1))); Rot3 Qz = Rot3::Rz(-z); Matrix3 R = C * Qz.matrix(); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d9d08a48e..2cdb9c4f0 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,6 +14,7 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -380,7 +381,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); + CHECK(assert_equal(actual, Rot3(R.transpose()))); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); @@ -502,6 +503,29 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,actual,1e-6)); } +/* ************************************************************************* */ +TEST( Rot3, RQStability) +{ + // Test case to check if xyz() is computed correctly when using quaternions. + Matrix actualR; + Vector actualXyz; + + // A is equivalent to the below + double kDegree = M_PI / 180; + const Rot3 nRy = Rot3::Yaw(315 * kDegree); + const Rot3 yRp = Rot3::Pitch(275 * kDegree); + const Rot3 pRb = Rot3::Roll(180 * kDegree); + const Rot3 nRb = nRy * yRp * pRb; + + // A(2, 1) ~= -0.0 + // Since A(2, 2) < 0, x-angle should be positive + Matrix A = nRb.matrix(); + boost::tie(actualR, actualXyz) = RQ(A); + + Vector3 expectedXyz(3.14159, -1.48353, -0.785398); + CHECK(assert_equal(expectedXyz,actualXyz,1e-6)); +} + /* ************************************************************************* */ TEST( Rot3, expmapStability ) { Vector w = Vector3(78e-9, 5e-8, 97e-7); From 7b6a80eba247843ae74a74b53cfe93f4d15d371b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 19:14:19 -0400 Subject: [PATCH 09/12] Revert "test for stability of RQ due to atan2 as well as fix" This reverts commit 1d5239dd251e91e4a12907d5ce4bc0d1ddd96703. --- gtsam/geometry/Rot3.cpp | 18 +++--------------- gtsam/geometry/tests/testRot3.cpp | 26 +------------------------- 2 files changed, 4 insertions(+), 40 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 96c714166..24bdd6f17 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -197,27 +197,15 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { /* ************************************************************************* */ pair RQ(const Matrix3& A) { - // atan2 has curious/unstable behavior near 0. - // If either x or y is close to zero, the results can vary depending on - // what the sign of either x or y is. - // E.g. for x = 1e-15, y = -0.08, atan2(x, y) != atan2(-x, y), - // even though arithmetically, they are both atan2(0, y). - // Suppressing small numbers to 0.0 doesn't work since the floating point - // representation still causes the issue due to a delta difference. - // The only fix is to convert to an int so we are guaranteed the value is 0. - // This lambda function checks if a number is close to 0. - // If yes, then return the integer representation, else do nothing. - auto suppress = [](auto x) { return abs(x) > 1e-15 ? x : int(x); }; - - double x = -atan2(-suppress(A(2, 1)), suppress(A(2, 2))); + double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); Matrix3 B = A * Qx.matrix(); - double y = -atan2(suppress(B(2, 0)), suppress(B(2, 2))); + double y = -atan2(B(2, 0), B(2, 2)); Rot3 Qy = Rot3::Ry(-y); Matrix3 C = B * Qy.matrix(); - double z = -atan2(-suppress(C(1, 0)), suppress(C(1, 1))); + double z = -atan2(-C(1, 0), C(1, 1)); Rot3 Qz = Rot3::Rz(-z); Matrix3 R = C * Qz.matrix(); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 2cdb9c4f0..d9d08a48e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,7 +14,6 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert - * @author Varun Agrawal */ #include @@ -381,7 +380,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal(actual, Rot3(R.transpose()))); + CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); @@ -503,29 +502,6 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,actual,1e-6)); } -/* ************************************************************************* */ -TEST( Rot3, RQStability) -{ - // Test case to check if xyz() is computed correctly when using quaternions. - Matrix actualR; - Vector actualXyz; - - // A is equivalent to the below - double kDegree = M_PI / 180; - const Rot3 nRy = Rot3::Yaw(315 * kDegree); - const Rot3 yRp = Rot3::Pitch(275 * kDegree); - const Rot3 pRb = Rot3::Roll(180 * kDegree); - const Rot3 nRb = nRy * yRp * pRb; - - // A(2, 1) ~= -0.0 - // Since A(2, 2) < 0, x-angle should be positive - Matrix A = nRb.matrix(); - boost::tie(actualR, actualXyz) = RQ(A); - - Vector3 expectedXyz(3.14159, -1.48353, -0.785398); - CHECK(assert_equal(expectedXyz,actualXyz,1e-6)); -} - /* ************************************************************************* */ TEST( Rot3, expmapStability ) { Vector w = Vector3(78e-9, 5e-8, 97e-7); From dec918c3d54ee24764c0ff1d59152d8f632075a9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 19:25:31 -0400 Subject: [PATCH 10/12] undo return type of Eigen::Transpose, and add back TODO to optimize RPY --- gtsam/geometry/Rot3.h | 11 ++++++++++- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/geometry/Rot3Q.cpp | 5 +++-- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 3e0e33609..fead20ea9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -411,7 +411,7 @@ namespace gtsam { /** * Return 3*3 transpose (inverse) rotation matrix */ - const Eigen::Transpose transpose() const; + Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -440,16 +440,25 @@ namespace gtsam { /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double yaw() const { return xyz()(2); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 85d9923fb..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -const Eigen::Transpose Rot3::transpose() const { +Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 64e8324b8..0116f137d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,9 +79,10 @@ namespace gtsam { } /* ************************************************************************* */ - const Eigen::Transpose Rot3::transpose() const { + Matrix3 Rot3::transpose() const { // `eval` for immediate evaluation (allows compilation). - return Rot3(matrix()).matrix().eval().transpose(); + // return Rot3(matrix()).matrix().eval().transpose(); + return matrix().eval().transpose(); } /* ************************************************************************* */ From c96300a7c6b06768cd1fdd44dbb9aa501b42d9ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Mar 2020 12:04:32 -0400 Subject: [PATCH 11/12] added const to matrix() function for Rot3 to be consistent with SOn and added note about using Eigen transpose expression --- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/Rot3M.cpp | 4 ++-- gtsam/geometry/Rot3Q.cpp | 15 ++++++++++----- gtsam/geometry/tests/testRot3.cpp | 2 +- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fead20ea9..9b3bff53a 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -406,12 +406,12 @@ namespace gtsam { /// @{ /** return 3*3 rotation matrix */ - Matrix3 matrix() const; + const Matrix3 matrix() const; /** * Return 3*3 transpose (inverse) rotation matrix */ - Matrix3 transpose() const; + const Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 46a07e50a..b39f3725e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -Matrix3 Rot3::transpose() const { +const Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } @@ -175,7 +175,7 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { } /* ************************************************************************* */ -Matrix3 Rot3::matrix() const { +const Matrix3 Rot3::matrix() const { return rot_.matrix(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 0116f137d..9c06d54e9 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,10 +79,15 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 Rot3::transpose() const { - // `eval` for immediate evaluation (allows compilation). - // return Rot3(matrix()).matrix().eval().transpose(); - return matrix().eval().transpose(); + // TODO: Maybe use return type `const Eigen::Transpose`? + // It works in Rot3M but not here, because of some weird behavior + // due to Eigen's expression templates which needs more investigation. + // For example, if we use matrix(), the value returned has a 1e-10, + // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary. + // Using eval() here doesn't help, it only helps if we use it in + // the downstream code. + const Matrix3 Rot3::transpose() const { + return matrix().transpose(); } /* ************************************************************************* */ @@ -115,7 +120,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} + const Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d9d08a48e..eeda3c3d3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -380,7 +380,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); + CHECK(assert_equal(actual.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); From b058adc675782354561ad94507e360020faa790f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Mar 2020 17:50:34 -0400 Subject: [PATCH 12/12] remove const from return types for Rot3 --- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/Rot3M.cpp | 4 ++-- gtsam/geometry/Rot3Q.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b900b8c82..fc3a8b3f2 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -407,12 +407,12 @@ namespace gtsam { /// @{ /** return 3*3 rotation matrix */ - const Matrix3 matrix() const; + Matrix3 matrix() const; /** * Return 3*3 transpose (inverse) rotation matrix */ - const Matrix3 transpose() const; + Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b39f3725e..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -const Matrix3 Rot3::transpose() const { +Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } @@ -175,7 +175,7 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { } /* ************************************************************************* */ -const Matrix3 Rot3::matrix() const { +Matrix3 Rot3::matrix() const { return rot_.matrix(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 9c06d54e9..d609c289c 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -86,7 +86,7 @@ namespace gtsam { // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary. // Using eval() here doesn't help, it only helps if we use it in // the downstream code. - const Matrix3 Rot3::transpose() const { + Matrix3 Rot3::transpose() const { return matrix().transpose(); } @@ -120,7 +120,7 @@ namespace gtsam { } /* ************************************************************************* */ - const Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} + Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }