diff --git a/include/manif/impl/se3/SE3_base.h b/include/manif/impl/se3/SE3_base.h index 42ca35ec..1066767f 100644 --- a/include/manif/impl/se3/SE3_base.h +++ b/include/manif/impl/se3/SE3_base.h @@ -374,28 +374,41 @@ struct RandomEvaluatorImpl> static void run(T& m) { // @note: + // We are using: + // http://mathworld.wolfram.com/HyperspherePointPicking.html + // which is faster than Quaternion::UnitRandom, and also, // Quaternion::UnitRandom is not available in Eigen 3.3-beta1 // which is the default version in Ubuntu 16.04 - // So we copy its implementation here. using std::sqrt; - using std::sin; - using std::cos; - - using Scalar = typename SE3Base::Scalar; - using Translation = typename SE3Base::Translation; - using Quaternion = typename SE3Base::QuaternionDataType; - - const Scalar u1 = Eigen::internal::random(0, 1), - u2 = Eigen::internal::random(0, 2*EIGEN_PI), - u3 = Eigen::internal::random(0, 2*EIGEN_PI); - const Scalar a = sqrt(1. - u1), - b = sqrt(u1); - - m = Derived(Translation::Random(), - Quaternion(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3))); - - //m = Derived(Translation::Random(), Quaternion::UnitRandom()); + using Scalar = typename SO3Base::Scalar; + using Quaternion = typename SO3Base::QuaternionDataType; + + while (true) + { + const Scalar u1 = Eigen::internal::random(-1, 1), + u2 = Eigen::internal::random(-1, 1); + if (u1 * u1 + u2 * u2 > 1.0) + { + continue; + } + + while (true) + { + const Scalar u3 = Eigen::internal::random(-1, 1), + u4 = Eigen::internal::random(-1, 1); + if (u3 * u3 + u4 * u4 > 1.0) + { + continue; + } + const Scalar zw_factor = sqrt((1 - u1 * u1 - u2 * u2) / (u3 * u3 + u4 * u4)); + const Scalar z = u3 * zw_factor; + const Scalar w = u4 * zw_factor; + m = Derived(Translation::Random(), Quaternion(u1, u2, z, w)); + break; + } + break; + } } }; diff --git a/include/manif/impl/so3/SO3_base.h b/include/manif/impl/so3/SO3_base.h index bd02caf3..06f1eca2 100644 --- a/include/manif/impl/so3/SO3_base.h +++ b/include/manif/impl/so3/SO3_base.h @@ -359,28 +359,42 @@ struct RandomEvaluatorImpl> template static void run(T& m) { - // @note: + // We are using: + // http://mathworld.wolfram.com/HyperspherePointPicking.html + // which is faster than Quaternion::UnitRandom, and also, // Quaternion::UnitRandom is not available in Eigen 3.3-beta1 // which is the default version in Ubuntu 16.04 - // So we copy its implementation here. using std::sqrt; - using std::sin; - using std::cos; - using Scalar = typename SO3Base::Scalar; using Quaternion = typename SO3Base::QuaternionDataType; - const Scalar u1 = Eigen::internal::random(0, 1), - u2 = Eigen::internal::random(0, 2*EIGEN_PI), - u3 = Eigen::internal::random(0, 2*EIGEN_PI); - const Scalar a = sqrt(1. - u1), - b = sqrt(u1); - m = Derived(Quaternion(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3))); - - - // m = Derived(Quaternion::UnitRandom()); + while (true) + { + const Scalar u1 = Eigen::internal::random(-1, 1), + u2 = Eigen::internal::random(-1, 1); + if (u1 * u1 + u2 * u2 > 1.0) + { + continue; + } + + while (true) + { + const Scalar u3 = Eigen::internal::random(-1, 1), + u4 = Eigen::internal::random(-1, 1); + if (u3 * u3 + u4 * u4 > 1.0) + { + continue; + } + const Scalar zw_factor = sqrt((1 - u1 * u1 - u2 * u2) / (u3 * u3 + u4 * u4)); + const Scalar z = u3 * zw_factor; + const Scalar w = u4 * zw_factor; + m = Derived(Quaternion(u1, u2, z, w)); + break; + } + break; + } } };