Commit 4f7ac8db by Justin Carpentier Committed by GitHub

### Merge pull request #332 from jmirabel/devel

Update quaternions related operations
parents 81ce16e0 aa6b5178
 ... ... @@ -65,21 +65,63 @@ namespace se3 /// of the normalization function. /// /// Only additions and multiplications are required. Neither square root nor /// division are used (except a division by 2). /// division are used (except a division by 2). Let \f$\delta = ||q||^2 - 1 \f$. /// Using the following limited development: /// \f[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \f] /// /// The output is /// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f] /// /// The output quaternion is guaranted to statisfy the following: /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f] /// where \f$M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$ /// and \f$\epsilon \f$ is the maximum tolerance of \f$||q_{in}||^2 - 1 \f$. /// /// \warning \f$||q||^2 - 1 \f$ should already be close to zero. /// /// \note See /// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3 /// to know the reason why the argument is const. template void firstOrderNormalize(const Eigen::QuaternionBase & q) template void firstOrderNormalize(const Eigen::QuaternionBase & q) { assert(std::fabs(q.norm() - 1) < 1e-2); typedef typename D::Scalar Scalar; const Scalar alpha = ((Scalar)3 - q.squaredNorm()) / 2; const Scalar N2 = q.squaredNorm(); #ifndef NDEBUG const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits::epsilon())); assert(std::fabs(N2-1.) <= epsilon); #endif const Scalar alpha = ((Scalar)3 - N2) / 2; const_cast &> (q).coeffs() *= alpha; #ifndef NDEBUG 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::epsilon())); #endif } /// Uniformly random quaternion sphere. template void uniformRandom (const Eigen::QuaternionBase & q) { typedef typename D::Scalar Scalar; typedef Eigen::QuaternionBase Base; // Rotational part const Scalar u1 = (Scalar)rand() / RAND_MAX; const Scalar u2 = (Scalar)rand() / RAND_MAX; const Scalar u3 = (Scalar)rand() / RAND_MAX; const Scalar mult1 = sqrt (1-u1); const Scalar mult2 = sqrt (u1); Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2); Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3); const_cast (q).w() = mult1 * s2; const_cast (q).x() = mult1 * c2; const_cast (q).y() = mult2 * s3; const_cast (q).z() = mult2 * c3; } } #endif //#ifndef __math_quaternion_hpp__
 ... ... @@ -213,7 +213,7 @@ namespace se3 typedef Eigen::Map ConstQuaternionMap_t; ConstQuaternionMap_t quat(q_joint.template tail<4>().data()); assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits::epsilon())); assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits::epsilon())); M.rotation(quat.matrix()); M.translation(q_joint.template head<3>()); ... ... @@ -314,7 +314,10 @@ namespace se3 ConfigVector_t random_impl() const { ConfigVector_t q(ConfigVector_t::Random()); q.segment<4>(3).normalize();// /= q.segment<4>(3).norm(); typedef Eigen::Map QuaternionMap_t; uniformRandom(QuaternionMap_t(q.segment<4>(3).data())); return q; } ... ... @@ -335,22 +338,9 @@ namespace se3 result[i] = lower_pos_limit[i] + (upper_pos_limit[i] - lower_pos_limit[i]) * (Scalar)(rand())/RAND_MAX; } // Rotational part const Scalar u1 = (Scalar)rand() / RAND_MAX; const Scalar u2 = (Scalar)rand() / RAND_MAX; const Scalar u3 = (Scalar)rand() / RAND_MAX; const Scalar mult1 = sqrt (1-u1); const Scalar mult2 = sqrt (u1); Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2); Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3); result.segment<4>(3) << mult1 * s2, mult1 * c2, mult2 * s3, mult2 * c3; typedef Eigen::Map QuaternionMap_t; uniformRandom(QuaternionMap_t(result.segment<4>(3).data())); return result; } ... ...
 ... ... @@ -356,31 +356,17 @@ namespace se3 ConfigVector_t random_impl() const { ConfigVector_t q(ConfigVector_t::Random()); q.normalize(); ConfigVector_t q; typedef Eigen::Map QuaternionMap_t; uniformRandom(QuaternionMap_t(q.data())); return q; } ConfigVector_t randomConfiguration_impl(const ConfigVector_t &, const ConfigVector_t &) const { ConfigVector_t result; // Rotational part const Scalar u1 = (Scalar)rand() / RAND_MAX; const Scalar u2 = (Scalar)rand() / RAND_MAX; const Scalar u3 = (Scalar)rand() / RAND_MAX; const Scalar mult1 = sqrt (1-u1); const Scalar mult2 = sqrt (u1); Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2); Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3); result << mult1 * s2, mult1 * c2, mult2 * s3, mult2 * c3; typedef Eigen::Map QuaternionMap_t; uniformRandom(QuaternionMap_t(result.data())); return result; } ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!