Composition is faster, composeTransform is trivial (and should probably go)

cpp/Pose3.cpp

@@ -108,7 +108,7 @@ Pose3 Pose3::inverse() const
 /* ************************************************************************* */
 bool Pose3::equals(const Pose3& pose, double tol) const
 {
-  return R_.equals(pose.rotation(),tol) && t_.equals(pose.translation(),tol);
+  return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
 }
 
 /* ************************************************************************* */
@@ -122,27 +122,13 @@ bool assert_equal(const Pose3& A, const Pose3& B, double tol)
 }
 
 /* ************************************************************************* */
-Pose3 Pose3::transformPose_to(const Pose3& transform) const
+Pose3 Pose3::transformPose_to(const Pose3& pose) const
 {
-		Rot3 cRv = rotation() * Rot3(transform.rotation().inverse());
-		Point3 t = transform_to(transform, translation());
+		Rot3 cRv = R_ * Rot3(pose.R_.inverse());
+		Point3 t = transform_to(pose, t_);
 		
 		return Pose3(cRv, t);
 }
-
 /* ************************************************************************* */
-Pose3 composeTransform(const Pose3& current, const Pose3& target)
-{
-		// reverse operation
-		Rot3 trans_rot = Rot3(target.rotation() * current.rotation().inverse()).inverse();
-
-		// get sub
-		Point3 sub = rotate(trans_rot, target.translation());
-		
-		// get final transform translation
-		Point3 trans_pt = current.translation() - sub;
-		
-		return Pose3(trans_rot, trans_pt);
-}
 
 } // namespace gtsam

cpp/Pose3.h

@@ -19,25 +19,36 @@ namespace gtsam {
 		Point3 t_;
 
 	public:
-		Pose3() {
-		} // default is origin
+
+		/**
+		 * Default constructor is origin
+		 */
+		Pose3() {}
+
+		/**
+		 * Copy constructor
+		 */
 		Pose3(const Pose3& pose) :
 			R_(pose.R_), t_(pose.t_) {
 		}
+
+		/**
+		 * Construct from R,t
+		 */
 		Pose3(const Rot3& R, const Point3& t) :
 			R_(R), t_(t) {
 		}
 
-		/** constructor from 4*4 matrix */
+		/** Constructor from 4*4 matrix */
 		Pose3(const Matrix &T) :
 			R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
 					T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
 		}
 
-		/** constructor from 12D vector */
+		/** Constructor from 12D vector */
 		Pose3(const Vector &V) :
-			R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), t_(V(9), V(10),
-					V(11)) {
+			R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
+			t_(V(9), V(10),V(11)) {
 		}
 
 		const Rot3& rotation() const {
@@ -61,7 +72,7 @@ namespace gtsam {
 
 		/** composition */
 		inline Pose3 operator*(const Pose3& B) const {
-			return Pose3(matrix() * B.matrix());
+			return Pose3(R_*B.R_, t_ + R_*B.t_);
 		}
 
 		/** return 12D vectorized form (column-wise) */
@@ -94,8 +105,12 @@ namespace gtsam {
 		}
 	}; // Pose3 class
 
-	/** finds a transform from the current frame to the target frame given two coordinates of the same point */
-	Pose3 composeTransform(const Pose3& current, const Pose3& target);
+	/**
+	 * Finds a transform from the current frame dTx to the target frame sTx
+	 */
+	inline Pose3 composeTransform(const Pose3& dTx, const Pose3& sTx) {
+		return dTx * sTx.inverse();
+	}
 
 	/** receives the point in Pose coordinates and transforms it to world coordinates */
 	Point3 transform_from(const Pose3& pose, const Point3& p);

cpp/testPose3.cpp

@@ -43,7 +43,7 @@ TEST( Pose3, exmap)
 }
 
 /* ************************************************************************* */
-TEST( Pose3, compose)
+TEST( Pose3, compose )
 {
   Matrix actual = (T*T).matrix();
   Matrix expected = T.matrix()*T.matrix();
@@ -180,7 +180,7 @@ TEST( Pose3, transformPose_to)
 }
 
 /* ************************************************************************* */
-TEST( Pose3, composeTransform)
+TEST( Pose3, composeTransform )
 {
 		// known transform
 		Rot3 r = rodriguez(0,0,-1.570796);