diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index de547210b..12ef9178a 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -84,9 +84,9 @@ Matrix Dtransform_to2(const Pose3& pose, const Point3& p) { // used as soft prior /* ************************************************************************* */ Vector hPose (const Vector& x) { - Pose3 pose(x); // transform from vector to Pose3 - Vector w = RQ(pose.rotation().matrix()); // get angle differences - Vector d = pose.translation().vector(); // get translation differences + Pose3 pose(x); // transform from vector to Pose3 + Vector w = pose.rotation().ypr(); // get angle differences + Vector d = pose.translation().vector(); // get translation differences return concatVectors(2,&w,&d); } diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index cbfbc98c4..ee3d3b4fb 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -67,6 +67,22 @@ namespace gtsam { r3_.x(), r3_.y(), r3_.z()); } + /* ************************************************************************* */ + Point3 Rot3::unrotate(const Point3& p) const { + return Point3( + 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() + ); + } + + /* ************************************************************************* */ + Vector Rot3::ypr() const { + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; + } + /* ************************************************************************* */ Rot3 rodriguez(const Vector& n, double t) { double n0 = n(0), n1=n(1), n2=n(2); @@ -136,64 +152,40 @@ namespace gtsam { } /* ************************************************************************* */ - /** This function receives a rotation 3 by 3 matrix and returns 3 rotation angles. - * The implementation is based on the algorithm in multiple view geometry - * the function returns a vector that its arguments are: thetax, thetay, thetaz in radians. - */ - /* ************************************************************************* */ - Vector RQ(Matrix R) { - double Cx = R(2, 2) / (double) ((sqrt(pow((double) (R(2, 2)), 2.0) + pow( - (double) (R(2, 1)), 2.0)))); //cosX - double Sx = -R(2, 1) / (double) ((sqrt(pow((double) (R(2, 2)), 2.0) + pow( - (double) (R(2, 1)), 2.0)))); //sinX - Matrix Qx(3, 3); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - Qx(i, j) = 0; + pair RQ(const Matrix& A) { + double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22); + double Cx = A22 / a; //cosX + double Sx = -A21 / a; //sinX + Matrix Qx = Matrix_(3, 3, + 1.0, 0.0, 0.0, + 0.0, Cx, -Sx, + 0.0, Sx, Cx); + Matrix B = A * Qx; - Qx(0, 0) = 1; - Qx(1, 1) = Cx; - Qx(1, 2) = -Sx; - Qx(2, 1) = Sx; - Qx(2, 2) = Cx; - R = R * Qx; - double Cy = R(2, 2) / (sqrt(pow((double) (R(2, 2)), 2.0) + pow((double) (R( - 2, 0)), 2.0))); //cosY - double Sy = R(2, 0) / (sqrt(pow((double) (R(2, 2)), 2.0) + pow((double) (R( - 2, 0)), 2.0))); //sinY - Matrix Qy(3, 3); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - Qy(i, j) = 0; + double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22); + double Cy = B22 / b; //cosY + double Sy = B20 / b; //sinY + Matrix Qy = Matrix_(3,3, + Cy, 0.0, Sy, + 0.0, 1.0, 0.0, + -Sy, 0.0, Cy); + Matrix C = B * Qy; - Qy(0, 0) = Cy; - Qy(0, 2) = Sy; - Qy(1, 1) = 1; - Qy(2, 0) = -Sy; - Qy(2, 2) = Cy; - R = R * Qy; - double Cz = R(1, 1) / (sqrt(pow((double) (R(1, 1)), 2.0) + pow((double) (R( - 1, 0)), 2.0))); //cosZ - double Sz = -R(1, 0) / (sqrt(pow((double) (R(1, 1)), 2.0) + pow( - (double) (R(1, 0)), 2.0)));//sinZ - Matrix Qz(3, 3); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - Qz(i, j) = 0; - Qz(0, 0) = Cz; - Qz(0, 1) = -Sz; - Qz(1, 0) = Sz; - Qz(1, 1) = Cz; - Qz(2, 2) = 1; - R = R * Qz; - double pi = atan2(sqrt(2.0) / 2.0, sqrt(2.0) / 2.0) * 4.0; + double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11); + double Cz = C11 / c; //cosZ + double Sz = -C10 / c; //sinZ + Matrix Qz = Matrix_(3, 3, + Cz, -Sz, 0.0, + Sz, Cz, 0.0, + 0.0, 0.0, 1.0); + Matrix R = C * Qz; - Vector result(3); - result(0) = -atan2(Sx, Cx); - result(1) = -atan2(Sy, Cy); - result(2) = -atan2(Sz, Cz); + Vector angles(3); + angles(0) = -atan2(Sx, Cx); + angles(1) = -atan2(Sy, Cy); + angles(2) = -atan2(Sz, Cz); - return result; + return make_pair(R,angles); } /* ************************************************************************* */ diff --git a/cpp/Rot3.h b/cpp/Rot3.h index e02d31c10..ef1695145 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -91,13 +91,10 @@ namespace gtsam { } /** rotate from world to rotated = R'*p */ - Point3 unrotate(const Point3& p) const { - return Point3( - 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() - ); - } + Point3 unrotate(const Point3& p) const; + + /** use RQ to calculate yaw-pitch-roll angle representation */ + Vector ypr() const; /** friends */ friend Matrix Dunrotate1(const Rot3& R, const Point3& p); @@ -150,7 +147,7 @@ namespace gtsam { * rotate point from rotated coordinate frame to * world = R*p */ - Point3 rotate(const Rot3& R, const Point3& p); + Point3 rotate (const Rot3& R, const Point3& p); Matrix Drotate1(const Rot3& R, const Point3& p); Matrix Drotate2(const Rot3& R); // does not depend on p ! @@ -158,11 +155,20 @@ namespace gtsam { * rotate point from world to rotated * frame = R'*p */ - Point3 unrotate(const Rot3& R, const Point3& p); + Point3 unrotate (const Rot3& R, const Point3& p); Matrix Dunrotate1(const Rot3& R, const Point3& p); Matrix Dunrotate2(const Rot3& R); // does not depend on p ! - /** receives a rotation 3 by 3 matrix and returns 3 rotation angles.*/ - Vector RQ(Matrix R); + /** + * [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' + * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will + * be the identity and Q is a yaw-pitch-roll decomposition of A. + * The implementation uses Givens rotations and is based on Hartley-Zisserman. + * @param a 3 by 3 matrix A=RQ + * @return an upper triangular matrix R + * @return a vector [thetax, thetay, thetaz] in radians. + */ + std::pair RQ(const Matrix& A); } diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 096a2e45d..47618c034 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -95,6 +95,19 @@ 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) { @@ -121,8 +134,6 @@ TEST( Rot3, Drotate2_DNrotate2) } /* ************************************************************************* */ -// unrotate - TEST( Rot3, unrotate) { Point3 w = R*P; @@ -146,6 +157,32 @@ TEST( Rot3, Dunrotate2_DNunrotate2) CHECK(assert_equal(numerical,computed,error)); } +/* ************************************************************************* */ +TEST( Rot3, RQ) +{ + // Try RQ on a pure rotation + Matrix actualK; Vector actual; + boost::tie(actualK,actual) = RQ(R.matrix()); + Vector expected = Vector_(3,0.14715, 0.385821, 0.231671); + CHECK(assert_equal(eye(3),actualK)); + CHECK(assert_equal(expected,actual,1e-6)); + + // Try using ypr call + actual = R.ypr(); + CHECK(assert_equal(expected,actual,1e-6)); + + // Try RQ to recover calibration from 3*3 sub-block of projection matrix + Matrix K = Matrix_(3,3, + 500.0, 0.0, 320.0, + 0.0, 500.0, 240.0, + 0.0, 0.0, 1.0 + ); + Matrix A = K*R.matrix(); + boost::tie(actualK,actual) = RQ(A); + CHECK(assert_equal(K,actualK)); + CHECK(assert_equal(expected,actual,1e-6)); +} + /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */