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; }