diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp index 58212c0e6b148cc9576cb75dfa2a44ce7ff38f2a..ecb3ddfcbe1091a3e6e8bbc5f0b4c4c29803731b 100644 --- a/src/math/quaternion.hpp +++ b/src/math/quaternion.hpp @@ -94,7 +94,7 @@ namespace se3 const Scalar alpha = ((Scalar)3 - N2) / 2; const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha; #ifndef NDEBUG - const Scalar M = 3 * std::pow((Scalar)1-epsilon, (Scalar)-5/2) / 4; + const Scalar M = 3 * std::pow((Scalar)1.-epsilon, ((Scalar)-5)/2) / 4; assert(std::fabs(q.norm() - 1) <= std::max(M * sqrt(N2) * (N2 - 1)*(N2 - 1) / 2, Eigen::NumTraits<Scalar>::epsilon())); #endif @@ -104,8 +104,8 @@ namespace se3 template <typename D> void uniformRandom (const Eigen::QuaternionBase<D> & q) { - // EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D, 4); typedef typename D::Scalar Scalar; + typedef Eigen::QuaternionBase<D> Base; // Rotational part const Scalar u1 = (Scalar)rand() / RAND_MAX; @@ -118,10 +118,10 @@ namespace se3 Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2); Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3); - const_cast <Eigen::QuaternionBase<D> &> (q).w() = mult1 * s2; - const_cast <Eigen::QuaternionBase<D> &> (q).x() = mult1 * c2; - const_cast <Eigen::QuaternionBase<D> &> (q).y() = mult2 * s3; - const_cast <Eigen::QuaternionBase<D> &> (q).z() = mult2 * c3; + const_cast <Base &> (q).w() = mult1 * s2; + const_cast <Base &> (q).x() = mult1 * c2; + const_cast <Base &> (q).y() = mult2 * s3; + const_cast <Base &> (q).z() = mult2 * c3; } } #endif //#ifndef __math_quaternion_hpp__