correct Qr coefficients in approximation

2020-09-25 20:26:24 +08:00 · 2020-09-25 20:26:24 +08:00 · 2550bf76a4
parent 52fc9cf4ba
commit 2550bf76a4
1 changed files with 2 additions and 2 deletions
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@ -222,8 +222,8 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
  }
  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