GroundPolyFitter<degree>::GroundPolyFitter(void) :
nrows(480), ncols(640), pixelToGround(nrows, ncols), allPixels(nrows * ncols)
{
double pixelPointInit[] = { 0.0, 0.0, 1.0 }; // homogeneous coordinates
cv::Mat_<double> pixelPoint(3, 1, pixelPointInit);
cv::Mat_<double> groundPoint(3, 1);
int i = 0;
for(int r = 0; r < nrows; r++) {
for(int c = 0; c < ncols; c++) {
pixelPoint(1, 1) = (double) r;
pixelPoint(1, 2) = (double) c;
groundPoint = homography * pixelPoint;
double x = groundPoint(1, 1);
double y = groundPoint(1, 2);
double scale = groundPoint(1, 3);
pixelToGround(r, c).setX(x /scale);
pixelToGround(r, c).setY(y /scale);
allPixels[i] = RowCol(r, c);
i++;
}
}
}
double PixelIterator::calculatePixelIndex()
{
double pixelIndex = -1;
if(usesTransform)
{
Vector2d pixelPoint = inverseTransform.transform(currentPoint);
pixelPoint.x *= ratio.x;
pixelPoint.y *= ratio.y;
if(mirrorHorizontal)
{
pixelPoint.x = srcRectD.x + (srcRectD.width - (pixelPoint.x-srcRectD.x));
}
else
{
pixelPoint.x = srcRectD.x + pixelPoint.x;
}
if(mirrorVertical)
{
pixelPoint.y = srcRectD.y + (srcRectD.height - (pixelPoint.y-srcRectD.y));
}
else
{
pixelPoint.y = srcRectD.y + pixelPoint.y;
}
if(pixelPoint.x < srcRectD.x || pixelPoint.y < srcRectD.y || pixelPoint.x > srcRectRight || pixelPoint.y > srcRectBottom)
{
pixelIndex = -1;
}
else
{
pixelIndex = (dimensions.x*Math::floor(pixelPoint.y))+pixelPoint.x;
}
}
else
{
Vector2d pixelPoint(currentPoint.x*ratio.x, currentPoint.y*ratio.y);
if(mirrorHorizontal)
{
pixelPoint.x = srcRectD.x + (srcRectD.width - (pixelPoint.x-srcRectD.x));
}
else
{
pixelPoint.x = srcRectD.x + pixelPoint.x;
}
if(mirrorVertical)
{
pixelPoint.y = srcRectD.y + (srcRectD.height - (pixelPoint.y-srcRectD.y));
row = Math::ceil(pixelPoint.y) - pixelPoint.y;
}
else
{
pixelPoint.y = srcRectD.y + pixelPoint.y;
row = pixelPoint.y - Math::floor(pixelPoint.y);
}
pixelIndex = (dimensions.x*Math::floor(pixelPoint.y))+pixelPoint.x;
}
return pixelIndex;
}
