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 }