#include "opencv/cv.h" #include "opencv2/highgui/highgui.hpp" #include using namespace cv; //using namespace std; int main(int argc, char* argv){ #define MARKER_SIZE (50) /* size of marker (Millimeter)*/ CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml"); CvMat *distortion = (CvMat*)cvLoad("Distortion.xml"); //----------------------------------------// CvMat *intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1 ); /* double fu = 794.54; double fv = 774.31; double cx = 320.0; double cy = 240.0; cvZero(intrinsic_matrix); cvSetReal2D(intrinsic_matrix,0,0,fu); cvSetReal2D(intrinsic_matrix,1,1,fv); cvSetReal2D(intrinsic_matrix,0,2,cx); cvSetReal2D(intrinsic_matrix,1,2,cy); cvSetReal2D(intrinsic_matrix,2,2, 1);*/ CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f; CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f; //----------------------------------------// int i; CvMat object_points; CvMat image_points; CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); // [ X X X ] (1 row, 3 columns), 32bit-float, 1-channel CvMat *translation = cvCreateMat (1 , 3, CV_32FC1); //Axis coordinate generation CvMat *srcPoints3D = cvCreateMat (4, 1, CV_32FC3);//The original three-dimensional coordinates CvMat *dstPoints2D = cvCreateMat (4, 1, CV_32FC2);//Two-dimensional coordinates on the screen when the projection CvPoint3D32f baseMarkerPoints[4]; //In the physical space that specifies whether a box which will coordinate. baseMarkerPoints[0].x =(float) 0 * MARKER_SIZE; baseMarkerPoints[0].y =(float) 0 * MARKER_SIZE; baseMarkerPoints[0].z = 0.0; baseMarkerPoints[1].x =(float) 0 * MARKER_SIZE; baseMarkerPoints[1].y =(float) 1 * MARKER_SIZE; baseMarkerPoints[1].z = 0.0; baseMarkerPoints[2].x =(float) 1 * MARKER_SIZE; baseMarkerPoints[2].y =(float) 1 * MARKER_SIZE; baseMarkerPoints[2].z = 0.0; baseMarkerPoints[3].x =(float) 1 * MARKER_SIZE; baseMarkerPoints[3].y =(float) 0 * MARKER_SIZE; baseMarkerPoints[3].z = 0.0; char text[255] = "Test"; CvFont axisfont; float axhscale = 0.8f; float axvscale = 0.8f; float italicscale = 0.0f; int thickness = 1; cvInitFont (&axisfont, CV_FONT_HERSHEY_SIMPLEX , axhscale, axvscale, italicscale, thickness, CV_AA); //Determine the coordinates of the axis base. for ( i=0;i<4;i++) { switch (i) { case 0: srcPoints3D->data.fl[0] =0; srcPoints3D->data.fl[1] =0; srcPoints3D->data.fl[2] =0; break; case 1: srcPoints3D->data.fl[0+i*3] =(float)MARKER_SIZE; srcPoints3D->data.fl[1+i*3] =0; srcPoints3D->data.fl[2+i*3] =0; break; case 2: srcPoints3D->data.fl[0+i*3] =0; srcPoints3D->data.fl[1+i*3] =(float)MARKER_SIZE; srcPoints3D->data.fl[2+i*3] =0; break; case 3: srcPoints3D->data.fl[0+i*3] =0; srcPoints3D->data.fl[1+i*3] =0; srcPoints3D->data.fl[2+i*3] =-(float)MARKER_SIZE;; break; } } CvCapture* capture = cvCaptureFromCAM(0); IplImage* frame = cvQueryFrame(capture); IplImage* image = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U,3); IplImage* gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U,1); IplImage* grayContour = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U,1); CvMemStorage *storage = cvCreateMemStorage (0); CvMemStorage *storagepoly = cvCreateMemStorage (0); CvSeq *firstcontour=NULL; //first contour found CvSeq *polycontour=NULL; int contourCount; IplImage *marker_inside=cvCreateImage(cvSize(50,50),IPL_DEPTH_8U,1); CvMat *map_matrix; CvPoint2D32f src_pnt[4], dst_pnt[4], tmp_pnt[4]; dst_pnt[0] = cvPoint2D32f (0, 0); dst_pnt[1] = cvPoint2D32f (marker_inside->width, 0); dst_pnt[2] = cvPoint2D32f (marker_inside->width, marker_inside->height); dst_pnt[3] = cvPoint2D32f (0, marker_inside->height); map_matrix = cvCreateMat (3, 3, CV_32FC1); cvNamedWindow ("marker_inside", CV_WINDOW_AUTOSIZE); //cvNamedWindow ("capture_image", CV_WINDOW_AUTOSIZE); char chr; cvNamedWindow("Camera"); cvNamedWindow("grayscale-threshold"); cvNamedWindow("grayscale-inverse"); cvNamedWindow("grayscale-smooth"); while(1){ frame = cvQueryFrame(capture); cvCopy(frame,image); cvCvtColor(image,gray,CV_BGR2GRAY); cvSmooth(gray,gray,CV_GAUSSIAN,3); cvShowImage("grayscale-smooth", gray); cvThreshold (gray, gray, 0, 255, CV_THRESH_BINARY | CV_THRESH_OTSU); cvShowImage("grayscale-threshold", gray); cvNot(gray,gray); cvShowImage("grayscale-inverse", gray); cvCopy(gray,grayContour); contourCount=cvFindContours (grayContour, storage, &firstcontour, sizeof (CvContour), CV_RETR_CCOMP); //printf("\nContours: %d", contourCount); polycontour=cvApproxPoly(firstcontour,sizeof(CvContour),storagepoly,CV_POLY_APPROX_DP,3,1); for(CvSeq* c=polycontour;c!=NULL;c=c->h_next) // loop till end of Seq (last found contour) { if(c->total==4) { int nearestindex=0; //Draw marker's contour cvDrawContours(image,c,CV_RGB(255,255,0),CV_RGB(200,255,255),0); float xlist[4]; float ylist[4]; for(int n=0;n<4;n++) //get point by point (total 4 points since we're detecting quadrilaterals) in Sequence { CvPoint* p=CV_GET_SEQ_ELEM(CvPoint,c,n); tmp_pnt[n].x=(float)p->x; tmp_pnt[n].y=(float)p->y; /* xlist[n]=(float)p->x; ylist[n]=(float)p->y;*/ } src_pnt[0].x=tmp_pnt[0].x; src_pnt[0].y=tmp_pnt[0].y; src_pnt[1].x=tmp_pnt[3].x; src_pnt[1].y=tmp_pnt[3].y; src_pnt[2].x=tmp_pnt[2].x; src_pnt[2].y=tmp_pnt[2].y; src_pnt[3].x=tmp_pnt[1].x; src_pnt[3].y=tmp_pnt[1].y; //Only pass the box information. I still do not know that either way cvGetPerspectiveTransform (tmp_pnt, dst_pnt, map_matrix); //Deforming the inner square of the marker cvWarpPerspective (gray, marker_inside, map_matrix, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll (0)); //Set the coordinates of the markers on the image. //create a Mat with points in src_pnt (perspective points) cvInitMatHeader (&image_points, 4, 1, CV_32FC2, src_pnt); //Underlying set of marker coordinates // create Mat with real points with actual distances) (unwarped real 50*50 square) cvInitMatHeader (&object_points, 4, 3, CV_32FC1, baseMarkerPoints); //cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic,distortion,rotation,translation); cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic_matrix,NULL,rotation,translation); //Those obtained by using calculations that do come to any location on the screen coordinates of real space //cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic,distortion,dstPoints2D); cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic_matrix,NULL,dstPoints2D); //Draw a number of markers cvPutText(image, text,cvPoint((int)(dstPoints2D->data.fl[3]+dstPoints2D->data.fl[6])/2,(int)(dstPoints2D->data.fl[4]+dstPoints2D->data.fl[7])/2), &axisfont, CV_RGB(255, 255, 100)); cvPutText(image, text,cvPoint(20,20), &axisfont, CV_RGB(255, 255, 100)); } } cvShowImage("Camera", image); cvShowImage("marker_inside",marker_inside); chr = cvWaitKey(50); if(chr ==27) break; } return 0; }