Fix SO(3) exponential for small angles.

- The Taylor expansion is missing a factor of 1/4, to go from theta_sq to theta_hald_sq.
wangyvliang · Nov 26, 2017 · c1a0ba1 · c1a0ba1
1 parent cb4ce2d
commit c1a0ba1
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/sophus/so3.hpp b/sophus/so3.hpp
@@ -336,7 +336,7 @@ class SO3Base {
       imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
                     Scalar(1.0 / 3840.0) * theta_po4;
       real_factor =
-          Scalar(1) - Scalar(0.5) * theta_sq + Scalar(1.0 / 384.0) * theta_po4;
+          Scalar(1) - Scalar(1.0 / 8.0) * theta_sq + Scalar(1.0 / 384.0) * theta_po4;
     } else {
       Scalar sin_half_theta = sin(half_theta);
       imag_factor = sin_half_theta / (*theta);