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

Changed logmap tolerance for failed platform

release/4.3a0
Binit Shah 2020-05-31 01:49:52 -04:00
parent 99cc2294e1
commit d0d80f55a6
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/SO3.cpp
View File

@ -262,12 +262,12 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (tr + 1.0 < 1e-10) { if (tr + 1.0 < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-5) if (std::abs(R33 + 1.0) > 1e-10)
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-5) else if (std::abs(R22 + 1.0) > 1e-10)
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else else
// if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else { } else {
double magnitude; double magnitude;