void QgsZonalStatistics::statisticsFromMiddlePointTest( void* band, const QgsGeometry& poly, int pixelOffsetX,
int pixelOffsetY, int nCellsX, int nCellsY, double cellSizeX, double cellSizeY, const QgsRectangle& rasterBBox, FeatureStats &stats )
{
double cellCenterX, cellCenterY;
float* scanLine = ( float * ) CPLMalloc( sizeof( float ) * nCellsX );
cellCenterY = rasterBBox.yMaximum() - pixelOffsetY * cellSizeY - cellSizeY / 2;
stats.reset();
GEOSGeometry* polyGeos = poly.exportToGeos();
if ( !polyGeos )
{
return;
}
GEOSContextHandle_t geosctxt = QgsGeometry::getGEOSHandler();
const GEOSPreparedGeometry* polyGeosPrepared = GEOSPrepare_r( geosctxt, polyGeos );
if ( !polyGeosPrepared )
{
GEOSGeom_destroy_r( geosctxt, polyGeos );
return;
}
GEOSCoordSequence* cellCenterCoords = nullptr;
GEOSGeometry* currentCellCenter = nullptr;
for ( int i = 0; i < nCellsY; ++i )
{
if ( GDALRasterIO( band, GF_Read, pixelOffsetX, pixelOffsetY + i, nCellsX, 1, scanLine, nCellsX, 1, GDT_Float32, 0, 0 )
!= CPLE_None )
{
continue;
}
cellCenterX = rasterBBox.xMinimum() + pixelOffsetX * cellSizeX + cellSizeX / 2;
for ( int j = 0; j < nCellsX; ++j )
{
if ( validPixel( scanLine[j] ) )
{
GEOSGeom_destroy_r( geosctxt, currentCellCenter );
cellCenterCoords = GEOSCoordSeq_create_r( geosctxt, 1, 2 );
GEOSCoordSeq_setX_r( geosctxt, cellCenterCoords, 0, cellCenterX );
GEOSCoordSeq_setY_r( geosctxt, cellCenterCoords, 0, cellCenterY );
currentCellCenter = GEOSGeom_createPoint_r( geosctxt, cellCenterCoords );
if ( GEOSPreparedContains_r( geosctxt, polyGeosPrepared, currentCellCenter ) )
{
stats.addValue( scanLine[j] );
}
}
cellCenterX += cellSizeX;
}
cellCenterY -= cellSizeY;
}
GEOSGeom_destroy_r( geosctxt, currentCellCenter );
CPLFree( scanLine );
GEOSPreparedGeom_destroy_r( geosctxt, polyGeosPrepared );
GEOSGeom_destroy_r( geosctxt, polyGeos );
}
void QgsZonalStatistics::statisticsFromMiddlePointTest( const QgsGeometry &poly, int pixelOffsetX,
int pixelOffsetY, int nCellsX, int nCellsY, double cellSizeX, double cellSizeY, const QgsRectangle &rasterBBox, FeatureStats &stats )
{
double cellCenterX, cellCenterY;
cellCenterY = rasterBBox.yMaximum() - pixelOffsetY * cellSizeY - cellSizeY / 2;
stats.reset();
GEOSGeometry *polyGeos = poly.exportToGeos();
if ( !polyGeos )
{
return;
}
GEOSContextHandle_t geosctxt = QgsGeometry::getGEOSHandler();
const GEOSPreparedGeometry *polyGeosPrepared = GEOSPrepare_r( geosctxt, polyGeos );
if ( !polyGeosPrepared )
{
GEOSGeom_destroy_r( geosctxt, polyGeos );
return;
}
GEOSCoordSequence *cellCenterCoords = nullptr;
GEOSGeometry *currentCellCenter = nullptr;
QgsRectangle featureBBox = poly.boundingBox().intersect( &rasterBBox );
QgsRectangle intersectBBox = rasterBBox.intersect( &featureBBox );
QgsRasterBlock *block = mRasterProvider->block( mRasterBand, intersectBBox, nCellsX, nCellsY );
for ( int i = 0; i < nCellsY; ++i )
{
cellCenterX = rasterBBox.xMinimum() + pixelOffsetX * cellSizeX + cellSizeX / 2;
for ( int j = 0; j < nCellsX; ++j )
{
if ( validPixel( block->value( i, j ) ) )
{
GEOSGeom_destroy_r( geosctxt, currentCellCenter );
cellCenterCoords = GEOSCoordSeq_create_r( geosctxt, 1, 2 );
GEOSCoordSeq_setX_r( geosctxt, cellCenterCoords, 0, cellCenterX );
GEOSCoordSeq_setY_r( geosctxt, cellCenterCoords, 0, cellCenterY );
currentCellCenter = GEOSGeom_createPoint_r( geosctxt, cellCenterCoords );
if ( GEOSPreparedContains_r( geosctxt, polyGeosPrepared, currentCellCenter ) )
{
stats.addValue( block->value( i, j ) );
}
}
cellCenterX += cellSizeX;
}
cellCenterY -= cellSizeY;
}
GEOSGeom_destroy_r( geosctxt, currentCellCenter );
GEOSPreparedGeom_destroy_r( geosctxt, polyGeosPrepared );
GEOSGeom_destroy_r( geosctxt, polyGeos );
delete block;
}
void QgsZonalStatistics::statisticsFromPreciseIntersection( const QgsGeometry &poly, int pixelOffsetX,
int pixelOffsetY, int nCellsX, int nCellsY, double cellSizeX, double cellSizeY, const QgsRectangle &rasterBBox, FeatureStats &stats )
{
stats.reset();
double currentY = rasterBBox.yMaximum() - pixelOffsetY * cellSizeY - cellSizeY / 2;
QgsGeometry pixelRectGeometry;
double hCellSizeX = cellSizeX / 2.0;
double hCellSizeY = cellSizeY / 2.0;
double pixelArea = cellSizeX * cellSizeY;
double weight = 0;
QgsRectangle featureBBox = poly.boundingBox().intersect( &rasterBBox );
QgsRectangle intersectBBox = rasterBBox.intersect( &featureBBox );
QgsRasterBlock *block = mRasterProvider->block( mRasterBand, intersectBBox, nCellsX, nCellsY );
for ( int i = 0; i < nCellsY; ++i )
{
double currentX = rasterBBox.xMinimum() + cellSizeX / 2.0 + pixelOffsetX * cellSizeX;
for ( int j = 0; j < nCellsX; ++j )
{
if ( !validPixel( block->value( i, j ) ) )
{
continue;
}
pixelRectGeometry = QgsGeometry::fromRect( QgsRectangle( currentX - hCellSizeX, currentY - hCellSizeY, currentX + hCellSizeX, currentY + hCellSizeY ) );
if ( !pixelRectGeometry.isNull() )
{
//intersection
QgsGeometry intersectGeometry = pixelRectGeometry.intersection( poly );
if ( !intersectGeometry.isNull() )
{
double intersectionArea = intersectGeometry.area();
if ( intersectionArea >= 0.0 )
{
weight = intersectionArea / pixelArea;
stats.addValue( block->value( i, j ), weight );
}
}
pixelRectGeometry = QgsGeometry();
}
currentX += cellSizeX;
}
currentY -= cellSizeY;
}
delete block;
}
void QgsZonalStatistics::statisticsFromPreciseIntersection( void* band, const QgsGeometry* poly, int pixelOffsetX,
int pixelOffsetY, int nCellsX, int nCellsY, double cellSizeX, double cellSizeY, const QgsRectangle& rasterBBox, FeatureStats &stats )
{
stats.reset();
double currentY = rasterBBox.yMaximum() - pixelOffsetY * cellSizeY - cellSizeY / 2;
float* pixelData = ( float * ) CPLMalloc( sizeof( float ) );
QgsGeometry* pixelRectGeometry = nullptr;
double hCellSizeX = cellSizeX / 2.0;
double hCellSizeY = cellSizeY / 2.0;
double pixelArea = cellSizeX * cellSizeY;
double weight = 0;
for ( int row = 0; row < nCellsY; ++row )
{
double currentX = rasterBBox.xMinimum() + cellSizeX / 2.0 + pixelOffsetX * cellSizeX;
for ( int col = 0; col < nCellsX; ++col )
{
if ( GDALRasterIO( band, GF_Read, pixelOffsetX + col, pixelOffsetY + row, nCellsX, 1, pixelData, 1, 1, GDT_Float32, 0, 0 ) != CE_None )
QgsDebugMsg( "Raster IO Error" );
if ( !validPixel( *pixelData ) )
continue;
pixelRectGeometry = QgsGeometry::fromRect( QgsRectangle( currentX - hCellSizeX, currentY - hCellSizeY, currentX + hCellSizeX, currentY + hCellSizeY ) );
if ( pixelRectGeometry )
{
//intersection
QgsGeometry *intersectGeometry = pixelRectGeometry->intersection( poly );
if ( intersectGeometry )
{
double intersectionArea = intersectGeometry->area();
if ( intersectionArea >= 0.0 )
{
weight = intersectionArea / pixelArea;
stats.addValue( *pixelData, weight );
}
delete intersectGeometry;
}
delete pixelRectGeometry;
pixelRectGeometry = nullptr;
}
currentX += cellSizeX;
}
currentY -= cellSizeY;
}
CPLFree( pixelData );
}
void MedicalWindow::floodFill(int** pixels,bool **visited,const int& h,const int& w){
bool follow = true;
for(int i=0;i<h;i++){
for(int j=0;j<w;j++){
if(pixels[i][j]==255){
q.push(make_pair(i,j));
follow = false;
break;
}
}
if(!follow) break;
}
while(!q.empty()){
pair<int,int> pos = q.front();
q.pop();
int i = pos.first;
int j = pos.second;
if(visited[i][j] || pixels[i][j] == 0) continue;
visited[i][j] = true;
pixels[i][j] = 0;
for(int k=0;k<D;k++){
int x = i+dx[k];
int y = j+dy[k];
if(validPixel(x,y,h,w))
q.push(make_pair(x,y));
}
}
int ** borders = new int*[h];
for(int i=0;i<h;i++){
borders[i] = new int[w];
for(int j=0;j<w;j++){
borders[i][j] = 1;
visited[i][j] = false;
}
}
q.push(make_pair(0,0));
while(!q.empty()){
pair<int,int> pos = q.front();
q.pop();
int i = pos.first;
int j = pos.second;
if(visited[i][j] || pixels[i][j] == 255) continue;
visited[i][j] = true;
borders[i][j] = 0;
for(int k=0;k<D;k++){
int x = i+dx[k];
int y = j+dy[k];
if(validPixel(x,y,h,w))
q.push(make_pair(x,y));
}
}
ofstream file("image.pgm",ios::out);
file<<"P2"<<endl;
file<<w<<" "<<h<<endl;
file<<1<<endl;
stringstream ss;
for(unsigned int i=0;i<h;i++)
{
for(unsigned int j=0;j<w;j++)
{
ss<<borders[i][j]<<" ";
}
ss<<endl;
}
file<<ss.str();
file.close();
}
int main(int argc, char *argv[])
{
//SET UP STUFF WE NEED
//PARAMS
unsigned w = 512;
unsigned h = 512;
unsigned bits = 2;
int levels = 1;
uint32_t bytesToRead = 0;
uint32_t bitMask = 0;
uint32_t pixPerChunk = 0;
switch (bits)
{
case 1:
bytesToRead = 1;
bitMask = 0x01;
pixPerChunk = 8;
break;
case 2:
bytesToRead = 1;
bitMask = 0x03;
pixPerChunk = 4;
break;
case 4:
bytesToRead = 2;
bitMask = 0x0F;
pixPerChunk = 4;
break;
case 8:
bytesToRead = 4;
bitMask = 0xFF;
pixPerChunk = 4;
break;
case 16:
bytesToRead = 8;
bitMask = 0xFFFF;
pixPerChunk = 4;
break;
case 32:
bytesToRead = 16;
bitMask = 0xFFFFFFFF;
pixPerChunk = 4;
break;
}
uint32_t chunksPerLine = w / pixPerChunk;
//512 pixels in a line, 4 pixels to a chunk
assert(chunksPerLine == 512 / 4);
uint32_t totalChunks = w * h / pixPerChunk;
//512 columns, 512 rows, 4 pixels per chunk
assert(totalChunks = 512 * 512 / 4);
uint32_t pixBufSize = sizeof(float) * ((w * (2 * levels)) + pixPerChunk);
//For levels = 1, need maximum of 2 full lines, plus 1 chunk;
assert(pixBufSize / sizeof(float) == 1028);
float *testStreamBuffer = (float *)malloc(pixBufSize);
levels = 3;
float *diamond = (float *)malloc(sizeof(float) * (2 * levels + 1));
for (int i = 0; i < (2 * levels + 1); i++)
{
for (int j = 0; j < (2 * levels + 1); j++)
{
diamond[(i * (2 * levels + 1)) + j] = (i * (2 * levels + 1)) + j;
}
}
printf("Levels = 3 - 2\n");
// ___
// 0 1 2__|3__|4__ 5 6
// 7 8__|9__|10_|11_|12_ 13
// 14_|15_|16_|17_|18_|19_|20_
//|21_|22_|23_|24_|25_|26_|27_|
// 28 |29_|30_|31_|32_|33_|34
// 35 36 |37_|38_|39_|40 41
// 42 43 44 |45_|46 47 48
fprintf(stderr, "\n ___\n");
fprintf(stderr, " 0 1 2__|3__|4__ 5 6\n");
fprintf(stderr, " 7 8__|9__|10_|11_|12_ 13\n");
fprintf(stderr, " 14_|15_|16_|17_|18_|19_|20_\n");
fprintf(stderr, "|21_|22_|23_|24_|25_|26_|27_|\n");
fprintf(stderr, " 28 |29_|30_|31_|32_|33_|34\n");
fprintf(stderr, " 35 36 |37_|38_|39_|40 41\n");
fprintf(stderr, " 42 43 44 |45_|46 47 48\n\n");
int squareCount = 0;
for (int i = 0; i < levels + 1; i++)
{
for (int j = 0; j < levels + 1; j++)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, 0, levels), mapIJKtoDY(i, j, 0, levels));
printf("i %d j %d k %d = %f\n", i, j, 0, MAXPIXIJK(7, 7, 3, 2, i, j, 0, levels, &diamond[0], &diamond[24], &diamond[48]));
if (i == 0)
{
for (int k = 1; k < 2 * j; k += 2)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels));
printf("i %d j %d k %d = %f\n", i, j, k, MAXPIXIJK(7, 7, 3, 2, i, j, k, levels, &diamond[0], &diamond[24], &diamond[48]));
// getPixel(7, 7, 3, 2, mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels), &diamond[0], &diamond[24], &diamond[48]
}
}
else if (i < j && i != 0 && j == levels)
{
for (int k = 1; k < 2 * (levels - i); k += 2)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels));
printf("i %d j %d k %d = %f\n", i, j, k, MAXPIXIJK(7, 7, 3, 2, i, j, k, levels, &diamond[0], &diamond[24], &diamond[48]));
}
}
}
}
fprintf(stderr, "Test if top is cropped... Should be no 3\n\n");
squareCount = 0;
for (int i = 0; i < levels + 1; i++)
{
for (int j = 0; j < levels + 1; j++)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, 0, levels), mapIJKtoDY(i, j, 0, levels));
printf("i %d j %d k %d = %f\n", i, j, 0, MAXPIXIJK(7, 7, 3, 2, i, j, 0, levels, &diamond[0], &diamond[24], &diamond[48]));
if (i == 0)
{
for (int k = 1; k < 2 * j; k += 2)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels));
printf("i %d j %d k %d = %f\n", i, j, k, MAXPIXIJK(7, 7, 3, 2, i, j, k, levels, &diamond[0], &diamond[24], &diamond[48]));
// getPixel(7, 7, 3, 2, mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels), &diamond[0], &diamond[24], &diamond[48]
}
}
else if (i < j && i != 0 && j == levels)
{
for (int k = 1; k < 2 * (levels - i); k += 2)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels));
printf("i %d j %d k %d = %f\n", i, j, k, MAXPIXIJK(7, 7, 3, 2, i, j, k, levels, &diamond[0], &diamond[24], &diamond[48]));
}
}
}
}
fprintf(stderr, "Test missing top half completely\n");
squareCount = 0;
for (int i = 0; i < levels + 1; i++)
{
for (int j = 0; j < levels + 1; j++)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, 0, levels), mapIJKtoDY(i, j, 0, levels));
printf("i %d j %d k %d = %f\n", i, j, 0, MAXPIXIJK(7, 7, 3, 0, i, j, 0, levels, &diamond[0], &diamond[24], &diamond[48]));
if (i == 0)
{
for (int k = 1; k < 2 * j; k += 2)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels));
printf("i %d j %d k %d = %f\n", i, j, k, MAXPIXIJK(7, 7, 3, 0, i, j, k, levels, &diamond[0], &diamond[24], &diamond[48]));
// getPixel(7, 7, 3, 2, mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels), &diamond[0], &diamond[24], &diamond[48]
}
}
else if (i < j && i != 0 && j == levels)
{
for (int k = 1; k < 2 * (levels - i); k += 2)
{
squareCount++;
// printf("dx: %d dy: %d\n", mapIJKtoDX(i, j, k, levels), mapIJKtoDY(i, j, k, levels));
printf("i %d j %d k %d = %f\n", i, j, k, MAXPIXIJK(7, 7, 3, 0, i, j, k, levels, &diamond[0], &diamond[24], &diamond[48]));
}
}
}
}
//Should get 25
printf("%d\n", squareCount);
// ___
// ___|_1_|___
// ___|11_|_3_|_2_|___
// ___|17_|_6_|12_|_5_|_4_|___
//|22_|10_|18_|_9_|13_|_8_|_7_|
// |23_|16_|19_|15_|14_|
// |24_|21_|20_|
// |25_|
// ___
// ___|___|___
// ___|___|___|___|___
// ___|___|___|___|___|___|___
//|___|___|___|___|___|___|___|
// |___|___|___|___|___|
// |___|___|___|
// |___|
//Test valid
levels = 2;
//TEST VALID PIXEL
assert(!validPixel(w, h, 0, 0, -1, -1));
assert(!validPixel(w, h, w, h, 0, 0));
assert(!validPixel(w, h, w / 2, h - 1, 5, 2));
assert(validPixel(w, h, w / 2, h / 2, (w / 2) - 1, (h / 2) - 1));
//Test validPixel and getPixel
testStreamBuffer[0] = 50;
testStreamBuffer[0 + w] = 100;
unsigned x, y;
calculateChunkXY(w, h, &x, &y, 128, chunksPerLine, pixPerChunk);
assert(x == 0);
assert(y == 1);
assert(validPixel(w, h, x, y, 0, -1));
assert(getPixel(w, h, x, y, 0, -1, &testStreamBuffer[0], &testStreamBuffer[512], &testStreamBuffer[1028]) == 50);
testStreamBuffer[0] = 5;
testStreamBuffer[1027] = 1;
assert(getPixel(w, h, 0, 0, 1, 0, &testStreamBuffer[0], &testStreamBuffer[1027], &testStreamBuffer[1028]) == 5);
// ERODE TEST TOP LEFT CORNER
// 5 1 5 3 7....
// 1 2 3 4....
// Should give
// 1 1 1 3 in results Buffer
levels = 1;
uint64_t chunksProcessed = 0; //Process top left corner
uint64_t chunksRead = 128; // We've read full width + 1 chunk
float *pixCalculate = &testStreamBuffer[0];
float *processedResultsBuffer = (float *)malloc(sizeof(float) * pixPerChunk);
testStreamBuffer[0] = 5;
testStreamBuffer[1] = 1;
testStreamBuffer[2] = 5;
testStreamBuffer[3] = 3;
testStreamBuffer[4] = 7;
testStreamBuffer[w] = 1;
testStreamBuffer[w + 1] = 2;
testStreamBuffer[w + 2] = 3;
testStreamBuffer[w + 3] = 4;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0);
assert(y == 0);
//Test correct Pixels are fetched
assert(getPixel(w, h, 0, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[0], &testStreamBuffer[1028]) == 1);
assert(getPixel(w, h, 1, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[1], &testStreamBuffer[1028]) == 2);
assert(getPixel(w, h, 2, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[2], &testStreamBuffer[1028]) == 3);
assert(getPixel(w, h, 3, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[3], &testStreamBuffer[1028]) == 4);
//Test the erode!
erodeChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], pixCalculate, &testStreamBuffer[1027], processedResultsBuffer);
assert(processedResultsBuffer[0] == 1);
assert(processedResultsBuffer[1] == 1);
assert(processedResultsBuffer[2] == 1);
assert(processedResultsBuffer[3] == 3);
assert(chunksProcessed == 1);
// DILATE TEST TOP LEFT CORNER
// 5 1 5 3 7....
// 1 2 3 4....
// Should give
// 5 5 5 7 in results Buffer
chunksProcessed = 0;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0);
assert(y == 0);
assert(getPixel(w, h, 0, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[0], &testStreamBuffer[1028]) == 1);
assert(getPixel(w, h, 1, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[1], &testStreamBuffer[1028]) == 2);
assert(getPixel(w, h, 2, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[2], &testStreamBuffer[1028]) == 3);
assert(getPixel(w, h, 3, 0, 0, 1, &testStreamBuffer[0], &testStreamBuffer[3], &testStreamBuffer[1028]) == 4);
dilateChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], pixCalculate, &testStreamBuffer[1027], processedResultsBuffer);
assert(processedResultsBuffer[0] == 5);
assert(processedResultsBuffer[1] == 5);
assert(processedResultsBuffer[2] == 5);
assert(processedResultsBuffer[3] == 7);
assert(chunksProcessed == 1);
//ERODE CENTER LEFT TEST
// 7 3 7 3
// 2 8 9 10 9
// 5 6 7 8
// Should give
// 2 2 7 3
testStreamBuffer[0] = 7;
testStreamBuffer[1] = 3;
testStreamBuffer[2] = 7;
testStreamBuffer[3] = 3;
testStreamBuffer[w] = 2;
testStreamBuffer[w + 1] = 8;
testStreamBuffer[w + 2] = 9;
testStreamBuffer[w + 3] = 10;
testStreamBuffer[w + 4] = 9;
testStreamBuffer[2 * w] = 5;
testStreamBuffer[2 * w + 1] = 6;
testStreamBuffer[2 * w + 2] = 7;
testStreamBuffer[2 * w + 3] = 8;
chunksProcessed = 128;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0);
assert(y == 1);
assert(testStreamBuffer[515] == 10);
//Above
assert(getPixel(w, h, x, y, 0, -1, &testStreamBuffer[0], &testStreamBuffer[512], &testStreamBuffer[1028]) == 7);
assert(getPixel(w, h, x + 1, y, 0, -1, &testStreamBuffer[0], &testStreamBuffer[513], &testStreamBuffer[1028]) == 3);
assert(getPixel(w, h, x + 2, y, 0, -1, &testStreamBuffer[0], &testStreamBuffer[514], &testStreamBuffer[1028]) == 7);
assert(getPixel(w, h, x + 3, y, 0, -1, &testStreamBuffer[0], &testStreamBuffer[515], &testStreamBuffer[1028]) == 3);
//Below
assert(getPixel(w, h, x, y, 0, 1, &testStreamBuffer[0], &testStreamBuffer[512], &testStreamBuffer[1028]) == 5);
assert(getPixel(w, h, x + 1, y, 0, 1, &testStreamBuffer[0], &testStreamBuffer[513], &testStreamBuffer[1028]) == 6);
assert(getPixel(w, h, x + 2, y, 0, 1, &testStreamBuffer[0], &testStreamBuffer[514], &testStreamBuffer[1028]) == 7);
assert(getPixel(w, h, x + 3, y, 0, 1, &testStreamBuffer[0], &testStreamBuffer[515], &testStreamBuffer[1028]) == 8);
assert(testStreamBuffer[512] == 2);
erodeChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[512], &testStreamBuffer[1027], processedResultsBuffer);
assert(processedResultsBuffer[0] == 2);
assert(processedResultsBuffer[1] == 2);
assert(processedResultsBuffer[2] == 7);
assert(processedResultsBuffer[3] == 3);
assert(chunksProcessed == 129);
w = 32;
h = 32;
bits = 2;
levels = 1;
bytesToRead = sizeof(uint64_t);
assert(bytesToRead == 8);
pixPerChunk = 64 / bits;
assert(pixPerChunk == 32);
chunksPerLine = w / pixPerChunk;
assert(chunksPerLine == 1);
totalChunks = (w * h) / pixPerChunk;
assert(totalChunks == 32);
pixBufSize = sizeof(float) * (w * ((2 * levels) + 1));
assert(pixBufSize == (3 * 32 * sizeof(float)));
free(testStreamBuffer);
testStreamBuffer = (float *)malloc(pixBufSize);
for (int i = 0; i < pixBufSize / sizeof(float); i++)
{
testStreamBuffer[i] = i + 1;
}
//Do some tests;
//GetChunkXY
chunksProcessed = 0;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 0);
chunksProcessed = 1;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 1);
chunksProcessed = 2;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 2);
chunksProcessed = 3;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 3);
//Valid Pixel
//First Line
chunksProcessed = 0;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 0);
//Top left
assert(!validPixel(w, h, x, y, 0, -1));
assert(!validPixel(w, h, x, y, -1, 0));
assert(!validPixel(w, h, x, y, -1, -1));
assert(validPixel(w, h, x, y, 0, 0));
assert(validPixel(w, h, x, y, 0, 1));
assert(validPixel(w, h, x, y, 1, 0));
//Top Right
assert(validPixel(w, h, x + 31, y, -1, 0));
assert(!validPixel(w, h, x + 31, y, 0, -1));
assert(!validPixel(w, h, x + 31, y, -1, -1));
assert(!validPixel(w, h, x + 31, y, 1, 0));
assert(!validPixel(w, h, x + 31, y, 1, -1));
assert(validPixel(w, h, x + 31, y, -1, 1));
//Left Edge
for (int i = 0; i < h; i++)
{
assert(validPixel(w, h, x, y + i, 1, 0));
assert(!validPixel(w, h, x, y + i, -1, 0));
}
//Right Edge
for (int i = 0; i < h; i++)
{
assert(validPixel(w, h, x + 31, y + i, -1, 0));
assert(!validPixel(w, h, x + 31, y + i, 1, 0));
}
//Along top
for (int i = 0; i < w; i++)
{
assert(validPixel(w, h, x + i, y, 0, 1));
assert(!validPixel(w, h, x + i, y, 0, -1));
}
chunksProcessed = 31;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 31);
//Bottom left
assert(validPixel(w, h, x, y, 0, -1));
assert(validPixel(w, h, x, y, 1, 0));
assert(!validPixel(w, h, x, y, -1, 0));
assert(!validPixel(w, h, x, y, -1, -1));
assert(validPixel(w, h, x, y, 1, -1));
assert(!validPixel(w, h, x, y, 0, 1));
assert(!validPixel(w, h, x, y, 1, 1));
//Bottom Right
assert(!validPixel(w, h, x + 31, y, 1, 0));
assert(!validPixel(w, h, x + 31, y, 0, 1));
assert(!validPixel(w, h, x + 31, y, 1, 1));
assert(validPixel(w, h, x + 31, y, -1, 0));
assert(validPixel(w, h, x + 31, y, 0, -1));
assert(validPixel(w, h, x + 31, y, -1, -1));
//Along Bottom
for (int i = 0; i < w; i++)
{
assert(validPixel(w, h, x + i, y, 0, -1));
assert(!validPixel(w, h, x + i, y, 0, 1));
}
//Test getting Values...
chunksProcessed = 0;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
for (int i = 0; i < w; i++)
{
assert(getPixel(w, h, x, y, 0, 1, &testStreamBuffer[0], &testStreamBuffer[i], &testStreamBuffer[96]) == w + 1 + i);
}
chunksProcessed = 3;
calculateChunkXY(w, h, &x, &y, chunksProcessed, chunksPerLine, pixPerChunk);
assert(x == 0 && y == 3);
for (int i = 0; i < w; i++)
{
assert(getPixel(w, h, x, y, 0, 1, &testStreamBuffer[0], &testStreamBuffer[(2 * w) + i], &testStreamBuffer[96]) == i + 1);
}
for (int i = 0; i < (w - 1); i++)
{
assert(getPixel(w, h, x, y, 1, 1, &testStreamBuffer[0], &testStreamBuffer[(2 * w) + i], &testStreamBuffer[96]) == i + 2);
}
printf("All Tests Run Successfully\n");
float *testResults = (float *)malloc(sizeof(float) * pixPerChunk * chunksPerLine);
assert(w == 32 && h == 32);
assert(pixPerChunk == 32);
assert(chunksPerLine == 1);
//Top Row
chunksProcessed = 0;
erodeChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[0], &testStreamBuffer[96], testResults);
fprintf(stderr, "Erode Top\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
chunksProcessed = 0;
dilateChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[0], &testStreamBuffer[96], testResults);
fprintf(stderr, "Dilate Top\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
//Middle Row
erodeChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[32], &testStreamBuffer[96], testResults);
fprintf(stderr, "Erode Middle\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
chunksProcessed = 1;
dilateChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[32], &testStreamBuffer[96], testResults);
fprintf(stderr, "Dilate Middle\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
//Bottom Row wrap
erodeChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[64], &testStreamBuffer[96], testResults);
fprintf(stderr, "Erode Bottom\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
chunksProcessed = 2;
dilateChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[64], &testStreamBuffer[96], testResults);
fprintf(stderr, "Dilate Bottom\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
//Top Row wrap
erodeChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[0], &testStreamBuffer[96], testResults);
fprintf(stderr, "Erode Top Wrap\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
chunksProcessed = 3;
dilateChunk(w, h, levels, pixPerChunk, chunksPerLine, &chunksProcessed, &testStreamBuffer[0], &testStreamBuffer[0], &testStreamBuffer[96], testResults);
fprintf(stderr, "Dilate Top Wrap\n");
for (int i = 0; i < (pixPerChunk * chunksPerLine); i++)
{
fprintf(stderr, "Value %d %lf\n", i, testResults[i]);
}
}
bool writePLY(const std::string& filename,const cv::Mat& disparityImage,cv::Mat img,const cv::Mat& Q,const cv::Mat& rH)
{
#define VALID(i,_x,_y) validPixel(i,mapped,_posImage,_x,_y)
std::ofstream file(filename.c_str());
if(!file)
{
std::cerr<<"Can't open file "<<filename<<std::endl;
return false;
}
int i=0;
cv::Mat mapped=cv::Mat(3,disparityImage.rows*disparityImage.cols,cv::DataType<float>::type,1.0);
if(!rH.empty())
{
//Only count pixels of disparity image which mapped via rH^(-1) are inside the unrectified image
cv::Mat p=cv::Mat(3,disparityImage.rows*disparityImage.cols,cv::DataType<float>::type,1.0);
for(float y=0;y<disparityImage.rows;y++)
{
for(float x=0;x<disparityImage.cols;x++,i++)
{
p.at<float>(0,i)=x;
p.at<float>(1,i)=y;
}
}
cv::Mat rHF;
rH.convertTo(rHF,cv::DataType<float>::type);
fprintf(stderr,"\nSolving Equation System %ix%i %ix%i %ix%i",rHF.rows,rHF.cols,mapped.rows,mapped.cols,p.rows,p.cols);
cv::solve(rHF,p,mapped);
rHF.release();
p.release();
for(i=0;i<mapped.cols;i++)
{
mapped.at<float>(0,i)=mapped.at<float>(0,i)/mapped.at<float>(2,i);
mapped.at<float>(1,i)=mapped.at<float>(1,i)/mapped.at<float>(2,i);
}
}else
{
fprintf(stderr,"\nRectifying Homography is empty");
mapped.create(2,disparityImage.rows*disparityImage.cols,cv::DataType<float>::type);
mapped.setTo(cv::Scalar(1.0));
}
i=0;
int noVertices=0;
int noTriangles=0;
cv::Mat _posImage(disparityImage.rows,disparityImage.cols,cv::DataType<cv::Vec3f>::type);
//Reproject image point
if(type_to_string(disparityImage)!="short" && type_to_string(disparityImage)!="float")
{
std::cerr<<"Converting disparity image to datatype float"<<std::endl;
cv::Mat tmp;
disparityImage.convertTo(tmp,CV_32F);
cv::reprojectImageTo3D(tmp,_posImage,Q,true);
}else
{
cv::reprojectImageTo3D(disparityImage,_posImage,Q,true);
}
std::stringstream verticesBuffer;
std::stringstream trianglesBuffer;
//Write vertices into buffer
int * indexToVertex = new int[disparityImage.rows*disparityImage.cols];
std::cout<<mapped.rows<<std::endl;
std::cout<<mapped.cols<<std::endl;
float * z=new float[mapped.cols];
for(int x=0;x<disparityImage.cols;x++)
{
for(int y=0;y<disparityImage.rows;y++,i++)
{
if(validPixel(i,mapped,_posImage,x,y))//Only those vertices which are valid
{
verticesBuffer<<_posImage.at<cv::Vec3f>(y,x)[0]<<" "<<_posImage.at<cv::Vec3f>(y,x)[1]<<" "<<_posImage.at<cv::Vec3f>(y,x)[2]<<
" "<<(int)img.at<cv::Vec3b>(y,x)[0]<<" "<<(int)img.at<cv::Vec3b>(y,x)[1]<<" "<<(int)img.at<cv::Vec3b>(y,x)[2]<<std::endl;
z[i]=_posImage.at<cv::Vec3f>(y,x)[2];
indexToVertex[i]=noVertices++; //Keep track of what pixel maps to which vertex
}
}
}
//Write triangles into a buffer
i=0;
bool first=true;
for(double x=0;x<disparityImage.cols;x++)
{
for(double y=0;y<disparityImage.rows;y++,i++)
{
if(x<disparityImage.rows-1 && y<disparityImage.cols-1 && VALID(i,x,y)) //Only those vertices which are valid
{
const int right_idx=(x+1)*disparityImage.rows + y;
//first triangle (i,i+1,right_idx+1)
if(VALID(i+1,x,y+1) && VALID(right_idx+1,x+1,y+1)) //Only those triangles which contain noly valid vertices
{
if(fabs(z[i]-z[i+1])<0.1 && fabs(z[i]-z[right_idx+1])<0.1)
{
noTriangles++;
if(!first)
{
trianglesBuffer<<std::endl;
}else
{
first=false;
}
trianglesBuffer<<"3 "<<indexToVertex[i]<<" "<<indexToVertex[i+1]<<" "<<indexToVertex[right_idx+1];
}
}
//second triangle (i,right_idx+1,right_idx)
if(VALID(right_idx+1,x+1,y+1) && VALID(right_idx,x+1,y)) //Only those triangles which contain noly valid vertices
{
if(fabs(z[right_idx+1]-z[i])<0.1 && fabs(z[right_idx]-z[i])<0.1)
{
noTriangles++;
if(!first)
{
trianglesBuffer<<std::endl;
}else
{
first=false;
}
trianglesBuffer<<"3 "<<indexToVertex[i]<<" "<<indexToVertex[right_idx+1]<<" "<<indexToVertex[right_idx];
}
}
}
}
}
delete [] z;
delete [] indexToVertex;
//Write PLY header
file << "ply\nformat ascii 1.0\nelement vertex "
<< noVertices << "\nproperty float x\nproperty float y\nproperty float z";
file << "\nproperty uchar red\nproperty uchar green\nproperty uchar blue";
file << "\nelement face " << noTriangles
<< "\nproperty list uint8 int32 vertex_indices\nend_header\n";
//Write vetices buffer to file
file<<verticesBuffer.str();
file.flush();
//Write triangles buffer to file
file<<trianglesBuffer.str();
file.flush();
file.close();
return true;
#undef VALID
}
bool writePLYBinary(const std::string& filename,const cv::Mat& disparityImage,cv::Mat img,const cv::Mat& Q,const cv::Mat& rH)
{
#define VALID(i,_x,_y) validPixel(i,mapped,_posImage,_x,_y)
std::ofstream file(filename.c_str(),std::ios::binary | std::ios::out);
if(!file)
{
std::cerr<<"Can't open file "<<filename<<std::endl;
return false;
}
int i=0;
cv::Mat mapped=cv::Mat(3,disparityImage.rows*disparityImage.cols,cv::DataType<float>::type,1.0);
if(!rH.empty())
{
//Only count pixels of disparity image which mapped via rH^(-1) are inside the unrectified image
cv::Mat p=cv::Mat(3,disparityImage.rows*disparityImage.cols,cv::DataType<float>::type,1.0);
for(float y=0;y<disparityImage.rows;y++)
{
for(float x=0;x<disparityImage.cols;x++,i++)
{
p.at<float>(0,i)=x;
p.at<float>(1,i)=y;
}
}
cv::Mat rHF;
rH.convertTo(rHF,cv::DataType<float>::type);
fprintf(stderr,"\nSolving Equation System %ix%i %ix%i %ix%i",rHF.rows,rHF.cols,mapped.rows,mapped.cols,p.rows,p.cols);
cv::solve(rHF,p,mapped);
rHF.release();
p.release();
for(i=0;i<mapped.cols;i++)
{
mapped.at<float>(0,i)=mapped.at<float>(0,i)/mapped.at<float>(2,i);
mapped.at<float>(1,i)=mapped.at<float>(1,i)/mapped.at<float>(2,i);
}
}else
{
fprintf(stderr,"\nRectifying Homography is empty");
mapped.create(2,disparityImage.rows*disparityImage.cols,cv::DataType<float>::type);
mapped.setTo(cv::Scalar(1.0));
}
i=0;
int noVertices=0;
int noTriangles=0;
cv::Mat _posImage(disparityImage.rows,disparityImage.cols,cv::DataType<cv::Vec3f>::type);
//Reproject image point
if(type_to_string(disparityImage)!="short" && type_to_string(disparityImage)!="float")
{
std::cerr<<"Converting disparity image to datatype float"<<std::endl;
cv::Mat tmp;
disparityImage.convertTo(tmp,CV_32F);
cv::reprojectImageTo3D(tmp,_posImage,Q,true);
}else
{
cv::reprojectImageTo3D(disparityImage,_posImage,Q,true);
}
//Write vertices into buffer
int * indexToVertex = new int[disparityImage.rows*disparityImage.cols];
memset(indexToVertex,0,disparityImage.rows*disparityImage.cols*sizeof(int));
float * z=new float[mapped.cols];
char * vertexBuffer =new char[disparityImage.rows*disparityImage.cols*(4*3+3)];
for(int x=0;x<disparityImage.cols;x++)
{
for(int y=0;y<disparityImage.rows;y++,i++)
{
if(validPixel(i,mapped,_posImage,x,y))//Only those vertices which are valid
{
char * buffC = vertexBuffer + (noVertices)*(4*3+3) + 4*3;
float * buffF = (float*) (vertexBuffer + (noVertices)*(4*3+3));
buffF[0]=_posImage.at<cv::Vec3f>(y,x)[0];
buffF[1]=_posImage.at<cv::Vec3f>(y,x)[1];
buffF[2]=_posImage.at<cv::Vec3f>(y,x)[2];
buffC[0]=img.at<cv::Vec3b>(y,x)[0];
buffC[1]=img.at<cv::Vec3b>(y,x)[1];
buffC[2]=img.at<cv::Vec3b>(y,x)[2];
z[i]=_posImage.at<cv::Vec3f>(y,x)[2];
indexToVertex[i]=noVertices++; //Keep track of what pixel maps to which vertex
}
}
}
//Write triangles into a buffer
i=0;
char * trianglesBuffer = new char[disparityImage.cols*disparityImage.cols*2*(1+3*4)];
for(double x=0;x<disparityImage.cols;x++)
{
for(double y=0;y<disparityImage.rows;y++,i++)
{
if(x<disparityImage.rows-1 && y<disparityImage.cols-1 && VALID(i,x,y)) //Only those vertices which are valid
{
const int right_idx=(x+1)*disparityImage.rows + y;
//first triangle (i,i+1,right_idx+1)
if(VALID(i+1,x,y+1) && VALID(right_idx+1,x+1,y+1)) //Only those triangles which contain noly valid vertices
{
if(fabs(z[i]-z[i+1])<0.1 && fabs(z[i]-z[right_idx+1])<0.1)
{
char * buffC= trianglesBuffer + noTriangles*(1+3*4);
int * buffI= (int*)(buffC+1);
buffC[0]=3;
buffI[0]=indexToVertex[i];
buffI[1]=indexToVertex[i+1];
buffI[2]=indexToVertex[right_idx+2];
if(buffI[0]>=noVertices || buffI[1]>=noVertices || buffI[2]>=noVertices)
{
std::cout<<"ARRRRGHHHHH "<<buffI[0]<<" "<<buffI[1]<<" "<<buffI[2]<<" "<<noVertices<<" "<<i<<std::endl;
}
noTriangles++;
}
}
//second triangle (i,right_idx+1,right_idx)
// if(VALID(right_idx+1,x+1,y+1) && VALID(right_idx,x+1,y)) //Only those triangles which contain noly valid vertices
// {
// if(fabs(z[right_idx+1]-z[i])<0.1 && fabs(z[right_idx]-z[i])<0.1)
// {
// char * buffC= trianglesBuffer + noTriangles*(1+3*4);
// int * buffI= (int*)(buffC+1);
// buffC[0]=3;
// buffI[0]=indexToVertex[i];
// buffI[1]=indexToVertex[right_idx+1];
// buffI[2]=indexToVertex[right_idx];
// noTriangles++;
// }
// }
}
}
}
delete [] z;
delete [] indexToVertex;
file << "ply\nformat binary_little_endian 1.0\n"
<< "element vertex "<< noVertices << "\nproperty float32 x\nproperty float32 y\nproperty float32 z"
<< "\nproperty uchar red\nproperty uchar green\nproperty uchar blue\n"
<< "element face " << noTriangles
<< "\nproperty list uint8 int32 vertex_indices\n"
<< "end_header\n";
file.write(vertexBuffer,(noVertices)*(sizeof(float)*3+3));
file.write(trianglesBuffer,(noTriangles)*(1+3*sizeof(int)));
// noVertices=0;
// char * buffC = vertexBuffer + (noVertices)*(4*3+3) + 4*3;
// float * buffF = (float*) (vertexBuffer + (noVertices)*(4*3+3));
// buffF[0]=100;
// buffF[1]=0;
// buffF[2]=0;
// buffC[0]=255;
// buffC[1]=0;
// buffC[2]=0;
// noVertices++;
// buffC = vertexBuffer + (noVertices)*(4*3+3) + 4*3;
// buffF = (float*) (vertexBuffer + (noVertices)*(4*3+3));
// buffF[0]=0;
// buffF[1]=100;
// buffF[2]=0;
// buffC[0]=0;
// buffC[1]=0;
// buffC[2]=255;
// noVertices++;
// buffC = vertexBuffer + (noVertices)*(4*3+3) + 4*3;
// buffF = (float*) (vertexBuffer + (noVertices)*(4*3+3));
// buffF[0]=0;
// buffF[1]=0;
// buffF[2]=100;
// buffC[0]=0;
// buffC[1]=255;
// buffC[2]=0;
// noVertices++;
// noTriangles=0;
// buffC= trianglesBuffer + noTriangles*(1+3*4);
// int * buffI= (int*)(buffC+1);
// buffC[0]=3;
// buffI[0]=0;
// buffI[1]=1;
// buffI[2]=2;
// noTriangles++;
// file << "ply\nformat binary_little_endian 1.0\n"
// << "element vertex "<< 3 << "\nproperty float32 x\nproperty float32 y\nproperty float32 z"
// << "\nproperty uchar red\nproperty uchar green\nproperty uchar blue\n"
// << "element face " << 1
// << "\nproperty list uint8 int32 vertex_indices\n"
// << "end_header\n";
// file.write(vertexBuffer,3*(sizeof(float)*3+3));
// file.write(trianglesBuffer,(1+3*sizeof(int)));
file.flush();
file.close();
delete [] vertexBuffer;
delete [] trianglesBuffer;
return true;
#undef VALID
}
