두 점 사이의 각도 계산

OpenCV 를 이용, 빨강, 검정 두 점을 인식하고 두 점 사이의 각도와 그 중심을 계산하여 출력하는 화면과 소스.

오브젝트의 방향성과 위치를 인식할 수 있게 되었다.

#include <cv.h>

#include <highgui.h>

#include <stdio.h>

#include <math.h>

void main()

{

double redmin_val, redmax_val;

double blkmin_val, blkmax_val;

CvPoint redmin_loc, redmax_loc;

CvPoint blkmin_loc, blkmax_loc;

CvPoint line_start, line_end, center;

CvSize reddst_size;

CvSize blkdst_size;

IplImage* src_img = 0;  //

CvCapture* capture = cvCaptureFromCAM(0);  //

cvGrabFrame( capture );

src_img = cvRetrieveFrame( capture );

IplImage* redtmp_img = cvLoadImage("reddot.jpg", -1);

IplImage* blktmp_img = cvLoadImage("blkdot.jpg", -1);

reddst_size = cvSize( src_img->width - redtmp_img->width+1, src_img->height - redtmp_img->height+1 );

blkdst_size = cvSize( src_img->width - blktmp_img->width+1, src_img->height - blktmp_img->height+1 );

IplImage* reddst_img = cvCreateImage( reddst_size , IPL_DEPTH_32F, 1 );

IplImage* blkdst_img = cvCreateImage( blkdst_size , IPL_DEPTH_32F, 1 );

cvNamedWindow( "Vision Test", 0 );

cvNamedWindow( "Vision Test#2", 0);

cvResizeWindow( "Vision Test", 640, 480 );

cvResizeWindow( "Vision Test#2", 640, 480 );

while(1)

{

cvGrabFrame( capture );

src_img = cvRetrieveFrame( capture );

cvMatchTemplate(src_img, redtmp_img, reddst_img, CV_TM_CCOEFF_NORMED);

cvMatchTemplate(src_img, blktmp_img, blkdst_img, CV_TM_CCOEFF_NORMED);

cvMinMaxLoc(reddst_img, &redmin_val, &redmax_val, &redmin_loc, &redmax_loc, NULL);

cvMinMaxLoc(blkdst_img, &blkmin_val, &blkmax_val, &blkmin_loc, &blkmax_loc, NULL);

cvRectangle(src_img, redmax_loc, cvPoint(redmax_loc.x + redtmp_img->width, redmax_loc.y + redtmp_img->height), CV_RGB(255,0,0), 1);

cvRectangle(src_img, blkmax_loc, cvPoint(blkmax_loc.x + blktmp_img->width, blkmax_loc.y + blktmp_img->height), CV_RGB(255,0,0), 1);

line_start = cvPoint(blkmax_loc.x + (blktmp_img->width)/2 , blkmax_loc.y + (blktmp_img->height)/2);

line_end = cvPoint(redmax_loc.x + (redtmp_img->width)/2 , redmax_loc.y + (redtmp_img->height)/2);

center = cvPoint((line_start.x + line_end.x)/2, (line_start.y + line_end.y)/2);

cvLine(src_img, line_start, line_end, CV_RGB(255,0,0));

cvCircle(src_img, center, 30, CV_RGB(0,0,255), 3);

int dx = line_end.x - line_start.x;

int dy = line_end.y - line_start.y;

double PI=3.14;

double rad= atan2(dx, dy);

double degree = (rad*180)/PI;

printf("센터 : X = %d, Y = %d\t\t각도 : %.0lfº\n", center.x, center.y, degree);

cvShowImage( "Vision Test", src_img );

cvShowImage( "Vision Test#2", reddst_img );

if( cvWaitKey(10) >= 0 ) break;

}

cvReleaseCapture( &capture );

cvReleaseImage ( &src_img );

cvReleaseImage ( &redtmp_img );

cvReleaseImage ( &blktmp_img );

cvReleaseImage ( &reddst_img );

cvReleaseImage ( &blkdst_img );

cvDestroyAllWindows();

}