diff --git a/src/multibody/liegroup/special-orthogonal.hpp b/src/multibody/liegroup/special-orthogonal.hpp index 938cd98bea2e622bfefb3a583f0c0fe7ed668443..6034673d4f2e3f69a13ab3530774859e205dfe8f 100644 --- a/src/multibody/liegroup/special-orthogonal.hpp +++ b/src/multibody/liegroup/special-orthogonal.hpp @@ -51,17 +51,18 @@ namespace se3 SE3_LIE_GROUP_PUBLIC_INTERFACE(SpecialOrthogonalOperation); typedef Eigen::Matrix<Scalar,2,2> Matrix2; - static Scalar log (const Matrix2& R) + static Scalar log(const Matrix2 & R) { Scalar theta; const Scalar tr = R.trace(); - const bool pos = (R (1, 0) > 0); - if (tr > 2) theta = 0; // acos((3-1)/2) - else if (tr < -2) theta = (pos ? PI : -PI); // acos((-1-1)/2) + const bool pos = (R (1, 0) > Scalar(0)); + const Scalar PI_value = PI<Scalar>(); + if (tr > Scalar(2)) theta = Scalar(0); // acos((3-1)/2) + else if (tr < Scalar(-2)) theta = (pos ? PI_value : -PI_value); // acos((-1-1)/2) // Around 0, asin is numerically more stable than acos because // acos(x) = PI/2 - x and asin(x) = x (the precision of x is not lost in PI/2). - else if (tr > 2 - 1e-2) theta = asin ((R(1,0) - R(0,1)) / 2); - else theta = (pos ? acos (tr/2) : -acos(tr/2)); + else if (tr > Scalar(2) - 1e-2) theta = asin ((R(1,0) - R(0,1)) / Scalar(2)); + else theta = (pos ? acos (tr/Scalar(2)) : -acos(tr/Scalar(2))); assert (theta == theta); // theta != NaN assert (fabs(theta - atan2 (R(1,0), R(0,0))) < 1e-6); return theta; @@ -88,7 +89,7 @@ namespace se3 ConfigVector_t neutral () const { - ConfigVector_t n; n.setZero (); n [0] = 1; + ConfigVector_t n; n.setZero(); n[0] = Scalar(1); return n; } @@ -167,8 +168,10 @@ namespace se3 Scalar sinTheta = q0(0)*q1(1) - q0(1)*q1(0); Scalar theta = atan2(sinTheta, cosTheta); assert (fabs (sin (theta) - sinTheta) < 1e-8); + + const Scalar PI_value = PI<Scalar>(); - if (fabs (theta) > 1e-6 && fabs (theta) < PI - 1e-6) + if (fabs (theta) > 1e-6 && fabs (theta) < PI_value - 1e-6) { out = (sin ((1-u)*theta)/sinTheta) * q0 + (sin ( u *theta)/sinTheta) * q1; @@ -199,8 +202,10 @@ namespace se3 void random_impl (const Eigen::MatrixBase<Config_t>& qout) const { Config_t& out = (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived(); - const Scalar angle = -PI + 2*PI * ((Scalar)rand())/RAND_MAX; - SINCOS (angle, &out(1), &out(0)); + + const Scalar PI_value = PI<Scalar>(); + const Scalar angle = -PI_value + Scalar(2)* PI_value * ((Scalar)rand())/RAND_MAX; + SINCOS(angle, &out(1), &out(0)); } template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> @@ -238,7 +243,7 @@ namespace se3 ConfigVector_t neutral () const { - ConfigVector_t n; n.setZero (); n [3] = 1; + ConfigVector_t n; n.setZero (); n[3] = Scalar(1); return n; } @@ -271,7 +276,7 @@ namespace se3 Jlog3 (R, J1); JacobianLOut_t& J0v = const_cast< JacobianLOut_t& > (J0.derived()); - J0v.noalias() = - J1.derived() * R.transpose(); + J0v.noalias() = - J1 * R.transpose(); } template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>