Commit 99ba549b by Justin Carpentier

### spatial/explog: correct taylor expansion precision computation

`It follows discussions in #584`
parent 0fdecc6f
 ... ... @@ -42,7 +42,7 @@ namespace se3 template static Scalar precision() { static Scalar value = std::pow(std::numeric_limits::epsilon(),Scalar(1.)/Scalar(degree)); static Scalar value = std::pow(std::numeric_limits::epsilon(),Scalar(1)/Scalar(degree+1)); return value; } }; // struct TaylorSeriesExpansion ... ...
 ... ... @@ -52,7 +52,7 @@ namespace se3 const Scalar t2 = v.squaredNorm(); const Scalar t = math::sqrt(t2); if(t > TaylorSeriesExpansion::template precision<4>()) if(t > TaylorSeriesExpansion::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::template precision<4>()) const Scalar t = ((theta > TaylorSeriesExpansion::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::template precision<4>()) if (n < TaylorSeriesExpansion::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(Jlog.derived()); if (theta < TaylorSeriesExpansion::template precision<4>()) if (theta < TaylorSeriesExpansion::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::template precision<4>()) if(t < TaylorSeriesExpansion::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::template precision<4>()) if (t < TaylorSeriesExpansion::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::template precision<4>()) if (t < TaylorSeriesExpansion::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::template precision<4>()) if(t < TaylorSeriesExpansion::template precision<3>()) { beta = Scalar(1)/Scalar(12) + t2/Scalar(720); beta_dot_over_theta = Scalar(1)/Scalar(360); ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!