Commit a4eade14 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Factorize uniform random configuration.

parent 81ce16e0
......@@ -81,5 +81,28 @@ namespace se3
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
}
/// Uniformly random quaternion sphere.
template <typename D>
void uniformRandom (const Eigen::QuaternionBase<D> & 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 <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;
}
}
#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<Motion_t::Quaternion_t> 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<Motion_t::Quaternion_t> 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<Motion_t::Quaternion_t> 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<Motion_t::Quaternion_t> QuaternionMap_t;
uniformRandom(QuaternionMap_t(result.data()));
return result;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment