From 578b5e6ec5da7b84c5ef7f07ecdfa12a24c10d2f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 3 Dec 2021 15:21:01 -0500 Subject: [PATCH] Only keep essentials --- gtsam/geometry/Quaternion.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index becb51d24..2ef47d58e 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -82,7 +82,7 @@ struct traits { using std::sin; if (H) *H = SO3::ExpmapDerivative(omega.template cast()); _Scalar theta2 = omega.dot(omega); - if (theta2 > 1e-8) { + if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { _Scalar theta = std::sqrt(theta2); _Scalar ha = _Scalar(0.5) * theta; Vector3 vec = (sin(ha) / theta) * omega; @@ -100,8 +100,8 @@ struct traits { using std::sqrt; // define these compile time constants to avoid std::abs: - static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-8, - NearlyNegativeOne = -1.0 + 1e-8; + static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, + NearlyNegativeOne = -1.0 + 1e-10; TangentVector omega;