converted Rot3M to fixed-size Matrix, and changed some methods elsewhere to return fixed-size Vector3 to avoid heap allocations for speedup.

gtsam/base/Matrix.h

@@ -37,6 +37,8 @@ namespace gtsam {
 typedef Eigen::MatrixXd Matrix;
 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
 
+typedef Eigen::Matrix3d Matrix3;
+
 // Matrix expressions for accessing parts of matrices
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;

gtsam/base/Vector.h

@@ -33,6 +33,10 @@ namespace gtsam {
 // Typedef arbitary length vector
 typedef Eigen::VectorXd Vector;
 
+// Commonly used fixed size vectors
+typedef Eigen::Vector3d Vector3;
+typedef Eigen::Matrix<double, 6, 1> Vector6;
+
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
 

gtsam/geometry/Point3.h

@@ -126,7 +126,7 @@ namespace gtsam {
   	inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
 
   	/// Returns inverse retraction
-  	inline Vector localCoordinates(const Point3& q) const { return (q -*this).vector(); }
+  	inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }
 
     /// @}
     /// @name Lie Group
@@ -136,7 +136,7 @@ namespace gtsam {
     static inline Point3 Expmap(const Vector& v) { return Point3(v); }
 
     /** Log map at identity - return the x,y,z of this point */
-    static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
+    static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
 
     /// @}
     /// @name Vector Space
@@ -170,9 +170,8 @@ namespace gtsam {
     bool   operator ==(const Point3& q) const;
 
     /** return vectorized form (column-wise)*/
-    Vector vector() const {
+    Vector3 vector() const {
-      Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_;
+      return Vector3(x_,y_,z_);
-      return v;
     }
 
     /// get x

gtsam/geometry/Pose3.cpp

@@ -136,7 +136,7 @@ namespace gtsam {
       return Logmap(between(T));
     } else if(mode == Pose3::FIRST_ORDER) {
       // R is always done exactly in all three retract versions below
-      Vector omega = R_.localCoordinates(T.rotation());
+      Vector3 omega = R_.localCoordinates(T.rotation());
 
       // Incorrect version
       // Independently computes the logmap of the translation and rotation

gtsam/geometry/Rot3.h

@@ -33,6 +33,7 @@
 #endif
 
 #include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/3rdparty/Eigen/Eigen/Geometry>
 
@@ -59,8 +60,7 @@ namespace gtsam {
     /** Internal Eigen Quaternion */
     Quaternion quaternion_;
 #else
-    /** We store columns ! */
+    Matrix3 rot_;
-    Point3 r1_, r2_, r3_;
 #endif
 
   public:
@@ -87,6 +87,9 @@ namespace gtsam {
     /** constructor from a rotation matrix */
     Rot3(const Matrix& R);
 
+//    /** constructor from a fixed size rotation matrix */
+//    Rot3(const Matrix3& R);
+
     /** Constructor from a quaternion.  This can also be called using a plain
      * Vector, due to implicit conversion from Vector to Quaternion
      * @param q The quaternion
@@ -183,7 +186,7 @@ namespace gtsam {
     /// @{
 
     /** print */
-    void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
+    void print(const std::string& s="R") const { gtsam::print((Matrix)matrix(), s);}
 
     /** equals with an tolerance */
     bool equals(const Rot3& p, double tol = 1e-9) const;
@@ -250,7 +253,7 @@ namespace gtsam {
     Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 
     /// Returns local retract coordinates in neighborhood around current rotation
-    Vector localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
+    Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 
     /// @}
     /// @name Lie Group
@@ -268,7 +271,7 @@ namespace gtsam {
     /**
      * Log map at identity - return the canonical coordinates of this rotation
      */
-    static Vector Logmap(const Rot3& R);
+    static Vector3 Logmap(const Rot3& R);
 
   	/// @}
   	/// @name Group Action on Point3
@@ -294,10 +297,10 @@ namespace gtsam {
   	/// @{
 
     /** return 3*3 rotation matrix */
-    Matrix matrix() const;
+    Matrix3 matrix() const;
 
     /** return 3*3 transpose (inverse) rotation matrix   */
-    Matrix transpose() const;
+    Matrix3 transpose() const;
 
     /** returns column vector specified by index */
     Point3 column(int index) const;
@@ -309,19 +312,19 @@ namespace gtsam {
      * Use RQ to calculate xyz angle representation
      * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
      */
-    Vector xyz() const;
+    Vector3 xyz() const;
 
     /**
      * Use RQ to calculate yaw-pitch-roll angle representation
      * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
      */
-    Vector ypr() const;
+    Vector3 ypr() const;
 
     /**
      * Use RQ to calculate roll-pitch-yaw angle representation
      * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
      */
-    Vector rpy() const;
+    Vector3 rpy() const;
 
     /**
      * Accessor to get to component of angle representations
@@ -365,9 +368,18 @@ namespace gtsam {
     	 ar & boost::serialization::make_nvp("Rot3",
     			 boost::serialization::base_object<Value>(*this));
 #ifndef GTSAM_DEFAULT_QUATERNIONS
-      ar & BOOST_SERIALIZATION_NVP(r1_);
+    	 ar & boost::serialization::make_nvp("rot11", rot_(0,0));
-      ar & BOOST_SERIALIZATION_NVP(r2_);
+       ar & boost::serialization::make_nvp("rot12", rot_(0,1));
-      ar & BOOST_SERIALIZATION_NVP(r3_);
+       ar & boost::serialization::make_nvp("rot13", rot_(0,2));
+       ar & boost::serialization::make_nvp("rot21", rot_(1,0));
+       ar & boost::serialization::make_nvp("rot22", rot_(1,1));
+       ar & boost::serialization::make_nvp("rot23", rot_(1,2));
+       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());
@@ -389,5 +401,5 @@ namespace gtsam {
    * @return an upper triangular matrix R
    * @return a vector [thetax, thetay, thetaz] in radians.
    */
-  std::pair<Matrix,Vector> RQ(const Matrix& A);
+  std::pair<Matrix3,Vector3> RQ(const Matrix& A);
 }

gtsam/geometry/Rot3M.cpp

@@ -28,42 +28,39 @@ using namespace std;
 
 namespace gtsam {
 
-static const Matrix I3 = eye(3);
+static const Matrix3 I3 = Matrix3::Identity();
 
 /* ************************************************************************* */
-Rot3::Rot3() :
+Rot3::Rot3() : rot_(Matrix3::Identity()) {}
-    r1_(Point3(1.0,0.0,0.0)),
-    r2_(Point3(0.0,1.0,0.0)),
-    r3_(Point3(0.0,0.0,1.0)) {}
 
 /* ************************************************************************* */
-Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
+Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) {
-    r1_(r1), r2_(r2), r3_(r3) {}
+  rot_.col(0) = r1.vector();
+  rot_.col(1) = r2.vector();
+  rot_.col(2) = r3.vector();
+}
 
 /* ************************************************************************* */
 Rot3::Rot3(double R11, double R12, double R13,
     double R21, double R22, double R23,
-    double R31, double R32, double R33) :
+    double R31, double R32, double R33) {
-      r1_(Point3(R11, R21, R31)),
+    rot_ << R11, R12, R13,
-      r2_(Point3(R12, R22, R32)),
+        R21, R22, R23,
-      r3_(Point3(R13, R23, R33)) {}
+        R31, R32, R33;
+}
 
 /* ************************************************************************* */
 Rot3::Rot3(const Matrix& R) {
 	if (R.rows()!=3 || R.cols()!=3)
 		throw invalid_argument("Rot3 constructor expects 3*3 matrix");
-	r1_ = Point3(R(0, 0), R(1, 0), R(2, 0));
+	rot_ = R;
-	r2_ = Point3(R(0, 1), R(1, 1), R(2, 1));
-	r3_ = Point3(R(0, 2), R(1, 2), R(2, 2));
 }
 
+///* ************************************************************************* */
+//Rot3::Rot3(const Matrix3& R) : rot_(R) {}
+
 /* ************************************************************************* */
-Rot3::Rot3(const Quaternion& q) {
+Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
-  Eigen::Matrix3d R = q.toRotationMatrix();
-  r1_ = Point3(R.col(0));
-  r2_ = Point3(R.col(1));
-  r3_ = Point3(R.col(2));
-}
 
 /* ************************************************************************* */
 Rot3 Rot3::Rx(double t) {
@@ -164,46 +161,39 @@ Point3 Rot3::operator*(const Point3& p) const { return rotate(p); }
 
 /* ************************************************************************* */
 Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
-  if (H1) *H1 = -matrix();
+  if (H1) *H1 = -rot_;
-  return Rot3(
+  return Rot3(rot_.transpose());
-      r1_.x(), r1_.y(), r1_.z(),
-      r2_.x(), r2_.y(), r2_.z(),
-      r3_.x(), r3_.y(), r3_.z());
 }
 
 /* ************************************************************************* */
 Rot3 Rot3::between (const Rot3& R2,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = -(R2.transpose()*matrix());
+  if (H1) *H1 = -(R2.transpose()*rot_);
   if (H2) *H2 = I3;
-  return between_default(*this, R2);
+  return Rot3(rot_.transpose()*R2.rot_);
+  //return between_default(*this, R2);
 }
 
 /* ************************************************************************* */
 Rot3 Rot3::operator*(const Rot3& R2) const {
-  return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
+  return Rot3(rot_*R2.rot_);
 }
 
 /* ************************************************************************* */
 Point3 Rot3::rotate(const Point3& p,
     boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
 	if (H1 || H2) {
-			const Matrix R(matrix());
+			if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
-			if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
+			if (H2) *H2 = rot_;
-			if (H2) *H2 = R;
 		}
-  return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
+  return Point3(rot_ * p.vector());
 }
 
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Point3 q(
+  Point3 q(rot_.transpose()*p.vector()); // q = Rt*p
-			r1_.x()*p.x()+r1_.y()*p.y()+r1_.z()*p.z(),
-			r2_.x()*p.x()+r2_.y()*p.y()+r2_.z()*p.z(),
-			r3_.x()*p.x()+r3_.y()*p.y()+r3_.z()*p.z()
-  		); // q = Rt*p
   if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
   if (H2) *H2 = transpose();
   return q;
@@ -211,25 +201,26 @@ Point3 Rot3::unrotate(const Point3& p,
 
 /* ************************************************************************* */
 // Log map at identity - return the canonical coordinates of this rotation
-Vector Rot3::Logmap(const Rot3& R) {
+Vector3 Rot3::Logmap(const Rot3& R) {
 
 	static const double PI = boost::math::constants::pi<double>();
 
+	const Matrix3& rot = R.rot_;
 	// Get trace(R)
-	double tr = R.r1_.x()+R.r2_.y()+R.r3_.z();
+	double tr = rot.trace();
 
 	// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
 	// we do something special
 	if (std::abs(tr+1.0) < 1e-10) {
-    if(std::abs(R.r3_.z()+1.0) > 1e-10)
+    if(std::abs(rot(2,2)+1.0) > 1e-10)
-      return (PI / sqrt(2.0+2.0*R.r3_.z())) *
+      return (PI / sqrt(2.0+2.0*rot(2,2) )) *
-          Vector_(3, R.r3_.x(), R.r3_.y(), 1.0+R.r3_.z());
+          Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
-    else if(std::abs(R.r2_.y()+1.0) > 1e-10)
+    else if(std::abs(rot(1,1)+1.0) > 1e-10)
-      return (PI / sqrt(2.0+2.0*R.r2_.y())) *
+      return (PI / sqrt(2.0+2.0*rot(1,1))) *
-          Vector_(3, R.r2_.x(), 1.0+R.r2_.y(), R.r2_.z());
+          Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
     else // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
-      return (PI / sqrt(2.0+2.0*R.r1_.x())) *
+      return (PI / sqrt(2.0+2.0*rot(0,0))) *
-          Vector_(3, 1.0+R.r1_.x(), R.r1_.y(), R.r1_.z());
+          Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
   } else {
 		double magnitude;
 		double tr_3 = tr-3.0; // always negative
@@ -241,10 +232,10 @@ Vector Rot3::Logmap(const Rot3& R) {
 			// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
 			magnitude = 0.5 - tr_3*tr_3/12.0;
 		}
-		return magnitude*Vector_(3,
+		return magnitude*Vector3(
-				R.r2_.z()-R.r3_.y(),
+				rot(2,1)-rot(1,2),
-				R.r3_.x()-R.r1_.z(),
+				rot(0,2)-rot(2,0),
-				R.r1_.y()-R.r2_.x());
+				rot(1,0)-rot(0,1));
   }
 }
 
@@ -276,7 +267,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
 }
 
 /* ************************************************************************* */
-Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
+Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
   if(mode == Rot3::EXPMAP) {
     return Logmap(between(T));
   } else if(mode == Rot3::CAYLEY) {
@@ -292,13 +283,13 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
     const double x = (a * f - cd + f) * K;
     const double y = (b * f - ce - c) * K;
     const double z = (fg - di - d) * K;
-    return -2 * Vector_(3, x, y, z);
+    return -2 * Vector3(x, y, z);
   } else if(mode == Rot3::SLOW_CAYLEY) {
     // Create a fixed-size matrix
     Eigen::Matrix3d A(between(T).matrix());
     // using templated version of Cayley
     Matrix Omega = Cayley<3>(A);
-    return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
+    return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
   } else {
     assert(false);
     exit(1);
@@ -306,89 +297,69 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
 }
 
 /* ************************************************************************* */
-Matrix Rot3::matrix() const {
+Matrix3 Rot3::matrix() const {
-	Matrix R(3,3);
+	return rot_;
-	R <<
-			r1_.x(), r2_.x(), r3_.x(),
-			r1_.y(), r2_.y(), r3_.y(),
-			r1_.z(), r2_.z(), r3_.z();
-	return R;
 }
 
 /* ************************************************************************* */
-Matrix Rot3::transpose() const {
+Matrix3 Rot3::transpose() const {
-	Matrix Rt(3,3);
+	return rot_.transpose();
-	Rt <<
-			r1_.x(), r1_.y(), r1_.z(),
-			r2_.x(), r2_.y(), r2_.z(),
-			r3_.x(), r3_.y(), r3_.z();
-	return Rt;
 }
 
 /* ************************************************************************* */
 Point3 Rot3::column(int index) const{
-	if(index == 3)
+  return Point3(rot_.col(index));
-		return r3_;
-	else if(index == 2)
-		return r2_;
-	else if(index == 1)
-		return r1_; // default returns r1
-	else
-	  throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
 }
 
 /* ************************************************************************* */
-Point3 Rot3::r1() const { return r1_; }
+Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
 
 /* ************************************************************************* */
-Point3 Rot3::r2() const { return r2_; }
+Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
 
 /* ************************************************************************* */
-Point3 Rot3::r3() const { return r3_; }
+Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
 
 /* ************************************************************************* */
-Vector Rot3::xyz() const {
+Vector3 Rot3::xyz() const {
-	Matrix I;Vector q;
+	Matrix I;Vector3 q;
 	boost::tie(I,q)=RQ(matrix());
 	return q;
 }
 
 /* ************************************************************************* */
-Vector Rot3::ypr() const {
+Vector3 Rot3::ypr() const {
-	Vector q = xyz();
+	Vector3 q = xyz();
-	return Vector_(3,q(2),q(1),q(0));
+	return Vector3(q(2),q(1),q(0));
 }
 
 /* ************************************************************************* */
-Vector Rot3::rpy() const {
+Vector3 Rot3::rpy() const {
-	Vector q = xyz();
+	Vector3 q = xyz();
-	return Vector_(3,q(0),q(1),q(2));
+	return Vector3(q(0),q(1),q(2));
 }
 
 /* ************************************************************************* */
 Quaternion Rot3::toQuaternion() const {
-  return Quaternion((Eigen::Matrix3d() <<
+  return Quaternion(rot_);
-      r1_.x(), r2_.x(), r3_.x(),
-      r1_.y(), r2_.y(), r3_.y(),
-      r1_.z(), r2_.z(), r3_.z()).finished());
 }
 
 /* ************************************************************************* */
-pair<Matrix, Vector> RQ(const Matrix& A) {
+pair<Matrix3, Vector3> RQ(const Matrix& A) {
 
 	double x = -atan2(-A(2, 1), A(2, 2));
 	Rot3 Qx = Rot3::Rx(-x);
-	Matrix B = A * Qx.matrix();
+	Matrix3 B = A * Qx.matrix();
 
 	double y = -atan2(B(2, 0), B(2, 2));
 	Rot3 Qy = Rot3::Ry(-y);
-	Matrix C = B * Qy.matrix();
+	Matrix3 C = B * Qy.matrix();
 
 	double z = -atan2(-C(1, 0), C(1, 1));
 	Rot3 Qz = Rot3::Rz(-z);
-	Matrix R = C * Qz.matrix();
+	Matrix3 R = C * Qz.matrix();
 
-	Vector xyz = Vector_(3, x, y, z);
+	Vector xyz = Vector3(x, y, z);
 	return make_pair(R, xyz);
 }
 

gtsam/geometry/Rot3Q.cpp

@@ -49,6 +49,10 @@ namespace gtsam {
   Rot3::Rot3(const Matrix& R) :
       quaternion_(Eigen::Matrix3d(R)) {}
 
+//  /* ************************************************************************* */
+//   Rot3::Rot3(const Matrix3& R) :
+//       quaternion_(R) {}
+
   /* ************************************************************************* */
   Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
 
@@ -140,7 +144,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   // Log map at identity - return the canonical coordinates of this rotation
-  Vector Rot3::Logmap(const Rot3& R) {
+  Vector3 Rot3::Logmap(const Rot3& R) {
     Eigen::AngleAxisd angleAxis(R.quaternion_);
     if(angleAxis.angle() > M_PI)      // Important:  use the smallest possible
       angleAxis.angle() -= 2.0*M_PI;  // angle, e.g. no more than PI, to keep
@@ -155,15 +159,15 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	Vector Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
+	Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
 		return Logmap(between(t2));
 	}
 
   /* ************************************************************************* */
-  Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); }
+  Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); }
 
   /* ************************************************************************* */
-  Matrix Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
+  Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
 
   /* ************************************************************************* */
   Point3 Rot3::column(int index) const{
@@ -187,29 +191,29 @@ namespace gtsam {
   Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
 
   /* ************************************************************************* */
-  Vector Rot3::xyz() const {
+  Vector3 Rot3::xyz() const {
-    Matrix I;Vector q;
+    Matrix I;Vector3 q;
     boost::tie(I,q)=RQ(matrix());
     return q;
   }
 
   /* ************************************************************************* */
-  Vector Rot3::ypr() const {
+  Vector3 Rot3::ypr() const {
-  	Vector q = xyz();
+  	Vector3 q = xyz();
-    return Vector_(3,q(2),q(1),q(0));
+    return Vector3(q(2),q(1),q(0));
   }
 
   /* ************************************************************************* */
-  Vector Rot3::rpy() const {
+  Vector3 Rot3::rpy() const {
-  	Vector q = xyz();
+  	Vector3 q = xyz();
-    return Vector_(3,q(0),q(1),q(2));
+    return Vector3(q(0),q(1),q(2));
   }
 
   /* ************************************************************************* */
   Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
   /* ************************************************************************* */
-  pair<Matrix, Vector> RQ(const Matrix& A) {
+  pair<Matrix, Vector3> RQ(const Matrix& A) {
 
     double x = -atan2(-A(2, 1), A(2, 2));
     Rot3 Qx = Rot3::Rx(-x);
@@ -223,7 +227,7 @@ namespace gtsam {
     Rot3 Qz = Rot3::Rz(-z);
     Matrix R = C * Qz.matrix();
 
-    Vector xyz = Vector_(3, x, y, z);
+    Vector xyz = Vector3(x, y, z);
     return make_pair(R, xyz);
   }