int doHoughLine(int argc, char** argv) { char *iamgePath = argv[1]; IplImage* src = cvLoadImage(iamgePath, 0 ); IplImage* dst; IplImage* color_dst; CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; int i; if( !src ) return -1; dst = cvCreateImage( cvGetSize(src), 8, 1 ); color_dst = cvCreateImage( cvGetSize(src), 8, 3 ); cvCanny( src, dst, 50, 200, 3 ); cvCvtColor( dst, color_dst, CV_GRAY2BGR ); #if 0 lines = cvHoughLines2( dst, storage, CV_HOUGH_STANDARD, 1, CV_PI/180, 100, 0, 0 ); for( i = 0; i < MIN(lines->total,100); i++ ) { float* line = (float*)cvGetSeqElem(lines,i); float rho = line[0]; float theta = line[1]; CvPoint pt1, pt2; double a = cos(theta), b = sin(theta); double x0 = a*rho, y0 = b*rho; pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); cvLine( color_dst, pt1, pt2, CV_RGB(255,0,0), 3, CV_AA, 0 ); } #else lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 50, 50, 10 ); for( i = 0; i < lines->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 3, CV_AA, 0 ); } #endif cvNamedWindow( "Source", 1 ); cvShowImage( "Source", src ); cvNamedWindow( "Hough", 1 ); cvShowImage( "Hough", color_dst ); cvWaitKey(0); return 0; }
void demo(Image &img) { IplImage *src = ImageToCv(img); IplImage* dst = cvCreateImage(cvGetSize(src), 8, 1); IplImage* color_dst = cvCreateImage(cvGetSize(src), 8, 3); CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; int i; cvCanny(src, dst, 50, 200, 3); cvCvtColor(dst, color_dst, CV_GRAY2BGR); lines = cvHoughLines2(dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 80, 30, 10); for(i = 0; i < lines->total; i++) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 3, 8); } cvNamedWindow("Source", 1); cvShowImage("Source", src); cvNamedWindow("Hough", 1); cvShowImage("Hough", color_dst); cvWaitKey(0); cvReleaseImage(&src); cvReleaseImage(&dst); cvReleaseImage(&color_dst); cvDestroyWindow("Source"); cvDestroyWindow("Hough"); }
int cvHoughLinesSDiv( CvArr* image, double rho, int srn, double theta, int stn, int threshold, float* lines, int linesNumber ) { CvMat linesMat = cvMat( 1, linesNumber, CV_32FC2, lines ); cvHoughLines2( image, &linesMat, CV_HOUGH_MULTI_SCALE, rho, theta, threshold, srn, stn ); return linesMat.cols; }
int cvHoughLines( CvArr* image, double rho, double theta, int threshold, float* lines, int linesNumber ) { CvMat linesMat = cvMat( 1, linesNumber, CV_32FC2, lines ); cvHoughLines2( image, &linesMat, CV_HOUGH_STANDARD, rho, theta, threshold, 0, 0 ); return linesMat.cols; }
int cvHoughLinesP( CvArr* image, double rho, double theta, int threshold, int lineLength, int lineGap, int* lines, int linesNumber ) { CvMat linesMat = cvMat( 1, linesNumber, CV_32SC4, lines ); cvHoughLines2( image, &linesMat, CV_HOUGH_PROBABILISTIC, rho, theta, threshold, lineLength, lineGap ); return linesMat.cols; }
void houghDetection() { IplImage *temporaryImage = cvCreateImage(cvGetSize(objectImage), IPL_DEPTH_32F, 1); IplImage *houghImage = cvLoadImage("object.jpg", CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR); IplImage *cannyImage = _canny(objectImage); CvMemStorage *storage = cvCreateMemStorage(0); CvSeq *lines = NULL; lines = cvHoughLines2(cannyImage, storage, CV_HOUGH_PROBABILISTIC, 1, (CV_PI/180), 50, 50, 10); for ( int i = 0; i < lines->total; i++ ) { CvPoint *line = (CvPoint *)cvGetSeqElem(lines, i); cvLine(houghImage, line[0], line[1], CV_RGB(255,0,0), 1, 1, 0); } cvShowImage(windowHoughName, houghImage); cvReleaseImage(&houghImage); }
void calcNecessaryImageRotation(IplImage *src) { #define MAX_LINES 100 CvMemStorage* storage = cvCreateMemStorage(0); CvSize img_sz = cvGetSize( src ); IplImage* color_dst = cvCreateImage( img_sz, 8, 3 ); IplImage* dst = cvCreateImage( img_sz, 8, 1 ); CvSeq* lines = 0; int i; cvCanny( src, dst, 50, 200, 3 ); cvCvtColor( dst, color_dst, CV_GRAY2BGR ); cvSaveImage("canny.png", dst); lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 80, 30, 10 ); for( i = 0; i < lines->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 1, 8 ); printf("%f\n", atan((double)(line[1].y-line[0].y) / (double)(line[1].x-line[0].x))); } // TODO(kroo): build up a smoothed histogram (cvHistogram) // TODO(kroo): find two peaks, assert that they are separated by roughly 90˚ // TODO(kroo): find smallest rotation necessary to cause the lines to point straight up/down/left/right cvSaveImage("hough.png", color_dst); cvNamedWindow( "Hough Transform", 1 ); cvShowImage( "Hough Transform", color_dst ); cvWaitKey(0); }
void rotate(IplImage *img) { CvRect rect; IplImage *imgLine; rect.x=GlobalGandhiji.x+GlobalGandhiji.width; rect.y=GlobalGandhiji.y-5; rect.width=(int)((GlobalGandhiji.width)-5); rect.height=GlobalGandhiji.height+15; if(GlobalGandhiji.matchval!=-1 && rect.x>0 && rect.y>0 && rect.y+rect.height<= img->height && rect.x+rect.width<= img->width) { imgLine=cropRectangle(img,rect); cvNamedWindow("imgLine",1); cvShowImage("imgLine",imgLine); IplImage* src1 = cvCreateImage( cvGetSize(imgLine), 8, 1 ); cvCvtColor( imgLine, src1, CV_RGB2GRAY ); IplImage* dst = cvCreateImage( cvGetSize(src1), 8, 1 ); IplImage* color_dst = cvCreateImage( cvGetSize(src1), 8, 3 ); CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; int i; cvCanny( src1, dst,50, 150, 3 ); //cvDilate( dst, dst, 0, 1 ); cvNamedWindow("edgedest",1); cvShowImage("edgedest",dst); cvCvtColor( dst, color_dst, CV_GRAY2BGR ); #if 1 lines = cvHoughLines2( dst, storage, CV_HOUGH_STANDARD, 1, CV_PI/180, 30, 0, 0 ); for( i = 0; i < MIN(lines->total,100); i++ ) { float* line = (float*)cvGetSeqElem(lines,i); float rho = line[0]; float theta = line[1]; printf("theta = %f",(theta*180/3.142)); CvPoint pt1, pt2; double a = cos(theta), b = sin(theta); double x0 = a*rho, y0 = b*rho; printf("a= %f b=%f x0=%f y0=%f roh=%f\n", a,b,x0,y0,rho); pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); printf(" x1 = %d, y1 = %d",pt1.x,pt1.y); printf(" x2 = %d, y2 = %d\n\n",pt2.x,pt2.y); //if((theta*180/3.142) < 100 && (theta*180/3.142) > 79 ) cvLine( color_dst, pt1, pt2, CV_RGB(255,0,0), 3, 8 ); } #else lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 30, 0, 0 ); for( i = 0; i < lines->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 3, 8 ); } #endif cvNamedWindow( "Hough", 1 ); cvShowImage( "Hough", color_dst ); } /* */ }
int _tmain(int argc, _TCHAR* argv[]) { //CvCapture *capture = cvCaptureFromCAM(0); int t = 10, n=0; double time = 0; while(1) { clock_t begin=clock(); //------------------------ Original Camera Feed -----------------------------------------// //IplImage *img = cvQueryFrame(capture); IplImage *img = cvLoadImage("../../Data/13 Oct/img13.jpg"); //cvNamedWindow("Camera Feed", 0); //cvShowImage("Camera Feed", img); //------------------------ Finding Gray Scale Image -------------------------------------// IplImage *gray = cvCreateImage(cvGetSize(img), 8, 1); cvCvtColor(img, gray, CV_BGR2GRAY); //cvReleaseImage(&img); //------------------------ Removing Salt and Pepper Noise -------------------------------// //cvSmooth(gray, gray, CV_MEDIAN, 3, 3, 0.0, 0.0); // For really poor images cvMorphologyEx(gray, gray, NULL, NULL, CV_MOP_OPEN, 1); // Removes Salt Noise for(int y = 0; y < gray->height; y++){ uchar* ptr = (uchar*) (gray->imageData + y * gray->widthStep); //implement inversion and pepper removal in single pass for(int x = 0; x < gray->width; x++) ptr[x] = 255-ptr[x]; // Pepper becomes Salt } cvMorphologyEx(gray, gray, NULL, NULL, CV_MOP_OPEN, 1); // Removes Pepper Noise //------------------------ Finding Gradient Image ---------------------------------------// int **A = (int **) calloc(gray->height, sizeof(int)); IplImage *gradient = cvCreateImage(cvGetSize(gray), 8, 1); float min=255, max=0; for(int i=1; i<gray->height-1; i++) { A[i] = (int*) calloc(gray->width, sizeof(int)); uchar* ptr1 = (uchar*) (gray->imageData + (i-1)*gray->widthStep); uchar* ptr2 = (uchar*) (gray->imageData + i*gray->widthStep); uchar* ptr3 = (uchar*) (gray->imageData + (i+1)*gray->widthStep); uchar* ptr = (uchar*) (gradient->imageData + i*gradient->widthStep); for(int j=1; j<gray->width-1; j++) { A[i][j] = (int)ptr1[j] + (int)ptr2[j-1] + (int)(-4)*ptr2[j] + (int)ptr2[j+1] + (int)ptr3[j]; if(A[i][j] <= min) min = (int)A[i][j]; if(A[i][j] >= max) max = (int)A[i][j]; } } //printf("max:%f min:%f\n", max, min); int *bins = (int *)calloc(256, sizeof(int)); for(int i=0; i<256; i++) bins[i] = 0; for(int i=1; i<gradient->height-1; i++) { uchar* ptr = (uchar*) (gradient->imageData + i*gradient->widthStep); for(int j=1; j<gradient->width-1; j++) { ptr[j] = (int)(A[i][j]-min)/(max-min)*255; bins[ptr[j]]++; } free(A[i]); } free(A); //------------ Applying 90 Percentile Threshold to get a Laplacian Edge Mask ------------// float sum=0; int pth=255; float percentile = 10; while((pth >= 0)&&(sum < gradient->imageSize*percentile/100)) sum += bins[pth--]; //printf("Percentile Threshold:%d\n", pth); //--------------- Applying the Laplacian Edge Mask to Original Gray Image ---------------// for(int i=0; i<256; i++) bins[i] = 0; for(int y = 0; y < gray->height; y++){ uchar* ptr = (uchar*) (gray->imageData + y * gray->widthStep); uchar* ptr1 = (uchar*) (gradient->imageData + y * gradient->widthStep); for(int x = 0; x < gray->width; x++){ if(ptr1[x] > pth){ bins[ptr[x]]++; } } } cvReleaseImage(&gradient); //cvReleaseImage(&gray); //------------- Thresholding the Original Gray Image using The Otsu Threshold -----------// int threshold = 255-OtsuThreshold(bins, 256); free(bins); //------------- Filtering points to get better line fittings ----------------------------// //img = cvQueryFrame(capture); //img = cvLoadImage("../../Data/13 Oct/img4.jpg"); //IplImage *otsu = cvCreateImage(cvGetSize(img), 8, 1); cvCvtColor(img, gray, CV_BGR2GRAY); //cvReleaseImage(&img); for(int i=0, d=t; i<gray->height; i++) { int flag = 0; uchar* ptr = (uchar*) (gray->imageData + i*gray->widthStep); for(int j=0; j<gray->width; j++) if(ptr[j] > threshold) ptr[j] = 255; else ptr[j] = 0; for(int x = 0; x < gray->width; x++) if(ptr[x]==255) { for(int i=0; i<d; i++) ptr[x+i] = 0; ptr[x+d/2] = 255; break; } } //---------------------------------- Hough Lines ----------------------------------------// CvMemStorage* line_storage = cvCreateMemStorage(0); CvSeq* results = cvHoughLines2(gray, line_storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 40, 5, 200); cvReleaseImage(&gray); printf("lines:%d\tt=%d\n", results->total, t); float angle = 0.0, temp; for(int i = 0; i < results->total; i++) { CvPoint* line = (CvPoint*)cvGetSeqElem(results, i); //cvLine(img, line[0], line[1], CV_RGB(255,0,0), 3, CV_AA, 0); if(line[0].y > line[1].y) temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x)); else temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x)); if(temp < 0) angle += 90 + 180/3.14*temp; else angle += 180/3.14*temp - 90; } printf("angle: %f\n", angle = angle/results->total); cvReleaseMemStorage(&line_storage); //cvNamedWindow("Hough Lines", 0); //cvShowImage("Hough Lines", img); send(FORWARD, angle); clock_t end=clock(); time = diffclock(end,begin); printf("Time Taken: %f fps\n", 1000/(time)); int c = cvWaitKey(0); if(c=='a') t++; else if(c == 'z') t--; else if(c == 'x') { send(BRAKE, 0); break; } } int c = cvWaitKey(0); return 0; }
int main() { IplImage* img = cvLoadImage("goal_arena.bmp"); CvSize imgSize = cvGetSize(img); IplImage* detected = cvCreateImage(imgSize, 8, 1); IplImage* imgBlue = cvCreateImage(imgSize, 8, 1); IplImage* imgGreen = cvCreateImage(imgSize, 8, 1); IplImage* imgRed = cvCreateImage(imgSize, 8, 1); cvSplit(img, imgBlue, imgGreen, imgRed, NULL); cvAnd(imgGreen, imgBlue, detected); cvAnd(detected, imgRed, detected); cvErode(detected, detected); cvDilate(detected, detected); // Opening // cvThreshold(detected, detected, 100, 250, CV_THRESH_BINARY); CvMat* lines = cvCreateMat(100, 1, CV_32FC2); cvHoughLines2(detected, lines, CV_HOUGH_STANDARD, 1, 0.001, 100); // CvMat* lines = cvCreateMat(100, 1, CV_32FC2); // cvHoughLines2(detected, lines, CV_HOUGH_STANDARD, 1, 0.001, 100); CvPoint left1 = cvPoint(0, 0); CvPoint left2 = cvPoint(0, 0); CvPoint right1 = cvPoint(0, 0); CvPoint right2 = cvPoint(0, 0); CvPoint top1 = cvPoint(0, 0); CvPoint top2 = cvPoint(0, 0); CvPoint bottom1 = cvPoint(0, 0); CvPoint bottom2 = cvPoint(0, 0); int numLines = lines->rows; int numTop = 0; int numBottom = 0; int numLeft = 0; int numRight = 0; for(int i=0;i<numLines;i++) { CvScalar dat = cvGet1D(lines, i); double rho = dat.val[0]; double theta = dat.val[1]; if(theta==0.0) continue; double degrees = theta*180.0/(3.1412); CvPoint pt1 = cvPoint(0, rho/sin(theta)); CvPoint pt2 = cvPoint(img->width, (-img->width/tan(theta)) + rho/sin(theta)); if(abs(rho)<50.0) { if(degrees>45.0 && degrees<135.0) { numTop++; // The line is vertical and near the top top1.x+=pt1.x; top1.y+=pt1.y; top2.x+=pt2.x; top2.y+=pt2.y; } else { numLeft++; // The line is vertical and near the left left1.x+=pt1.x; left1.y+=pt1.y; left2.x+=pt2.x; left2.y+=pt2.y; } } else { // We're in the right portion if(degrees>45.0 && degrees<135.0) { numBottom++; //The line is horizontal and near the bottom bottom1.x+=pt1.x; bottom1.y+=pt1.y; bottom2.x+=pt2.x; bottom2.y+=pt2.y; } else { numRight++; // The line is vertical and near the right right1.x+=pt1.x; right1.y+=pt1.y; right2.x+=pt2.x; right2.y+=pt2.y; } } } left1.x/=numLeft; left1.y/=numLeft; left2.x/=numLeft; left2.y/=numLeft; right1.x/=numRight; right1.y/=numRight; right2.x/=numRight; right2.y/=numRight; top1.x/=numTop; top1.y/=numTop; top2.x/=numTop; top2.y/=numTop; bottom1.x/=numBottom; bottom1.y/=numBottom; bottom2.x/=numBottom; bottom2.y/=numBottom; cvLine(img, left1, left2, CV_RGB(255, 0,0), 1); cvLine(img, right1, right2, CV_RGB(255, 0,0), 1); cvLine(img, top1, top2, CV_RGB(255, 0,0), 1); cvLine(img, bottom1, bottom2, CV_RGB(255, 0,0), 1); // Next, we need to figure out the four intersection points double leftA = left2.y-left1.y; double leftB = left1.x-left2.x; double leftC = leftA*left1.x + leftB*left1.y; double rightA = right2.y-right1.y; double rightB = right1.x-right2.x; double rightC = rightA*right1.x + rightB*right1.y; double topA = top2.y-top1.y; double topB = top1.x-top2.x; double topC = topA*top1.x + topB*top1.y; double bottomA = bottom2.y-bottom1.y; double bottomB = bottom1.x-bottom2.x; double bottomC = bottomA*bottom1.x + bottomB*bottom1.y; // Intersection of left and top double detTopLeft = leftA*topB - leftB*topA; CvPoint ptTopLeft = cvPoint((topB*leftC - leftB*topC)/detTopLeft, (leftA*topC - topA*leftC)/detTopLeft); // Intersection of top and right double detTopRight = rightA*topB - rightB*topA; CvPoint ptTopRight = cvPoint((topB*rightC-rightB*topC)/detTopRight, (rightA*topC-topA*rightC)/detTopRight); // Intersection of right and bottom double detBottomRight = rightA*bottomB - rightB*bottomA; CvPoint ptBottomRight = cvPoint((bottomB*rightC-rightB*bottomC)/detBottomRight, (rightA*bottomC-bottomA*rightC)/detBottomRight); // Intersection of bottom and left double detBottomLeft = leftA*bottomB-leftB*bottomA; CvPoint ptBottomLeft = cvPoint((bottomB*leftC-leftB*bottomC)/detBottomLeft, (leftA*bottomC-bottomA*leftC)/detBottomLeft); cvLine(img, ptTopLeft, ptTopLeft, CV_RGB(0,255,0), 5); cvLine(img, ptTopRight, ptTopRight, CV_RGB(0,255,0), 5); cvLine(img, ptBottomRight, ptBottomRight, CV_RGB(0,255,0), 5); cvLine(img, ptBottomLeft, ptBottomLeft, CV_RGB(0,255,0), 5); IplImage* imgMask = cvCreateImage(imgSize, 8, 3); cvZero(imgMask); CvPoint* pts = new CvPoint[4]; pts[0] = ptTopLeft; pts[1] = ptTopRight; pts[2] = ptBottomRight; pts[3] = ptBottomLeft; cvFillConvexPoly(imgMask, pts, 4, cvScalar(255,255,255)); cvAnd(img, imgMask, img); cvNamedWindow("Original"); cvNamedWindow("Detected"); cvShowImage("Original", img); cvShowImage("Detected", detected); cvWaitKey(0); return 0; }
UINT WINAPI //DWORD WINAPI #elif defined(POSIX_SYS) // using pthread void * #endif ChessRecognition::HoughLineThread( #if defined(WINDOWS_SYS) LPVOID #elif defined(POSIX_SYS) void * #endif Param) { // 실제로 뒤에서 동작하는 windows용 thread함수. // 함수 인자로 클래스를 받아옴. ChessRecognition *_TChessRecognition = (ChessRecognition *)Param; _TChessRecognition->_HoughLineBased = new HoughLineBased(); CvSeq *_TLineX, *_TLineY; double _TH[] = { -1, -7, -15, 0, 15, 7, 1 }; CvMat _TDoGX = cvMat(1, 7, CV_64FC1, _TH); CvMat* _TDoGY = cvCreateMat(7, 1, CV_64FC1); cvTranspose(&_TDoGX, _TDoGY); // transpose(&DoGx) -> DoGy double _TMinValX, _TMaxValX, _TMinValY, _TMaxValY, _TMinValT, _TMaxValT; int _TKernel = 1; // Hough 사용되는 Image에 대한 Initialize. IplImage *iplTemp = cvCreateImage(cvSize(_TChessRecognition->_Width, _TChessRecognition->_Height), IPL_DEPTH_32F, 1); IplImage *iplDoGx = cvCreateImage(cvGetSize(iplTemp), IPL_DEPTH_32F, 1); IplImage *iplDoGy = cvCreateImage(cvGetSize(iplTemp), IPL_DEPTH_32F, 1); IplImage *iplDoGyClone = cvCloneImage(iplDoGy); IplImage *iplDoGxClone = cvCloneImage(iplDoGx); IplImage *iplEdgeX = cvCreateImage(cvGetSize(iplTemp), 8, 1); IplImage *iplEdgeY = cvCreateImage(cvGetSize(iplTemp), 8, 1); CvMemStorage* _TStorageX = cvCreateMemStorage(0), *_TStorageY = cvCreateMemStorage(0); while (_TChessRecognition->_EnableThread != false) { // 이미지를 받아옴. main루프와 동기를 맞추기 위해서 critical section 사용. _TChessRecognition->_ChessBoardDetectionInternalImageProtectMutex.lock(); //EnterCriticalSection(&(_TChessRecognition->cs)); cvConvert(_TChessRecognition->_ChessBoardDetectionInternalImage, iplTemp); //LeaveCriticalSection(&_TChessRecognition->cs); _TChessRecognition->_ChessBoardDetectionInternalImageProtectMutex.unlock(); // 각 X축 Y축 라인을 검출해 내기 위해서 filter 적용. cvFilter2D(iplTemp, iplDoGx, &_TDoGX); // 라인만 축출해내고. cvFilter2D(iplTemp, iplDoGy, _TDoGY); cvAbs(iplDoGx, iplDoGx); cvAbs(iplDoGy, iplDoGy); // 이미지 내부에서 최댓값과 최소값을 구하여 정규화. cvMinMaxLoc(iplDoGx, &_TMinValX, &_TMaxValX); cvMinMaxLoc(iplDoGy, &_TMinValY, &_TMaxValY); cvMinMaxLoc(iplTemp, &_TMinValT, &_TMaxValT); cvScale(iplDoGx, iplDoGx, 2.0 / _TMaxValX); // 정규화. cvScale(iplDoGy, iplDoGy, 2.0 / _TMaxValY); cvScale(iplTemp, iplTemp, 2.0 / _TMaxValT); cvCopy(iplDoGy, iplDoGyClone); cvCopy(iplDoGx, iplDoGxClone); // NMS진행후 추가 작업 _TChessRecognition->_HoughLineBased->NonMaximumSuppression(iplDoGx, iplDoGyClone, _TKernel); _TChessRecognition->_HoughLineBased->NonMaximumSuppression(iplDoGy, iplDoGxClone, _TKernel); cvConvert(iplDoGx, iplEdgeY); // IPL_DEPTH_8U로 다시 재변환. cvConvert(iplDoGy, iplEdgeX); double rho = 1.0; // distance resolution in pixel-related units. double theta = 1.0; // angle resolution measured in radians. int threshold = 20; if (threshold == 0) threshold = 1; // detecting 해낸 edge에서 hough line 검출. _TLineX = cvHoughLines2(iplEdgeX, _TStorageX, CV_HOUGH_STANDARD, 1.0 * rho, CV_PI / 180 * theta, threshold, 0, 0); _TLineY = cvHoughLines2(iplEdgeY, _TStorageY, CV_HOUGH_STANDARD, 1.0 * rho, CV_PI / 180 * theta, threshold, 0, 0); // cvSeq를 vector로 바꾸기 위한 연산. _TChessRecognition->_Vec_ProtectionMutex.lock(); _TChessRecognition->_HoughLineBased->CastSequence(_TLineX, _TLineY); _TChessRecognition->_Vec_ProtectionMutex.unlock(); Sleep(2); } // mat 할당 해제. cvReleaseMat(&_TDoGY); // 내부 연산에 사용된 이미지 할당 해제. cvReleaseImage(&iplTemp); cvReleaseImage(&iplDoGx); cvReleaseImage(&iplDoGy); cvReleaseImage(&iplDoGyClone); cvReleaseImage(&iplDoGxClone); cvReleaseImage(&iplEdgeX); cvReleaseImage(&iplEdgeY); // houghline2에 사용된 opencv 메모리 할당 해제. cvReleaseMemStorage(&_TStorageX); cvReleaseMemStorage(&_TStorageY); delete _TChessRecognition->_HoughLineBased; #if defined(WINDOWS_SYS) _endthread(); #elif defined(POSIX_SYS) #endif _TChessRecognition->_EndThread = true; return 0; }
void main(int argc, char** argv) { cvNamedWindow("src",0 ); cvNamedWindow("warp image",0 ); cvNamedWindow("warp image (grey)",0 ); cvNamedWindow("Smoothed warped gray",0 ); cvNamedWindow("threshold image",0 ); cvNamedWindow("canny",0 ); cvNamedWindow("final",1 ); CvPoint2D32f srcQuad[4], dstQuad[4]; CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1); float Z=1; dstQuad[0].x = 216; //src Top left dstQuad[0].y = 15; dstQuad[1].x = 392; //src Top right dstQuad[1].y = 6; dstQuad[2].x = 12; //src Bottom left dstQuad[2].y = 187; dstQuad[3].x = 620; //src Bot right dstQuad[3].y = 159; srcQuad[0].x = 100; //dst Top left srcQuad[0].y = 120; srcQuad[1].x = 540; //dst Top right srcQuad[1].y = 120; srcQuad[2].x = 100; //dst Bottom left srcQuad[2].y = 360; srcQuad[3].x = 540; //dst Bot right srcQuad[3].y = 360; cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix); //CvCapture *capture = cvCaptureFromCAM(0); /*double fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS); IplImage* image = cvRetrieveFrame(capture); CvSize imgSize; imgSize.width = image->width; imgSize.height = image->height; CvVideoWriter *writer = cvCreateVideoWriter("out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, imgSize);*/ int ik=0; while(1) { //IplImage* img = cvQueryFrame(capture); IplImage* img = cvLoadImage( "../../Data/6 Dec/009.jpg", CV_LOAD_IMAGE_COLOR); cvShowImage( "src", img ); //cvWriteFrame(writer, img); //cvSaveImage(nameGen(ik++), img, 0); IplImage* warp_img = cvCloneImage(img); CV_MAT_ELEM(*warp_matrix, float, 2, 2) = Z; cvWarpPerspective(img, warp_img, warp_matrix, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS); cvShowImage( "warp image", warp_img ); IplImage* grayimg = cvCreateImage(cvGetSize(warp_img),IPL_DEPTH_8U,1); cvCvtColor( warp_img, grayimg, CV_RGB2GRAY ); cvShowImage( "warp image (grey)", grayimg ); cvSmooth(grayimg, grayimg, CV_GAUSSIAN, 3, 3, 0.0, 0.0); cvShowImage( "Smoothed warped gray", grayimg ); IplImage* thresholded_img=simplethreshold(grayimg, 220); cvShowImage("threshold image",thresholded_img); //grayimg = doCanny( thresholded_img, 50, 100, 3 ); grayimg = cvCloneImage(thresholded_img); cvShowImage("canny",grayimg); IplImage* finalimg = cvCreateImage(cvGetSize(grayimg),IPL_DEPTH_8U,3); CvMemStorage* line_storage=cvCreateMemStorage(0); CvSeq* results = cvHoughLines2(grayimg,line_storage,CV_HOUGH_PROBABILISTIC,10,CV_PI/180*5,350,100,10); double angle = 0.0, temp; double lengthSqd, wSum=0; double xc = 0, yc = 0; for( int i = 0; i < results->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(results,i); cvLine( finalimg, line[0], line[1], CV_RGB(0,0,255), 1, CV_AA, 0 ); //lengthSqd = (line[0].x - line[1].x)*(line[0].x - line[1].x) + (line[0].y - line[1].y)*(line[0].y - line[1].y); wSum += 1;//lengthSqd; if(line[0].y > line[1].y) temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x)); else temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x)); if(temp < 0) angle += (90 + 180/3.14*temp)/* * lengthSqd*/; else angle += (180/3.14*temp - 90)/* * lengthSqd*/; xc += line[0].x + line[1].x; yc += line[0].y + line[1].y; } angle=angle/wSum; //angle+=10; printf("total: %d, angle: % f\n", results->total, angle); xc /= 2*results->total; yc /= 2*results->total; double m = (angle != 0) ? 1/tan(angle*3.14/180) : 100; // 100 represents a very large slope (near vertical) m=-m; double x1, y1, x2, y2; // The Center Line y1 = 0; y2 = finalimg->height; x1 = xc + (y1-yc)/m; x2 = xc + (y2-yc)/m; cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(0,255,0), 1, CV_AA, 0); printf("point: %f\t%f\n", xc, yc); double lx=0, ly=0, lm=0, lc=0, rx=0, ry=0, rm=0, rc=0; for( int i = 0; i < results->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(results,i); double xm = (line[0].x + line[1].x)/2.0, ym = (line[0].y + line[1].y)/2.0; if(ym - yc - m*(xm - xc) > 0) { lx += xm; ly += ym; lm += (line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001); lc++; } else { rx += xm; ry += ym; rm += (line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001); rc++; } } // The Left Line lx /= lc; ly /= lc; lm /= lc; rx /= rc; ry /= rc; rm /= rc; printf("lins: %f\t%f\t%f\n", lx, ly, lm); printf("lins: %f\t%f\t%f\n", rx, ry, rm); y1 = 0; y2 = finalimg->height-5; x1 = lx + (y1-ly)/lm; x2 = lx + (y2-ly)/lm; cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(255,255,0), 1, CV_AA, 0); // The Right Line y1 = 0; y2 = finalimg->height-5; x1 = rx + (y1-ry)/rm; x2 = rx + (y2-ry)/rm; cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(0,255,255), 1, CV_AA, 0); // The Center Point CvPoint vpt = cvPoint(finalimg->width/2, 416); printf("center point: %d\t%d\n", vpt.x, vpt.y); // The Dl and Dr int dl = vpt.x - lx + (ly-vpt.y+0.0)/lm; int dr = (vpt.y-ry+0.0)/rm + rx - vpt.x; printf("dl-dr: %d\n", dl-dr); cvShowImage("final",finalimg); if(dl-dr < SAFEZONE_LL) // Assume that the bot lies just on the boundary of the safe zone { if(angle < -10) { navCommand(7, angle); } else { navCommand(7, angle); } } else if(dl-dr > SAFEZONE_RL) { if(angle > 10) { navCommand(-7, angle); } else { navCommand(-7, angle); } } else { if((angle < 10) && (angle > -10)) { navCommand(angle, angle); } else { navCommand(0, angle); } } cvWaitKey(0); } }
void MaizeDetector::processLaserScan( const sensor_msgs::LaserScan::ConstPtr& laser_scan) { float rthetamean = 0, rrhomean = 0, lthetamean = 0, lrhomean = 0, theta = 0, rho = 0; double x0 = 0, y0 = 0, a, b; int lmc = 0, rmc = 0; static int right_count = 0; static int left_count = 0; clearRawImage(); tf::TransformListener listener_; sensor_msgs::PointCloud cloud; try { projector_.projectLaser(*laser_scan, cloud); // projector_.transformLaserScanToPointCloud("base_link",*laser_scan,cloud,listener_); } catch (tf::TransformException& e) { std::cout << e.what(); return; } //ROS_INFO("Got it right"); int size = cloud.points.size(); for (int i = 0; i < size; i++) { //if (((abs(cloud.points[i].y))-((cloud.points[i].x-300)*0.5))<0.65) { if (abs((cloud.points[i].y))<0.65 && cloud.points[i].x >0 ) { point1.x = ((int)(cloud.points[i].x * 50) + 300); point1.y = ((int)(cloud.points[i].y * 50) + 300); point2.x = point1.x-4; point2.y = point1.y; cvLine(rawData_, point1, point2, CV_RGB(255,255,255), 3, CV_AA, 0); } } cvCvtColor(rawData_, workingImg_, CV_BGR2GRAY); cvDilate(rawData_,rawData_,NULL,6); cvErode(rawData_,rawData_,NULL,4); lines_ = cvHoughLines2(workingImg_, storage_, CV_HOUGH_STANDARD, 1, CV_PI/ 180, 60, 0, 0); //lines_ = cvHoughLines2(workingImg_, storage_, CV_HOUGH_PROBABILISTIC, 1, CV_PI/360, 30, 10, 30); for(int i = 0; i < MIN(lines_->total,20); i++){ //for (int i = 0; i < MIN(lines_->total,15); i++) { float* line = (float*) cvGetSeqElem(lines_, i); rho = line[0]; theta = line[1]; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)); point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)); point2.y = cvRound(y0 - 600 * (a)); point3.x = 300, point3.y = 300; point4.x = 300, point4.y = 600; point5.x = 300, point5.y = 0; //cvLine(rawData_, point1, point2, CV_RGB(255,0,0), 1, CV_AA,0); cvLine(rawData_, point3, point4, CV_RGB(0,0,255), 1, CV_AA, 0); cvLine(rawData_, point3, point5, CV_RGB(0,0,255), 1, CV_AA, 0); if (intersect(point1, point2, point3, point4)) { { rrhomean += rho; rthetamean += theta; rmc++; //cvLine(workingImg_, point1, point2, CV_RGB(0,0,255), 1, CV_AA,0); } } else if (intersect(point1, point2, point3, point5)) { { lrhomean += rho; lthetamean += theta; lmc++; //cvLine(workingImg_, point1, point2, CV_RGB(255,255,255), 1,CV_AA, 0); } } } theta = lthetamean / lmc; rho = lrhomean / lmc; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)), point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)), point2.y = cvRound(y0 - 600 * (a)); //cvLine(rawData_, point1, point2, CV_RGB(255,0,0), 3, CV_AA, 0); point4.x = 300; point4.y = 300; point5.x = point4.x + 800 * sin(CV_PI - (theta - CV_PI / 2)); point5.y = point4.y + 800 * cos(CV_PI - (theta - CV_PI / 2)); cvLine(rawData_, point5, point4, CV_RGB(255,255,255), 1, CV_AA, 0); rows_.header.stamp = ros::Time::now(); rows_.leftvalid = false; rows_.rightvalid = false; //detect valid lines if (intersect(point1, point2, point4, point5)) { right_distance_ = sqrt(((intersection_.y - 300) * (intersection_.y- 300)) + ((intersection_.x - 300) * (intersection_.x - 300)))* 2; right_angle_ = (theta) - CV_PI / 2; right_count++; right_angle_rolling_mean_[right_count%10] = right_angle_; right_dist_rolling_mean_[right_count%10] = right_distance_; right_angle_ = 0; right_distance_ = 0; for(int i=0;i < 10;i++){ right_angle_ += right_angle_rolling_mean_[i]; right_distance_ += right_dist_rolling_mean_[i]; } right_angle_ = right_angle_/10; right_distance_ = right_distance_ /10; ROS_WARN("right_distance_: %f",right_distance_); cvLine(rawData_, point1, point2, CV_RGB(0,255,0), 1, CV_AA, 0); marker_r.header.frame_id = "/laser_front_link"; marker_r.header.stamp = ros::Time::now(); marker_r.ns = "kf_shapes"; // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER marker_r.type = visualization_msgs::Marker::CUBE; // Set the marker action. Options are ADD and DELETE marker_r.action = visualization_msgs::Marker::ADD; // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header marker_r.id = 2; geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(right_angle_); marker_r.pose.position.x = 0; marker_r.pose.position.y = -((float) right_distance_) / 100; marker_r.pose.position.z = 0; marker_r.pose.orientation = pose_quat; // Set the scale of the marker -- 1x1x1 here means 1m on a side marker_r.scale.x = 10.0; marker_r.scale.y = 0.1; marker_r.scale.z = 0.5; marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 0.0f; marker_r.color.a = 0.5; marker_r.lifetime = ros::Duration(.1); marker_r_pub.publish(marker_r); // Set the color -- be sure to set alpha to something non-zero! // Publish the marker rows_.rightvalid = true; rows_.rightdistance = ((float) right_distance_) / 100; rows_.rightangle = right_angle_; }else{ rows_.rightvalid = false; rows_.rightdistance = 0; rows_.rightangle = 0; } theta = rthetamean / rmc; rho = rrhomean / rmc; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)), point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)), point2.y = cvRound(y0 - 600 * (a)); point4.x = 300; point4.y = 300; point5.x = point4.x - 800 * sin(CV_PI - (theta - CV_PI / 2)); point5.y = point4.y - 800 * cos(CV_PI - (theta - CV_PI / 2)); cvLine(rawData_, point5, point4, CV_RGB(255,255,255), 1, CV_AA, 0); //detect valid lines if (intersect(point1, point2, point4, point5)) { left_distance_ = sqrt(((intersection_.y - 300) * (intersection_.y- 300)) + ((intersection_.x - 300) * (intersection_.x - 300)))* 2; left_angle_ = (theta) - CV_PI / 2; left_count++; left_angle_rolling_mean_[left_count%10] = left_angle_; left_dist_rolling_mean_[left_count%10] = left_distance_; left_angle_ = 0; left_distance_ = 0; for(int i=0;i < 10;i++){ left_angle_ += left_angle_rolling_mean_[i]; left_distance_ += left_dist_rolling_mean_[i]; } left_angle_ = left_angle_/10; left_distance_ = left_distance_ /10; ROS_WARN("left_distance_: %f",left_distance_); marker_r.id = 1; geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(left_angle_); marker_r.color.r = 0.0f; marker_r.color.g = 0.0f; marker_r.color.b = 1.0f; marker_r.pose.position.x = 0; marker_r.pose.position.y = ((float) left_distance_) / 100; marker_r.pose.position.z = 0; marker_r.pose.orientation = pose_quat; marker_r_pub.publish(marker_r); //rolling_mean_[count%10] = right_angle_; //left_angle_ = 0; //for(int i=0;i < 10;i++){ // left_angle_ += rolling_mean_[i]; //} //right_angle_ = right_angle_/10; cvLine(rawData_, point1, point2, CV_RGB(0,255,255), 1, CV_AA, 0); rows_.leftvalid = true; rows_.leftdistance = ((float) left_distance_) / 100; rows_.leftangle = left_angle_; }else{ rows_.leftvalid = false; rows_.leftdistance = 0; rows_.leftangle = 0; } if (rows_.leftvalid && rows_.rightvalid){ e_angle = (rows_.leftangle + rows_.rightangle) / 2; e_distance = (rows_.leftdistance - rows_.rightdistance); row_dist_est =( (rows_.leftdistance + rows_.rightdistance) + row_dist_est)/2; ROS_INFO("2ROWS row_dist_est %f, e_dist %f",row_dist_est,e_distance); rows_.error_angle = e_angle; rows_.error_distance = e_distance; rows_.var = 5^2; marker_r.id = 3; geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle); marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 1.0f; marker_r.pose.orientation = pose_quat; marker_r.pose.position.x = 0; marker_r.pose.position.y = (e_distance); marker_r.pose.position.z = 0; marker_r_pub.publish(marker_r); }else if (rows_.leftvalid && !rows_.rightvalid){ e_angle = (rows_.leftangle); e_distance = 0;//row_dist_est/2-(rows_.leftdistance); //ROS_INFO("e_angle %f, e_dist %f",e_angle,e_distance); rows_.error_angle = e_angle; rows_.error_distance = e_distance;//e_distance-(0.75/2); rows_.var = 10^2; marker_r.id = 3; ROS_INFO("LEFTROW row_dist_est %f, e_dist %f",row_dist_est,e_distance); geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle); marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 1.0f; marker_r.pose.orientation = pose_quat; marker_r.pose.position.x = 0; marker_r.pose.position.y = (e_distance); marker_r.pose.position.z = 0; marker_r_pub.publish(marker_r); }else if (!rows_.leftvalid && rows_.rightvalid){ e_angle = (rows_.rightangle); e_distance = 0;//row_dist_est/2-(rows_.rightdistance); //ROS_INFO("e_angle %f, e_dist %f",e_angle,e_distance); ROS_INFO("LRIGHTROW row_dist_est %f, e_dist %f",row_dist_est,e_distance); rows_.error_angle = (0.75/2)-e_angle; rows_.error_distance = e_distance;//e_distance-(0.75/2); rows_.var = 10^2; marker_r.id = 3; geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle); marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 1.0f; marker_r.pose.orientation = pose_quat; marker_r.pose.position.x = 0; marker_r.pose.position.y = (e_distance); marker_r.pose.position.z = 0; marker_r_pub.publish(marker_r); }else{ e_angle = 0; e_distance = 0; ROS_INFO("e_angle %f, e_dist %f",e_angle,e_distance); ROS_INFO("NOROW row_dist_est %f, e_dist %f",row_dist_est,e_distance); rows_.error_angle = e_angle; rows_.error_distance = e_distance; rows_.var = 4000^2; marker_r.id = 3; geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle); marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 1.0f; marker_r.pose.orientation = pose_quat; marker_r.pose.position.x = 0; marker_r.pose.position.y = (e_distance); marker_r.pose.position.z = 0; marker_r_pub.publish(marker_r); } //cvLine(rawData_, cvPoint(0,300+150), cvPoint(600,300+150), CV_RGB(255,255,255), 1, CV_AA, 0); //cvLine(rawData_, cvPoint(0,300-150), cvPoint(600,300-150), CV_RGB(255,255,255), 1, CV_AA, 0); row_pub.publish(rows_); //cvShowImage("TEST", rawData_); //cvWaitKey(10); //pc_pub.publish(cloud); }
unsigned short int* find2 (IplImage* frame2) { //--------------------------------Область алгоритма--------------------------------------------------- unsigned short int xy[6]={0}; int up = brightness_spot (frame2, (frame2->width/2)-block_size/2, delta_edge); int right = brightness_spot (frame2, frame2->width-block_size-delta_edge, (frame2->height/2)-block_size/2); int down = brightness_spot (frame2, (frame2->width/2)-block_size/2, frame2->height-block_size-delta_edge); int left = brightness_spot (frame2, delta_edge, (frame2->height/2)-block_size/2); if ( up>=getW-delta_brightness_max || up<=getB+delta_brightness_max) { xy[0]++; xy[1]=1; CvPoint pt = cvPoint( cvRound( frame2->width/2 ), cvRound( delta_edge*3 ) ); cvCircle(frame2, pt, cvRound( Radius ), CV_RGB(255,0,0), 10 ); if ( up>=getW-delta_brightness_max ) { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(255,255,255), 5 ); } else { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(0,0,0), 5 ); } } //if ( abs(right-w)<=delta_brightness_max || abs(right-b)<=delta_brightness_max) if ( right>=getW-delta_brightness_max || right<=getB+delta_brightness_max) { xy[0]++; xy[2]=1; CvPoint pt = cvPoint( cvRound(frame2->width - delta_edge*3 ), cvRound( frame2->height/2 ) ); cvCircle(frame2, pt, cvRound( Radius ), CV_RGB(255,0,0), 10 ); if ( right>=getW-delta_brightness_max) { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(255,255,255), 5 ); } else { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(0,0,0), 5 ); } } //if ( abs(down-w)<=delta_brightness_max || abs(down-b)<=delta_brightness_max) if ( down>=getW-delta_brightness_max || down<=getB+delta_brightness_max) { xy[0]++; xy[3]=1; CvPoint pt = cvPoint( cvRound( frame2->width/2 ), cvRound(frame2->height - delta_edge*3 ) ); cvCircle(frame2, pt, cvRound( Radius ), CV_RGB(255,0,0), 10 ); if ( down>=getW-delta_brightness_max) { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(255,255,255), 5 ); } else { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(0,0,0), 5 ); } } //if ( abs(left-w)<=delta_brightness_max || abs(left-b)<=delta_brightness_max) if ( left>=getW-delta_brightness_max || left<=getB+delta_brightness_max) { xy[0]++; xy[4]=1; CvPoint pt = cvPoint( cvRound( delta_edge*3 ), cvRound(frame2->height/2 ) ); cvCircle(frame2, pt, cvRound( Radius ), CV_RGB(255,0,0), 10 ); if (left>=getW-delta_brightness_max) { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(255,255,255), 5 ); } else { cvCircle(frame2, pt, cvRound( Radius/2 ), CV_RGB(0,0,0), 5 ); } } // if(xy[0]>0)//Поиск перпендикулярных линий. // { class Vect///Ограничение на количество найденных линий!!! Может быть ошибка, если вдруг превысит. { public: int x[1000], y[1000]; Vect() { for (int i=0; i<1000; i++) { x[i]=0; y[i]=0; } } } Vec; IplImage* dst=0; // хранилище памяти для хранения найденных линий CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; int i = 0; dst = cvCreateImage( cvGetSize(frame2), 8, 1 ); // детектирование границ cvCanny( frame2, dst, 50, 100, 3 ); // конвертируем в цветное изображение //cvCvtColor( dst, color_dst, CV_GRAY2BGR ); // нахождение линий lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 50, 200, 50 ); std::list<short unsigned int> MXX,MYY; // Нарисуем найденные линии и найдём соответствующие им вектора. for( i = 0; i < lines->total; i++ ){ CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( frame2, line[0], line[1], CV_RGB(255,0,0), 3, CV_AA, 0 ); Vec.x[i]=line[1].x-line[0].x;///////// Для каждого отрезка найдём вектор (xn-ck,yn-yk) Vec.y[i]=line[1].y-line[0].y; MXX.push_back(line[0].x); MXX.push_back(line[1].x); MYY.push_back(line[0].y); MYY.push_back(line[1].y); } int countPer=0; const short int Os=100; for (int i=0; i<lines->total; i++) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); /////for (std::list<short unsigned int>::iterator x2=VX.begin(),std::list<short unsigned int>::iterator y2=VY.begin();x2!=VX.end(),y2!=VY.end();x2++,y2++) for (int j=i+1; j<lines->total; j++) { CvPoint* line2 = (CvPoint*)cvGetSeqElem(lines,j); if ( ((Vec.x[i])*(Vec.x[j])+(Vec.y[i])*(Vec.y[j]))>=-Os && ((Vec.x[i])*(Vec.x[j])+(Vec.y[i])*(Vec.y[j]))<=Os)//Перпендикулярность { countPer++; } } } if (lines->total!=0 && countPer>4) { int MX=0,MY=0; for(std::list<short unsigned int>::iterator i=MXX.begin();i!=MXX.end();i++) { MX+=(*i); } for(std::list<short unsigned int>::iterator i=MYY.begin();i!=MYY.end();i++) { MY+=(*i); } MX/=(lines->total*2); MY/=(lines->total*2); //const short int k2=10; //Рисуем крестик /*CvPoint* line=new CvPoint(); line[0].x=MX-k2; line[0].y=MY; line[1].x=MX+k2; line[1].y=MY; cvLine( frame2, line[0], line[1], CV_RGB(0,255,0), 3, CV_AA, 0 ); line[0].x=MX; line[0].y=MY-k2; line[1].x=MX; line[1].y=MY+k2; cvLine( frame2, line[0], line[1], CV_RGB(0,255,0), 3, CV_AA, 0 ); */ xy[5]=1; } // освобождаем ресурсы cvReleaseMemStorage(&storage); cvReleaseImage(&dst); MXX.clear(); MYY.clear(); // } //---------------------------------------------------------------------------------------------------- return xy; }
t_jit_err cv_jit_lines_matrix_calc(t_cv_jit_lines *x, void *inputs, void *outputs) { t_jit_err err = JIT_ERR_NONE; long in_savelock,out_savelock; t_jit_matrix_info in_minfo,out_minfo; char *in_bp,*out_bp; long i,dimcount,dim[JIT_MATRIX_MAX_DIMCOUNT]; void *in_matrix,*out_matrix; t_int32 *out; double thresh1, thresh2, theta, rho; int houghThresh; CvMat source; CvPoint *ln; in_matrix = jit_object_method(inputs,_jit_sym_getindex,0); out_matrix = jit_object_method(outputs,_jit_sym_getindex,0); if (x&&in_matrix&&out_matrix) { in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1); out_savelock = (long) jit_object_method(out_matrix,_jit_sym_lock,1); jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo); jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo); jit_object_method(in_matrix,_jit_sym_getdata,&in_bp); if (!in_bp) { err=JIT_ERR_INVALID_INPUT; goto out;} //compatible types? if (in_minfo.type!=_jit_sym_char) { err=JIT_ERR_MISMATCH_TYPE; goto out; } //compatible planes? if ((in_minfo.planecount!=1)||(out_minfo.planecount!=4)) { err=JIT_ERR_MISMATCH_PLANE; goto out; } //get dimensions/planecount dimcount = in_minfo.dimcount; for (i=0;i<dimcount;i++) { dim[i] = MIN(in_minfo.dim[i],out_minfo.dim[i]); } //Convert input matrix to OpenCV matrices cvJitter2CvMat(in_matrix, &source); //Adjust size of edge matrix if need be if((x->edges->rows != source.rows)||(x->edges->cols != source.cols)) { cvReleaseMat(&(x->edges)); x->edges = cvCreateMat( source.rows, source.cols, CV_8UC1 ); } //Calculate parameter values for Hough and Canny algorithms thresh1 = x->threshold - THRESHOLD_RANGE; thresh2 = x->threshold + THRESHOLD_RANGE; CLIP_ASSIGN(thresh1,0,255); CLIP_ASSIGN(thresh2,0,255); theta = CV_PI / (180 / (double)x->resolution); rho = (double)x->resolution; houghThresh = x->sensitivity; x->gap = MAX(0,x->gap); x->length = MAX(0,x->length); //calculate edges using Canny algorithm cvCanny( &source, x->edges, thresh1, thresh2, 3 ); //Find lines using the probabilistic Hough transform method x->lines = cvHoughLines2( x->edges, x->storage, CV_HOUGH_PROBABILISTIC, rho, theta, houghThresh, x->length, x->gap ); //Transfer line information to output matrix //First adjust matrix size out_minfo.dim[0] = x->lines->total; jit_object_method(out_matrix,_jit_sym_setinfo,&out_minfo); jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo); jit_object_method(out_matrix,_jit_sym_getdata,&out_bp); if (!out_bp) { err=JIT_ERR_INVALID_OUTPUT; goto out;} //Copy... out = (t_int32 *)out_bp; for( i = 0; i < x->lines->total; i++ ) { ln = (CvPoint*)cvGetSeqElem(x->lines,i); out[0] = ln[0].x; out[1] = ln[0].y; out[2] = ln[1].x; out[3] = ln[1].y; out+=4; } } else { return JIT_ERR_INVALID_PTR; } out: jit_object_method(out_matrix,gensym("lock"),out_savelock); jit_object_method(in_matrix,gensym("lock"),in_savelock); return err; }
void main(int argc, char** argv) { CvPoint2D32f srcQuad[4], dstQuad[4]; CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1); float Z=1; /*cvNamedWindow("img", CV_WINDOW_AUTOSIZE); cvNamedWindow("warp", CV_WINDOW_AUTOSIZE);*/ dstQuad[0].x = 250; //src Top left dstQuad[0].y = 100; dstQuad[1].x = 430; //src Top right dstQuad[1].y = 115; dstQuad[2].x = 50; //src Bottom left dstQuad[2].y = 170; dstQuad[3].x = 630; //src Bot right dstQuad[3].y = 250; int lOff = 50, tOff = 150; srcQuad[0].x = tOff; //dst Top left srcQuad[0].y = lOff; srcQuad[1].x = 640-tOff; //dst Top right srcQuad[1].y = lOff; srcQuad[2].x = tOff; //dst Bottom left srcQuad[2].y = 480-lOff; srcQuad[3].x = 640-tOff; //dst Bot right srcQuad[3].y = 480-lOff; cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix); int ik=0, ni = 0, niX = 22-1; char names[22][25] = { "../../Data/6 Dec/009.jpg", "../../Data/6 Dec/011.jpg", "../../Data/6 Dec/012.jpg", "../../Data/6 Dec/016.jpg", "../../Data/6 Dec/018.jpg", "../../Data/6 Dec/019.jpg", "../../Data/6 Dec/020.jpg", "../../Data/6 Dec/022.jpg", "../../Data/6 Dec/024.jpg", "../../Data/6 Dec/064.jpg", "../../Data/6 Dec/065.jpg", "../../Data/6 Dec/066.jpg", "../../Data/6 Dec/067.jpg", "../../Data/6 Dec/068.jpg", "../../Data/6 Dec/069.jpg", "../../Data/6 Dec/070.jpg", "../../Data/6 Dec/071.jpg", "../../Data/6 Dec/072.jpg", "../../Data/6 Dec/073.jpg", "../../Data/6 Dec/074.jpg", "../../Data/6 Dec/075.jpg", "../../Data/6 Dec/076.jpg" }; int lwSum = 0, nopf = 0; //CvCapture *capture = cvCaptureFromCAM(0); /*double fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS); IplImage* image = cvRetrieveFrame(capture); CvSize imgSize; imgSize.width = image->width; imgSize.height = image->height; CvVideoWriter *writer = cvCreateVideoWriter("out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, imgSize);*/ while(1) { //IplImage* img = cvQueryFrame(capture); IplImage* img = cvLoadImage( "../../Data/23 Jan/c.jpg", CV_LOAD_IMAGE_COLOR); //cvSaveImage(nameGen(ik++), img, 0); //cvShowImage("img", img); IplImage* warp_img = cvCloneImage(img); CV_MAT_ELEM(*warp_matrix, float, 2, 2) = Z; cvWarpPerspective(img, warp_img, warp_matrix, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS); //cvReleaseImage(&img); //cvWaitKey(0); IplImage* grayimg = cvCreateImage(cvGetSize(warp_img),IPL_DEPTH_8U,1); cvCvtColor( warp_img, grayimg, CV_RGB2GRAY ); cvReleaseImage(&warp_img); cvSmooth(grayimg, grayimg, CV_GAUSSIAN, 3, 3, 0.0, 0.0); cvEqualizeHist(grayimg, grayimg); cvThreshold(grayimg, grayimg, PercentileThreshold(grayimg, 10.0), 255, CV_THRESH_BINARY); IplImage* finalimg = cvCreateImage(cvGetSize(grayimg),IPL_DEPTH_8U,3); CvMemStorage* line_storage=cvCreateMemStorage(0); CvSeq* results = cvHoughLines2(grayimg,line_storage,CV_HOUGH_PROBABILISTIC,10,CV_PI/180*5,350,100,10); cvReleaseImage(&grayimg); double angle = 0.0, temp; double lengthSqd, wSum=0; CvPoint center = cvPoint(0, 0); for( int i = 0; i < results->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(results,i); //lengthSqd = (line[0].x - line[1].x)*(line[0].x - line[1].x) + (line[0].y - line[1].y)*(line[0].y - line[1].y); wSum += 1;//lengthSqd; if(line[0].y > line[1].y) temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x)); else temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x)); if(temp < 0) angle += (90 + 180/3.14*temp)/* * lengthSqd*/; else angle += (180/3.14*temp - 90)/* * lengthSqd*/; center.x += (line[0].x + line[1].x)/2; center.y += (line[0].y + line[1].y)/2; } angle /= wSum; // Angle Direction: Left == -ve and Right == +ve // Angle is calculated w.r.t Vertical //angle+=10; // Angle Offset (Depends on camera's position) center.x /= results->total; center.y /= results->total; double m = (angle != 0) ? tan(CV_PI*(0.5-angle/180)) : 100000; // 100000 represents a very large slope (near vertical) //m=-m; // Slope Correction CvPoint leftCenter = cvPoint(0, 0), rightCenter = cvPoint(0, 0); double leftSlope = 0, rightSlope = 0, leftCount = 0, rightCount = 0; for( int i = 0; i < results->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(results,i); CvPoint midPoint = cvPoint((line[0].x + line[1].x)/2, (line[0].y + line[1].y)/2); double L11 = (0-center.y + m*(0-center.x + 0.0))/m; double L22 = (midPoint.y-center.y + m*(midPoint.x-center.x + 0.0))/m; if(L11*L22 > 0) { leftCenter.x += midPoint.x; leftCenter.y += midPoint.y; leftSlope += -(line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001); leftCount++; } else { rightCenter.x += midPoint.x; rightCenter.y += midPoint.y; rightSlope += -(line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001); rightCount++; } } cvReleaseMemStorage(&line_storage); leftCenter.x /= leftCount; leftCenter.y /= leftCount; leftSlope /= leftCount; rightCenter.x /= rightCount; rightCenter.y /= rightCount; rightSlope /= rightCount; CvPoint botCenter = cvPoint(finalimg->width/2, finalimg->height); int dL = abs(botCenter.y-leftCenter.y + m * (botCenter.x-leftCenter.x)) / sqrt(m*m + 1); int dR = abs(botCenter.y-rightCenter.y + m * (botCenter.x-rightCenter.x)) / sqrt(m*m + 1); int lw = abs((leftCenter.y - rightCenter.y) + m*(leftCenter.x - rightCenter.x)) / sqrt(m*m + 1); lwSum += lw; nopf++; if(lw <= SINGLE_LANE_WIDTH) { double L11 = (0-leftCenter.y + m*(0-leftCenter.x + 0.0))/m; double L22 = (botCenter.y-leftCenter.y + m*(botCenter.x-leftCenter.x + 0.0))/m; if(L11*L22 < 0) dR = lwSum/nopf - dL; // Only Left Lane is visible else dL = lwSum/nopf - dR; // Only Right Lane is visible } //cvSaveImage("test.jpg", finalimg, 0); printf("Bot:\t(%d, %d, %.3f)\n", dL, (finalimg->height)/10, 90.0-angle); printf("Target:\t(%d, %d, %.3f)\n", (dL+dR)/2, (finalimg->height)*9/10, 90.0); location bot, target; bot.x = dL; bot.y = (finalimg->height)/10; bot.theta = 90.0-angle; target.x = (dL+dR)/2; target.y = (finalimg->height)*9/10; target.theta = 90.0; cvReleaseImage(&finalimg); list *ol = NULL, *cl = NULL; elem e,vare; e.l = bot; e.g = 0; e.h = 0; e.id = UNDEFINED; int n = 15; elem* np = loadPosData(n); while(1) { cl = append(cl, e); //printList(cl); if(isNear(e.l, target)) break; ol = update(ol, e, target, np, n); //printList(ol); e = findMin(ol); //printf("Min: (%.3f, %.3f, %.3f, %d)\n", e.l.x, e.l.y, e.l.theta, e.id); ol = detach(ol, e); //printList(ol); //getchar(); } free(np); vare = e; printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id); while(!((abs(vare.l.x-bot.x) < 1.25) && (abs(vare.l.y-bot.y) < 1.25))) { vare=search(cl,vare.parent.x,vare.parent.y); if(vare.id != -1) { printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id); e = vare; } } printf("\n(%.3f, %.3f, %.3f) : %d\n", e.l.x, e.l.y, e.l.theta, e.id); //navCommand(10-e.id, e.id); releaseList(ol); releaseList(cl); getchar(); int c = cvWaitKey(0); if(c == '4') { if(ni != 0) ni--; } else if(c == '6') { if(ni != niX) ni++; } } }
bool Robot::adjustWorldCoordinate(IplImage* image, double coordAdjustRate) { IplImage *img; IplImage* src1=cvCreateImage(cvGetSize(image),IPL_DEPTH_8U,1); if(image->nChannels==3) { IplImage *hsv_img = get_hsv(image); img=worldMap.getField(hsv_img); cvReleaseImage(&hsv_img); src1=img; } else { img=image; src1=img; //cvCvtColor(img, src1, CV_BGR2GRAY); } if( img != 0 ) { IplImage* dst = cvCreateImage( cvGetSize(img), 8, 1 ); IplImage* color_dst = cvCreateImage( cvGetSize(img), 8, 3 ); CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* ls = 0; int i; cvCanny( src1, dst, 50, 200, 3 ); cvCvtColor( dst, color_dst, CV_GRAY2BGR ); ls = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 2, CV_PI/90, 20, 5, 30 ); //ls = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 5, CV_PI/30, 10, 20, 5 ); vector<myLine> tmplines; for( i = 0; i < ls->total; i++ ) { CvPoint* tmpl = (CvPoint*)cvGetSeqElem(ls,i); cvLine( color_dst, tmpl[0], tmpl[1], CV_RGB(255,0,0), 1, 8 ); cv::Point2f tmpp[2]; cv::Point2f scrPos(tmpl[0].x,tmpl[0].y); cv::Point2f roboPos=worldMap.coord_screen2robot(scrPos,true); cv::Point2f worldPos=worldMap.coord_robot2world(roboPos); tmpp[0]=worldPos; scrPos=cv::Point2f(tmpl[1].x,tmpl[1].y); roboPos=worldMap.coord_screen2robot(scrPos,true); worldPos=worldMap.coord_robot2world(roboPos); tmpp[1]=worldPos; myLine templ(tmpp[0],tmpp[1]); if(templ.l>LINE_LENGTH_LBOUND) tmplines.push_back(templ); // //printf("length=%f angle=%f\n",sqrt(float((tmpl[1].y-tmpl[0].y)*(tmpl[1].y-tmpl[0].y)); // +float((tmpl[1].x-tmpl[0].x)*(tmpl[1].x-tmpl[0].x))) // ,atan2(float(tmpl[1].y-tmpl[0].y),float(tmpl[1].x-tmpl[0].x))); } //printf("\n"); cvNamedWindow( "Source", 1 ); cvShowImage( "Source", img ); cvNamedWindow( "Hough", 1 ); cvShowImage( "Hough", color_dst ); cvWaitKey(10); cvReleaseImage(&dst); cvReleaseImage(&src1); cvReleaseImage(&color_dst); cvReleaseMemStorage(&storage); if(coordAdjustRate==0) { for(i=0;i<tmplines.size();++i) { lines.push_back(tmplines[i]); } } else if(coordAdjustRate==2) { for(i=0;i<tmplines.size();++i) { lines.push_back(tmplines[i]); } //vector<double> oris; vector<int> lineNums; vector<double> lineValues; int groupId=0; for(i=0;i<lines.size();++i) { bool classified=false; int j; for(j=0;j<i;++j) { double angle=lines[i].theta-lines[j].theta+CV_PI/4.0; //to make the process simple, add 45 degree //to turn the cared angles to the middle of a phase if(angle<0) angle+=CV_PI*2.0; int phase=(int)(angle/(CV_PI/2.0)); double angle90=angle-CV_PI/2.0*(double)phase; phase%=2; if(abs(angle90-CV_PI/4.0)<CV_PI/60.0)//subtract the added 45 degree { lines[i].clsId=lines[j].clsId/2*2+phase; ++lineNums[lines[i].clsId]; lineValues[lines[i].clsId]+=lines[i].l; classified=true; break; } } if(classified==false) { lines[i].clsId=groupId; lineNums.push_back(1); lineNums.push_back(0); lineValues.push_back(lines[i].l); lineValues.push_back(0); groupId+=2; } } int maxValueGroup=0; double maxValue=0; for(i=0;i<lineNums.size();i+=2) { if(lineValues[i]+lineValues[i+1]>maxValue) { maxValue=lineValues[i]+lineValues[i+1]; maxValueGroup=i; } } maxValueGroup/=2; double sumAngle=0; double sumL=0; for(i=0;i<lines.size();++i) { if(lines[i].clsId/2==maxValueGroup) { double angle=lines[i].theta+CV_PI/4.0;//similar strategy, add 45 degree if(angle<0) angle+=CV_PI*2.0; double angle90=angle-CV_PI/2.0*(double)((int)(angle/(CV_PI/2.0))); sumAngle+=(angle90-CV_PI/4.0)*lines[i].l;//subtract 45 degree sumL+=lines[i].l; } } if(sumL==0) { //printf("false 2 sumL=0\n"); return false; } mainAngle=sumAngle/sumL; mainGroupId=maxValueGroup; //printf("mainAngle=%f mainGroupId=%d\n",mainAngle,mainGroupId); } else if(coordAdjustRate==1) { CvRect bBox=worldMap.getMap_bbox(); //printf("in func param=1\n"); //printf("tmplines.size=%d\n",tmplines.size()); for(i=0;i<tmplines.size();++i) { cv::Point2f imgPos=world2image(tmplines[i].p[0]); if(!(imgPos.x>bBox.x-BBOX_DELTA && imgPos.x<bBox.x+bBox.width+BBOX_DELTA && imgPos.y>bBox.y-BBOX_DELTA && imgPos.y<bBox.y+bBox.height+BBOX_DELTA)) continue; bool classified=false; double minAngle=CV_PI; int minAnglePhase=0; int bestJ=-1; int j; for(j=0;j<lines.size();++j) { if(lines[j].clsId/2!=mainGroupId) continue; double angle=tmplines[i].theta-lines[j].theta+CV_PI/4.0; //to make the process simple, add 45 degree //to turn the cared angles to the middle of a phase if(angle<0) angle+=CV_PI*2.0; int phase=(int)(angle/(CV_PI/2.0)); double angle90=angle-CV_PI/2.0*(double)phase; phase%=2; if(abs(angle90-CV_PI/4.0)<minAngle)//subtract the added 45 degree { minAngle=abs(angle90-CV_PI/4.0); bestJ=j; minAnglePhase=phase; } } if(bestJ>-1) { //if(minAngle<CV_PI/6.0) tmplines[i].clsId=mainGroupId*2+minAnglePhase; classified=true; //printf("nearest main ori found. angle diff=%f\n",minAngle); } } double sumAngle=0; double sumL=0; for(i=0;i<tmplines.size();++i) { if(tmplines[i].clsId/2==mainGroupId) { //printf("comparing with a main line..i=%d\n",i); double angle=tmplines[i].theta+CV_PI/4.0;//similar strategy, add 45 degree if(angle<0) angle+=CV_PI*2.0; double angle90=angle-CV_PI/2.0*double((int)(angle/(CV_PI/2.0))); sumAngle+=angle90*tmplines[i].l;//use the 45 degree to balance the unwanted lines sumL+=tmplines[i].l; } } if(sumL<LINE_LENGTH_SUM_LBOUND) { //printf("false sumL=%f<%d\n",sumL,LINE_LENGTH_SUM_LBOUND); return false; } double curAngle=sumAngle/sumL-CV_PI/4.0;//subtract 45 degree ori+=curAngle-mainAngle; //printf("true oriChange=%f\n",curAngle-mainAngle); } } return true; }
//--------------------------------HOUGHLINES--------------------------------- void sendtoHoughLines(IplImage * img) { IplImage* src = cvCreateImage( cvGetSize(img), 8, 1 ); cvCvtColor(img, src, CV_RGB2GRAY); //-------------CREATING A IMAGE NAMED dst FOR EDGE SHOWING ON IT FROM CANNY RESULT-------------------- IplImage* dst = cvCreateImage( cvGetSize(src), 8, 1 ); //---------------CREATING color_dst IMAGE FOR RESULT OF LINE DISPLAY PURPOSE--------------------------- IplImage *color_dst = cvCreateImage( cvGetSize(src), 8, 3 ); //---------------CEATING A STORE POOL FOR LINE PURPOSE AND LINE DECLARATIVE PARAMETER------------- CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; int i; //---------------APPLYING THE CANNY FUNCTION ON SRC AND STORING THE EDGE RESULTS IN DST------------- cvCanny( src, dst,30, 90, 3 ); cvDilate( dst, dst, 0, 1 ); //--------------CONVERTING THE CANNY RESULT IMAGE DST INTO RGB AND STORE IT IN COLORDST AND SHOWING A WINDOW OF IT-------------- cvCvtColor( dst, color_dst, CV_GRAY2BGR ); /* //-----hough lines algo-------- lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 30, 30, 0 ); //---------------ACCESING THE POINTS OF THE LINES AND DRAWING THOSE LINES ON THE GRAY TO RGB CONVERTED IMAGE NAMED color_dst-------------- for( i = 0; i < lines->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 3, 8 ); //printf("\n i = %d x1 = %d y1 = %d x2 = %d y2 = %d ",i,line[0].x,line[0].y,line[1].x,line[1].y); } */ lines = cvHoughLines2( dst, storage, CV_HOUGH_STANDARD, 1, CV_PI/180, 30, 0, 0 ); for( i = 0; i < MIN(lines->total,100); i++ ) { float* line = (float*)cvGetSeqElem(lines,i); float rho = line[0]; float theta = line[1]; printf("theta = %f",(theta*180/3.142)); CvPoint pt1, pt2; double a = cos(theta), b = sin(theta); double x0 = a*rho, y0 = b*rho; printf("a= %f b=%f x0=%f y0=%f roh=%f\n", a,b,x0,y0,rho); pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); printf(" x1 = %d, y1 = %d",pt1.x,pt1.y); printf(" x2 = %d, y2 = %d\n\n",pt2.x,pt2.y); //if((theta*180/3.142) < 100 && (theta*180/3.142) > 79 ) cvLine( color_dst, pt1, pt2, CV_RGB(255,0,0), 3, 8 ); } cvNamedWindow("HoughLinesShow",1); cvShowImage("HoughLinesShow",color_dst); cvWaitKey(1000); }
const CvPoint2D32f* locate_puzzle(IplImage *in, IplImage **annotated) { IplImage *grid_image = _grid(in); *annotated = cvCloneImage(in); // find lines using Hough transform CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; double distance_resolution = 1; double angle_resolution = CV_PI / 60; int threshold = 60; int minimum_line_length = in->width / 2; int maximum_join_gap = in->width / 10; lines = cvHoughLines2(grid_image, storage, CV_HOUGH_PROBABILISTIC, distance_resolution, angle_resolution, threshold, minimum_line_length, maximum_join_gap); cvCvtColor(grid_image, *annotated, CV_GRAY2RGB); cvReleaseImage(&grid_image); double most_horizontal = INFINITY; for (int i = 0; i < lines->total; ++i) { CvPoint *line = (CvPoint*)cvGetSeqElem(lines,i); double dx = abs(line[1].x - line[0].x); double dy = abs(line[1].y - line[0].y); double slope = INFINITY; if (dx != 0) { slope = dy / dx; } if (slope != INFINITY) { if (slope < most_horizontal) { //printf("most horizontal seen: %0.2f\n", slope); most_horizontal = slope; } } } int top = -1; int left = -1; int bottom = -1; int right = -1; for (int i = 0; i < lines->total; i++) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); double dx = abs(line[1].x - line[0].x); double dy = abs(line[1].y - line[0].y); double slope = INFINITY; if (dx) { slope = dy / dx; } cvLine(*annotated, line[0], line[1], CV_RGB(255, 0, 0), 1, 8, 0); if (abs(slope - most_horizontal) <= 1) { if ((top == -1) || (line[1].y < ((CvPoint*)cvGetSeqElem(lines,top))[1].y)) { top = i; } if ((bottom == -1) || (line[1].y > ((CvPoint*)cvGetSeqElem(lines,bottom))[1].y)) { bottom = i; } } else { if ((left == -1) || (line[1].x < ((CvPoint*)cvGetSeqElem(lines,left))[1].x)) { left = i; } if ((right == -1) || (line[1].x > ((CvPoint*)cvGetSeqElem(lines,right))[1].x)) { right = i; } } } //printf("number of lines: %d\n", lines->total); if ((top == -1) || (left == -1) || (bottom == -1) || (right == -1)) { return NULL; } CvPoint *top_line = (CvPoint*)cvGetSeqElem(lines,top); cvLine(*annotated, top_line[0], top_line[1], CV_RGB(0, 0, 255), 6, 8, 0); CvPoint *bottom_line = (CvPoint*)cvGetSeqElem(lines,bottom); cvLine(*annotated, bottom_line[0], bottom_line[1], CV_RGB(0, 255, 255), 6, 8, 0); CvPoint *left_line = (CvPoint*)cvGetSeqElem(lines,left); cvLine(*annotated, left_line[0], left_line[1], CV_RGB(0, 255, 0), 6, 8, 0); CvPoint *right_line = (CvPoint*)cvGetSeqElem(lines,right); cvLine(*annotated, right_line[0], right_line[1], CV_RGB(255, 255, 0), 6, 8, 0); CvPoint2D32f *coordinates; coordinates = malloc(sizeof(CvPoint2D32f) * 4); // top left intersect(top_line, left_line, &(coordinates[0])); cvLine(*annotated, cvPointFrom32f(coordinates[0]), cvPointFrom32f(coordinates[0]), CV_RGB(255, 255, 0), 10, 8, 0); //printf("top_left: %.0f, %.0f\n", coordinates[0].x, coordinates[0].y); // top right intersect(top_line, right_line, &(coordinates[1])); cvLine(*annotated, cvPointFrom32f(coordinates[1]), cvPointFrom32f(coordinates[1]), CV_RGB(255, 255, 0), 10, 8, 0); //printf("top_right: %.0f, %.0f\n", coordinates[1].x, coordinates[1].y); // bottom right intersect(bottom_line, right_line, &(coordinates[2])); cvLine(*annotated, cvPointFrom32f(coordinates[2]), cvPointFrom32f(coordinates[2]), CV_RGB(255, 255, 0), 10, 8, 0); //printf("bottom_right: %.0f, %.0f\n", coordinates[2].x, coordinates[2].y); // bottom left intersect(bottom_line, left_line, &(coordinates[3])); cvLine(*annotated, cvPointFrom32f(coordinates[3]), cvPointFrom32f(coordinates[3]), CV_RGB(255, 255, 0), 10, 8, 0); //printf("bottom_left: %.0f, %.0f\n", coordinates[3].x, coordinates[3].y); return coordinates; }
int main( int argc, char** argv ) { if (argc>=2){ //Load image IplImage*src = cvLoadImage( argv[1]); //Create Windows and Position cvNamedWindow("Input",0); cvResizeWindow("Input",500,350); cvMoveWindow("Input", 0, 0); cvNamedWindow("Output",0); cvResizeWindow("Output",500,350); cvMoveWindow("Output", 0, 600); cvNamedWindow( "Hough", 0 ); cvResizeWindow("Hough",500,350); cvMoveWindow("Hough",700,0); cvNamedWindow( "FloodFill", 0 ); cvResizeWindow("FloodFill",500,350); cvMoveWindow("FloodFill",700,600); //Display Original Image cvShowImage( "Input", src ); IplImage*srcCopy = cvCloneImage(src); //Flood Fill CvPoint seedPoint= cvPoint((srcCopy->width)/2,(srcCopy->height)/2); CvScalar pixColor=avgpixelcolor(srcCopy,seedPoint,5); //Takes avg pixel color value (5x5 grid) cvLine( srcCopy, cvPoint(seedPoint.x,srcCopy->height*.9), cvPoint(seedPoint.x,srcCopy->height*.1), pixColor, 3, 8 ); cvFloodFill(srcCopy,seedPoint,cvScalarAll(255),cvScalarAll(50),cvScalarAll(50),NULL,8|CV_FLOODFILL_FIXED_RANGE, NULL); cvShowImage("FloodFill",srcCopy); //Convert to Grayscale IplImage*dst = cvCreateImage( cvGetSize(src), IPL_DEPTH_8U, 1 ); cvCvtColor( srcCopy, dst , CV_BGR2GRAY ); //Display Flood Fill Results cvCircle(srcCopy,seedPoint,5,cvScalarAll(0),3,8,0); //Threshold IplImage*thresh = cvCreateImage(cvGetSize(dst), IPL_DEPTH_8U,1); cvThreshold(dst,thresh, 230, //Set Manually during Initialization 255, //Max Pixel Intensity Value (Do not Change) CV_THRESH_TOZERO ); //Canny Edge Detection cvCanny( thresh, dst, 0, //Low Threshold 255, //High Threshold 3 ); //Storage for Hough Line Endpoints CvMemStorage* storage = cvCreateMemStorage(0); //Hough CvSeq* lines = cvHoughLines2( dst,storage,CV_HOUGH_PROBABILISTIC, 1, //rho 1*CV_PI/180, //theta 150, //Accumulator threshold 500, //Min Line Length 200 //Min Colinear Separation ); //Draw Vertical Lines on src image for(int i = 0; i < lines->total; i++ ) { CvPoint* Point = (CvPoint*)cvGetSeqElem(lines,i); cvLine( src, Point[0], Point[1], CV_RGB(0,220,20), 3, 8 ); //Reject Horizontal lines float slope=(Point[0].y-Point[1].y)/(Point[0].x-Point[1].x); } //Create a Trapazodal Mask //Detect Horizontal Lines //Isolate 4 points //Display Image cvShowImage( "Output", src); //For Calibration Purposes "what the Hough transform sees" cvShowImage( "Hough", dst ); //Wait for User 10sec cvWaitKey(10000); //Deallocate Memory cvReleaseImage( &src ); cvReleaseImage( &dst ); cvReleaseImage( &thresh ); cvDestroyWindow( "Input" ); cvDestroyWindow( "Output" ); cvDestroyWindow("Hough"); } else{ printf("Hough Transform Code Requires \n"); return 0; } }
unsigned long Test() { bool visualize = true; std::string RangeWindowName = "Range"; std::string HoughWindowName = "Hough"; IplImage* visualizationReferenceImage = cvLoadImage("Pictures/building.jpg"); CvMemStorage* storage = cvCreateMemStorage(0); /// Line endings storage CvSeq* lines = 0; int AngleBins = 45; /// Controls angle resolution of Hough trafo (bins per pi) IplImage* Image = cvCreateImage(cvGetSize(visualizationReferenceImage), IPL_DEPTH_8U, 3); /// Visualization image cvCopyImage(visualizationReferenceImage,Image); IplImage* GrayImage = cvCreateImage(cvGetSize(Image), IPL_DEPTH_8U, 1); cvCvtColor(Image, GrayImage, CV_RGB2GRAY); IplImage* CannyImage = cvCreateImage(cvGetSize(Image), IPL_DEPTH_8U, 1); /// Edge image cvCanny(GrayImage, CannyImage, 25, 50); CvPoint ROIp1 = cvPoint(100,10); /// Tablet ROI CvPoint ROIp2 = cvPoint(visualizationReferenceImage->width-40+ROIp1.x,visualizationReferenceImage->height-200+ROIp1.y); cvSetImageROI(CannyImage,cvRect(ROIp1.x,ROIp1.y,ROIp2.x-ROIp1.x,ROIp2.y-ROIp1.y)); cvRectangle(Image, ROIp1, ROIp2, CV_RGB(0,255,0)); //int maxd = cvRound(sqrt(sqrt((double)ROIp2.x-ROIp1.x)+sqrt((double)ROIp2.y-ROIp1.y))); /// Maximum of possible distance value in Hough space int maxd = cvRound(sqrt((double)(((ROIp2.x-ROIp1.x)*(ROIp2.x-ROIp1.x))+((ROIp2.y-ROIp1.y)*(ROIp2.y-ROIp1.y))))); /// Maximum of possible distance value in Hough space IplImage* HoughSpace = cvCreateImage(cvSize(maxd,AngleBins+1),IPL_DEPTH_8U, 1); /// Hough space image (black=no line, white=there are lines at these bins) cvZero(HoughSpace); /// Hough transformation int AccumulatorThreshold = 100; /// Threshold parameter. A line is returned by the function if the corresponding accumulator value is greater than threshold. double MinimumLineLength = 50; /// For probabilistic Hough transform it is the minimum line length. double MaximumGap = 4; /// For probabilistic Hough transform it is the maximum gap between line segments lieing on the same line to treat them as the single line segment (i.e. to join them). lines = cvHoughLines2(CannyImage, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/AngleBins, AccumulatorThreshold, MinimumLineLength, MaximumGap); for(int i = 0; i < lines->total; i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); /// Endings of a line CvPoint p0 = cvPoint(line[0].x+ROIp1.x,line[0].y+ROIp1.y); CvPoint p1 = cvPoint(line[1].x+ROIp1.x,line[1].y+ROIp1.y); cvLine(Image, p0, p1, CV_RGB(255,0,0), 3, 8 ); /// Slope/angle of line double phi = CV_PI/2; if(p0.x != p1.x) phi = atan((double)(p1.y-p0.y)/(double)(p1.x-p0.x)); phi += (phi < 0)*CV_PI; /// Hessian normal form parameters: d = x*cos(alpha) + y*sin(alpha) /// with alpha in [0...pi], d in [0...maxd] double alpha = phi+CV_PI/2; alpha -= (alpha > CV_PI)*CV_PI; double d = p0.x; if(p0.x != p1.x) { double n = p1.y - (p1.y-p0.y)/(p1.x-p0.x) * p1.x; d = abs(n * cos(phi)); } /// Write Line into Hough space cvLine(HoughSpace, cvPoint(cvRound(d),cvRound(alpha/CV_PI*AngleBins)),cvPoint(cvRound(d),cvRound(alpha/CV_PI*AngleBins)),CV_RGB(255,255,255)); } if(visualize) { cvNamedWindow(RangeWindowName.c_str()); cvNamedWindow(HoughWindowName.c_str()); cvShowImage(RangeWindowName.c_str(), Image); cvShowImage(HoughWindowName.c_str(), HoughSpace); cvWaitKey(0); } cvCopyImage(Image,visualizationReferenceImage); cvReleaseImage(&GrayImage); cvReleaseImage(&CannyImage); /* IplImage* img1 = cvLoadImage("Cob3.jpg"); IplImage* img2 = cvCreateImage(cvGetSize(img1),img1->depth,1); IplImage* img3 = cvCreateImage(cvGetSize(img1),img1->depth,1); IplImage* img4 = cvCreateImage(cvGetSize(img1),img1->depth,1); cvNamedWindow("Img1"); cvNamedWindow("Img2"); cvNamedWindow("Img3"); cvCvtColor(img1, img2, CV_RGB2GRAY); cvCanny(img2, img3, 100, 200); cvShowImage("Img1", img1); cvShowImage("Img2", img2); cvShowImage("Img3", img3); cvWaitKey(0); cvReleaseImage(&img1); cvReleaseImage(&img2); cvReleaseImage(&img3); cvDestroyAllWindows(); /* IplImage* Img1; IplImage* Img2; IplImage* Img3; cvNamedWindow("Img"); while (cvGetWindowHandle("Img")) { if(cvWaitKey(10)=='q') break; /// Uncomment when using <code>GetColorImage</code> instead of <code>GetColorImage2</code> //ColorImage = cvCreateImage(cvSize(1388,1038),IPL_DEPTH_8U,3); if (colorCamera->GetColorImage2(&Img1) == libCameraSensors::RET_FAILED) //if (colorCamera->GetColorImage(ColorImage, true) == libCameraSensors::RET_FAILED) { std::cerr << "TestCameraSensors: Color image acquisition failed\n"; getchar(); return ipa_utils::RET_FAILED; } if (colorCamera->GetColorImage2(&Img2) == libCameraSensors::RET_FAILED) { std::cerr << "TestCameraSensors: Color image acquisition failed\n"; getchar(); return ipa_utils::RET_FAILED; } Img3 = cvCreateImage(cvGetSize(Img1),Img1->depth,Img1->nChannels); cvSub(Img1, Img2, Img3); cvShowImage("Img", Img3); cvReleaseImage(&Img1); cvReleaseImage(&Img2); cvReleaseImage(&Img3); }*/ return ipa_utils::RET_OK; }
int main() { // Load the image we'll work on IplImage* img = cvLoadImage("C:\\goal_arena.jpg"); CvSize imgSize = cvGetSize(img); // This will hold the white parts of the image IplImage* detected = cvCreateImage(imgSize, 8, 1); // These hold the three channels of the loaded image IplImage* imgBlue = cvCreateImage(imgSize, 8, 1); IplImage* imgGreen = cvCreateImage(imgSize, 8, 1); IplImage* imgRed = cvCreateImage(imgSize, 8, 1); cvSplit(img, imgBlue, imgGreen, imgRed, NULL); // Extract white parts into detected cvAnd(imgGreen, imgBlue, detected); cvAnd(detected, imgRed, detected); // Morphological opening cvErode(detected, detected); cvDilate(detected, detected); // Thresholding (I knew you wouldn't catch this one... so i wrote a comment here // I mean the command can be so decieving at times) cvThreshold(detected, detected, 100, 250, CV_THRESH_BINARY); // Do the hough thingy CvMat* lines = cvCreateMat(100, 1, CV_32FC2); cvHoughLines2(detected, lines, CV_HOUGH_STANDARD, 1, 0.001, 100); // The two endpoints for each boundary line CvPoint left1 = cvPoint(0, 0); CvPoint left2 = cvPoint(0, 0); CvPoint right1 = cvPoint(0, 0); CvPoint right2 = cvPoint(0, 0); CvPoint top1 = cvPoint(0, 0); CvPoint top2 = cvPoint(0, 0); CvPoint bottom1 = cvPoint(0, 0); CvPoint bottom2 = cvPoint(0, 0); // Some numbers we're interested in int numLines = lines->rows; int numTop = 0; int numBottom = 0; int numLeft = 0; int numRight = 0; // Iterate through each line for(int i=0;i<numLines;i++) { // Get the parameters for the current line CvScalar dat = cvGet1D(lines, i); double rho = dat.val[0]; double theta = dat.val[1]; if(theta==0.0) { // This is an obviously vertical line... and we can't approximate it... NEXT continue; } // Convert from radians to degrees double degrees = theta*180/(3.1412); // Generate two points on this line (one at x=0 and one at x=image's width) CvPoint pt1 = cvPoint(0, rho/sin(theta)); CvPoint pt2 = cvPoint(img->width, (-img->width/tan(theta)) + rho/sin(theta)); if(abs(rho)<50) // Top + left { if(degrees>45 && degrees<135) // Top { numTop++; // The line is horizontal and near the top top1.x+=pt1.x; top1.y+=pt1.y; top2.x+=pt2.x; top2.y+=pt2.y; } else // left { numLeft++; //The line is vertical and near the left left1.x+=pt1.x; left1.y+=pt1.y; left2.x+=pt2.x; left2.y+=pt2.y; } } else // bottom+right { if(degrees>45 && degrees<135) // Bottom { numBottom++; //The line is horizontal and near the bottom bottom1.x+=pt1.x; bottom1.y+=pt1.y; bottom2.x+=pt2.x; bottom2.y+=pt2.y; } else // Right { numRight++; // The line is vertical and near the right right1.x+=pt1.x; right1.y+=pt1.y; right2.x+=pt2.x; right2.y+=pt2.y; } } } // we've done the adding... now the dividing to get the "averaged" point left1.x/=numLeft; left1.y/=numLeft; left2.x/=numLeft; left2.y/=numLeft; right1.x/=numRight; right1.y/=numRight; right2.x/=numRight; right2.y/=numRight; top1.x/=numTop; top1.y/=numTop; top2.x/=numTop; top2.y/=numTop; bottom1.x/=numBottom; bottom1.y/=numBottom; bottom2.x/=numBottom; bottom2.y/=numBottom; // Render these lines onto the image cvLine(img, left1, left2, CV_RGB(255, 0,0), 1); cvLine(img, right1, right2, CV_RGB(255, 0,0), 1); cvLine(img, top1, top2, CV_RGB(255, 0,0), 1); cvLine(img, bottom1, bottom2, CV_RGB(255, 0,0), 1); // Next, we need to figure out the four intersection points double leftA = left2.y-left1.y; double leftB = left1.x-left2.x; double leftC = leftA*left1.x + leftB*left1.y; double rightA = right2.y-right1.y; double rightB = right1.x-right2.x; double rightC = rightA*right1.x + rightB*right1.y; double topA = top2.y-top1.y; double topB = top1.x-top2.x; double topC = topA*top1.x + topB*top1.y; double bottomA = bottom2.y-bottom1.y; double bottomB = bottom1.x-bottom2.x; double bottomC = bottomA*bottom1.x + bottomB*bottom1.y; // Intersection of left and top double detTopLeft = leftA*topB - leftB*topA; CvPoint ptTopLeft = cvPoint((topB*leftC - leftB*topC)/detTopLeft, (leftA*topC - topA*leftC)/detTopLeft); // Intersection of top and right double detTopRight = rightA*topB - rightB*topA; CvPoint ptTopRight = cvPoint((topB*rightC-rightB*topC)/detTopRight, (rightA*topC-topA*rightC)/detTopRight); // Intersection of right and bottom double detBottomRight = rightA*bottomB - rightB*bottomA; CvPoint ptBottomRight = cvPoint((bottomB*rightC-rightB*bottomC)/detBottomRight, (rightA*bottomC-bottomA*rightC)/detBottomRight); // Intersection of bottom and left double detBottomLeft = leftA*bottomB-leftB*bottomA; CvPoint ptBottomLeft = cvPoint((bottomB*leftC-leftB*bottomC)/detBottomLeft, (leftA*bottomC-bottomA*leftC)/detBottomLeft); // Render the points onto the image cvLine(img, ptTopLeft, ptTopLeft, CV_RGB(0,255,0), 5); cvLine(img, ptTopRight, ptTopRight, CV_RGB(0,255,0), 5); cvLine(img, ptBottomRight, ptBottomRight, CV_RGB(0,255,0), 5); cvLine(img, ptBottomLeft, ptBottomLeft, CV_RGB(0,255,0), 5); // Initialize a mask IplImage* imgMask = cvCreateImage(imgSize, 8, 3); cvZero(imgMask); // Generate the mask CvPoint* pts = new CvPoint[4]; pts[0] = ptTopLeft; pts[1] = ptTopRight; pts[2] = ptBottomRight; pts[3] = ptBottomLeft; cvFillConvexPoly(imgMask, pts, 4, cvScalar(255,255,255)); // Delete anything thats outside the mask cvAnd(img, imgMask, img); // Show all images in windows cvNamedWindow("Original"); cvNamedWindow("Detected"); cvShowImage("Original", img); cvShowImage("Detected", detected); cvWaitKey(0); return 0; }
int main(int argc, char* argv[]) { IplImage* src = 0; IplImage* dst = 0; IplImage* color_dst = 0; char* filename = argc >= 2 ? argv[1] : "Image0.jpg"; //get gray image src = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE); if (!src){ printf("[!] Error: cant load image: %s \n", filename); return -1; } printf("[i] image: %s\n", filename); //array for lines CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; int i, p, i1, i2, x, y; double angle1, angle2, phi1, phi2, tetta, b1, b2, k1, k2; //phi - starting(worst) result, angle - needed result phi1 = 0; phi2 = CV_PI / 2; angle1 = CV_PI / 2; angle2 = 0; dst = cvCreateImage(cvGetSize(src), 8, 1); color_dst = cvCreateImage(cvGetSize(src), 8, 3); //Canny edge detector cvCanny(src, dst, 50, 70, 3); //convert into color picture cvCvtColor(dst, color_dst, CV_GRAY2BGR); //finding lines lines = cvHoughLines2(dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI / 180, 100, 50, 10); //Finding needed lines (by angle between axis 0x and this line). For finding the point it's necessary to find 2 two out of 3 lines for (i = 0; i < lines->total; i++) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines, i); //check for correct finding tg if (line[0].x >= line[1].x) { p = line[0].x; line[0].x = line[1].x; line[1].x = p; p = line[0].y; line[0].y = line[1].y; line[1].y = p; } tetta = atan((double)(line[1].y - line[0].y) / (double)(line[1].x - line[0].x)); //first line if (abs(angle1 - abs(tetta)) < abs(angle1 - abs(phi1))) { phi1 = tetta; i1 = i; } //second line if (abs(angle2 - abs(tetta)) < abs(angle2 - abs(phi2))) { //abs(phi2-tetta) <= check threshold between old line and new one if ((abs(phi2-tetta)<=0.25) && (i != 0)) { CvPoint* lastline = (CvPoint*)cvGetSeqElem(lines, i2); //needed lower of lines with similar angle if (lastline[0].y < line[0].y) { i2 = i; phi2 = tetta; } } else { i2 = i; phi2 = tetta; } } } CvPoint* line1 = (CvPoint*)cvGetSeqElem(lines, i1); CvPoint* line2 = (CvPoint*)cvGetSeqElem(lines, i2); //Building lines cvLine(color_dst, line1[0], line1[1], CV_RGB(255, 0, 0), 1, CV_AA, 0); cvLine(color_dst, line2[0], line2[1], CV_RGB(255, 0, 0), 1, CV_AA, 0); //Finding and building point k1 = tan(phi1); k2 = tan(phi2); b1 = line1[0].y - k1*line1[0].x; b2 = line2[0].y - k2*line2[0].x; x = (b2 - b1) / (k1 - k2); y = k1*x + b1; CvPoint* point = new CvPoint(); point[0].x = x; point[1].x = x; point[0].y = y; point[1].y = y; cvLine(color_dst, point[0], point[1], CV_RGB(0, 255, 0), 5, CV_AA, 0); //Showing cvNamedWindow("Source", 1); cvShowImage("Source", src); cvNamedWindow("Point", 1); cvShowImage("Point", color_dst); //Waiting for key cvWaitKey(0); cvSaveImage("test.jpg", color_dst); //Releasing resources cvReleaseMemStorage(&storage); cvReleaseImage(&src); cvReleaseImage(&dst); cvReleaseImage(&color_dst); cvDestroyAllWindows(); return 0; }
JNIEXPORT void JNICALL Java_com_stead_alistair_soundcoder_OpenCV_extractHoughFeature( JNIEnv* env, jobject thiz) { /* This code works perfectly on RGB but does not detect yellow because it assumes its too light IplImage* grayImage = cvCreateImage(cvGetSize(pImage), IPL_DEPTH_8U, 1); IplImage* cannyImage = cvCreateImage(cvGetSize(pImage), IPL_DEPTH_8U, 1); cvCvtColor(pImage, grayImage, CV_BGR2GRAY); cvCanny(grayImage, cannyImage, 20, 30); //Hough bit CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; lines = cvHoughLines2(cannyImage, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 100, 100, 100); int y = 0; for(int i = 0; i < MIN(lines->total, 100); i++ ) { y++; CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( pImage, line[0], line[1], CV_RGB(255,0,0), 3, 8 ); } cvReleaseMemStorage(&storage); cvReleaseImage(&grayImage); cvReleaseImage(&cannyImage); *********************************/ IplImage* hsv; IplImage* h; IplImage* s; IplImage* v; hsv = cvCreateImage( cvGetSize(pImage), 8, 3 ) ; h = cvCreateImage( cvGetSize(pImage), 8, 1 ); s = cvCreateImage( cvGetSize(pImage), 8, 1 ); v = cvCreateImage( cvGetSize(pImage), 8, 1 ) ; // Convert from BGR to HSV cvCvtColor( pImage, hsv, CV_BGR2HSV); // Get individual planes cvSplit( hsv, h, s, v, 0 ); //use canny on h and then hough IplImage* cannyImage = cvCreateImage(cvGetSize(pImage), IPL_DEPTH_8U, 1); cvThreshold(h,h,-50,200,CV_THRESH_BINARY); cvCanny(h, cannyImage, 20, 100); //cvMerge(cannyImage, s, v, 0, pImage); //cvCvtColor(pImage, pImage, CV_BGR2HSV); //Finally, get the Hough Lines CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* lines = 0; lines = cvHoughLines2(h, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 1, //Threshold 100, //Minimum Line Length 5); //maximum gap between line segments lying on the same line to treat them as a single line segment LOGI("Number: %d", MIN(lines->total, 20)); for(int i = 0; i < MIN(lines->total, 20); i++ ) { CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); cvLine( pImage, line[0], line[1], CV_RGB(255,0,0), 3, 8 ); } cvReleaseMemStorage(&storage); //End hough cvReleaseImage(&cannyImage); cvReleaseImage(&hsv); cvReleaseImage(&h); cvReleaseImage(&s); cvReleaseImage(&v); }
static void process_image_common(IplImage *frame) { CvFont font; cvInitFont(&font, CV_FONT_VECTOR0, 0.25f, 0.25f); CvSize video_size; #if defined(USE_POSIX_SHARED_MEMORY) video_size.height = *shrd_ptr_height; video_size.width = *shrd_ptr_width; #else // XXX These parameters should be set ROS parameters video_size.height = frame->height; video_size.width = frame->width; #endif CvSize frame_size = cvSize(video_size.width, video_size.height/2); IplImage *temp_frame = cvCreateImage(frame_size, IPL_DEPTH_8U, 3); IplImage *gray = cvCreateImage(frame_size, IPL_DEPTH_8U, 1); IplImage *edges = cvCreateImage(frame_size, IPL_DEPTH_8U, 1); IplImage *half_frame = cvCreateImage(cvSize(video_size.width/2, video_size.height/2), IPL_DEPTH_8U, 3); CvMemStorage *houghStorage = cvCreateMemStorage(0); cvPyrDown(frame, half_frame, CV_GAUSSIAN_5x5); // Reduce the image by 2 /* we're intersted only in road below horizont - so crop top image portion off */ crop(frame, temp_frame, cvRect(0, frame_size.height, frame_size.width, frame_size.height)); cvCvtColor(temp_frame, gray, CV_BGR2GRAY); // contert to grayscale /* Perform a Gaussian blur & detect edges */ // smoothing image more strong than original program cvSmooth(gray, gray, CV_GAUSSIAN, 15, 15); cvCanny(gray, edges, CANNY_MIN_TRESHOLD, CANNY_MAX_TRESHOLD); /* do Hough transform to find lanes */ double rho = 1; double theta = CV_PI/180; CvSeq *lines = cvHoughLines2(edges, houghStorage, CV_HOUGH_PROBABILISTIC, rho, theta, HOUGH_TRESHOLD, HOUGH_MIN_LINE_LENGTH, HOUGH_MAX_LINE_GAP); processLanes(lines, edges, temp_frame, frame); #ifdef SHOW_DETAIL /* show middle line */ cvLine(temp_frame, cvPoint(frame_size.width/2, 0), cvPoint(frame_size.width/2, frame_size.height), CV_RGB(255, 255, 0), 1); // cvShowImage("Gray", gray); // cvShowImage("Edges", edges); // cvShowImage("Color", temp_frame); // cvShowImage("temp_frame", temp_frame); // cvShowImage("frame", frame); #endif #if defined(USE_POSIX_SHARED_MEMORY) setImage_toSHM(frame); #endif #ifdef SHOW_DETAIL // cvMoveWindow("Gray", 0, 0); // cvMoveWindow("Edges", 0, frame_size.height+25); // cvMoveWindow("Color", 0, 2*(frame_size.height+25)); #endif cvReleaseMemStorage(&houghStorage); cvReleaseImage(&gray); cvReleaseImage(&edges); cvReleaseImage(&temp_frame); cvReleaseImage(&half_frame); }
//-------------------------------------------------------------- void testApp::update(){ bool bNewFrame = false; #ifdef _USE_LIVE_VIDEO vidGrabber.grabFrame(); bNewFrame = vidGrabber.isFrameNew(); #else vidPlayer.idleMovie(); bNewFrame = vidPlayer.isFrameNew(); #endif if (bNewFrame){ #ifdef _USE_LIVE_VIDEO colorImg.setFromPixels(vidGrabber.getPixels(), cw,ch); #else colorImg.setFromPixels(vidPlayer.getPixels(), cw,ch); #endif kx = (float) ofGetWidth() / cw; ky = (float) ofGetHeight() / ch; cvSmooth(colorImg.getCvImage(), medianImg.getCvImage(), CV_MEDIAN, medianValue, medianValue); medianImg.flagImageChanged(); grayImage = medianImg; cvCvtColor(colorImg.getCvImage(), hsvImage.getCvImage(), CV_RGB2HSV); hsvImage.flagImageChanged(); cvSetImageCOI(hsvImage.getCvImage(), 2); cvCopy(hsvImage.getCvImage(), satImage.getCvImage()); satImage.flagImageChanged(); cvSetImageCOI(hsvImage.getCvImage(), 0); //cvSmooth(satImage.getCvImage(), satImage.getCvImage(), CV_BLUR, 3, 3, 0, 0); cvAdaptiveThreshold(grayImage.getCvImage(), trsImage.getCvImage(), 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, adaptiveThreshValue); //cvCanny(trsImage.getCvImage(), trsImage.getCvImage(), sb, sb*4, 3); trsImage.flagImageChanged(); // cvSmooth(satImage.getCvImage(), satImage.getCvImage(), CV_MEDIAN, 7, 7); // cvSmooth( iplImage, iplImage, CV_BLUR, br, br, 0, 0 ); // cvSmooth( iplImage, iplImage, CV_MEDIAN, 7, 7); cvCanny( grayImage.getCvImage(), cannyImage.getCvImage(), cannyThresh1Value, cannyThresh2Value, cannyApertureValue); cannyImage.flagImageChanged(); //cvPyrMeanShiftFiltering(colorImg.getCvImage(), colorImg.getCvImage(), 20, 40, 2); if (mode==MODE_DRAWING) { if (draw_edges) { #if PROBABILISTIC_LINE lines = cvHoughLines2( cannyImage.getCvImage(), linesStorage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, lineThreshValue, lineMinLengthValue, lineMaxGapValue ); #else lines = cvHoughLines2( cannyImage.getCvImage(), linesStorage, CV_HOUGH_STANDARD, 1, CV_PI/180, 100, 0, 0 ); #endif } if (draw_contours || draw_approx) { cvFindContours(cannyImage.getCvImage(), edgesStorage, &edgeContours); CvSeq* contour = edgeContours; while (contour!=NULL) { for (int j = 0; j < contour->total; j++){ CvPoint* p1 = CV_GET_SEQ_ELEM(CvPoint, contour, j); p1->x = p1->x*(float)kx; p1->y = p1->y*(float)ky; } contour = contour->h_next; } } if (draw_fills) { cvFindContours(trsImage.getCvImage(), fillsStorage, &fillContours); CvSeq* contour = fillContours; while (contour!=NULL) { for (int j = 0; j < contour->total; j++){ CvPoint* p1 = CV_GET_SEQ_ELEM(CvPoint, contour, j); p1->x = p1->x*(float)kx; p1->y = p1->y*(float)ky; } contour = contour->h_next; } } } } // update scope // float* rand = new float[50]; // for(int i=0 ;i<50; i++){ // rand[i] = ofRandom(-1.0,1); // // } // // gui->update(scope, kofxGui_Set_FloatArray, rand, sizeof(float*)); // // // make 3 seconds loop // float f = ((ofGetElapsedTimeMillis()%3000) / 3000.0); // gui->update(points, kofxGui_Set_Float, &f, sizeof(float)); }
void OrchardDetector::processLaserScan( const sensor_msgs::LaserScan::ConstPtr& laser_scan) { float rthetamean = 0, rrhomean = 0, lthetamean = 0, lrhomean = 0, theta = 0, rho = 0; double x0 = 0, y0 = 0, a, b; int lmc = 0, rmc = 0; static int count = 0; clearRawImage(); sensor_msgs::PointCloud cloud; projector_.projectLaser(*laser_scan, cloud); int size = cloud.points.size(); for (int i = 0; i < size; i++) { if (abs(cloud.points[i].y) < 1.5 && abs(cloud.points[i].y) > 0.5 &&cloud.points[i].y < 0 && cloud.points[i].x > -1 && cloud.points[i].x < 3) { point1.x = ((int)(cloud.points[i].x * 50) + 300); point1.y = ((int)(cloud.points[i].y * 50) + 300); point2.x = point1.x - 4; point2.y = point1.y; cvLine(rawData_, point1, point2, CV_RGB(255,255,255), 2, CV_AA, 0); } } cvCvtColor(rawData_, workingImg_, CV_BGR2GRAY); //cvThreshold(workingImg_,workingImg_,) cvThreshold(workingImg_,workingImg_, 100, 255, CV_THRESH_BINARY); CvMemStorage* stor = cvCreateMemStorage(0); CvSeq* cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), stor); // find external contours cvFindContours(workingImg_, stor, &cont, sizeof(CvContour), CV_RETR_EXTERNAL, 2, cvPoint(0, 0)); for (; cont; cont = cont->h_next) { // size of pointArray and polygon int point_cnt = cont->total; // no small contours if (point_cnt > 20) { CvPoint* PointArray = (CvPoint*) malloc(point_cnt * sizeof(CvPoint)); // Get contour point set. cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); for (int i = 0; i <= point_cnt; i++) { // Show the Pixelcoordinates // have some fun with the color cvLine(rawData_, PointArray[i % point_cnt], PointArray[(i + 1) % point_cnt], cvScalar(0, 0, 0), 10); cvLine(workingImg_, PointArray[i % point_cnt], PointArray[(i + 1) % point_cnt], cvScalar(0, 0, 0), 50); } continue; } // Allocate memory for contour point set. CvPoint* PointArray = (CvPoint*) malloc(point_cnt * sizeof(CvPoint)); // Get contour point set. cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); for (int i = 0; i <= point_cnt; i++) { // Show the Pixelcoordinates //cout << PointArray[i].x << " " << PointArray[i].y << endl; // have some fun with the color int h_value = int(i * 3.0 / point_cnt * i) % 100; cvLine(rawData_, PointArray[i % point_cnt], PointArray[(i + 1) % point_cnt], cvScalar(0, 255, 0), 4); } } //cvDilate(workingImg_,workingImg_, 0, 3); //cvErode(workingImg_,workingImg_, 0, 3); //cvShowImage("Wporking", workingImg_); cvWaitKey(10); lines_ = cvHoughLines2(workingImg_, storage_, CV_HOUGH_STANDARD, 1, CV_PI / 180, 15, 0, 0); //cvHoughLines2(edgesImage, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/360, 30, 10, MAXIMUM_GAP); for (int i = 0; i < MIN(lines_->total,15); i++) { float* line = (float*) cvGetSeqElem(lines_, i); rho = line[0]; theta = line[1]; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)); point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)); point2.y = cvRound(y0 - 600 * (a)); point3.x = 300, point3.y = 300; point4.x = 300, point4.y = 600; point5.x = 300, point5.y = 0; cvLine(rawData_, point3, point4, CV_RGB(0,0,255), 1, CV_AA, 0); cvLine(rawData_, point3, point5, CV_RGB(0,0,255), 1, CV_AA, 0); if (intersect(point1, point2, point3, point4)) { { if(abs(left_angle_ -( (theta) - CV_PI / 2)) < 0.2){ rrhomean += rho; rthetamean += theta; rmc++; cvLine(workingImg_, point1, point2, CV_RGB(0,0,255), 1, CV_AA, 0); } } } else if (intersect(point1, point2, point3, point5)) { { if(abs(right_angle_ -( (theta) - CV_PI / 2)) < 0.5){ lrhomean += rho; lthetamean += theta; lmc++; cvLine(workingImg_, point1, point2, CV_RGB(255,255,255), 1,CV_AA, 0); } } } } if(lmc > 5){ theta = lthetamean / lmc; rho = lrhomean / lmc; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)), point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)), point2.y = cvRound(y0 - 600 * (a)); cvLine(rawData_, point1, point2, CV_RGB(255,0,0), 3, CV_AA, 0); point4.x = 300; point4.y = 300; point5.x = point4.x + 200 * sin(CV_PI - (theta - CV_PI / 2)); point5.y = point4.y + 200 * cos(CV_PI - (theta - CV_PI / 2)); cvLine(rawData_, point5, point4, CV_RGB(255,255,255), 1, CV_AA, 0); rows_.header.stamp = ros::Time::now(); rows_.leftvalid = false; rows_.rightvalid = false; if (intersect(point1, point2, point4, point5)) { right_distance_ = sqrt(((intersection_.y - 300) * (intersection_.y - 300)) + ((intersection_.x - 300) * (intersection_.x - 300))) * 2; right_angle_ = (theta) - CV_PI / 2; count++; rolling_mean_[count % 100][0] = right_angle_; right_angle_ = 0; for (int i = 0; i < 100; i++) { right_angle_ += rolling_mean_[i][0]; } right_angle_ = right_angle_ / 100; rolling_mean_[count % 50][1] = right_distance_; right_distance_ = 0; for (int i = 0; i < 50; i++) { right_distance_ += rolling_mean_[i][1]; } right_distance_ = right_distance_ / 50; inrow_cnt++; if(inrow_cnt > 10) inrow_cnt = 10; /* ROS_INFO("angle: %f",right_angle_); //cvLine(rawData_, point1, point2, CV_RGB(0,255,0), 1, CV_AA, 0); */ geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw( right_angle_); marker_r.header.frame_id = "/laser_link"; marker_r.header.stamp = ros::Time::now(); marker_r.ns = "basic_shapes"; marker_r.id = 0; // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER marker_r.type = visualization_msgs::Marker::CUBE; // Set the marker action. Options are ADD and DELETE marker_r.action = visualization_msgs::Marker::ADD; // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header marker_r.pose.position.x = 0; marker_r.pose.position.y = -((float) right_distance_) / 100; marker_r.pose.position.z = 0; marker_r.pose.orientation = pose_quat; // Set the scale of the marker -- 1x1x1 here means 1m on a side marker_r.scale.x = 10.0; marker_r.scale.y = 0.1; marker_r.scale.z = 0.5; // Set the color -- be sure to set alpha to something non-zero! marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 0.0f; marker_r.color.a = 0.5; marker_r.lifetime = ros::Duration(.5); // Publish the marker marker_r_pub.publish(marker_r); rows_.rightvalid = true; rows_.rightdistance = ((float) right_distance_) / 100; rows_.rightangle = right_angle_; } else { inrow_cnt--; inrow_cnt--; if(inrow_cnt < -5){ inrow_cnt = -5; } /* left_distance_ = -1; left_angle_ = 6000; rows_.rightdistance = ((float) left_distance_) / 100; rows_.rightangle = theta - CV_PI / 2; */ // printf("\nDistance from right:%dmm",hough_lines.right_distance); //sprintf(textright, "Distance from right: %dmm, angle: %d",hough_lines.right_distance, hough_lines.right_angle); } }else{ ROS_INFO_THROTTLE(1,"lmc = %d", lmc); inrow_cnt--; inrow_cnt--; if(inrow_cnt < -5){ inrow_cnt = -5; } } CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, CV_AA); if(inrow_cnt > 0){ twist_msg_.twist.angular.z = -((right_angle_*1.5) - (((float)right_distance_/100) - 1.2) ); cvPutText(rawData_, "INROW-NAVIGATION", cvPoint(10, 130), &font, cvScalar(255, 255, 255, 0)); cvLine(rawData_, cvPoint(10, 140), cvPoint(10 + abs(inrow_cnt * 20),140),CV_RGB(0,255,0), 3, CV_AA, 0); rows_.headland = false; }else{ twist_msg_.twist.angular.z = M_PI/4; cvPutText(rawData_, "HEADLAND", cvPoint(10, 130), &font, cvScalar(255, 255, 255, 0)); cvLine(rawData_, cvPoint(10, 140), cvPoint(10 + abs(inrow_cnt * 20), 140 ),CV_RGB(255,0,0), 3, CV_AA, 0); rows_.headland = true; } twist_pub.publish(twist_msg_); cvLine(rawData_, cvPoint(0, 300 + 150), cvPoint(600, 300 + 150),CV_RGB(255,255,255), 1, CV_AA, 0); cvLine(rawData_, cvPoint(0, 300 - 150), cvPoint(600, 300 - 150),CV_RGB(255,255,255), 1, CV_AA, 0); rows_.header.stamp = ros::Time::now(); row_pub.publish(rows_); cvShowImage("TEST", rawData_); cvWaitKey(10); //pc_pub.publish(cloud); }
void BoatDetector::houghline(IplImage* edgeImage, IplImage* image, IplImage* lineImage) { //validation int points = 50; // points per row int rows = 3; // number of rows int ver_dist = 10; // vertical distance between points int hor_dist = image->width / points; // horizontal distance between points cvCopy(edgeImage, lineImage); CvSeq* hough_lines = 0; CvScalar line_color = cvScalar(120); hough_lines = cvHoughLines2( edgeImage, hough_storage, CV_HOUGH_STANDARD, 1, CV_PI/180, 100, 0, 0 ); if(hough_lines->total == 0) { return; } bool find = false; CvPoint pt1, pt2; float* line; float theta; float rho; double a, b, x0, y0; for( int i = 0; i < min(hough_lines->total, 100); i++ ) { line = (float*)cvGetSeqElem(hough_lines, i); theta = line[1]; if(theta < 1.50 || theta > 1.60) { continue; } rho = line[0]; a = cos(theta); b = sin(theta); x0 = a*rho; y0 = b*rho; pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); cvLine( lineImage, pt1, pt2, line_color, 2, CV_AA, 0 ); find = true; } if(!find) { return; } bool run = true; int search_limit = lineImage->height - (ver_dist * rows); int line_step = lineImage->widthStep/sizeof(char); int img_step = image->widthStep/sizeof(char); int max_left, max_right; int tmp_limit; double count; while(run) { max_left = 0; max_right = 0; for(int i = ver_dist * rows; i < search_limit; i++) { if(((uchar)lineImage->imageData[i*line_step+3]) == line_color.val[0]) { if(i > max_left) { max_left = i; } } if(((uchar)lineImage->imageData[i*line_step + (lineImage->width-3)]) == line_color.val[0]) { if(i > max_right) { max_right = i; } } } if(max_left == 0 || max_right == 0) { run = false; continue; } tmp_limit = (max_left + max_right) / 2; //limit validation count = 0; if(abs(max_left - max_right) < 10) { for(int i = tmp_limit - (ver_dist * rows), k = 0, t = rows*2; k < rows; i+=ver_dist, k++, t-=2) { for(int j = hor_dist; j < image->width; j+=hor_dist) { if(abs(image->imageData[i*img_step + j] - image->imageData[(i+t*ver_dist)*img_step + j] ) > 10 ) { count++; } } } } if((count / (points * rows)) > 0.9 ) { sea_limit = tmp_limit; /* IplImage* ao = cvCloneImage(image); for(int i = tmp_limit - (ver_dist * rows), k = 0, t = rows*2; k < rows; i+=ver_dist, k++, t-=2) { for(int j = hor_dist; j < image->width; j+=hor_dist) { if(abs(image->imageData[i*img_step + j] - image->imageData[(i+t*ver_dist)*img_step + j] ) > 10 ) { cvLine(ao, cvPoint(j,i), cvPoint(j,i), CV_RGB(0,0,0), 3); cvLine(ao, cvPoint(j,i+t*ver_dist), cvPoint(j,i+t*ver_dist), CV_RGB(255,255,255), 3); } } } cvShowImage("ao",ao); cvWaitKey(0); */ run = false; } else { search_limit = max(max_left, max_right); } } }