faster implementation of SO3 random

artivis · Sep 19, 2019 · bf38e2c · bf38e2c
1 parent 70066d0
commit bf38e2c
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 32 deletions.
diff --git a/include/manif/impl/se3/SE3_base.h b/include/manif/impl/se3/SE3_base.h
@@ -374,28 +374,41 @@ struct RandomEvaluatorImpl<SE3Base<Derived>>
   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<Derived>::Scalar;
-    using Translation = typename SE3Base<Derived>::Translation;
-    using Quaternion  = typename SE3Base<Derived>::QuaternionDataType;
-
-    const Scalar u1 = Eigen::internal::random<Scalar>(0, 1),
-                 u2 = Eigen::internal::random<Scalar>(0, 2*EIGEN_PI),
-                 u3 = Eigen::internal::random<Scalar>(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<Derived>::Scalar;
+    using Quaternion = typename SO3Base<Derived>::QuaternionDataType;
+
+    while (true)
+    {
+      const Scalar u1 = Eigen::internal::random<Scalar>(-1, 1),
+            u2 = Eigen::internal::random<Scalar>(-1, 1);
+      if (u1 * u1 + u2 * u2 > 1.0)
+      {
+        continue;
+      }
+
+      while (true)
+      {
+        const Scalar u3 = Eigen::internal::random<Scalar>(-1, 1),
+              u4 = Eigen::internal::random<Scalar>(-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
@@ -359,28 +359,42 @@ struct RandomEvaluatorImpl<SO3Base<Derived>>
   template <typename T>
   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<Derived>::Scalar;
     using Quaternion = typename SO3Base<Derived>::QuaternionDataType;
 
-    const Scalar u1 = Eigen::internal::random<Scalar>(0, 1),
-                 u2 = Eigen::internal::random<Scalar>(0, 2*EIGEN_PI),
-                 u3 = Eigen::internal::random<Scalar>(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<Scalar>(-1, 1),
+            u2 = Eigen::internal::random<Scalar>(-1, 1);
+      if (u1 * u1 + u2 * u2 > 1.0)
+      {
+        continue;
+      }
+
+      while (true)
+      {
+        const Scalar u3 = Eigen::internal::random<Scalar>(-1, 1),
+              u4 = Eigen::internal::random<Scalar>(-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;
+    }
   }
 };