inline void
pcl::PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
{
//cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
if (isNearlyZero (a))
{
//cout << "Highest order element is 0 => Calling solveLineaqrEquation.\n";
solveLinearEquation (b, c, roots);
return;
}
if (isNearlyZero (c))
{
roots.push_back (0.0);
//cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n";
std::vector<real> tmpRoots;
solveLinearEquation (a, b, tmpRoots);
for (unsigned int i=0; i<tmpRoots.size (); i++)
if (!isNearlyZero (tmpRoots[i]))
roots.push_back (tmpRoots[i]);
return;
}
real tmp = b*b - 4*a*c;
if (tmp>0)
{
tmp = sqrt (tmp);
real tmp2 = 1.0/ (2*a);
roots.push_back ( (-b + tmp)*tmp2);
roots.push_back ( (-b - tmp)*tmp2);
}
else if (sqrtIsNearlyZero (tmp))
{
roots.push_back (-b/ (2*a));
}
#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;
real result = a*x2 + b*x + c;
if (!isNearlyZero (result))
{
cout << "Something went wrong during solving of polynomial "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
//roots.clear ();
}
//cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^2 + "<<b<<"x + "<<c<<" = "<<result<<")\n";
}
#endif
}
void BarrelCorrection::calculatePixelMapping()
{
LOG_TRACE("BarrelCorrection::calculatePixelMapping()");
if(m_outputWidth <= 0 || m_outputHeight <= 0)
return;
int correctedRectangle[8][2] = {
{0, 0},
{m_outputWidth / 2, 0},
{m_outputWidth - 1, 0},
{0, m_outputHeight / 2},
{m_outputWidth - 1, m_outputHeight / 2},
{0, m_outputHeight - 1},
{m_outputWidth / 2, m_outputHeight - 1},
{m_outputWidth - 1, m_outputHeight - 1}
};
float M[8][9];
float correctionVectorA[8];
float correctionVectorB[8];
// Calculate correctionVectorA
for (int r = 0; r < 8; r++)
{
M[r][0] = (float) (correctedRectangle[r][1]*correctedRectangle[r][1]*correctedRectangle[r][0]);
M[r][1] = (float) (correctedRectangle[r][1]*correctedRectangle[r][0]*correctedRectangle[r][0]);
M[r][2] = (float) (correctedRectangle[r][1]*correctedRectangle[r][1]);
M[r][3] = (float) (correctedRectangle[r][0]*correctedRectangle[r][0]);
M[r][4] = (float) (correctedRectangle[r][1]*correctedRectangle[r][0]);
M[r][5] = (float) (correctedRectangle[r][1]);
M[r][6] = (float) (correctedRectangle[r][0]);
M[r][7] = 1;
M[r][8] = (float) (m_distortedRectangle[r][1]);
}
solveLinearEquation(M);
for (int r = 0; r < 8; r++)
{
correctionVectorA[r] = M[r][8];
}
// Calculate correctionVectorB
for (int r = 0; r < 8; r++)
{
M[r][0] = (float) (correctedRectangle[r][1]*correctedRectangle[r][1]*correctedRectangle[r][0]);
M[r][1] = (float) (correctedRectangle[r][1]*correctedRectangle[r][0]*correctedRectangle[r][0]);
M[r][2] = (float) (correctedRectangle[r][1]*correctedRectangle[r][1]);
M[r][3] = (float) (correctedRectangle[r][0]*correctedRectangle[r][0]);
M[r][4] = (float) (correctedRectangle[r][1]*correctedRectangle[r][0]);
M[r][5] = (float) (correctedRectangle[r][1]);
M[r][6] = (float) (correctedRectangle[r][0]);
M[r][7] = 1;
M[r][8] = (float) (m_distortedRectangle[r][0]);
}
solveLinearEquation(M);
for (int r = 0; r < 8; r++)
{
correctionVectorB[r] = M[r][8];
}
// Calculate pixel mapping
int dx, dy;
int *mapping = m_pixelMapping;
m_pixelMappingMin = 0;
m_pixelMappingMax = 0;
for (int y = 0; y < m_outputHeight; y++)
{
for (int x = 0; x < m_outputWidth; x++)
{
dy = (int) (correctionVectorA[0]*x*y*y +
correctionVectorA[1]*x*x*y +
correctionVectorA[2]*y*y +
correctionVectorA[3]*x*x +
correctionVectorA[4]*x*y +
correctionVectorA[5]*y +
correctionVectorA[6]*x +
correctionVectorA[7]);
dx = (int) (correctionVectorB[0]*x*y*y +
correctionVectorB[1]*x*x*y +
correctionVectorB[2]*y*y +
correctionVectorB[3]*x*x +
correctionVectorB[4]*x*y +
correctionVectorB[5]*y +
correctionVectorB[6]*x +
correctionVectorB[7]);
// TODO: Fix this bug!
// BUG: m_outputWidth should be the with of the input image. Only output with the same width as the input is supported now!
*mapping = dy * m_outputWidth + dx;
m_pixelMappingMin = (*mapping) < m_pixelMappingMin ? (*mapping) : m_pixelMappingMin;
m_pixelMappingMax = (*mapping) > m_pixelMappingMax ? (*mapping) : m_pixelMappingMax;
mapping++;
}
}
}