Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Guest

Untitled

By: a guest on Oct 1st, 2011  |  syntax: C++  |  size: 7.49 KB  |  views: 339  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. #include "opencv/cv.h"
  2. #include "opencv2/highgui/highgui.hpp"
  3. #include <stdio.h>
  4.  
  5. using namespace cv;
  6. //using namespace std;
  7.  
  8.  
  9.        
  10.  
  11. int main(int argc, char* argv){
  12.  
  13.         #define MARKER_SIZE (50)
  14.         /* size of marker (Millimeter)*/
  15.         CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
  16.         CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");
  17.  
  18. //----------------------------------------//
  19.          CvMat *intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1 );
  20.  /* double fu = 794.54;
  21.     double fv = 774.31;
  22.     double cx = 320.0;
  23.     double cy = 240.0;
  24.  
  25.     cvZero(intrinsic_matrix);
  26.     cvSetReal2D(intrinsic_matrix,0,0,fu);
  27.     cvSetReal2D(intrinsic_matrix,1,1,fv);
  28.     cvSetReal2D(intrinsic_matrix,0,2,cx);
  29.     cvSetReal2D(intrinsic_matrix,1,2,cy);
  30.     cvSetReal2D(intrinsic_matrix,2,2, 1);*/
  31.  CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
  32.  CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;
  33.         //----------------------------------------//
  34.  
  35.         int i;
  36.  
  37.         CvMat object_points;
  38.         CvMat image_points;
  39.  
  40.         CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); // [ X X X ] (1 row, 3 columns), 32bit-float, 1-channel
  41.         CvMat *translation = cvCreateMat (1 , 3, CV_32FC1);
  42.  
  43.         //Axis coordinate generation
  44.         CvMat *srcPoints3D = cvCreateMat (4, 1, CV_32FC3);//The original three-dimensional coordinates
  45.         CvMat *dstPoints2D = cvCreateMat (4, 1, CV_32FC3);//Two-dimensional coordinates on the screen when the projection
  46.  
  47.         CvPoint3D32f baseMarkerPoints[4];
  48.  
  49.         //In the physical space that specifies whether a box which will coordinate.
  50.         baseMarkerPoints[0].x =(float) 0 * MARKER_SIZE;
  51.         baseMarkerPoints[0].y =(float) 0 * MARKER_SIZE;
  52.         baseMarkerPoints[0].z = 0.0;
  53.  
  54.         baseMarkerPoints[1].x =(float) 0 * MARKER_SIZE;
  55.         baseMarkerPoints[1].y =(float) 1 * MARKER_SIZE;
  56.         baseMarkerPoints[1].z = 0.0;
  57.  
  58.         baseMarkerPoints[2].x =(float) 1 * MARKER_SIZE;
  59.         baseMarkerPoints[2].y =(float) 1 * MARKER_SIZE;
  60.         baseMarkerPoints[2].z = 0.0;
  61.  
  62.         baseMarkerPoints[3].x =(float) 1 * MARKER_SIZE;
  63.         baseMarkerPoints[3].y =(float) 0 * MARKER_SIZE;
  64.         baseMarkerPoints[3].z = 0.0;
  65.  
  66.          char text[255] = "Test";
  67.  
  68.         CvFont axisfont;
  69.     float axhscale      = 0.8f;
  70.     float axvscale      = 0.8f;
  71.         float italicscale = 0.0f;
  72.     int  thickness    = 1;
  73.     cvInitFont (&axisfont, CV_FONT_HERSHEY_SIMPLEX , axhscale, axvscale, italicscale, thickness, CV_AA);
  74.  
  75.   //Determine the coordinates of the axis base.
  76.         for ( i=0;i<4;i++)
  77.         {
  78.                 switch (i)
  79.                 {
  80.                         case 0: srcPoints3D->data.fl[0]     =0;
  81.                                     srcPoints3D->data.fl[1]     =0;
  82.                                         srcPoints3D->data.fl[2]     =0;
  83.                                         break;
  84.                         case 1: srcPoints3D->data.fl[0+i*3] =(float)MARKER_SIZE;
  85.                                         srcPoints3D->data.fl[1+i*3] =0;
  86.                                         srcPoints3D->data.fl[2+i*3] =0;
  87.                                         break;
  88.                         case 2: srcPoints3D->data.fl[0+i*3] =0;
  89.                                         srcPoints3D->data.fl[1+i*3] =(float)MARKER_SIZE;
  90.                                         srcPoints3D->data.fl[2+i*3] =0;
  91.                                         break;
  92.                         case 3: srcPoints3D->data.fl[0+i*3] =0;
  93.                                         srcPoints3D->data.fl[1+i*3] =0;
  94.                                         srcPoints3D->data.fl[2+i*3] =-(float)MARKER_SIZE;;
  95.                                         break;
  96.  
  97.                 }
  98.         }
  99.  
  100.  
  101.         CvCapture* capture = cvCaptureFromCAM(1);
  102.         IplImage* frame = cvQueryFrame(capture);
  103.  
  104.         IplImage* image = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U,3);
  105.         IplImage* gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U,1);
  106.         IplImage* grayContour = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U,1);
  107.  
  108.         CvMemStorage *storage = cvCreateMemStorage (0);
  109.         CvMemStorage *storagepoly = cvCreateMemStorage (0);
  110.  
  111.         CvSeq *firstcontour=NULL; //first contour found
  112.         CvSeq *polycontour=NULL;
  113.  
  114.         int contourCount;
  115.  
  116.         IplImage *marker_inside=cvCreateImage(cvSize(50,50),IPL_DEPTH_8U,1);
  117.  
  118.         CvMat *map_matrix;
  119.         CvPoint2D32f src_pnt[4], dst_pnt[4], tmp_pnt[4];
  120.  
  121.         dst_pnt[0] = cvPoint2D32f (0, 0);
  122.         dst_pnt[1] = cvPoint2D32f (marker_inside->width, 0);
  123.         dst_pnt[2] = cvPoint2D32f (marker_inside->width, marker_inside->height);
  124.     dst_pnt[3] = cvPoint2D32f (0, marker_inside->height);
  125.         map_matrix = cvCreateMat (3, 3, CV_32FC1);
  126.  
  127.         cvNamedWindow ("marker_inside", CV_WINDOW_AUTOSIZE);
  128.         //cvNamedWindow ("capture_image", CV_WINDOW_AUTOSIZE);
  129.  
  130.  
  131.         char chr;
  132.  
  133.         cvNamedWindow("Camera");
  134.         cvNamedWindow("grayscale-threshold");
  135.         cvNamedWindow("grayscale-inverse");
  136.         cvNamedWindow("grayscale-smooth");
  137.  
  138.         while(1){
  139.                 frame = cvQueryFrame(capture);
  140.                 cvCopy(frame,image);
  141.                
  142.                
  143.                
  144.                 cvCvtColor(image,gray,CV_BGR2GRAY);
  145.  
  146.                 cvSmooth(gray,gray,CV_GAUSSIAN,3);
  147.                 cvShowImage("grayscale-smooth", gray);
  148.  
  149.                 cvThreshold (gray, gray, 0, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
  150.                 cvShowImage("grayscale-threshold", gray);
  151.  
  152.                 cvNot(gray,gray);
  153.                 cvShowImage("grayscale-inverse", gray);
  154.  
  155.                 cvCopy(gray,grayContour);
  156.  
  157.                 contourCount=cvFindContours (grayContour, storage, &firstcontour, sizeof (CvContour), CV_RETR_CCOMP);
  158.                 //printf("\nContours: %d", contourCount);
  159.                
  160.                 polycontour=cvApproxPoly(firstcontour,sizeof(CvContour),storagepoly,CV_POLY_APPROX_DP,3,1);
  161.  
  162.  
  163.                 for(CvSeq* c=polycontour;c!=NULL;c=c->h_next) // loop till end of Seq (last found contour)
  164.                 {
  165.                
  166.                         if(c->total==4)
  167.                         {
  168.                                
  169.                                         int nearestindex=0;
  170.  
  171.                                        
  172.                                         //Draw marker's contour
  173.                                         cvDrawContours(image,c,CV_RGB(255,255,0),CV_RGB(200,255,255),0);
  174.  
  175.                                         float xlist[4];
  176.                                                 float ylist[4];
  177.                                                 for(int n=0;n<4;n++) //get point by point (total 4 points since we're detecting quadrilaterals) in Sequence
  178.                                                 {
  179.                                                         CvPoint* p=CV_GET_SEQ_ELEM(CvPoint,c,n);
  180.                                                         tmp_pnt[n].x=(float)p->x;
  181.                                                         tmp_pnt[n].y=(float)p->y;
  182.                                                 /*      xlist[n]=(float)p->x;
  183.                                                         ylist[n]=(float)p->y;*/
  184.                                                 }
  185.  
  186.                                                         src_pnt[0].x=tmp_pnt[0].x;
  187.                                                         src_pnt[0].y=tmp_pnt[0].y;
  188.                                                         src_pnt[1].x=tmp_pnt[3].x;
  189.                                                         src_pnt[1].y=tmp_pnt[3].y;
  190.                                                         src_pnt[2].x=tmp_pnt[2].x;
  191.                                                         src_pnt[2].y=tmp_pnt[2].y;
  192.                                                         src_pnt[3].x=tmp_pnt[1].x;
  193.                                                         src_pnt[3].y=tmp_pnt[1].y;
  194.  
  195.                                                 //Only pass the box information. I still do not know that either way
  196.                                                 cvGetPerspectiveTransform (tmp_pnt, dst_pnt, map_matrix);
  197.  
  198.                                                 //Deforming the inner square of the marker
  199.                                                 cvWarpPerspective (gray, marker_inside, map_matrix, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll (0));
  200.  
  201.                                                
  202.                                                 //Set the coordinates of the markers on the image.
  203.                                                 //create a Mat with points in src_pnt (perspective points)
  204.                                                 cvInitMatHeader (&image_points, 4, 1, CV_32FC2, src_pnt);
  205.  
  206.                                                 //Underlying set of marker coordinates
  207.                                                 // create Mat with real points with actual distances) (unwarped real 50*50 square)
  208.                                                 cvInitMatHeader (&object_points, 4, 3, CV_32FC1, baseMarkerPoints);
  209.  
  210.                                                 //cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic,distortion,rotation,translation);
  211. cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic_matrix,NULL,rotation,translation);
  212.  
  213.                                                 //Those obtained by using calculations that do come to any location on the screen coordinates of real space
  214.                                                 //cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic,distortion,dstPoints2D);
  215.  
  216.  int total = srcPoints3D->rows * srcPoints3D->cols * CV_MAT_CN(srcPoints3D->type);
  217.  
  218.  printf("%d",total);
  219.        
  220. cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic_matrix,NULL,dstPoints2D);
  221.                                                 //Draw a number of markers
  222.                                                 //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));
  223.                                                 //cvPutText(image, text,cvPoint(20,20), &axisfont, CV_RGB(255, 255, 100));
  224.                         }
  225.                 }
  226.  
  227.                 cvShowImage("Camera", image);
  228.  
  229.                 cvShowImage("marker_inside",marker_inside);
  230.  
  231.                 chr = cvWaitKey(50);
  232.                 if(chr ==27)
  233.                         break;
  234.  
  235.         }
  236.  
  237.         return 0;
  238. }
  239.