Added static methods to construct rotations, coded up convention for yaw-pitch-roll, modernized RQ
Frank Dellaert
parent
d4f92c7bb6
commit
6851b31fad
85
cpp/Rot3.cpp
85
cpp/Rot3.cpp
|
|
@ -12,22 +12,38 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// static member functions to construct rotations
|
||||
|
||||
Rot3 Rot3::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
|
||||
Rot3 Rot3::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
|
||||
Rot3 Rot3::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Vector Rot3::vector() const {
|
||||
// double r[] = { r1_.x(), r1_.y(), r1_.z(),
|
||||
// r2_.x(), r2_.y(), r2_.z(),
|
||||
// r3_.x(), r3_.y(), r3_.z() };
|
||||
// Vector v(9);
|
||||
// copy(r,r+9,v.begin());
|
||||
// return v;
|
||||
// }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3::matrix() const {
|
||||
double r[] = { r1_.x(), r2_.x(), r3_.x(),
|
||||
|
|
@ -55,12 +71,17 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::ypr() const {
|
||||
Vector Rot3::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
Vector Rot3::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 rodriguez(const Vector& n, double t) {
|
||||
double n0 = n(0), n1=n(1), n2=n(2);
|
||||
|
|
@ -150,39 +171,21 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
|
||||
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;
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
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;
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
Vector angles(3);
|
||||
angles(0) = -atan2(Sx, Cx);
|
||||
angles(1) = -atan2(Sy, Cy);
|
||||
angles(2) = -atan2(Sz, Cz);
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
return make_pair(R,angles);
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
34
cpp/Rot3.h
34
cpp/Rot3.h
|
|
@ -54,6 +54,29 @@ namespace gtsam {
|
|||
r2_(Point3(R(0,1), R(1,1), R(2,1))),
|
||||
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
|
||||
|
||||
/** Static member function to generate some well known rotations */
|
||||
|
||||
/**
|
||||
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
|
||||
* Counterclockwise when looking from unchanging axis.
|
||||
*/
|
||||
static Rot3 Rx(double t);
|
||||
static Rot3 Ry(double t);
|
||||
static Rot3 Rz(double t);
|
||||
static Rot3 RzRyRx(double x, double y, double z) { return Rz(z)*Ry(y)*Rx(x);}
|
||||
|
||||
/**
|
||||
* Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
|
||||
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
|
||||
* Differs from one defined by Justin, who uses, (x,y,z) = (roll,-pitch,yaw)
|
||||
* and http://en.wikipedia.org/wiki/Yaw,_pitch,_and_roll, where (x,y,z) = (-roll,pitch,yaw)
|
||||
* Assumes vehicle coordinate frame X forward, Y right, Z down
|
||||
*/
|
||||
static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
|
||||
static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
|
||||
static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
|
||||
static Rot3 ypr (double y, double p, double r) { return yaw(y)*pitch(p)*roll(r);}
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
||||
|
||||
|
|
@ -72,7 +95,16 @@ namespace gtsam {
|
|||
Point3 r2() const { return r2_; }
|
||||
Point3 r3() const { return r3_; }
|
||||
|
||||
/** use RQ to calculate yaw-pitch-roll angle representation */
|
||||
/**
|
||||
* Use RQ to calculate xyz angle representation
|
||||
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
|
||||
*/
|
||||
Vector xyz() const;
|
||||
|
||||
/**
|
||||
* Use RQ to calculate yaw-pitch-roll angle representation
|
||||
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
|
||||
*/
|
||||
Vector ypr() const;
|
||||
|
||||
private:
|
||||
|
|
|
|||
106
cpp/testRot3.cpp
106
cpp/testRot3.cpp
|
|
@ -103,6 +103,40 @@ TEST( Rot3, expmap)
|
|||
CHECK(assert_equal(expmap(R,v), R));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, log)
|
||||
{
|
||||
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
|
||||
Rot3 R1 = rodriguez(w1);
|
||||
CHECK(assert_equal(w1, logmap(R1)));
|
||||
|
||||
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
|
||||
Rot3 R2 = rodriguez(w2);
|
||||
CHECK(assert_equal(w2, logmap(R2)));
|
||||
|
||||
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
|
||||
Rot3 R3 = rodriguez(w3);
|
||||
CHECK(assert_equal(w3, logmap(R3)));
|
||||
|
||||
Vector w = Vector_(3, 0.1, 0.4, 0.2);
|
||||
Rot3 R = rodriguez(w);
|
||||
CHECK(assert_equal(w, logmap(R)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 = logmap(t1, t2);
|
||||
CHECK(assert_equal(t2, expmap(t1,d12)));
|
||||
CHECK(assert_equal(t2, expmap<Rot3>(d12)*t1));
|
||||
Vector d21 = logmap(t2, t1);
|
||||
CHECK(assert_equal(t1, expmap(t2,d21)));
|
||||
CHECK(assert_equal(t1, expmap<Rot3>(d21)*t2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// rotate derivatives
|
||||
|
||||
|
|
@ -128,8 +162,6 @@ TEST( Rot3, Drotate2_DNrotate2)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// unrotate
|
||||
|
||||
TEST( Rot3, unrotate)
|
||||
{
|
||||
Point3 w = R*P;
|
||||
|
|
@ -175,6 +207,11 @@ TEST( Rot3, compose )
|
|||
/* ************************************************************************* */
|
||||
TEST( Rot3, between )
|
||||
{
|
||||
Rot3 R = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 origin;
|
||||
CHECK(assert_equal(R, between(origin,R)));
|
||||
CHECK(assert_equal(inverse(R), between(R,origin)));
|
||||
|
||||
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
|
|
@ -191,6 +228,71 @@ TEST( Rot3, between )
|
|||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, xyz )
|
||||
{
|
||||
double t = 0.1, st = sin(t), ct = cos(t);
|
||||
|
||||
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3::Rx(t)));
|
||||
|
||||
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3::Ry(t)));
|
||||
|
||||
// yaw is around z axis
|
||||
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3::Rz(t)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, yaw_pitch_roll )
|
||||
{
|
||||
double t = 0.1;
|
||||
|
||||
// yaw is around z axis
|
||||
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
|
||||
|
||||
// pitch is around y axis
|
||||
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
|
||||
|
||||
// roll is around x axis
|
||||
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 xyz call
|
||||
CHECK(assert_equal(expected,R.xyz(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz(),1e-6));
|
||||
|
||||
// Try using xyz call
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr(),1e-6));
|
||||
|
||||
// Try ypr for pure yaw-pitch-roll matrices
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr(),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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue