Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "opencv/cv.h"
- #include "opencv2/highgui/highgui.hpp"
- #include <stdio.h>
- 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;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement