Commit a4eade14 by Joseph Mirabel Committed by Joseph Mirabel

### [C++] Factorize uniform random configuration.

parent 81ce16e0
 ... ... @@ -81,5 +81,28 @@ namespace se3 const_cast &> (q).coeffs() *= alpha; } /// Uniformly random quaternion sphere. template void uniformRandom (const Eigen::QuaternionBase & q) { // EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D, 4); typedef typename D::Scalar Scalar; // 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__
 ... ... @@ -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!