log and between

cpp/Rot3.cpp

@@ -23,6 +23,18 @@ namespace gtsam {
 		return rodriguez(v) * (*this);
 	}
 
+	/* ************************************************************************* */
+	Vector Rot3::log() const {
+		double tr = r1_.x()+r2_.y()+r3_.z();
+		if (tr==3.0) return ones(3);
+		if (tr==-1.0) throw domain_error("Rot3::log: trace == -1 not yet handled :-(");;
+		double theta = acos((tr-1.0)/2.0);
+		return (theta/2.0/sin(theta))*Vector_(3,
+				r2_.z()-r3_.y(),
+				r3_.x()-r1_.z(),
+				r1_.y()-r2_.x());
+	}
+
   /* ************************************************************************* */
   Vector Rot3::vector() const {
     double r[] = { r1_.x(), r1_.y(), r1_.z(),
@@ -118,6 +130,10 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+  Vector log(const Rot3& R, const Rot3& S) {
+  	return between(R,S).log();
+  }
+	/* ************************************************************************* */
 	Point3 rotate(const Rot3& R, const Point3& p) {
 		return R * p;
 	}
@@ -151,6 +167,11 @@ namespace gtsam {
 		return R.transpose();
 	}
 
+	/* ************************************************************************* */
+	Rot3 between(const Rot3& R1, const Rot3& R2) {
+		return R2 * R1.inverse();
+	}
+
 	/* ************************************************************************* */
 	pair<Matrix,Vector> RQ(const Matrix& A) {
 		double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22);

cpp/Rot3.h

@@ -64,9 +64,17 @@ namespace gtsam {
     /** return DOF, dimensionality of tangent space */
     size_t dim() const { return 3;}
 		
-    /** Given 3-dim tangent vector, create new rotation */
+    /**
+     * @param a 3-dim tangent vector d (canonical coordinates of between(R,S))
+     * @return new rotation S=exp(d)*R
+     */
     Rot3 exmap(const Vector& d) const;
 		
+    /**
+     * @return log(R), i.e. canonical coordinates of R
+     */
+    Vector log() const;
+
     /** return vectorized form (column-wise)*/
     Vector vector() const;
 
@@ -143,6 +151,13 @@ namespace gtsam {
    */
   Rot3 exmap(const Rot3& R, const Vector& v);
 
+  /**
+   * @param a rotation R
+   * @param a rotation S
+   * @return log(S*R'), i.e. canonical coordinates of between(R,S)
+   */
+  Vector log(const Rot3& R, const Rot3& S);
+
   /**
    * rotate point from rotated coordinate frame to 
    * world = R*p
@@ -159,6 +174,11 @@ namespace gtsam {
   Matrix Dunrotate1(const Rot3& R, const Point3& p);
   Matrix Dunrotate2(const Rot3& R); // does not depend on p !
 
+	/**
+	 * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
+	 */
+	Rot3 between(const Rot3& R1, const Rot3& R2);
+
 	/**
 	 * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
 	 * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'

cpp/testRot3.cpp

@@ -95,25 +95,55 @@ TEST( Rot3, rodriguez3) {
   CHECK(assert_equal(R1,R2));
 }
 
-/* ************************************************************************* */
-//TEST(Rot3, manifold) {
-//	Rot3 t1 = rodriguez(0.1,0.4,0.2);
-//	Rot3 t2 = rodriguez(0.3,0.1,0.7);
-//	Rot3 origin;
-//	Vector d12 = t1.log(t2);
-//	CHECK(assert_equal(t2, t1.exmap(d12)));
-//	CHECK(assert_equal(t2, origin.exmap(d12)*t1));
-//	Vector d21 = t2.log(t1);
-//	CHECK(assert_equal(t1, t2.exmap(d21)));
-//	CHECK(assert_equal(t1, origin.exmap(d21)*t2));
-//}
-
 /* ************************************************************************* */
 TEST( Rot3, exmap)
 {
-  Vector v(3);
+	Vector v(3);
-  fill(v.begin(), v.end(), 0);
+	fill(v.begin(), v.end(), 0);
-  CHECK(assert_equal(R.exmap(v), R));
+	CHECK(assert_equal(R.exmap(v), R));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, log)
+{
+	Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
+	Rot3 R1 = rodriguez(w1);
+	CHECK(assert_equal(w1, R1.log()));
+
+	Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
+	Rot3 R2 = rodriguez(w2);
+	CHECK(assert_equal(w2, R2.log()));
+
+	Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
+	Rot3 R3 = rodriguez(w3);
+	CHECK(assert_equal(w3, R3.log()));
+
+	Vector w = Vector_(3, 0.1, 0.4, 0.2);
+	Rot3 R = rodriguez(w);
+	CHECK(assert_equal(w, R.log()));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, between)
+{
+	Rot3 R = rodriguez(0.1, 0.4, 0.2);
+	Rot3 origin;
+	CHECK(assert_equal(R, between(origin,R)));
+	CHECK(assert_equal(R.inverse(), between(R,origin)));
+}
+
+/* ************************************************************************* */
+	TEST(Rot3, manifold)
+{
+	Rot3 t1 = rodriguez(0.1, 0.4, 0.2);
+	Rot3 t2 = rodriguez(0.3, 0.1, 0.7);
+	Rot3 origin;
+	Vector d12 = log(t1, t2);
+	CHECK(assert_equal(t2, t1.exmap(d12)));
+	CHECK(assert_equal(t2, origin.exmap(d12)*t1));
+	Vector d21 = log(t2, t1);
+	CHECK(assert_equal(t1, t2.exmap(d21)));
+	CHECK(assert_equal(t1, origin.exmap(d21)*t2));
 }
 
 /* ************************************************************************* */