ccImage* ccCalibratedImage::orthoRectifyAsImage(CCLib::GenericIndexedCloud* keypoints3D,
std::vector<KeyPoint>& keypointsImage,
double& pixelSize,
double* minCorner/*=0*/,
double* maxCorner/*=0*/,
double* realCorners/*=0*/) const
{
double a[3],b[3],c[3];
if (!computeOrthoRectificationParams(keypoints3D,keypointsImage,a,b,c))
return 0;
const double& a0 = a[0];
const double& a1 = a[1];
const double& a2 = a[2];
const double& b0 = b[0];
const double& b1 = b[1];
const double& b2 = b[2];
//const double& c0 = c[0];
const double& c1 = c[1];
const double& c2 = c[2];
//first, we compute the ortho-rectified image corners
double corners[8];
double xi,yi,qi;
double halfWidth = (double)m_width/2.0;
double halfHeight = (double)m_height/2.0;
//top-left
xi = -halfWidth;
yi = -halfHeight;
qi = 1.0+c1*xi+c2*yi;
corners[0] = (a0+a1*xi+a2*yi)/qi;
corners[1] = (b0+b1*xi+b2*yi)/qi;
//top-right
xi = halfWidth;
yi = -halfHeight;
qi = 1.0+c1*xi+c2*yi;
corners[2] = (a0+a1*xi+a2*yi)/qi;
corners[3] = (b0+b1*xi+b2*yi)/qi;
//bottom-right
xi = halfWidth;
yi = halfHeight;
qi = 1.0+c1*xi+c2*yi;
corners[4] = (a0+a1*xi+a2*yi)/qi;
corners[5] = (b0+b1*xi+b2*yi)/qi;
//bottom-left
xi = -halfWidth;
yi = halfHeight;
qi = 1.0+c1*xi+c2*yi;
corners[6] = (a0+a1*xi+a2*yi)/qi;
corners[7] = (b0+b1*xi+b2*yi)/qi;
if (realCorners)
memcpy(realCorners,corners,8*sizeof(double));
//we look for min and max bounding box
double minC[2] = {corners[0],corners[1]};
double maxC[2] = {corners[0],corners[1]};
for (unsigned k=1;k<4;++k)
{
const double* C = corners+2*k;
if (minC[0] > C[0])
minC[0] = C[0];
else if (maxC[0] < C[0])
maxC[0] = C[0];
if (minC[1] > C[1])
minC[1] = C[1];
else if (maxC[1] < C[1])
maxC[1] = C[1];
}
//output 3D boundaries (optional)
if (minCorner)
{
minCorner[0]=minC[0];
minCorner[1]=minC[1];
}
if (maxCorner)
{
maxCorner[0]=maxC[0];
maxCorner[1]=maxC[1];
}
double dx = maxC[0]-minC[0];
double dy = maxC[1]-minC[1];
double _pixelSize = pixelSize;
if (_pixelSize<=0.0)
{
unsigned maxSize = std::max(m_width,m_height);
_pixelSize = std::max(dx,dy)/(double)maxSize;
}
unsigned w = (unsigned)((double)dx/_pixelSize);
unsigned h = (unsigned)((double)dy/_pixelSize);
QImage orthoImage(w,h,QImage::Format_ARGB32);
if (orthoImage.isNull()) //not enough memory!
return 0;
const QRgb blackValue = QColor( Qt::black ).rgb();
for (unsigned i=0;i<w;++i)
{
double xip = minC[0]+(double)i*_pixelSize;
for (unsigned j=0;j<h;++j)
{
double yip = minC[1]+(double)j*_pixelSize;
double q = (c2*xip-a2)*(c1*yip-b1)-(c2*yip-b2)*(c1*xip-a1);
double p = (a0-xip)*(c1*yip-b1)-(b0-yip)*(c1*xip-a1);
double yi = p/q;
yi += halfHeight;
int y = (int)yi;
if (y>=0 && y<(int)m_height)
{
q = (c1*xip-a1)*(c2*yip-b2)-(c1*yip-b1)*(c2*xip-a2);
p = (a0-xip)*(c2*yip-b2)-(b0-yip)*(c2*xip-a2);
double xi = p/q;
xi += halfWidth;
int x = (int)xi;
if (x>=0 && x<(int)m_width)
{
QRgb rgb = m_image.pixel(x,y);
//pure black pixels are treated as transparent ones!
if (rgb != blackValue)
orthoImage.setPixel(i,h-1-j,rgb);
else
orthoImage.setPixel(i,h-1-j,qRgba(qRed(rgb),qGreen(rgb),qBlue(rgb),0));
}
else
{
orthoImage.setPixel(i,h-1-j,qRgba(0,0,0,0)); //black by default
}
}
else
{
orthoImage.setPixel(i,h-1-j,qRgba(0,0,0,0)); //black by default
}
}
}
//output pixel size (auto)
pixelSize = _pixelSize;
return new ccImage(orthoImage,getName());
}
ccPointCloud* ccCalibratedImage::orthoRectifyAsCloud(CCLib::GenericIndexedCloud* keypoints3D, std::vector<KeyPoint>& keypointsImage) const
{
double a[3],b[3],c[3];
if (!computeOrthoRectificationParams(keypoints3D,keypointsImage,a,b,c))
return 0;
const double& a0 = a[0];
const double& a1 = a[1];
const double& a2 = a[2];
const double& b0 = b[0];
const double& b1 = b[1];
const double& b2 = b[2];
//const double& c0 = c[0];
const double& c1 = c[1];
const double& c2 = c[2];
PointCoordinateType defaultZ = 0;
ccPointCloud* proj = new ccPointCloud(getName()+QString(".ortho-rectified"));
if (!proj->reserve(m_width*m_height))
{
delete proj;
return 0;
}
if (!proj->reserveTheRGBTable())
{
delete proj;
return 0;
}
proj->showColors(true);
unsigned realCount = 0;
//ortho rectification
{
for (unsigned pi = 0; pi<m_width; ++pi)
{
double xi = static_cast<PointCoordinateType>(pi) - 0.5*static_cast<PointCoordinateType>(m_width);
for (unsigned pj = 0; pj<m_height; ++pj)
{
double yi = static_cast<PointCoordinateType>(pj) - 0.5*static_cast<PointCoordinateType>(m_height);
double qi = 1.0 + c1*xi + c2*yi;
CCVector3 P(static_cast<PointCoordinateType>((a0+a1*xi+a2*yi)/qi),
static_cast<PointCoordinateType>((b0+b1*xi+b2*yi)/qi),
defaultZ);
//and color?
QRgb rgb = m_image.pixel(pi,pj);
int r = qRed(rgb);
int g = qGreen(rgb);
int b = qBlue(rgb);
if (r+g+b > 0)
{
//add point
proj->addPoint(P);
//and color
colorType C[3] = { static_cast<colorType>(r),
static_cast<colorType>(g),
static_cast<colorType>(b) };
proj->addRGBColor(C);
++realCount;
}
}
}
}
if (realCount == 0)
{
delete proj;
proj = 0;
}
else
{
proj->resize(realCount);
}
return proj;
}
ccPointCloud* ccCalibratedImage::orthoRectifyAsCloud(CCLib::GenericIndexedCloud* keypoints3D, std::vector<KeyPoint>& keypointsImage) const
{
double a[3],b[3],c[3];
if (!computeOrthoRectificationParams(keypoints3D,keypointsImage,a,b,c))
return 0;
const double& a0 = a[0];
const double& a1 = a[1];
const double& a2 = a[2];
const double& b0 = b[0];
const double& b1 = b[1];
const double& b2 = b[2];
//const double& c0 = c[0];
const double& c1 = c[1];
const double& c2 = c[2];
PointCoordinateType defaultZ = 0.0;
ccPointCloud* proj = new ccPointCloud(qPrintable(QString("%1.ortho-rectified").arg(getName())));
if (!proj->reserve(m_width*m_height))
{
delete proj;
return 0;
}
if (!proj->reserveTheRGBTable())
{
delete proj;
return 0;
}
proj->showColors(true);
colorType C[3];
unsigned realCount=0;
//ortho rectification
{
for (unsigned pi = 0; pi<(unsigned)m_width; ++pi)
{
double xi = (double)pi-0.5*(double)m_width;
for (unsigned pj = 0; pj<(unsigned)m_height; ++pj)
{
double yi = (double)pj-0.5*(double)m_height;
double qi = (1.0+c1*xi+c2*yi);
CCVector3 P((a0+a1*xi+a2*yi)/qi,
(b0+b1*xi+b2*yi)/qi,
defaultZ);
//and color?
QRgb rgb = m_image.pixel(pi,pj);
C[0]=qRed(rgb);
C[1]=qGreen(rgb);
C[2]=qBlue(rgb);
if ((int)C[0]+(int)C[1]+(int)C[2]>0)
{
//add point
proj->addPoint(P);
//and color
proj->addRGBColor(C);
++realCount;
}
}
}
}
if (realCount==0)
{
delete proj;
proj=0;
}
else
{
proj->resize(realCount);
}
return proj;
}
