added const to matrix() function for Rot3 to be consistent with SOn and added note about using Eigen transpose expression

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;

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

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<const Matrix3>`?
+  // 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)); }

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<Rot3>, R);
   CHECK(assert_equal(numericalH,actualH));