Unverified Commit a42477a7 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #585 from jcarpent/devel

spatial/explog: correct taylor expansion precision computation
parents 0fdecc6f 99ba549b
......@@ -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
......
......@@ -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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment