Merge pull request #585 from jcarpent/devel

spatial/explog: correct taylor expansion precision computation

src/math/taylor-expansion.hpp

@@ -42,7 +42,7 @@ namespace se3
     template<int degree>
     static Scalar precision()
     {
-      static Scalar value = std::pow(std::numeric_limits<Scalar>::epsilon(),Scalar(1.)/Scalar(degree));
+      static Scalar value = std::pow(std::numeric_limits<Scalar>::epsilon(),Scalar(1)/Scalar(degree+1));
       return value;
     }
   }; // struct TaylorSeriesExpansion

src/spatial/explog.hpp

@@ -52,7 +52,7 @@ namespace se3
     const Scalar t2 = v.squaredNorm();
     
     const Scalar t = math::sqrt(t2);
-    if(t > TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if(t > TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       Scalar ct,st; SINCOS(t,&st,&ct);
       const Scalar alpha_vxvx = (1 - ct)/t2;
@@ -108,7 +108,7 @@ namespace se3
     // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
     if (theta < PI_value - 1e-2)
     {
-      const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<4>())
+      const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
                         ? theta / sin(theta)
                         : Scalar(1)) / Scalar(2);
       res(0) = t * (R (2, 1) - R (1, 2));
@@ -171,7 +171,7 @@ namespace se3
     Scalar n2 = r.squaredNorm(),a,b,c;
     Scalar n = math::sqrt(n2);
     
-    if (n < TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if (n < TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       a =   Scalar(1)           - n2/Scalar(6);
       b = - Scalar(1)/Scalar(2) - n2/Scalar(24);
@@ -207,7 +207,7 @@ namespace se3
 
     Matrix3Like & Jout = const_cast<Matrix3Like &>(Jlog.derived());
 
-    if (theta < TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if (theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       const Scalar alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
       Jout.noalias() = alpha * log * log.transpose();
@@ -275,7 +275,7 @@ namespace se3
     typename SE3::AngularType & rot = res.rotation();
     
     const Scalar t = math::sqrt(t2);
-    if(t < TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       // Taylor expansion
       const Scalar alpha_wxv = Scalar(1)/Scalar(2) - t2/24;
@@ -356,7 +356,7 @@ namespace se3
     Vector3 w(log3(R,t)); // t in [0,π]
     const Scalar t2 = t*t;
     Scalar alpha, beta;
-    if (t < TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
       beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
@@ -419,7 +419,7 @@ namespace se3
     Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
 
     Scalar beta, beta_dot_over_theta;
-    if (t < TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
       beta_dot_over_theta = Scalar(1)/Scalar(360);
@@ -498,7 +498,7 @@ namespace se3
 
     const Scalar t2 = t*t;
     Scalar beta, beta_dot_over_theta;
-    if(t < TaylorSeriesExpansion<Scalar>::template precision<4>())
+    if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
     {
       beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
       beta_dot_over_theta = Scalar(1)/Scalar(360);