Fix SO3/SE3Tangent random (#182)

* fix SO3/SE3Tangent random * fix exp.log test
artivis · Dec 11, 2020 · f656469 · f656469
1 parent 3044d29
commit f656469
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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)        \