コード例 #1
0
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
}
コード例 #2
0
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++;
		}
	}
}