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

fixed round off error in Rot3.rodriguez

release/4.3a0
Kai Ni 2009-09-16 02:20:57 +00:00
parent 7fb691183d
commit 354f9de64e
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Rot3.cpp
View File

@ -32,9 +32,9 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 rodriguez(double wx, double wy, double wz) {
if (wx == 0.0 && wy == 0.0 && wz == 0.0) return Rot3();
Matrix J = skewSymmetric(wx, wy, wz);
double t2 = wx * wx + wy * wy + wz * wz;
if (t2 < 1e-10) return Rot3();
double t = sqrt(t2);
Matrix R = eye(3, 3) + sin(t) / t * J + (1.0 - cos(t)) / t2 * (J * J);
return R; // matrix constructor will be tripped