From 19d1ac42b95664530b276744f2ad19da34207793 Mon Sep 17 00:00:00 2001
From: JIanzhu Huai <huai.3@osu.edu>
Date: Thu, 3 Sep 2020 15:21:15 +0800
Subject: [PATCH] correct coefficients of approximated SE3 Q_r

---
 gtsam/geometry/Pose3.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 0a56e2ef5..ecc0598eb 100644
--- 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