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

Revert "Improved CayleyChart Local" 

This reverts commit b5284e4b3f.
release/4.3a0
Varun Agrawal 2020-11-12 10:57:40 -05:00
parent 3db5983c16
commit 6bf410c0d8
1 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/Rot3M.cpp
View File

@ -176,13 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
// Create a fixed-size matrix // Create a fixed-size matrix
Matrix3 A = R.matrix(); Matrix3 A = R.matrix();
const Matrix3 P = A + I_3x3; // Mathematica closed form optimization (procrastination?) gone wild:
// Check if (A+I) is invertible. Same as checking for -1 eigenvalue. const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
if (P.determinant() == 0.0) { const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
} const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
Matrix3 Pinv = (A + I_3x3).inverse(); const double M = 1 + e - f * h + i + e * i;
return SO3::Vee(Pinv * (A - I_3x3)) * 2; const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
const double x = a * f - cd + f;
const double y = b * f - ce - c;
const double z = fg - di - d;
return K * Vector3(x, y, z);
} }
/* ************************************************************************* */ /* ************************************************************************* */