inline void
pcl::PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
{
//cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n";
if (isNearlyZero (a))
{
//cout << "Highest order element is 0 => Calling solveQuadraticEquation.\n";
solveQuadraticEquation (b, c, d, roots);
return;
}
if (isNearlyZero (d))
{
roots.push_back (0.0);
//cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n";
std::vector<real> tmpRoots;
solveQuadraticEquation (a, b, c, tmpRoots);
for (unsigned int i=0; i<tmpRoots.size (); i++)
if (!isNearlyZero (tmpRoots[i]))
roots.push_back (tmpRoots[i]);
return;
}
double a2 = a*a,
a3 = a2*a,
b2 = b*b,
b3 = b2*b,
alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
alpha2 = alpha*alpha,
alpha3 = alpha2*alpha,
beta2 = beta*beta;
// Value for resubstitution:
double resubValue = b/ (3*a);
//cout << "Trying to solve y^3 + "<<alpha<<"y + "<<beta<<"\n";
double discriminant = (alpha3/27.0) + 0.25*beta2;
//cout << "Discriminant is "<<discriminant<<"\n";
if (isNearlyZero (discriminant))
{
if (!isNearlyZero (alpha) || !isNearlyZero (beta))
{
roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
roots.push_back ( (3.0*beta)/alpha - resubValue);
}
else
{
roots.push_back (-resubValue);
}
}
else if (discriminant > 0)
{
double sqrtDiscriminant = sqrt (discriminant);
double d1 = -0.5*beta + sqrtDiscriminant,
d2 = -0.5*beta - sqrtDiscriminant;
if (d1 < 0)
d1 = -pow (-d1, 1.0/3.0);
else
d1 = pow (d1, 1.0/3.0);
if (d2 < 0)
d2 = -pow (-d2, 1.0/3.0);
else
d2 = pow (d2, 1.0/3.0);
//cout << PVAR (d1)<<", "<<PVAR (d2)<<"\n";
roots.push_back (d1 + d2 - resubValue);
}
else
{
double tmp1 = sqrt (- (4.0/3.0)*alpha),
tmp2 = acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
roots.push_back (tmp1*cos (tmp2) - resubValue);
roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
}
#if 0
cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
for (unsigned int i=0; i<roots.size (); i++)
{
real x=roots[i], x2=x*x, x3=x2*x;
real result = a*x3 + b*x2 + c*x + d;
if (fabs (result) > 1e-4)
{
cout << "Something went wrong:\n";
//roots.clear ();
}
cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = "<<result<<")\n";
}
cout << "\n\n";
#endif
}
inline void
pcl::PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e,
std::vector<real>& roots) const
{
//cout << "Trying to solve "<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = 0\n";
if (isNearlyZero (a))
{
//cout << "Highest order element is 0 => Calling solveCubicEquation.\n";
solveCubicEquation (b, c, d, e, roots);
return;
}
if (isNearlyZero (e))
{
roots.push_back (0.0);
//cout << "Constant element is 0 => Adding root 0 and calling solveCubicEquation.\n";
std::vector<real> tmpRoots;
solveCubicEquation (a, b, c, d, tmpRoots);
for (unsigned int i=0; i<tmpRoots.size (); i++)
if (!isNearlyZero (tmpRoots[i]))
roots.push_back (tmpRoots[i]);
return;
}
double root1, root2, root3, root4,
a2 = a*a,
a3 = a2*a,
a4 = a2*a2,
b2 = b*b,
b3 = b2*b,
b4 = b2*b2,
alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
alpha2 = alpha*alpha;
// Value for resubstitution:
double resubValue = b/ (4*a);
//cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n";
if (isNearlyZero (beta))
{ // y^4 + alpha*y^2 + gamma\n";
//cout << "Using beta=0 condition\n";
std::vector<real> tmpRoots;
solveQuadraticEquation (1.0, alpha, gamma, tmpRoots);
for (unsigned int i=0; i<tmpRoots.size (); i++)
{
double qudraticRoot = tmpRoots[i];
if (sqrtIsNearlyZero (qudraticRoot))
{
roots.push_back (-resubValue);
}
else if (qudraticRoot > 0.0)
{
root1 = sqrt (qudraticRoot);
roots.push_back (root1 - resubValue);
roots.push_back (-root1 - resubValue);
}
}
}
else
{
//cout << "beta != 0\n";
double alpha3 = alpha2*alpha,
beta2 = beta*beta,
p = (-alpha2/12.0)-gamma,
q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
q2 = q*q,
p3 = p*p*p,
u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
if (u > 0.0)
u = pow (u, 1.0/3.0);
else if (isNearlyZero (u))
u = 0.0;
else
u = -pow (-u, 1.0/3.0);
double y = (-5.0/6.0)*alpha - u;
if (!isNearlyZero (u))
y += p/ (3.0*u);
double w = alpha + 2.0*y;
if (w > 0)
{
w = sqrt (w);
}
else if (isNearlyZero (w))
{
w = 0;
}
else
{
//cout << "Found no roots\n";
return;
}
double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
if (tmp1 > 0)
{
tmp1 = sqrt (tmp1);
root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
roots.push_back (root1);
roots.push_back (root2);
}
else if (isNearlyZero (tmp1))
{
root1 = - (b/ (4.0*a)) + 0.5*w;
roots.push_back (root1);
}
if (tmp2 > 0)
{
tmp2 = sqrt (tmp2);
root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
roots.push_back (root3);
roots.push_back (root4);
}
else if (isNearlyZero (tmp2))
{
root3 = - (b/ (4.0*a)) - 0.5*w;
roots.push_back (root3);
}
//cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n";
}
#if 0
cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
for (unsigned int i=0; i<roots.size (); i++)
{
real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
real result = a*x4 + b*x3 + c*x2 + d*x + e;
if (fabs (result) > 1e-4)
{
cout << "Something went wrong:\n";
//roots.clear ();
}
cout << "Root "<<i<<" = "<<roots[i]
<< ". ("<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = "<<result<<")\n";
}
cout << "\n\n";
#endif
}
void
ExtendedHandEyeCalibration::solve(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& H1,
const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& H2,
Eigen::Matrix4d& H_12,
double& s) const
{
int motionCount = H1.size();
Eigen::MatrixXd T(motionCount * 6, 8);
T.setZero();
for (int i = 0; i < motionCount; ++i)
{
Eigen::AngleAxisd aa1(H1.at(i).block<3,3>(0,0));
Eigen::Vector3d rvec1 = aa1.angle() * aa1.axis();
Eigen::Vector3d tvec1 = H1.at(i).block<3,1>(0,3);
Eigen::AngleAxisd aa2(H2.at(i).block<3,3>(0,0));
Eigen::Vector3d rvec2 = aa2.angle() * aa2.axis();
Eigen::Vector3d tvec2 = H2.at(i).block<3,1>(0,3);
double theta1, d1;
Eigen::Vector3d l1, m1;
AngleAxisAndTranslationToScrew(rvec1, tvec1, theta1, d1, l1, m1);
double theta2, d2;
Eigen::Vector3d l2, m2;
AngleAxisAndTranslationToScrew(rvec2, tvec2, theta2, d2, l2, m2);
Eigen::Vector3d a = l1;
Eigen::Vector3d a_prime = m1;
Eigen::Vector3d b = l2;
Eigen::Vector3d b_prime = m2;
T.block<3,1>(i * 6, 0) = a - b;
T.block<3,3>(i * 6, 1) = skew(Eigen::Vector3d(a + b));
T.block<3,1>(i * 6 + 3, 0) = a_prime - b_prime;
T.block<3,3>(i * 6 + 3, 1) = skew(Eigen::Vector3d(a_prime + b_prime));
T.block<3,1>(i * 6 + 3, 4) = a - b;
T.block<3,3>(i * 6 + 3, 5) = skew(Eigen::Vector3d(a + b));
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeFullU | Eigen::ComputeFullV);
// v7 and v8 span the null space of T, v6 may also be one
// if rank = 5.
Eigen::Matrix<double, 8, 1> v6 = svd.matrixV().block<8,1>(0, 5);
Eigen::Matrix<double, 8, 1> v7 = svd.matrixV().block<8,1>(0,6);
Eigen::Matrix<double, 8, 1> v8 = svd.matrixV().block<8,1>(0,7);
Eigen::Vector4d u1 = v7.block<4,1>(0,0);
Eigen::Vector4d v1 = v7.block<4,1>(4,0);
Eigen::Vector4d u2 = v8.block<4,1>(0,0);
Eigen::Vector4d v2 = v8.block<4,1>(4,0);
double lambda1 = 0;
double lambda2 = 0.0;
if (u1.dot(v1) == 0.0)
{
std::swap(u1, u2);
std::swap(v1, v2);
}
if (u1.dot(v1) != 0.0)
{
double s[2];
solveQuadraticEquation(u1.dot(v1), u1.dot(v2) + u2.dot(v1), u2.dot(v2), s[0], s[1]);
// find better solution for s
double t[2];
for (int i = 0; i < 2; ++i)
{
t[i] = s[i] * s[i] * u1.dot(u1) + 2 * s[i] * u1.dot(u2) + u2.dot(u2);
}
int idx;
if (t[0] > t[1])
{
idx = 0;
}
else
{
idx = 1;
}
lambda2 = sqrt(1.0 / t[idx]);
lambda1 = s[idx] * lambda2;
}
else
{
if (u1.norm() == 0.0 && u2.norm() > 0.0)
{
lambda1 = 0.0;
lambda2 = 1.0 / u2.norm();
}
else if (u2.norm() == 0.0 && u1.norm() > 0.0)
{
lambda1 = 1.0 / u1.norm();
lambda2 = 0.0;
}
}
// rotation
Eigen::Vector4d q_coeffs = lambda1 * u1 + lambda2 * u2;
Eigen::Vector4d q_prime_coeffs = lambda1 * v1 + lambda2 * v2;
Eigen::Quaterniond q(q_coeffs(0), q_coeffs(1), q_coeffs(2), q_coeffs(3));
Eigen::Quaterniond d(q_prime_coeffs(0), q_prime_coeffs(1), q_prime_coeffs(2), q_prime_coeffs(3));
DualQuaterniond dq(q, d);
H_12 = dq.toMatrix().inverse();
s = 1.0;
refine(H1, H2, H_12, s);
}
