From f656469365908d924406d0c8c7d25a1acc73e524 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <deray.jeremie@gmail.com>
Date: Fri, 11 Dec 2020 11:59:55 -0500
Subject: [PATCH] Fix SO3/SE3Tangent random (#182)

* fix SO3/SE3Tangent random
* fix exp.log test
---
 include/manif/impl/eigen.h                    | 30 +++++++++++++++++++
 include/manif/impl/se3/SE3Tangent_base.h      |  5 ++--
 .../manif/impl/se_2_3/SE_2_3Tangent_base.h    |  6 ++--
 include/manif/impl/so3/SO3Tangent_base.h      |  4 +--
 test/common_tester.h                          |  2 +-
 5 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/include/manif/impl/eigen.h b/include/manif/impl/eigen.h
index d0cfe9b9..b5b21736 100644
--- a/include/manif/impl/eigen.h
+++ b/include/manif/impl/eigen.h
@@ -5,6 +5,8 @@
 #include <Eigen/LU> // for mat.inverse()
 #include <Eigen/Geometry>
 
+#include <manif/constants.h>
+
 /**
  * @note static_cast<int> to avoid -Wno-enum-compare
  */
@@ -163,6 +165,34 @@ skew(const Eigen::MatrixBase<_Derived>& v)
             -v(1),   +v(0),    T(0.) ).finished();
 }
 
+template <typename Scalar>
+Eigen::Matrix<Scalar, 3, 1> randPointInBall(Scalar radius)
+{
+  // See https://stackoverflow.com/a/5408843/9709397
+
+  using std::acos;
+  using std::sin;
+  using std::cos;
+  using std::cbrt;
+
+  // random(0, 2pi)
+  Scalar phi = static_cast<Scalar>(rand()) / (static_cast<Scalar>(RAND_MAX / (Scalar(2) * MANIF_PI)));
+  // random(-1, 1)
+  Scalar costheta = Scalar(-1) + static_cast<Scalar>(rand()) / (static_cast<Scalar>(RAND_MAX / Scalar(2)));
+  // random(0, 1)
+  Scalar u = static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX);
+
+  Scalar theta = acos(costheta);
+  Scalar r = radius * cbrt(u);
+  Scalar rsintheta = r * sin(theta);
+
+  return Eigen::Matrix<Scalar, 3, 1>(
+    rsintheta * cos(phi),
+    rsintheta * sin(phi),
+    r * costheta
+  );
+}
+
 } /* namespace manif */
 
 #endif /* _MANIF_MANIF_EIGEN_H_ */
diff --git a/include/manif/impl/se3/SE3Tangent_base.h b/include/manif/impl/se3/SE3Tangent_base.h
index 2fef9b76..ac1fca04 100644
--- a/include/manif/impl/se3/SE3Tangent_base.h
+++ b/include/manif/impl/se3/SE3Tangent_base.h
@@ -451,8 +451,9 @@ struct RandomEvaluatorImpl<SE3TangentBase<Derived>>
 {
   static void run(SE3TangentBase<Derived>& m)
   {
-    m.coeffs().setRandom();                // in [-1,1]
-    m.coeffs().template tail<3>() *= MANIF_PI; // in [-PI,PI]
+    m.coeffs().template head<3>().setRandom();
+    // In ball of radius PI
+    m.coeffs().template tail<3>() = randPointInBall(MANIF_PI);
   }
 };
 
diff --git a/include/manif/impl/se_2_3/SE_2_3Tangent_base.h b/include/manif/impl/se_2_3/SE_2_3Tangent_base.h
index 4db0dda9..d1edd1d4 100644
--- a/include/manif/impl/se_2_3/SE_2_3Tangent_base.h
+++ b/include/manif/impl/se_2_3/SE_2_3Tangent_base.h
@@ -390,8 +390,10 @@ struct RandomEvaluatorImpl<SE_2_3TangentBase<Derived>>
 {
   static void run(SE_2_3TangentBase<Derived>& m)
   {
-    m.coeffs().setRandom();                // in [-1,1]
-    m.coeffs().template segment<3>(3) *= MANIF_PI; // in [-PI,PI]
+    // in [-1,1]
+    m.coeffs().setRandom();
+    // In ball of radius PI
+    m.coeffs().template segment<3>(3) = randPointInBall(MANIF_PI);
   }
 };
 
diff --git a/include/manif/impl/so3/SO3Tangent_base.h b/include/manif/impl/so3/SO3Tangent_base.h
index b259a6ea..33952afc 100644
--- a/include/manif/impl/so3/SO3Tangent_base.h
+++ b/include/manif/impl/so3/SO3Tangent_base.h
@@ -305,8 +305,8 @@ struct RandomEvaluatorImpl<SO3TangentBase<Derived>>
 {
   static void run(SO3TangentBase<Derived>& m)
   {
-    // in [-1,1]  / in [-PI,PI]
-    m.coeffs().setRandom() *= MANIF_PI;
+    // In ball of radius PI
+    m.coeffs() = randPointInBall(MANIF_PI);
   }
 };
 
diff --git a/test/common_tester.h b/test/common_tester.h
index 49d419c9..01cf1e1d 100644
--- a/test/common_tester.h
+++ b/test/common_tester.h
@@ -37,7 +37,7 @@
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_LIFT_RETRACT)        \
   { evalLiftRetr(); }                                                     \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_RETRACT_LIFT)        \
-  { evalLiftRetr(); }                                                     \
+  { evalRetrLift(); }                                                     \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_COMPOSE_WITH_INV)    \
   { evalComposeWithInv(); }                                               \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_BETWEEN_SELF)        \