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

Bugfix in Rot3::logmap. The function checked for a trace of 3, but the check could fail due to rounding errors, causing the function to try to take acos(something larger than 1) resulting in NaNs being returned.

release/4.3a0
justinca 2010-01-21 19:49:12 +00:00
parent ac746ccead
commit f89a53ed91
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cpp/Rot3.cpp
View File

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