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

more refactoring, use different Rot3 constructor so no confusion

release/4.3a0
Frank Dellaert 2010-03-02 06:16:11 +00:00
parent 680a6a28c1
commit 32beb8c712
1 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/Rot3.cpp
View File

@ -140,25 +140,22 @@ namespace gtsam {
Rot3 rodriguez(const Vector& w, double theta) { Rot3 rodriguez(const Vector& w, double theta) {
// get components of axis \omega // get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2); double wx = w(0), wy=w(1), wz=w(2);
double w2_xx = wx*wx, w2_yy = wy*wy, w2_zz = wz*wz; double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG #ifndef NDEBUG
double l_n = w2_xx + w2_yy + w2_zz; double l_n = wwTxx + wwTyy + wwTzz;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif #endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c; double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double swx = wx * s, swy = wy * s, swz = wz * s; 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 C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
double C11 = c_1*w2_yy, C12 = c_1*wy*wz; double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*w2_zz; double C22 = c_1*wwTzz;
// Important: these are columns, so this reads transposed !!!! return Rot3( c + C00, -swz + C01, swy + C02,
Point3 r1 = Point3( c + C00, swz + C01, -swy + C02); swz + C01, c + C11, -swx + C12,
Point3 r2 = Point3(-swz + C01, c + C11, swx + C12); -swy + C02, swx + C12, c + C22);
Point3 r3 = Point3( swy + C02, -swx + C12, c + C22);
return Rot3(r1, r2, r3);
} }
/* ************************************************************************* */ /* ************************************************************************* */