void PushbroomStereo::RunStereoPushbroomStereo2(Mat leftImage, Mat rightImage,Mat laplacian_left,Mat laplacian_right, std::vector<Point3f> *pointVector3d,std::vector<Point3i> *pointVector2d,std::vector<uchar> *pointColors) { int row_start = 0;//statet->row_start; int row_end = leftImage.rows;//statet->row_end; //PushbroomStereoState state = statet->state; // we will do this by looping through every block in the left image // (defined by blockSize) and checking for a matching value on // the right image std::vector<Point3f> localHitPoints; //待确认 int startJ = 0; int stopJ = leftImage.cols - (m_iDisparity + m_iBlockSize); if (m_iDisparity < 0) { startJ = -m_iDisparity; stopJ = leftImage.cols - m_iBlockSize; } //printf("row_start: %d, row_end: %d, startJ: %d, stopJ: %d, rows: %d, cols: %d\n", row_start, row_end, startJ, stopJ, leftImage.rows, leftImage.cols); int hitCounter = 0; //if (state.random_results < 0) //{ for (int i=row_start; i < row_end;i+=m_iBlockSize) { for (int j=startJ; j < stopJ; j+=m_iBlockSize) { // get the sum of absolute differences for this location on both images int sad = GetSAD(leftImage, rightImage, laplacian_left, laplacian_right, j, i); // check to see if the SAD is below the threshold, // indicating a hit if (sad < m_iSadThreshold && sad >= 0) { // got a hit // now check for horizontal invariance (ie check for parts of the image that look the same as this which would indicate that this might be a false-positive) if (!m_bCheck_horizontal_invariance || (CheckHorizontalInvariance(leftImage, rightImage, laplacian_left, laplacian_right, j, i)== false)) { // add it to the vector of matches // don't forget to offset it by the blockSize,so we match the center of the block instead of the top left corner localHitPoints.push_back(Point3f(j+m_iBlockSize/2.0, i+m_iBlockSize/2.0, -m_iDisparity)); //localHitPoints.push_back(Point3f(state.debugJ, state.debugI, -disparity)); uchar pxL = leftImage.at<uchar>(i,j); pointColors->push_back(pxL); // this is the corner of the box, not the center hitCounter ++; if (m_bShow_display) pointVector2d->push_back(Point3i(j, i, sad)); } // check horizontal invariance } } } // now we have an array of hits -- transform them to 3d points if (hitCounter > 0) perspectiveTransform(localHitPoints, *pointVector3d, m_matQ); }
void PushbroomStereo::RunStereoPushbroomStereo( Mat leftImage, Mat rightImage, Mat laplacian_left, Mat laplacian_right, cv::vector<Point3f> *pointVector3d, cv::vector<Point3i> *pointVector2d, cv::vector<uchar> *pointColors, int row_start, int row_end, PushbroomStereoState state ) { // we will do this by looping through every block in the left image // (defined by blockSize) and checking for a matching value on // the right image cv::vector<Point3f> localHitPoints; int blockSize = state.blockSize; int disparity = state.disparity; int sadThreshold = state.sadThreshold; int startJ = 0; int stopJ = leftImage.cols - (disparity + blockSize); if (disparity < 0) { startJ = -disparity; stopJ = leftImage.cols - blockSize; } //printf("row_start: %d, row_end: %d, startJ: %d, stopJ: %d, rows: %d, cols: %d\n", row_start, row_end, startJ, stopJ, leftImage.rows, leftImage.cols); int hitCounter = 0; if (state.random_results < 0) { int *sadArray = new int[ leftImage.rows * leftImage.step ]; int iStep, jStep; #ifdef USE_GPU StopWatchInterface *timer; sdkCreateTimer( &timer ); sdkResetTimer( &timer ); sdkStartTimer( &timer ); //GetSADBlock(row_start, row_end, blockSize, startJ, stopJ, sadArray, leftImage, rightImage, laplacian_left, laplacian_right, state); m_sadCalculator.runGetSAD( row_start, row_end, startJ, stopJ, sadArray, leftImage.data, rightImage.data, laplacian_left.data, laplacian_right.data, leftImage.step, state.blockSize, state.disparity, state.sobelLimit ); sdkStopTimer( &timer ); //printf("RunStereo bottleneck timer: %.2f ms \n", sdkGetTimerValue( &timer) ); sdkDeleteTimer( &timer ); #endif int gridY = (row_end - row_start)/blockSize; int gridX = (stopJ - startJ)/blockSize; for (int y=0; y< gridY; y++) { for (int x=0; x< gridX; x++) { // check to see if the SAD is below the threshold, // indicating a hit int i = row_start + y * blockSize; int j = startJ + x * blockSize; #ifdef USE_GPU int sad = sadArray[ y * gridX + x]; #else int sad= GetSAD(leftImage, rightImage, laplacian_left, laplacian_right, j, i, state); #endif if (sad < sadThreshold && sad >= 0) { // got a hit // now check for horizontal invariance // (ie check for parts of the image that look the same as this // which would indicate that this might be a false-positive) if (!state.check_horizontal_invariance || CheckHorizontalInvariance(leftImage, rightImage, laplacian_left, laplacian_right, j, i, state) == false) { // add it to the vector of matches // don't forget to offset it by the blockSize, // so we match the center of the block instead // of the top left corner localHitPoints.push_back(Point3f(j+blockSize/2.0, i+blockSize/2.0, -disparity)); //localHitPoints.push_back(Point3f(state.debugJ, state.debugI, -disparity)); uchar pxL = leftImage.at<uchar>(i,j); pointColors->push_back(pxL); // TODO: this is the corner of the box, not the center hitCounter ++; if (state.show_display) { pointVector2d->push_back(Point3i(j, i, sad)); } } // check horizontal invariance } } } } else { double intpart; float fractpart = modf(state.random_results , &intpart); hitCounter = int(intpart); // determine if this is a time we'll use that last point std::random_device rd; std::default_random_engine generator(rd()); // rd() provides a random seed std::uniform_real_distribution<float> distribution(0, 1); if (fractpart > distribution(generator)) { hitCounter ++; } for (int i = 0; i < hitCounter; i++) { int randx = rand() % (stopJ - startJ) + startJ; int randy = rand() % (row_end - row_start) + row_start; localHitPoints.push_back(Point3f(randx, randy, -disparity)); } } // now we have an array of hits -- transform them to 3d points if (hitCounter > 0) { perspectiveTransform(localHitPoints, *pointVector3d, state.Q); } }