From b4edfc257672a80a9cc744e697eeb567e8e50376 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 1 Feb 2016 13:04:12 -0800 Subject: [PATCH 1/2] One last improvement before Duy works out the *true* solution :-) --- gtsam/geometry/SO3.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2d274c278..ceb9eae6a 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -86,13 +86,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, } if (H1) { // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = omega.cross(v / theta); - const Vector3 KKv = omega.cross(Kv / theta); - const Matrix3 T = skewSymmetric(v / theta); + const Vector3 Kv = K * v; const double Da = (sin_theta - 2.0 * a) / theta2; const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T - - skewSymmetric(Kv * b / theta) - b * K * T; + *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - + skewSymmetric(Kv * b / theta) + + (a * I_3x3 - b * K) * skewSymmetric(v / theta); } if (H2) *H2 = dexp_; return dexp_ * v; From e3954d541aa69a0ea9091e552fe903a0ac4a9470 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 1 Feb 2016 21:07:50 +0000 Subject: [PATCH 2/2] SO3.cpp edited online with Bitbucket: KK not K*K --- gtsam/geometry/SO3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index ceb9eae6a..4d993de8f 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -63,7 +63,7 @@ SO3 ExpmapFunctor::expmap() const { if (nearZero) return I_3x3 + W; else - return I_3x3 + sin_theta * K + one_minus_cos * K * K; + return I_3x3 + sin_theta * K + one_minus_cos * KK; } DexpFunctor::DexpFunctor(const Vector3& omega)