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

Logmap fix from last week. Rot3 trace > 3 is now handled, previously caused NaN

release/4.3a0
Chris Beall 2011-05-02 16:59:33 +00:00
parent 7e901f4fad
commit 729b35fe11
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Rot3.cpp
View File

@ -161,7 +161,7 @@ namespace gtsam {
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (fabs(tr-3.0) < 1e-10) { // when theta = 0, +-2pi, +-4pi, etc.
if (tr > 3.0 - 1e-10) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3);
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(fabs(R.r3().z() - -1.0) > 1e-10)