From 73f9b9d3fb75386997d2b08953efea7d6a50ec08 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:26:43 -0800 Subject: [PATCH] Better behaved numerics --- gtsam/geometry/SO3.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a417164e4..ca80167f4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -47,7 +47,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { // get components of axis \omega, where is a unit vector const double& wx = axis.x(), wy = axis.y(), wz = axis.z(); - const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta; + const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2; const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta; @@ -130,7 +130,6 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { /* ************************************************************************* */ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { - using std::cos; using std::sin; const double theta2 = omega.dot(omega);