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

refactored and thoroughly checked rodrgues, added two more unit test

release/4.3a0
Frank Dellaert 2010-03-02 05:45:19 +00:00
parent f956bae6a4
commit 33c6c51658
2 changed files with 40 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
cpp/Rot3.cpp
View File

@ -137,23 +137,26 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& n, double t) {
double n0 = n(0), n1=n(1), n2=n(2);
double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2;
Rot3 rodriguez(const Vector& w, double theta) {
// get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2);
double w2_xx = wx*wx, w2_yy = wy*wy, w2_zz = wz*wz;
#ifndef NDEBUG
double l_n = n00+n11+n22;
double l_n = w2_xx + w2_yy + w2_zz;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif
double ct = cos(t), st = sin(t), ct_1 = 1 - ct;
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st;
double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2;
double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22;
double swx = wx * s, swy = wy * s, swz = wz * s;
double C00 = c_1*w2_xx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
double C11 = c_1*w2_yy, C12 = c_1*wy*wz;
double C22 = c_1*w2_zz;
Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02);
Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12);
Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22);
// Important: these are columns, so this reads transposed !!!!
Point3 r1 = Point3( c + C00, swz + C01, -swy + C02);
Point3 r2 = Point3(-swz + C01, c + C11, swx + C12);
Point3 r3 = Point3( swy + C02, -swx + C12, c + C22);
return Rot3(r1, r2, r3);
}

37
cpp/testRot3.cpp
View File

@ -68,12 +68,13 @@ TEST( Rot3, equals)
}
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = eye(3, 3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R; // matrix constructor will be tripped
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
/* ************************************************************************* */
@ -82,19 +83,19 @@ TEST( Rot3, rodriguez)
Rot3 R1 = rodriguez(epsilon, 0, 0);
Vector w = Vector_(3, epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R1,R2));
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector v(3);
v(0) = 0;
v(1) = 1;
v(2) = 0;
Rot3 R1 = rodriguez(v, 3.14 / 4.0);
Rot3 R2(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388);
CHECK(assert_equal(R1,R2,1e-5));
Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
CHECK(assert_equal(expected,actual,1e-5));
}
/* ************************************************************************* */
@ -103,7 +104,21 @@ TEST( Rot3, rodriguez3)
Vector w = Vector_(3, 0.1, 0.2, 0.3);
Rot3 R1 = rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R1,R2));
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
double angle = M_PI_2;
Rot3 actual = rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0,
s, c, 0,
0, 0, 1);
CHECK(assert_equal(expected,actual,1e-5));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
}
/* ************************************************************************* */