diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index a948fd5d9..93e276de7 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -377,9 +377,6 @@ namespace gtsam {
        ar & boost::serialization::make_nvp("rot31", rot_(2,0));
        ar & boost::serialization::make_nvp("rot32", rot_(2,1));
        ar & boost::serialization::make_nvp("rot33", rot_(2,2));
-//      ar & BOOST_SERIALIZATION_NVP(r1_);
-//      ar & BOOST_SERIALIZATION_NVP(r2_);
-//      ar & BOOST_SERIALIZATION_NVP(r3_);
 #else
       ar & boost::serialization::make_nvp("w", quaternion_.w());
       ar & boost::serialization::make_nvp("x", quaternion_.x());
@@ -401,5 +398,5 @@ namespace gtsam {
    * @return an upper triangular matrix R
    * @return a vector [thetax, thetay, thetaz] in radians.
    */
-  std::pair<Matrix3,Vector3> RQ(const Matrix& A);
+  std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
 }
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index b698a77a5..aba684a95 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -322,8 +322,8 @@ Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
 
 /* ************************************************************************* */
 Vector3 Rot3::xyz() const {
-	Matrix I;Vector3 q;
-	boost::tie(I,q)=RQ(matrix());
+	Matrix3 I;Vector3 q;
+	boost::tie(I,q)=RQ(rot_);
 	return q;
 }
 
@@ -335,8 +335,7 @@ Vector3 Rot3::ypr() const {
 
 /* ************************************************************************* */
 Vector3 Rot3::rpy() const {
-	Vector3 q = xyz();
-	return Vector3(q(0),q(1),q(2));
+  return xyz();
 }
 
 /* ************************************************************************* */
@@ -345,7 +344,7 @@ Quaternion Rot3::toQuaternion() const {
 }
 
 /* ************************************************************************* */
-pair<Matrix3, Vector3> RQ(const Matrix& A) {
+pair<Matrix3, Vector3> RQ(const Matrix3& A) {
 
 	double x = -atan2(-A(2, 1), A(2, 2));
 	Rot3 Qx = Rot3::Rx(-x);
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index dfa2d546d..3a68e1a92 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -213,7 +213,7 @@ namespace gtsam {
   Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
   /* ************************************************************************* */
-  pair<Matrix3, Vector3> RQ(const Matrix& A) {
+  pair<Matrix3, Vector3> RQ(const Matrix3& A) {
 
     double x = -atan2(-A(2, 1), A(2, 2));
     Rot3 Qx = Rot3::Rx(-x);