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