12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Modernized and tested RQ and added Rot3::ypr. Note this yaw-pitch-roll is different from ML version, which is focused on cameras. Let the fun begin...

release/4.3a0
Frank Dellaert 2010-01-06 15:52:43 +00:00
parent 9845a5ae37
commit f77da96caf
4 changed files with 105 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cpp/Pose3.cpp
View File

@ -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);
}

100
cpp/Rot3.cpp
View File

@ -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<Matrix,Vector> 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);
}
/* ************************************************************************* */

28
cpp/Rot3.h
View File

@ -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<Matrix,Vector> RQ(const Matrix& A);
}

41
cpp/testRot3.cpp
View File

@ -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); }
/* ************************************************************************* */