#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;
}