correct coefficients of approximated SE3 Q_r

2020-09-03 15:21:15 +08:00 · 2020-09-03 15:21:15 +08:00 · 19d1ac42b9
parent 915702116f
commit 19d1ac42b9
1 changed files with 2 additions and 2 deletions
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@ -230,8 +230,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
  }
  else {
    Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
-        + 1./24.*(W*W*V + V*W*W - 3*W*V*W)
-        - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
+        - 1./24.*(W*W*V + V*W*W - 3*W*V*W)
+        + 1./120.*(W*V*W*W + W*W*V*W);
  }
 #endif