diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index ee3d3b4fb..3bd8d0c1b 100644 --- a/cpp/Rot3.cpp +++ b/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 RQ(const Matrix& A) { double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22); diff --git a/cpp/Rot3.h b/cpp/Rot3.h index ef1695145..b290f3892 100644 --- a/cpp/Rot3.h +++ b/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' diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 47618c034..9f49a9132 100644 --- a/cpp/testRot3.cpp +++ b/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); - fill(v.begin(), v.end(), 0); - CHECK(assert_equal(R.exmap(v), R)); + Vector v(3); + fill(v.begin(), v.end(), 0); + 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)); } /* ************************************************************************* */