From 4ef43387bd2c1a7620c019ebc3d490ccb05130fe Mon Sep 17 00:00:00 2001
From: Tim McGrath <t.mike.mcgrath@gmail.com>
Date: Tue, 1 Jun 2021 09:38:42 -0400
Subject: [PATCH] fix bug on computation of SO(3) logmap

when theta near 0 (or 2pi, 4pi...)
---
 gtsam/geometry/SO3.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index c86b9b860a..80c0171ad1 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -278,7 +278,8 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
     } else {
       // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
       // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
-      magnitude = 0.5 - tr_3 * tr_3 / 12.0;
+      // see https://github.com/borglab/gtsam/issues/746 for details
+      magnitude = 0.5 - tr_3 / 12.0;
     }
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }