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;