--- /dev/null
+/*\r
+ * A Demo of automatic samples capturing\r
+ * Author: Alexey Latyshev\r
+ */\r
+\r
+\r
+#ifdef WIN32\r
+#include "cv.h"\r
+#include "highgui.h"\r
+#else\r
+#include "opencv/cv.h"\r
+#include "opencv/highgui.h"\r
+#endif\r
+#include <vector>\r
+#include <string>\r
+#include <algorithm>\r
+#include <stdio.h>\r
+#include <ctype.h>\r
+#include "tracker_calibration.h"\r
+#include "tracker3D.h"\r
+#include "object_tracker.h"\r
+\r
+\r
+int VIDEO_CALIBRATION_DELAY = 1000;\r
+bool AUTOFIND_CHESSBOARD=true;\r
+bool USE_DEINTERLACING=false;\r
+bool DRAW_CORNERS = true;\r
+bool DRAW_CHESSBOARD = true;\r
+bool USE_UNDISTORTION = false;\r
+bool CALIBRATE_CAMERA = false;\r
+bool IS_VIDEO_CAPTURE = false;\r
+bool SAVE_DRAWINGS = false;\r
+bool SAVE_SAMPLES = false;\r
+bool SAVE_ALL_FRAMES = false;\r
+bool SHOW_TEST_SQUARE = false;\r
+bool USE_3D = false;\r
+char* SAMPLES_PATH;\r
+char* INPUT_VIDEO;\r
+char* OUTPUT_DIRECTORY;\r
+char* CAMERA_PARAMETERS_PATH;\r
+\r
+CvCapture* capture;\r
+IplImage* frame;\r
+IplImage* workImage;\r
+CvSize innerCornersCount;\r
+CvMat* IntrinsicMatrix;\r
+CvMat* DistortionCoeffs;\r
+\r
+\r
+\r
+// Image deinterlacing\r
+IplImage* Deinterlace(IplImage* src)\r
+{\r
+ IplImage* res = cvCloneImage(src);\r
+ uchar* linea;\r
+ uchar* lineb;\r
+ uchar* linec;\r
+\r
+ for (int i = 1; i < res->height-1; i+=2)\r
+ {\r
+ linea = (uchar*)res->imageData + ((i-1) * res->widthStep);\r
+ lineb = (uchar*)res->imageData + ((i) * res->widthStep);\r
+ linec = (uchar*)res->imageData + ((i+1) * res->widthStep);\r
+\r
+ for (int j = 0; j < res->width * res->nChannels; j++)\r
+ {\r
+ lineb[j] = (uchar)((linea[j] + linec[j])/2);\r
+ }\r
+ }\r
+\r
+ if (res->height > 1 && res->height % 2 == 0)\r
+ {\r
+ linea = (uchar*)res->imageData + ((res->height-2) * res->widthStep);\r
+ lineb = (uchar*)res->imageData + ((res->height-1) * res->widthStep);\r
+ memcpy(lineb, linea, res->width);\r
+ }\r
+ return res;\r
+\r
+}\r
+\r
+void InverseUndistortMap(const CvMat* mapx,const CvMat* mapy, CvMat** invmapx, CvMat** invmapy, bool interpolate)\r
+{\r
+ *invmapx=cvCreateMat(mapx->rows,mapx->cols,mapx->type);\r
+ *invmapy=cvCreateMat(mapy->rows,mapy->cols,mapy->type);\r
+ int x,y;\r
+ for (int i=0;i<mapx->cols;i++)\r
+ for (int j=0;j<mapy->rows;j++)\r
+ {\r
+ (*invmapx)->data.fl[i+(*invmapx)->cols*j]=-1e20f;\r
+ (*invmapy)->data.fl[i+(*invmapy)->cols*j]=-1e20f;\r
+ }\r
+\r
+ for (int i=0;i<mapx->cols;i++)\r
+ for (int j=0;j<mapy->rows;j++)\r
+ {\r
+ x = (int) mapx->data.fl[i+mapx->cols*j];\r
+ y = (int) mapy->data.fl[i+mapy->cols*j];\r
+ if ((x>=0) && (x<(*invmapx)->cols) && (y>=0) && (y < (*invmapy)->rows))\r
+ {\r
+ (*invmapx)->data.fl[x+(*invmapx)->cols*y]=(float)i;\r
+ (*invmapy)->data.fl[x+(*invmapy)->cols*y]=(float)j;\r
+ }\r
+ }\r
+ if (interpolate)\r
+ {\r
+ for (int i=1;i<mapx->cols-1;i++)\r
+ for (int j=1;j<mapy->rows-1;j++)\r
+ {\r
+ if ((*invmapx)->data.fl[i+(*invmapx)->cols*j]==-1e20)\r
+ {\r
+ (*invmapx)->data.fl[i+(*invmapx)->cols*j] = ((*invmapx)->data.fl[i-1+(*invmapx)->cols*j]+(*invmapx)->data.fl[i+1+(*invmapx)->cols*j]\r
+ + (*invmapx)->data.fl[i+(*invmapx)->cols*(j-1)]+(*invmapx)->data.fl[i+(*invmapx)->cols*(j+1)])/4;\r
+ }\r
+ if ((*invmapy)->data.fl[i+(*invmapy)->cols*j]==-1e20)\r
+ {\r
+ (*invmapy)->data.fl[i+(*invmapy)->cols*j] = ((*invmapy)->data.fl[i-1+(*invmapy)->cols*j]+(*invmapy)->data.fl[i+1+(*invmapy)->cols*j]\r
+ + (*invmapy)->data.fl[i+(*invmapy)->cols*(j-1)]+(*invmapy)->data.fl[i+(*invmapy)->cols*(j+1)])/4;\r
+ }\r
+ }\r
+ }\r
+}\r
+\r
+// left button click sets new point;\r
+// (!!!removed!!!)right button click removes last point\r
+void on_mouse( int event, int x, int y, int flags, void* param )\r
+{\r
+ switch( event )\r
+ {\r
+ case CV_EVENT_LBUTTONUP:\r
+ {\r
+\r
+ int n=0;\r
+ //for (n=0;(n<4) && ((*((CvPoint**)param))[n].x != -1);n++);\r
+ for (n=0;(n<4) && (((CvPoint*)param)[n].x != -1);n++);\r
+ if (n<4)\r
+ {\r
+ ((CvPoint*)param)[n].x = x;\r
+ ((CvPoint*)param)[n].y = y;\r
+ }\r
+ }\r
+ break;\r
+ }\r
+}\r
+\r
+\r
+//Load camera params from yaml file\r
+int LoadCameraParams(char* filename, CvMat** intrinsic_matrix, CvMat** distortion_coeffs)\r
+{\r
+ CvFileStorage* fs = cvOpenFileStorage( filename, 0, CV_STORAGE_READ );\r
+ if (fs==NULL) return 0;\r
+\r
+ *intrinsic_matrix = (CvMat*)cvReadByName( fs,0,"camera_matrix");\r
+ *distortion_coeffs = (CvMat*)cvReadByName( fs,0,"distortion_coefficients");\r
+\r
+ return 1;\r
+}\r
+\r
+\r
+int LoadApplicationParams(char* filename)\r
+{\r
+ CvFileStorage* fs = cvOpenFileStorage( filename, 0, CV_STORAGE_READ );\r
+ if (fs==NULL) return 0;\r
+\r
+ USE_DEINTERLACING = cvReadIntByName( fs,0,"USE_DEINTERLACING",0) != 0;\r
+ USE_UNDISTORTION = cvReadIntByName( fs,0,"USE_UNDISTORTION",0) != 0;\r
+ CALIBRATE_CAMERA = cvReadIntByName( fs,0,"CALIBRATE_CAMERA",0) != 0;\r
+ DRAW_CORNERS = cvReadIntByName( fs,0,"DRAW_CORNERS",1) != 0;\r
+ DRAW_CHESSBOARD = cvReadIntByName( fs,0,"DRAW_CHESSBOARD",1) != 0;\r
+ AUTOFIND_CHESSBOARD = cvReadIntByName( fs,0,"AUTOFIND_CHESSBOARD",1) != 0;\r
+ IS_VIDEO_CAPTURE = cvReadIntByName( fs,0,"IS_VIDEO_CAPTURE",0) != 0;\r
+ SAVE_DRAWINGS = cvReadIntByName( fs,0,"SAVE_DRAWINGS",0) != 0;\r
+ SAVE_SAMPLES = cvReadIntByName( fs,0,"SAVE_SAMPLES",0) != 0;\r
+ SAVE_ALL_FRAMES = cvReadIntByName( fs,0,"SAVE_ALL_FRAMES",0) != 0;\r
+ SHOW_TEST_SQUARE = cvReadIntByName( fs,0,"SHOW_TEST_SQUARE",0) != 0;\r
+ USE_3D = cvReadIntByName( fs,0,"USE_3D",0) != 0;\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ SAMPLES_PATH = new char[500];\r
+ if (cvReadStringByName( fs,0,"SAMPLES_PATH"))\r
+ strcpy(SAMPLES_PATH, cvReadStringByName( fs,0,"SAMPLES_PATH"));\r
+ else return 0;\r
+ }\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ INPUT_VIDEO = new char[500];\r
+ if (cvReadStringByName( fs,0,"INPUT_VIDEO"))\r
+ strcpy(INPUT_VIDEO,cvReadStringByName( fs,0,"INPUT_VIDEO") );//input video filename\r
+ else return 0;\r
+ }\r
+\r
+ OUTPUT_DIRECTORY = new char[500];\r
+ if (cvReadStringByName( fs,0,"OUTPUT_DIRECTORY"))\r
+ strcpy(OUTPUT_DIRECTORY,cvReadStringByName( fs,0,"OUTPUT_DIRECTORY"));//output directory for images and selected regions path\r
+ else return 0;\r
+ //chessboard inner corners count (width x height)\r
+ int CHESSBOARD_WIDTH=cvReadIntByName( fs,0,"CHESSBOARD_WIDTH");\r
+ int CHESSBOARD_HEIGHT=cvReadIntByName( fs,0,"CHESSBOARD_HEIGHT");\r
+ innerCornersCount=cvSize(CHESSBOARD_WIDTH,CHESSBOARD_HEIGHT);\r
+\r
+\r
+ CAMERA_PARAMETERS_PATH = new char[500];\r
+ if (cvReadStringByName( fs,0,"CAMERA_PARAMETERS_PATH"))\r
+ strcpy(CAMERA_PARAMETERS_PATH, cvReadStringByName( fs,0,"CAMERA_PARAMETERS_PATH"));//camera parameters filename\r
+ else \r
+ return 0;\r
+ if (LoadCameraParams(CAMERA_PARAMETERS_PATH,&IntrinsicMatrix,&DistortionCoeffs))\r
+ {\r
+ printf("\nCamera parameters loaded successfully\n");\r
+ }\r
+ else\r
+ {\r
+ printf("\nUnable to load parameters\n");\r
+ if (USE_UNDISTORTION && (!CALIBRATE_CAMERA))\r
+ return 0;\r
+ }\r
+\r
+\r
+ //cvReleaseFileStorage(&fs);\r
+ return 1;\r
+}\r
+\r
+\r
+IplImage* Undistort(IplImage* src,const CvMat* intrinsic_matrix,const CvMat* distortion_coeffs)\r
+{\r
+ IplImage* undistortImg;\r
+ undistortImg = cvCreateImage(cvSize(src->width,src->height),src->depth,src->nChannels);\r
+ cvUndistort2(src,undistortImg,intrinsic_matrix,distortion_coeffs);\r
+\r
+ return undistortImg;\r
+}\r
+\r
+int ShowFrame(int pos)\r
+{\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,pos);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ IplImage* undistortImg = 0;\r
+ cvReleaseImage(&workImage);\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+\r
+ undistortImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs); \r
+ workImage = cvCloneImage(undistortImg);\r
+ cvReleaseImage(&undistortImg);\r
+ }\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ return 1;\r
+ }\r
+ return 0;\r
+}\r
+\r
+\r
+//Calculates selected points coordinates (by x and y) in coordinate system connected with chessboard\r
+//(zero point is in the upper-left corner of the chessboard, axises are horizontal and vertical chessboard edges)\r
+// in: nFrame (number of frame on which points were selected), points (CvPoint[4] array with selected points)\r
+// out: relCoords (pointer to the array[4] with (x1,y1) coordinate of every point)\r
+// Function returns 0 if there is no chessboard found and array 2xN with corners otherwise\r
+// Camera Matrix is for 3D relative position estimation with z start coordinate\r
+// We also can give found Chessboard to the method instead of running chessboard finding algorithm\r
+CvMat* CalcRelativePosition(IplImage* workImage, CvPoint* points, CvPoint2D32f** relCoords, CvMat* cameraMatrix=NULL, float z = 0, CvMat* Chessboard=NULL)\r
+{\r
+ CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ int count;\r
+\r
+ if (!Chessboard)\r
+ {\r
+ if (cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0)\r
+ {\r
+ delete[] corners;\r
+ return NULL;\r
+ }\r
+\r
+ IplImage* view_gray = cvCreateImage( cvGetSize(workImage), 8, 1 );\r
+ cvCvtColor(workImage, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+ }\r
+ else\r
+ {\r
+ for (int i=0;i<Chessboard->cols;i++)\r
+ {\r
+ corners[i].x = (float)Chessboard->data.db[i];\r
+ corners[i].y = (float)Chessboard->data.db[Chessboard->cols+i];\r
+ } \r
+ }\r
+\r
+ if (!cameraMatrix)\r
+ {\r
+ CvPoint2D32f src[3];\r
+ CvPoint2D32f dst[3];\r
+ src[0].x = corners[0].x;\r
+ src[0].y = corners[0].y;\r
+ src[1].x = corners[innerCornersCount.width-1].x;\r
+ src[1].y = corners[innerCornersCount.width-1].y;\r
+ src[2].x = corners[innerCornersCount.width*(innerCornersCount.height-1)].x;\r
+ src[2].y = corners[innerCornersCount.width*(innerCornersCount.height-1)].y;\r
+ dst[0].x = 0;\r
+ dst[0].y = 0;\r
+ dst[1].x = innerCornersCount.width-1.f;\r
+ dst[1].y = 0;\r
+ dst[2].x = 0;\r
+ dst[2].y = innerCornersCount.height-1.f;\r
+\r
+ CvMat* map_matrix = cvCreateMat(2,3,CV_64F);\r
+ cvGetAffineTransform(src,dst,map_matrix);\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ (*relCoords)[i].x=(float)(points[i].x*(map_matrix->data.db[0])+points[i].y*(map_matrix->data.db[1])+(map_matrix->data.db[2]));\r
+ (*relCoords)[i].y=(float)(points[i].x*(map_matrix->data.db[3])+points[i].y*(map_matrix->data.db[4])+(map_matrix->data.db[5]));\r
+ }\r
+ cvReleaseMat(&map_matrix);\r
+ }\r
+ else\r
+ {\r
+ CvMat* image_points = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ CvMat* object_points = cvCreateMat(3,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+\r
+ // Sets object points and image points\r
+ for (int i=0; i< innerCornersCount.height;i++)\r
+ for (int j=0; j < innerCornersCount.width;j++)\r
+ {\r
+ object_points->data.db[(i*innerCornersCount.width+j)]=j;\r
+ object_points->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=i;\r
+ object_points->data.db[(i*innerCornersCount.width+j)+2*innerCornersCount.width*innerCornersCount.height]=0.0f;\r
+\r
+ image_points->data.db[(i*innerCornersCount.width+j)]=corners[(i*innerCornersCount.width+j)].x;\r
+ image_points->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=corners[(i*innerCornersCount.width+j)].y;\r
+ }\r
+\r
+ CvMat* R = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* T = cvCreateMat(3, 1, CV_64FC1);\r
+ CvMat* rotation_vector = cvCreateMat(3,1,CV_64FC1);\r
+ //Calculating Exrinsic camera parameters\r
+ CvMat* distCoeffs = cvCreateMat(5,1,CV_64FC1);\r
+ cvZero(distCoeffs);\r
+ cvFindExtrinsicCameraParams2(object_points,image_points,cameraMatrix, distCoeffs,rotation_vector,T);\r
+ cvReleaseMat(&distCoeffs);\r
+ cvRodrigues2(rotation_vector,R);\r
+ CvMat* M = cvCreateMat(3, 4, CV_64FC1);\r
+ CvMat* A = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* invA = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* point3D = cvCreateMat(3, 1, CV_64FC1);\r
+ CvMat* t = cvCreateMat(3, 1, CV_64FC1);\r
+\r
+ cvGEMM(cameraMatrix,R,1.0,NULL,0,A);\r
+ cvGEMM(cameraMatrix,T,1.0,NULL,0,t);\r
+\r
+ t->data.db[0]+=z*A->data.db[2];\r
+ t->data.db[1]+=z*A->data.db[5];\r
+ t->data.db[2]+=z*A->data.db[8];\r
+\r
+ t->data.db[0]=-t->data.db[0];\r
+ t->data.db[1]=-t->data.db[1];\r
+ t->data.db[2]=-t->data.db[2];\r
+\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ A->data.db[2]=-points[i].x;\r
+ A->data.db[5]=-points[i].y;\r
+ A->data.db[8]=-1.0f;\r
+\r
+ cvInvert(A,invA);\r
+ cvGEMM(invA,t,1.0,NULL,0,point3D);\r
+ (*relCoords)[i].x = (float)(point3D->data.db[0]);\r
+ (*relCoords)[i].y = (float)(point3D->data.db[1]);\r
+\r
+ //TEST\r
+ CvMat* d3 = cvCreateMat(1,1,CV_64FC3);\r
+ d3->data.db[0] = point3D->data.db[0];\r
+ d3->data.db[1] = point3D->data.db[1];\r
+ d3->data.db[2] = z;\r
+ distCoeffs = cvCreateMat(5,1,CV_64FC1);\r
+ CvMat* imP = cvCreateMat(1,1,CV_64FC2);\r
+ cvZero(distCoeffs);\r
+ \r
+ cvProjectPoints2(d3,rotation_vector,T,cameraMatrix,distCoeffs,imP);\r
+ cvReleaseMat(&imP);\r
+ cvReleaseMat(&d3);\r
+ cvReleaseMat(&distCoeffs);\r
+ //END OF\r
+ }\r
+\r
+ cvReleaseMat(&T);\r
+ cvReleaseMat(&t);\r
+ cvReleaseMat(&R);\r
+ cvReleaseMat(&rotation_vector);\r
+ cvReleaseMat(&M);\r
+ cvReleaseMat(&A);\r
+ cvReleaseMat(&invA);\r
+ cvReleaseMat(&point3D);\r
+ \r
+ }\r
+ if (!Chessboard)\r
+ {\r
+ CvMat* result = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ for (int i=0;i<result->cols;i++)\r
+ {\r
+ result->data.db[i]=corners[i].x;\r
+ result->data.db[result->cols+i]=corners[i].y;\r
+ } \r
+ \r
+ delete[] corners;\r
+ return result;\r
+ }\r
+ else\r
+ {\r
+ return NULL;\r
+ }\r
+\r
+}\r
+\r
+// if chessboardPoints = NULL using simle affine transform otherwise we must have correct oldPoints pointer\r
+// chessBoardPoints is array 2xN\r
+// returns new points location\r
+CvPoint* GetCurrentPointsPosition(IplImage* workImage, CvPoint2D32f* relCoords, CvMat* chessboardPoints, CvPoint* oldPoints, CvPoint2D32f* outCorners)\r
+{\r
+ CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ int count;\r
+\r
+ if (cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0)\r
+ {\r
+ delete[] corners;\r
+ corners = NULL;\r
+ return NULL;\r
+ }\r
+\r
+ IplImage* view_gray = cvCreateImage( cvGetSize(workImage), 8, 1 );\r
+ cvCvtColor(workImage, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+\r
+ CvPoint* result = new CvPoint[4];\r
+\r
+ if (chessboardPoints && oldPoints)\r
+ {\r
+ CvMat* chessboardPoints2 = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ for (int i=0;i<(chessboardPoints2)->cols;i++)\r
+ {\r
+ chessboardPoints2->data.db[i]=(corners)[i].x;\r
+ chessboardPoints2->data.db[chessboardPoints2->cols+i]=(corners)[i].y;\r
+ }\r
+ CvMat* homography = cvCreateMat(3,3,CV_64FC1);\r
+\r
+ cvFindHomography(chessboardPoints,chessboardPoints2,homography);\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ double t = oldPoints[i].x*(homography->data.db[6])+oldPoints[i].y*(homography->data.db[7])+(homography->data.db[8]);\r
+ result[i].x = (int)((oldPoints[i].x*(homography->data.db[0])+oldPoints[i].y*(homography->data.db[1])+(homography->data.db[2]))/t);\r
+ result[i].y = (int)((oldPoints[i].x*(homography->data.db[3])+oldPoints[i].y*(homography->data.db[4])+(homography->data.db[5]))/t);\r
+\r
+ }\r
+ cvReleaseMat(&homography);\r
+ }\r
+\r
+ else\r
+ {\r
+ CvPoint2D32f src[3];\r
+ CvPoint2D32f dst[3];\r
+ dst[0].x = (corners)[0].x;\r
+ dst[0].y = (corners)[0].y;\r
+ dst[1].x = (corners)[innerCornersCount.width-1].x;\r
+ dst[1].y = (corners)[innerCornersCount.width-1].y;\r
+ dst[2].x = (corners)[innerCornersCount.width*(innerCornersCount.height-1)].x;\r
+ dst[2].y = (corners)[innerCornersCount.width*(innerCornersCount.height-1)].y;\r
+ src[0].x = 0;\r
+ src[0].y = 0;\r
+ src[1].x = innerCornersCount.width-1.f;\r
+ src[1].y = 0;\r
+ src[2].x = 0;\r
+ src[2].y = innerCornersCount.height-1.f;\r
+\r
+ CvMat* map_matrix = cvCreateMat(2,3,CV_64F);\r
+ cvGetAffineTransform(src,dst,map_matrix);\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ result[i].x=(int)(relCoords[i].x*(map_matrix->data.db[0])+relCoords[i].y*(map_matrix->data.db[1])+(map_matrix->data.db[2]));\r
+ result[i].y=(int)(relCoords[i].x*(map_matrix->data.db[3])+relCoords[i].y*(map_matrix->data.db[4])+(map_matrix->data.db[5]));\r
+ }\r
+ cvReleaseMat(&map_matrix);\r
+ }\r
+\r
+ if (outCorners)\r
+ {\r
+ for (int i=0;i<innerCornersCount.height*innerCornersCount.width;i++)\r
+ {\r
+ outCorners[i].x=corners[i].x;\r
+ outCorners[i].y=corners[i].y;\r
+ }\r
+ }\r
+\r
+ delete[] corners;\r
+\r
+ return result;\r
+}\r
+\r
+\r
+\r
+\r
+IplImage* GetSample(const IplImage* src,CvSize innerCornersCount, const CvPoint* points, CvPoint2D32f* chessboardCorners)\r
+{\r
+ CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ int count;\r
+\r
+ if (!chessboardCorners)\r
+ {\r
+ if (cvFindChessboardCorners(src,innerCornersCount,corners,&count)==0)\r
+ {\r
+ delete[] corners;\r
+ return NULL;\r
+ }\r
+\r
+ IplImage* view_gray = cvCreateImage( cvGetSize(src), 8, 1 );\r
+ cvCvtColor(src, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+ }\r
+ else\r
+ {\r
+ for (int i=0;i<innerCornersCount.height*innerCornersCount.width;i++)\r
+ {\r
+ corners[i]=chessboardCorners[i];\r
+ }\r
+\r
+ }\r
+\r
+ CvPoint result[4];\r
+ CvPoint2D32f top;\r
+ CvPoint2D32f bot;\r
+ top.x = corners[0].x < corners[(innerCornersCount.height-1)*innerCornersCount.width].x ? corners[0].x : corners[(innerCornersCount.height-1)*innerCornersCount.width].x;\r
+ top.y = corners[0].y < corners[innerCornersCount.width-1].y ? corners[0].y : corners[innerCornersCount.width-1].y;\r
+ bot.x = corners[innerCornersCount.width-1].x < corners[innerCornersCount.width*innerCornersCount.height-1].x ? corners[innerCornersCount.width*innerCornersCount.height-1].x : corners[innerCornersCount.width-1].x;\r
+ bot.y = corners[innerCornersCount.height*innerCornersCount.width-1].y < corners[(innerCornersCount.height-1)*innerCornersCount.width].y ?\r
+ corners[(innerCornersCount.height-1)*innerCornersCount.width].y : corners[innerCornersCount.height*innerCornersCount.width-1].y;\r
+\r
+ CvMat* chessboardPoints = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ CvMat* chessboardPoints2 = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+\r
+ //assumes that chessboard squares are squared\r
+ float step = (bot.x-top.x)/(innerCornersCount.width-1) > (bot.y-top.y)/(innerCornersCount.height-1) ? (bot.x-top.x)/(innerCornersCount.width-1) : (bot.y-top.y)/(innerCornersCount.height-1);\r
+ for (int i=0;i<innerCornersCount.width;i++)\r
+ for (int j=0;j<innerCornersCount.height;j++)\r
+ {\r
+ chessboardPoints2->data.db[i+j*innerCornersCount.width]=top.x+i*step;\r
+ chessboardPoints2->data.db[i+j*(innerCornersCount.width)+innerCornersCount.width*innerCornersCount.height]=\r
+ top.y+j*step;\r
+ }\r
+\r
+ for (int i=0;i<(chessboardPoints)->cols;i++)\r
+ {\r
+ chessboardPoints->data.db[i]=(corners)[i].x;\r
+ chessboardPoints->data.db[chessboardPoints->cols+i]=(corners)[i].y;\r
+ }\r
+ CvMat* homography = cvCreateMat(3,3,CV_64FC1);\r
+\r
+ cvFindHomography(chessboardPoints,chessboardPoints2,homography);\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ double t = points[i].x*(homography->data.db[6])+points[i].y*(homography->data.db[7])+(homography->data.db[8]);\r
+ result[i].x = (int)((points[i].x*(homography->data.db[0])+points[i].y*(homography->data.db[1])+(homography->data.db[2]))/t);\r
+ result[i].y = (int)((points[i].x*(homography->data.db[3])+points[i].y*(homography->data.db[4])+(homography->data.db[5]))/t);\r
+ }\r
+\r
+ IplImage* resImage= cvCloneImage(src);\r
+ cvWarpPerspective( src, resImage, homography);\r
+ CvRect rect;\r
+ rect.x = result[0].x<result[2].x ? result[0].x : result[2].x;\r
+ rect.y = result[0].y<result[1].y ? result[0].y : result[1].y;\r
+ rect.width = result[1].x<result[3].x ? (result[3].x-rect.x) : (result[1].x-rect.x);\r
+ rect.height = result[2].y<result[3].y ? (result[3].y-rect.y) : (result[2].y-rect.y);\r
+ if ((rect.x>=0) && (rect.y >=0) && ((rect.x+rect.width)<resImage->width)&&((rect.y+rect.height)<resImage->height))\r
+ cvSetImageROI(resImage,rect);\r
+ else\r
+ return NULL;\r
+\r
+ cvReleaseMat(&homography);\r
+ return resImage;\r
+ delete[] corners;\r
+}\r
+\r
+void createSamples2DObject(int argc, char** argv)\r
+{\r
+ CvPoint points[4];\r
+ CvPoint pointsChessboardSquare[4];\r
+\r
+ CvPoint2D32f* relCoords = new CvPoint2D32f[4];\r
+ CvPoint2D32f* relCoordsChessboardSquare = new CvPoint2D32f[4];\r
+ CvPoint * newPoints;\r
+ CvPoint * newPointsChessboardSquare;\r
+ int currentFrame=0;\r
+ int key;\r
+ IplImage* undistortedImg;\r
+\r
+ if (CALIBRATE_CAMERA)\r
+ {\r
+ int v = IS_VIDEO_CAPTURE ? 10 : 7;\r
+ char** c = new char*[v];\r
+ c[0]=new char[11]; c[0]="calibration";\r
+ c[1]=new char[2]; c[1]="-w";\r
+ c[2] = new char[2]; sprintf(c[2],"%d",innerCornersCount.width);\r
+ c[3]=new char[2]; c[3]="-h";\r
+ c[4] = new char[2]; sprintf(c[4],"%d",innerCornersCount.height);\r
+ c[5] = new char[2]; c[5]="-d";\r
+ c[6] = new char[5]; sprintf(c[6],"%d",VIDEO_CALIBRATION_DELAY);\r
+ c[7]=new char[2]; c[7]="-o";\r
+ c[8]=new char[100];\r
+ sprintf(c[8],"%scamera.yml",OUTPUT_DIRECTORY);\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ c[9]=new char[strlen(INPUT_VIDEO)+1];\r
+ strcpy(c[9],INPUT_VIDEO);\r
+ }\r
+\r
+ calibrate(v,c);\r
+ LoadCameraParams(c[8],&IntrinsicMatrix, &DistortionCoeffs);\r
+\r
+ // Add functionality for camera calibration\r
+ }\r
+\r
+ cvNamedWindow(FRAME_WINDOW,1);\r
+ capture = IS_VIDEO_CAPTURE ? cvCreateFileCapture(INPUT_VIDEO) : cvCreateCameraCapture(0);\r
+\r
+ if (!IS_VIDEO_CAPTURE)\r
+ {\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,640);\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,480);\r
+ }\r
+\r
+ cvResizeWindow(FRAME_WINDOW,(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH),(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT));\r
+\r
+ CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ int count;\r
+\r
+ do\r
+ {\r
+ if (IS_VIDEO_CAPTURE)\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,++currentFrame);\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ //if (!IS_VIDEO_CAPTURE)\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ key = cvWaitKey(30);\r
+ if (key==27) return;\r
+ }\r
+\r
+ }\r
+ while ( frame && (((AUTOFIND_CHESSBOARD && IS_VIDEO_CAPTURE)|| (!IS_VIDEO_CAPTURE)) && cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0));\r
+\r
+ if (frame == NULL)\r
+ {\r
+ printf("\n Unable to load video with chessboard or connect to the camera");\r
+ return;\r
+ }\r
+\r
+ delete[] corners;\r
+ currentFrame--;\r
+\r
+ IplImage* firstImage=cvCloneImage(workImage);\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+\r
+ points[0].x =-1;\r
+ points[1].x =-1;\r
+ points[2].x =-1;\r
+ points[3].x =-1;\r
+\r
+ printf("\n Select the quadrangle region by selectig its vertices on the frame with a mouse\n");\r
+ cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points);\r
+\r
+ // Getting initial points position\r
+ for (int currPoint=0;currPoint<4;)\r
+ {\r
+ if (points[currPoint].x != -1)\r
+ {\r
+ cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),2,cvScalar(255,255,255));\r
+ cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),3,cvScalar(0));\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ currPoint++;\r
+ }\r
+ key = cvWaitKey(30);\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ switch (key)\r
+ {\r
+ case 32: // Space symbol\r
+ if (ShowFrame(++currentFrame))\r
+ {\r
+ points[0].x =-1;\r
+ points[1].x =-1;\r
+ points[2].x =-1;\r
+ points[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame--;\r
+ }\r
+ break;\r
+ case 8: // Backspace symbol\r
+ if (currentFrame>0)\r
+ {\r
+ if (ShowFrame(--currentFrame))\r
+ {\r
+ points[0].x =-1;\r
+ points[1].x =-1;\r
+ points[2].x =-1;\r
+ points[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame++;\r
+ }\r
+ }\r
+ break;\r
+ }\r
+ }\r
+ }\r
+\r
+ // sorting points\r
+\r
+ for (int i=1;i<4;i++)\r
+ {\r
+\r
+ if (points[i].y<points[0].y)\r
+ {\r
+ CvPoint temp;\r
+ temp = points[0];\r
+ points[0]=points[i];\r
+ points[i]=temp;\r
+ }\r
+ }\r
+ if (points[1].x<points[0].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points[0];\r
+ points[0]=points[1];\r
+ points[1]=temp;\r
+ }\r
+\r
+ if (points[3].x<points[2].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points[3];\r
+ points[3]=points[2];\r
+ points[2]=temp;\r
+ }\r
+\r
+ //end of sorting\r
+\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(firstImage);\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvReleaseImage(&firstImage);\r
+\r
+\r
+ CvMat* chessboardPointsMat = CalcRelativePosition(workImage, points,&relCoords);\r
+ if (SHOW_TEST_SQUARE && chessboardPointsMat)\r
+ {\r
+ pointsChessboardSquare[0].x = (int)chessboardPointsMat->data.db[(innerCornersCount.height-1)*innerCornersCount.width-2];\r
+ pointsChessboardSquare[1].x = (int)chessboardPointsMat->data.db[(innerCornersCount.height-1)*innerCornersCount.width-1];\r
+ pointsChessboardSquare[3].x = (int)chessboardPointsMat->data.db[innerCornersCount.height*innerCornersCount.width-2];\r
+ pointsChessboardSquare[2].x = (int)chessboardPointsMat->data.db[innerCornersCount.height*innerCornersCount.width-1];\r
+\r
+ pointsChessboardSquare[0].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+(innerCornersCount.height-1)*innerCornersCount.width-2];\r
+ pointsChessboardSquare[1].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+(innerCornersCount.height-1)*innerCornersCount.width-1];\r
+ pointsChessboardSquare[3].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+innerCornersCount.height*innerCornersCount.width-2];\r
+ pointsChessboardSquare[2].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+innerCornersCount.height*innerCornersCount.width-1];\r
+ CalcRelativePosition(workImage,pointsChessboardSquare,&relCoordsChessboardSquare);\r
+ }\r
+\r
+ char* PATH = OUTPUT_DIRECTORY;\r
+ FILE* f;\r
+ char path[100];\r
+ char path_frames[100];\r
+ sprintf(path_frames,"%sframes.txt",PATH);\r
+ remove(path_frames);\r
+ char cmd[100];\r
+#ifdef WIN32\r
+ sprintf(cmd,"mkdir %s",PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"del %s*.* /q",PATH);\r
+ system(cmd);\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(cmd,"mkdir %s",SAMPLES_PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"del %s*.* /q",SAMPLES_PATH);\r
+ system(cmd);\r
+ }\r
+#else\r
+\r
+ sprintf(cmd,"mkdir %s",PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"rm -f %s*.*",PATH);\r
+ system(cmd);\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(cmd,"mkdir %s",SAMPLES_PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"rm -f %s*.*",SAMPLES_PATH);\r
+ system(cmd);\r
+ }\r
+#endif\r
+\r
+ do\r
+ {\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+\r
+ if (chessboardPointsMat)\r
+ {\r
+ CvPoint2D32f* ChessboardCorners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ newPoints = GetCurrentPointsPosition(workImage,relCoords,chessboardPointsMat,points,ChessboardCorners/*&newPointsMat*/);\r
+\r
+ if (newPoints)\r
+ {\r
+ if (SHOW_TEST_SQUARE)\r
+ {\r
+ newPointsChessboardSquare = GetCurrentPointsPosition(workImage,relCoordsChessboardSquare,chessboardPointsMat,pointsChessboardSquare,ChessboardCorners);\r
+ cvFillConvexPoly(workImage,newPointsChessboardSquare,4,cvScalar(10,10,250));\r
+ }\r
+ bool areOnFrame=true; // are points on frame or not\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ if ((newPoints[i].x>workImage->width)||(newPoints[i].x<0)||(newPoints[i].y>workImage->height)||(newPoints[i].y<0))\r
+ areOnFrame=false;\r
+ }\r
+\r
+ if (areOnFrame)\r
+ {\r
+ f = fopen(path_frames,"a");\r
+ sprintf(path,"%s%d.jpg",PATH,currentFrame+1);\r
+ fprintf(f,"%s,%d,%d,%d,%d,%d,%d,%d,%d\n",path,newPoints[0].x,newPoints[0].y,newPoints[1].x,newPoints[1].y,newPoints[2].x,newPoints[2].y,newPoints[3].x,newPoints[3].y);\r
+ fclose(f);\r
+\r
+ if (DRAW_CHESSBOARD)\r
+ cvDrawChessboardCorners(workImage,innerCornersCount,ChessboardCorners,innerCornersCount.height*innerCornersCount.width,1);\r
+ if (DRAW_CORNERS)\r
+ {\r
+ cvCircle(workImage,newPoints[0],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[0],3,cvScalar(0));\r
+ cvCircle(workImage,newPoints[1],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[1],3,cvScalar(0));\r
+ cvCircle(workImage,newPoints[2],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[2],3,cvScalar(0));\r
+ cvCircle(workImage,newPoints[3],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[3],3,cvScalar(0));\r
+ }\r
+ if (SAVE_DRAWINGS)\r
+ cvSaveImage(path,workImage);\r
+ else\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ {\r
+ IplImage* tmp = Deinterlace(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(tmp,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&tmp);\r
+ tmp = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvSaveImage(path,tmp);\r
+ cvReleaseImage(&tmp);\r
+ }\r
+ else\r
+ {\r
+ if (!USE_UNDISTORTION)\r
+ cvSaveImage(path,frame);\r
+ else\r
+ {\r
+ undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);\r
+ cvSaveImage(path,undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ }\r
+ }\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(path,"%s%d.jpg",SAMPLES_PATH,currentFrame+1);\r
+ IplImage* tmp;\r
+ if (!USE_UNDISTORTION)\r
+ tmp = GetSample(frame,innerCornersCount,newPoints,ChessboardCorners);\r
+ else\r
+ {\r
+ undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);\r
+ tmp = GetSample(undistortedImg,innerCornersCount,newPoints,ChessboardCorners);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ if (tmp)\r
+ {\r
+ cvSaveImage(path,tmp);\r
+ cvReleaseImage(&tmp);\r
+ }\r
+ }\r
+ printf("Frame %d successfully saved to %s\n",currentFrame+1,path);\r
+ delete[] ChessboardCorners;\r
+ ChessboardCorners = NULL;\r
+ }\r
+\r
+ delete[] newPoints;\r
+ newPoints = NULL;\r
+ }\r
+ else\r
+ if (SAVE_ALL_FRAMES)\r
+ {\r
+ sprintf(path,"%s%d.jpg",PATH,currentFrame+1);\r
+ cvSaveImage(path,workImage);\r
+ }\r
+\r
+ }\r
+\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ key = cvWaitKey(30);\r
+ }\r
+ currentFrame++;\r
+ }\r
+ while (frame && (key!=27));\r
+\r
+ if (capture)\r
+ cvReleaseCapture(&capture);\r
+}\r
+//--------------------------\r
+\r
+void createSamples3DObject(int argc, char** argv)\r
+{\r
+ CvPoint points1[4];\r
+ CvPoint points2[4];\r
+\r
+ IplImage* i1;\r
+ IplImage* i2;\r
+ IplImage* undistortedImg;\r
+\r
+ CvPoint3D32f* points3D = new CvPoint3D32f[8];\r
+ CvPoint* newPoints = new CvPoint[8];\r
+\r
+ int currentFrame=0;\r
+ int key;\r
+ int boxDepthValue=10;\r
+ int maxErrorValue=10;\r
+\r
+\r
+ if (CALIBRATE_CAMERA)\r
+ {\r
+ int v = 10;/*IS_VIDEO_CAPTURE ? 10 : 7*/;\r
+ char** c = new char*[v];\r
+ c[0] = new char[11]; c[0]="calibration";\r
+ c[1] = new char[2]; c[1]="-w";\r
+ c[2] = new char[2]; sprintf(c[2],"%d",innerCornersCount.width);\r
+ c[3] = new char[2]; c[3]="-h";\r
+ c[4] = new char[2]; sprintf(c[4],"%d",innerCornersCount.height);\r
+ c[5] = new char[2]; c[5]="-d";\r
+ c[6] = new char[5]; sprintf(c[6],"%d",VIDEO_CALIBRATION_DELAY);\r
+ c[7] = new char[2]; c[7]="-o";\r
+ c[8] = new char[100];\r
+ sprintf(c[8],"%scamera.yml",OUTPUT_DIRECTORY);\r
+ c[9]=new char[strlen(INPUT_VIDEO)+1];\r
+ strcpy(c[9],INPUT_VIDEO);\r
+\r
+ calibrate(v,c);\r
+ LoadCameraParams(c[8],&IntrinsicMatrix, &DistortionCoeffs);\r
+\r
+ // Add functionality for camera calibration\r
+ }\r
+\r
+ cvNamedWindow(FRAME_WINDOW,1);\r
+ cvCreateTrackbar(DEPTH_TRACKBAR,FRAME_WINDOW,&boxDepthValue,MAX_DEPTH_VALUE,NULL);\r
+ cvCreateTrackbar(ERROR_TRACKBAR,FRAME_WINDOW,&maxErrorValue,MAX_ERROR_VALUE,NULL);\r
+ capture =cvCreateFileCapture(INPUT_VIDEO);\r
+\r
+ cvResizeWindow(FRAME_WINDOW,(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH),(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT));\r
+\r
+ CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+\r
+ if (capture == NULL)\r
+ {\r
+ printf("\n Unable to load video with chessboard or connect to the camera");\r
+ return;\r
+ }\r
+\r
+ delete[] corners;\r
+\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ }\r
+\r
+ IplImage* firstImage=cvCloneImage(workImage);\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+\r
+ do\r
+ {\r
+ points1[0].x =-1;\r
+ points1[1].x =-1;\r
+ points1[2].x =-1;\r
+ points1[3].x =-1;\r
+ points2[0].x =-1;\r
+ points2[1].x =-1;\r
+ points2[2].x =-1;\r
+ points2[3].x =-1;\r
+\r
+ printf("\nSelect the quadrangle region by selecting its vertices on the frame with a mouse\nUse Space and Backspace to go forward or backward\nPress Esc key to exit");\r
+\r
+ cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points1);\r
+\r
+ // Getting initial points position\r
+ for (int currPoint=0;currPoint<4;)\r
+ {\r
+ if (points1[currPoint].x != -1)\r
+ {\r
+ cvCircle(workImage,cvPoint(points1[currPoint].x,points1[currPoint].y),2,cvScalar(255,255,255));\r
+ cvCircle(workImage,cvPoint(points1[currPoint].x,points1[currPoint].y),3,cvScalar(0));\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ currPoint++;\r
+ }\r
+ key = cvWaitKey(30);\r
+ switch (key)\r
+ {\r
+ case 32: // Space symbol\r
+ if (ShowFrame(++currentFrame))\r
+ {\r
+ points1[0].x =-1;\r
+ points1[1].x =-1;\r
+ points1[2].x =-1;\r
+ points1[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame--;\r
+ }\r
+ break;\r
+ case 8: // Backspace symbol\r
+ if (currentFrame>0)\r
+ {\r
+ if (ShowFrame(--currentFrame))\r
+ {\r
+ points1[0].x =-1;\r
+ points1[1].x =-1;\r
+ points1[2].x =-1;\r
+ points1[3].x =-1;\r
+\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame++;\r
+ }\r
+ }\r
+ break;\r
+ }\r
+ }\r
+\r
+ i1 = cvCloneImage(workImage);\r
+\r
+ // sorting points\r
+\r
+ for (int i=1;i<4;i++)\r
+ {\r
+\r
+ if (points1[i].y<points1[0].y)\r
+ {\r
+ CvPoint temp;\r
+ temp = points1[0];\r
+ points1[0]=points1[i];\r
+ points1[i]=temp;\r
+ }\r
+ }\r
+ if (points1[1].x<points1[0].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points1[0];\r
+ points1[0]=points1[1];\r
+ points1[1]=temp;\r
+ }\r
+\r
+ if (points1[3].x<points1[2].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points1[3];\r
+ points1[3]=points1[2];\r
+ points1[2]=temp;\r
+ }\r
+\r
+ //end of sorting\r
+\r
+\r
+ ShowFrame(currentFrame);\r
+ //second frame\r
+ cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points2);\r
+\r
+ printf("\nSelect one more time on another frame\n");\r
+ // Getting initial points position\r
+ for (int currPoint=0;currPoint<4;)\r
+ {\r
+ if (points2[currPoint].x != -1)\r
+ {\r
+ cvCircle(workImage,cvPoint(points2[currPoint].x,points2[currPoint].y),2,cvScalar(255,255,255));\r
+ cvCircle(workImage,cvPoint(points2[currPoint].x,points2[currPoint].y),3,cvScalar(0));\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ currPoint++;\r
+ }\r
+ key = cvWaitKey(30);\r
+ switch (key)\r
+ {\r
+ case 32: // Space symbol\r
+ if (ShowFrame(++currentFrame))\r
+ {\r
+ points2[0].x =-1;\r
+ points2[1].x =-1;\r
+ points2[2].x =-1;\r
+ points2[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame--;\r
+ }\r
+ break;\r
+ case 8: // Backspace symbol\r
+ if (currentFrame>0)\r
+ {\r
+ if (ShowFrame(--currentFrame))\r
+ {\r
+ points2[0].x =-1;\r
+ points2[1].x =-1;\r
+ points2[2].x =-1;\r
+ points2[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame++;\r
+ }\r
+ }\r
+ break;\r
+ }\r
+ }\r
+\r
+ i2 = cvCloneImage(workImage);\r
+ // sorting points\r
+\r
+ for (int i=1;i<4;i++)\r
+ {\r
+\r
+ if (points2[i].y<points2[0].y)\r
+ {\r
+ CvPoint temp;\r
+ temp = points2[0];\r
+ points2[0]=points2[i];\r
+ points2[i]=temp;\r
+ }\r
+ }\r
+ if (points2[1].x<points2[0].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points2[0];\r
+ points2[0]=points2[1];\r
+ points2[1]=temp;\r
+ }\r
+\r
+ if (points2[3].x<points2[2].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points2[3];\r
+ points2[3]=points2[2];\r
+ points2[2]=temp;\r
+ }\r
+\r
+ //end of sorting\r
+\r
+\r
+\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ if (USE_DEINTERLACING)\r
+ {\r
+ workImage = Deinterlace(i1);\r
+ cvReleaseImage(&i1);\r
+ i1=cvCloneImage(workImage);\r
+ cvReleaseImage(&workImage);\r
+ workImage = Deinterlace(i2);\r
+ cvReleaseImage(&i2);\r
+ i2=cvCloneImage(workImage);\r
+ cvReleaseImage(&workImage);\r
+ }\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ workImage = Undistort(i1,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&i1);\r
+ i1=cvCloneImage(workImage);\r
+ cvReleaseImage(&workImage);\r
+ workImage = Undistort(i2,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&i2);\r
+ i2=cvCloneImage(workImage);\r
+ cvReleaseImage(&workImage);\r
+ }\r
+\r
+ points3D = calc3DPoints(i1,i2,points1,points2,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,\r
+ !USE_UNDISTORTION,(float)(cvGetTrackbarPos(DEPTH_TRACKBAR,FRAME_WINDOW))/IDENTITY_DEPTH_VALUE,(float)(cvGetTrackbarPos(ERROR_TRACKBAR,FRAME_WINDOW))/IDENTITY_ERROR_VALUE);\r
+ if (!points3D)\r
+ {\r
+ printf("\nUnable to find correct stereo correspondence, please try again\n");\r
+ ShowFrame(currentFrame);\r
+ }\r
+ }\r
+ while (!points3D);\r
+\r
+\r
+\r
+ workImage = cvCloneImage(firstImage);\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvReleaseImage(&firstImage);\r
+\r
+\r
+ char* PATH = OUTPUT_DIRECTORY;\r
+ FILE* f;\r
+ char path[100];\r
+ char path_frames[100];\r
+ sprintf(path_frames,"%sframes.txt",PATH);\r
+ remove(path_frames);\r
+ char cmd[100];\r
+#ifdef WIN32\r
+ sprintf(cmd,"mkdir %s",PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"del %s*.* /q",PATH);\r
+ system(cmd);\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(cmd,"mkdir %s",SAMPLES_PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"del %s*.* /q",SAMPLES_PATH);\r
+ system(cmd);\r
+ }\r
+#else\r
+\r
+ sprintf(cmd,"mkdir %s",PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"rm -f %s*.*",PATH);\r
+ system(cmd);\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(cmd,"mkdir %s",SAMPLES_PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"rm -f %s*.*",SAMPLES_PATH);\r
+ system(cmd);\r
+ }\r
+#endif\r
+\r
+ do\r
+ {\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+\r
+ if (points3D)\r
+ {\r
+\r
+ newPoints = Find3DObject(workImage,points3D,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,!USE_UNDISTORTION);\r
+\r
+ if (newPoints)\r
+ {\r
+ bool areOnFrame=true; // are points on frame or not\r
+ if (areOnFrame=true)\r
+ {\r
+ f = fopen(path_frames,"a");\r
+ sprintf(path,"%s%d.jpg",PATH,currentFrame+1);\r
+ fprintf(f,"%s,%d,%d,%d,%d,%d,%d,%d,%d,",path,newPoints[0].x,newPoints[0].y,newPoints[1].x,newPoints[1].y,newPoints[2].x,newPoints[2].y,newPoints[3].x,newPoints[3].y);\r
+ fprintf(f,"%d,%d,%d,%d,%d,%d,%d,%d\n",newPoints[4].x,newPoints[4].y,newPoints[5].x,newPoints[5].y,newPoints[6].x,newPoints[6].y,newPoints[7].x,newPoints[7].y);\r
+ fclose(f);\r
+\r
+ if (DRAW_CHESSBOARD)\r
+ { \r
+ CvPoint2D32f* ChessboardCorners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ cvFindChessboardCorners(workImage,innerCornersCount,ChessboardCorners);\r
+ cvDrawChessboardCorners(workImage,innerCornersCount,ChessboardCorners,innerCornersCount.height*innerCornersCount.width,1);\r
+ delete[] ChessboardCorners;\r
+ }\r
+ if (DRAW_CORNERS)\r
+ {\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ cvCircle(workImage,newPoints[i],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[i],3,cvScalar(0)); \r
+ }\r
+ cvLine(workImage,newPoints[0],newPoints[1], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[0],newPoints[2], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[1],newPoints[3], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[2],newPoints[3], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[4],newPoints[5], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[5],newPoints[7], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[6],newPoints[7], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[4],newPoints[6], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[0],newPoints[4], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[1],newPoints[5], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[2],newPoints[6], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[3],newPoints[7], cvScalar(100,255,100));\r
+ }\r
+ if (SAVE_DRAWINGS)\r
+ cvSaveImage(path,workImage);\r
+ else\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ {\r
+ IplImage* tmp = Deinterlace(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(tmp,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&tmp);\r
+ tmp = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvSaveImage(path,tmp);\r
+ cvReleaseImage(&tmp);\r
+ }\r
+ else\r
+ {\r
+ if (!USE_UNDISTORTION)\r
+ cvSaveImage(path,frame);\r
+ else\r
+ {\r
+ undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);\r
+ cvSaveImage(path,undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ }\r
+ }\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(path,"%s%d.jpg",SAMPLES_PATH,currentFrame+1);\r
+ IplImage* tmp;\r
+ if (!USE_UNDISTORTION)\r
+ tmp = GetSample3D(frame,newPoints);\r
+ else\r
+ {\r
+ undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);\r
+ tmp = GetSample3D(undistortedImg,newPoints);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ if (tmp)\r
+ {\r
+ cvSaveImage(path,tmp);\r
+ cvReleaseImage(&tmp);\r
+ }\r
+ }\r
+ printf("Frame %d successfully saved to %s\n",currentFrame+1,path);\r
+ }\r
+\r
+ delete[] newPoints;\r
+ newPoints = NULL;\r
+ }\r
+ else\r
+ if (SAVE_ALL_FRAMES)\r
+ {\r
+ sprintf(path,"%s%d.jpg",PATH,currentFrame+1);\r
+ cvSaveImage(path,workImage);\r
+ }\r
+\r
+ }\r
+\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ key = cvWaitKey(30);\r
+ }\r
+ currentFrame++;\r
+ }\r
+ while (frame && (key!=27));\r
+\r
+ if (capture)\r
+ cvReleaseCapture(&capture);\r
+}\r
+\r
+//-------------\r
+\r
+void createSamples3DObject2(int argc, char** argv)\r
+{\r
+ CvPoint points[4];\r
+\r
+ CvPoint2D32f* relCoords = new CvPoint2D32f[4];\r
+ CvPoint3D32f* objectPoints = new CvPoint3D32f[8];\r
+\r
+ CvPoint * newPoints;\r
+ int currentFrame=0;\r
+ int key;\r
+ IplImage* undistortedImg;\r
+\r
+ if (CALIBRATE_CAMERA)\r
+ {\r
+ int v = IS_VIDEO_CAPTURE ? 10 : 7;\r
+ char** c = new char*[v];\r
+ c[0]=new char[11]; c[0]="calibration";\r
+ c[1]=new char[2]; c[1]="-w";\r
+ c[2] = new char[2]; sprintf(c[2],"%d",innerCornersCount.width);\r
+ c[3]=new char[2]; c[3]="-h";\r
+ c[4] = new char[2]; sprintf(c[4],"%d",innerCornersCount.height);\r
+ c[5] = new char[2]; c[5]="-d";\r
+ c[6] = new char[5]; sprintf(c[6],"%d",VIDEO_CALIBRATION_DELAY);\r
+ c[7]=new char[2]; c[7]="-o";\r
+ c[8]=new char[100];\r
+ sprintf(c[8],"%scamera.yml",OUTPUT_DIRECTORY);\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ c[9]=new char[strlen(INPUT_VIDEO)+1];\r
+ strcpy(c[9],INPUT_VIDEO);\r
+ }\r
+\r
+ calibrate(v,c);\r
+ LoadCameraParams(c[8],&IntrinsicMatrix, &DistortionCoeffs);\r
+\r
+ // Add functionality for camera calibration\r
+ }\r
+\r
+ int boxDepthValue = IDENTITY_DEPTH_VALUE;\r
+ int initialBoxDepthValue = (MAX_INITAL_DEPTH_VALUE - MIN_INITAL_DEPTH_VALUE)/2;\r
+ cvNamedWindow(FRAME_WINDOW,1);\r
+ cvCreateTrackbar(DEPTH_TRACKBAR,FRAME_WINDOW,&boxDepthValue,MAX_DEPTH_VALUE,NULL);\r
+ cvCreateTrackbar(INITIAL_DEPTH_TRACKBAR,FRAME_WINDOW,&initialBoxDepthValue,MAX_INITAL_DEPTH_VALUE-MIN_INITAL_DEPTH_VALUE,NULL);\r
+\r
+\r
+ capture = IS_VIDEO_CAPTURE ? cvCreateFileCapture(INPUT_VIDEO) : cvCreateCameraCapture(0);\r
+\r
+ if (!IS_VIDEO_CAPTURE)\r
+ {\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,640);\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,480);\r
+ }\r
+\r
+ cvResizeWindow(FRAME_WINDOW,(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH),(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT));\r
+\r
+ CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ int count;\r
+\r
+ do\r
+ {\r
+ if (IS_VIDEO_CAPTURE)\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,++currentFrame);\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ \r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ key = cvWaitKey(30);\r
+ if (key==27) return;\r
+ }\r
+\r
+ }\r
+ while ( frame && (((AUTOFIND_CHESSBOARD && IS_VIDEO_CAPTURE)|| (!IS_VIDEO_CAPTURE)) && cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0));\r
+\r
+ if (frame == NULL)\r
+ {\r
+ printf("\n Unable to load video with chessboard or connect to the camera");\r
+ return;\r
+ }\r
+\r
+ delete[] corners;\r
+ currentFrame--;\r
+\r
+ IplImage* firstImage=cvCloneImage(workImage);\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+\r
+ points[0].x =-1;\r
+ points[1].x =-1;\r
+ points[2].x =-1;\r
+ points[3].x =-1;\r
+\r
+ printf("\nSelect the quadrangle region by selecting its vertices on the frame with a mouse\nUse Space and Backspace to go forward or backward (video only)\nPress Esc key to exit");\r
+ cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points);\r
+\r
+ // Getting initial points position\r
+ for (int currPoint=0;currPoint<4;)\r
+ {\r
+ if (points[currPoint].x != -1)\r
+ {\r
+ cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),2,cvScalar(255,255,255));\r
+ cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),3,cvScalar(0));\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ currPoint++;\r
+ }\r
+ key = cvWaitKey(30);\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ switch (key)\r
+ {\r
+ case 32: // Space symbol\r
+ if (ShowFrame(++currentFrame))\r
+ {\r
+ points[0].x =-1;\r
+ points[1].x =-1;\r
+ points[2].x =-1;\r
+ points[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame--;\r
+ }\r
+ break;\r
+ case 8: // Backspace symbol\r
+ if (currentFrame>0)\r
+ {\r
+ if (ShowFrame(--currentFrame))\r
+ {\r
+ points[0].x =-1;\r
+ points[1].x =-1;\r
+ points[2].x =-1;\r
+ points[3].x =-1;\r
+ currPoint = 0;\r
+ }\r
+ else\r
+ {\r
+ currentFrame++;\r
+ }\r
+ }\r
+ break;\r
+ }\r
+ }\r
+ }\r
+\r
+ // sorting points\r
+\r
+ for (int i=1;i<4;i++)\r
+ {\r
+\r
+ if (points[i].y<points[0].y)\r
+ {\r
+ CvPoint temp;\r
+ temp = points[0];\r
+ points[0]=points[i];\r
+ points[i]=temp;\r
+ }\r
+ }\r
+ if (points[1].x<points[0].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points[0];\r
+ points[0]=points[1];\r
+ points[1]=temp;\r
+ }\r
+\r
+ if (points[3].x<points[2].x)\r
+ {\r
+ CvPoint temp;\r
+ temp = points[3];\r
+ points[3]=points[2];\r
+ points[2]=temp;\r
+ }\r
+\r
+ //end of sorting\r
+\r
+\r
+\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(firstImage);\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvReleaseImage(&firstImage);\r
+\r
+\r
+ CvMat* Chessboard = CalcRelativePosition(workImage,points,&relCoords,IntrinsicMatrix, (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE);\r
+\r
+ if (relCoords)\r
+ {\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ objectPoints[i+4].x = objectPoints[i].x = relCoords[i].x;\r
+ objectPoints[i+4].y = objectPoints[i].y = relCoords[i].y;\r
+ objectPoints[i].z = (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE;\r
+ objectPoints[i+4].z = objectPoints[i].z + (float)(boxDepthValue)/IDENTITY_DEPTH_VALUE; \r
+ }\r
+ }\r
+ else\r
+ {\r
+ delete[] objectPoints;\r
+ objectPoints = NULL;\r
+ return;\r
+ }\r
+\r
+\r
+ printf("\n\nCheck box depth and position\nUse Space and Backspace to go forward or backward (video only)\nPress Esc key to exit and Enter to approve");\r
+ if (IS_VIDEO_CAPTURE)\r
+ currentFrame--;\r
+ //waiting for box depth approving\r
+ IplImage* startImage = cvCloneImage(workImage);\r
+ key=-1;\r
+ while (key != 13)\r
+ {\r
+ key = cvWaitKey(30);\r
+ if (key==27) \r
+ return;\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ switch (key)\r
+ {\r
+ case 32: // Space symbol\r
+ if (ShowFrame(++currentFrame))\r
+ {\r
+\r
+ }\r
+ else\r
+ {\r
+ currentFrame--;\r
+ }\r
+ break;\r
+ case 8: // Backspace symbol\r
+ if (currentFrame>0)\r
+ {\r
+ if (ShowFrame(--currentFrame))\r
+ {\r
+\r
+ }\r
+ else\r
+ {\r
+ currentFrame++;\r
+ }\r
+ }\r
+ break;\r
+ default:\r
+ ShowFrame(currentFrame);\r
+ break;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+\r
+\r
+ }\r
+ else\r
+ return;\r
+ }\r
+\r
+\r
+ CalcRelativePosition(workImage,points,&relCoords,IntrinsicMatrix, (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE,Chessboard);\r
+\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ objectPoints[i+4].x = objectPoints[i].x = relCoords[i].x;\r
+ objectPoints[i+4].y = objectPoints[i].y = relCoords[i].y;\r
+ objectPoints[i].z = (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE;\r
+ objectPoints[i+4].z = objectPoints[i].z + (float)(boxDepthValue)/IDENTITY_DEPTH_VALUE; \r
+ }\r
+\r
+\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ objectPoints[i].z = (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE;\r
+ objectPoints[i+4].z = objectPoints[i].z +(float)(boxDepthValue)/IDENTITY_DEPTH_VALUE; \r
+ }\r
+ newPoints = Find3DObject(workImage,objectPoints,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,false);\r
+ if (newPoints)\r
+ {\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ cvCircle(workImage,newPoints[i],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[i],3,cvScalar(0)); \r
+ }\r
+ cvLine(workImage,newPoints[0],newPoints[1], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[0],newPoints[2], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[1],newPoints[3], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[2],newPoints[3], cvScalar(100,255,100));\r
+\r
+ cvLine(workImage,newPoints[4],newPoints[5], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[5],newPoints[7], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[6],newPoints[7], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[4],newPoints[6], cvScalar(50,150,50));\r
+\r
+ cvLine(workImage,newPoints[0],newPoints[4], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[1],newPoints[5], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[2],newPoints[6], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[3],newPoints[7], cvScalar(50,150,50));\r
+ }\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ }\r
+ cvReleaseImage(&startImage);\r
+\r
+\r
+ char* PATH = OUTPUT_DIRECTORY;\r
+ FILE* f;\r
+ char path[100];\r
+ char path_frames[100];\r
+ sprintf(path_frames,"%sframes.txt",PATH);\r
+ remove(path_frames);\r
+ char cmd[100];\r
+#ifdef WIN32\r
+ sprintf(cmd,"mkdir %s",PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"del %s*.* /q",PATH);\r
+ system(cmd);\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(cmd,"mkdir %s",SAMPLES_PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"del %s*.* /q",SAMPLES_PATH);\r
+ system(cmd);\r
+ }\r
+#else\r
+\r
+ sprintf(cmd,"mkdir %s",PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"rm -f %s*.*",PATH);\r
+ system(cmd);\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(cmd,"mkdir %s",SAMPLES_PATH);\r
+ system(cmd);\r
+ sprintf(cmd,"rm -f %s*.*",SAMPLES_PATH);\r
+ system(cmd);\r
+ }\r
+#endif\r
+ if (IS_VIDEO_CAPTURE)\r
+ {\r
+ currentFrame = 0;\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,currentFrame);\r
+ }\r
+ do\r
+ {\r
+ if (workImage) cvReleaseImage(&workImage);\r
+ frame = cvQueryFrame(capture);\r
+ if (frame)\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ workImage = Deinterlace(frame);\r
+ else\r
+ workImage = cvCloneImage(frame);\r
+\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&workImage);\r
+ workImage = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+\r
+ if (objectPoints)\r
+ {\r
+ newPoints = Find3DObject(workImage,objectPoints,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,false);\r
+\r
+ if (newPoints)\r
+ {\r
+ bool areOnFrame=true; // are points on frame or not\r
+ if (areOnFrame=true)\r
+ {\r
+ f = fopen(path_frames,"a");\r
+ sprintf(path,"%s%d.jpg",PATH,currentFrame+1);\r
+ fprintf(f,"%s,%d,%d,%d,%d,%d,%d,%d,%d,",path,newPoints[0].x,newPoints[0].y,newPoints[1].x,newPoints[1].y,newPoints[2].x,newPoints[2].y,newPoints[3].x,newPoints[3].y);\r
+ fprintf(f,"%d,%d,%d,%d,%d,%d,%d,%d\n",newPoints[4].x,newPoints[4].y,newPoints[5].x,newPoints[5].y,newPoints[6].x,newPoints[6].y,newPoints[7].x,newPoints[7].y);\r
+ fclose(f);\r
+\r
+ if (DRAW_CHESSBOARD)\r
+ { \r
+ CvPoint2D32f* ChessboardCorners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];\r
+ cvFindChessboardCorners(workImage,innerCornersCount,ChessboardCorners);\r
+ cvDrawChessboardCorners(workImage,innerCornersCount,ChessboardCorners,innerCornersCount.height*innerCornersCount.width,1);\r
+ delete[] ChessboardCorners;\r
+ }\r
+ if (DRAW_CORNERS)\r
+ {\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ cvCircle(workImage,newPoints[i],2,cvScalar(255,255,255));\r
+ cvCircle(workImage,newPoints[i],3,cvScalar(0)); \r
+ }\r
+ cvLine(workImage,newPoints[0],newPoints[1], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[0],newPoints[2], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[1],newPoints[3], cvScalar(100,255,100));\r
+ cvLine(workImage,newPoints[2],newPoints[3], cvScalar(100,255,100));\r
+\r
+ cvLine(workImage,newPoints[4],newPoints[5], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[5],newPoints[7], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[6],newPoints[7], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[4],newPoints[6], cvScalar(50,150,50));\r
+\r
+ cvLine(workImage,newPoints[0],newPoints[4], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[1],newPoints[5], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[2],newPoints[6], cvScalar(50,150,50));\r
+ cvLine(workImage,newPoints[3],newPoints[7], cvScalar(50,150,50));\r
+ }\r
+ if (SAVE_DRAWINGS)\r
+ cvSaveImage(path,workImage);\r
+ else\r
+ {\r
+ if (USE_DEINTERLACING)\r
+ {\r
+ IplImage* tmp = Deinterlace(frame);\r
+ if (USE_UNDISTORTION)\r
+ {\r
+ undistortedImg = Undistort(tmp,IntrinsicMatrix,DistortionCoeffs);\r
+ cvReleaseImage(&tmp);\r
+ tmp = cvCloneImage(undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ cvSaveImage(path,tmp);\r
+ cvReleaseImage(&tmp);\r
+ }\r
+ else\r
+ {\r
+ if (!USE_UNDISTORTION)\r
+ cvSaveImage(path,frame);\r
+ else\r
+ {\r
+ undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);\r
+ cvSaveImage(path,undistortedImg);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ }\r
+ }\r
+ if (SAVE_SAMPLES)\r
+ {\r
+ sprintf(path,"%s%d.jpg",SAMPLES_PATH,currentFrame+1);\r
+ IplImage* tmp;\r
+ if (!USE_UNDISTORTION)\r
+ tmp = GetSample3D(frame,newPoints);\r
+ else\r
+ {\r
+ undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);\r
+ tmp = GetSample3D(undistortedImg,newPoints);\r
+ cvReleaseImage(&undistortedImg);\r
+ }\r
+ if (tmp)\r
+ {\r
+ cvSaveImage(path,tmp);\r
+ cvReleaseImage(&tmp);\r
+ }\r
+ }\r
+ printf("Frame %d successfully saved to %s\n",currentFrame+1,path);\r
+ }\r
+\r
+ delete[] newPoints;\r
+ newPoints = NULL;\r
+ }\r
+ else\r
+ if (SAVE_ALL_FRAMES)\r
+ {\r
+ sprintf(path,"%s%d.jpg",PATH,currentFrame+1);\r
+ cvSaveImage(path,workImage);\r
+ }\r
+\r
+ }\r
+\r
+\r
+ cvShowImage(FRAME_WINDOW,workImage);\r
+ key = cvWaitKey(30);\r
+ }\r
+ currentFrame++;\r
+ }\r
+ while (frame && (key!=27));\r
+ if (capture)\r
+ cvReleaseCapture(&capture);\r
+}\r
+//-------------\r
+\r
+// Command line arguments: <config_file>\r
+int main( int argc, char** argv )\r
+{\r
+ if(argc != 2)\r
+ {\r
+ \r
+ printf("Usage: createsamples arguments.yml\n");\r
+ printf("Read readme.txt to get an additional information about the sample\n");\r
+ return 0;\r
+ }\r
+ if (LoadApplicationParams(argv[1]))\r
+ {\r
+ if (USE_3D)\r
+ createSamples3DObject2(argc,argv);\r
+ else\r
+ createSamples2DObject(argc,argv);\r
+ }\r
+ else\r
+ {\r
+ printf("\nUnable to load configuration file from %s\nClosing application...\n",argv[1]);\r
+ }\r
+\r
+ return 0;\r
+}\r
--- /dev/null
+/*\r
+ * A Demo of automatic samples capturing\r
+ * Author: Alexey Latyshev\r
+ */\r
+#ifdef WIN32\r
+#include "cv.h"\r
+#include "highgui.h"\r
+#else\r
+#include "opencv/cv.h"\r
+#include "opencv/highgui.h"\r
+#endif\r
+#include <vector>\r
+#include <string>\r
+#include <algorithm>\r
+#include <stdio.h>\r
+#include <ctype.h>\r
+#include <float.h>\r
+#include "object_tracker.h"\r
+#include "tracker3D.h"\r
+\r
+/////////----------------------------------------------------------------\r
+/////////-----------------------------------------------\r
+// return NULL if no there was no chessboard founded\r
+\r
+// input: the same points on two images\r
+// output: 3D coordinates of selected points\r
+// boxDepthCoeff is box depth (relative to max of width or height)\r
+// maxRelativeError - max reprojection error of upper-left corner of the schessboard (in chessboard square sizes)\r
+CvPoint3D32f* calc3DPoints(const IplImage* _img1, const IplImage* _img2,CvPoint* points1, CvPoint* points2, CvSize innerCornersCount,\r
+ const CvMat* intrinsic_matrix, const CvMat* _distortion_coeffs, bool undistortImage,\r
+ float boxDepthCoeff, float maxRelativeError)\r
+{\r
+ bool isOK = true;\r
+ IplImage* img1 = cvCreateImage(cvSize(_img1->width,_img1->height),_img1->depth,_img1->nChannels);\r
+ IplImage* img2 = cvCreateImage(cvSize(_img2->width,_img2->height),_img2->depth,_img2->nChannels);\r
+ if (undistortImage)\r
+ {\r
+ cvUndistort2(_img1,img1,intrinsic_matrix,_distortion_coeffs);\r
+ cvUndistort2(_img2,img2,intrinsic_matrix,_distortion_coeffs);\r
+ }\r
+ else\r
+ {\r
+ img1=cvCloneImage(_img1);\r
+ img2=cvCloneImage(_img2);\r
+ }\r
+\r
+ CvMat* image_points1 = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ CvMat* image_points2 = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ CvMat* object_points = cvCreateMat(3,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+\r
+ CvPoint2D32f* corners1 = new CvPoint2D32f[ innerCornersCount.height*innerCornersCount.width ];\r
+ CvPoint2D32f* corners2 = new CvPoint2D32f[ innerCornersCount.height*innerCornersCount.width ];\r
+ int count1 = 0;\r
+ int count2 = 0;\r
+\r
+ //Find chessboard corners\r
+ int found1 = cvFindChessboardCorners(img1,innerCornersCount,corners1,&count1);\r
+ int found2 = cvFindChessboardCorners(img2,innerCornersCount,corners2,&count2);\r
+ if ((found1 == 0)||(found2 == 0))\r
+ {\r
+ delete[] corners1;\r
+ corners1 = NULL;\r
+ delete[] corners2;\r
+ corners2 = NULL;\r
+ cvReleaseMat(&image_points1);\r
+ cvReleaseMat(&image_points2);\r
+ cvReleaseMat(&object_points);\r
+ cvReleaseImage(&img1);\r
+ cvReleaseImage(&img2);\r
+ return NULL;\r
+ }\r
+\r
+ CvMat* distortion_coeffs = cvCloneMat(_distortion_coeffs);\r
+ cvZero(distortion_coeffs);\r
+\r
+ IplImage* view_gray = cvCreateImage( cvGetSize(img1), 8, 1 );\r
+ cvCvtColor(img1, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, corners1, count1, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+\r
+ view_gray = cvCreateImage( cvGetSize(img2), 8, 1 );\r
+ cvCvtColor(img2, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, corners2, count2, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+\r
+ //assumes that chessboard squares are squared\r
+ float step = 1.0f;\r
+\r
+ // Sets object points and image points\r
+ for (int i=0; i< innerCornersCount.height;i++)\r
+ for (int j=0; j < innerCornersCount.width;j++)\r
+ {\r
+ object_points->data.db[(i*innerCornersCount.width+j)]=j*step;\r
+ object_points->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=i*step;\r
+ object_points->data.db[(i*innerCornersCount.width+j)+2*innerCornersCount.width*innerCornersCount.height]=0.0f;\r
+\r
+ image_points1->data.db[(i*innerCornersCount.width+j)]=(int)corners1[(i*innerCornersCount.width+j)].x;\r
+ image_points1->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=(int)corners1[(i*innerCornersCount.width+j)].y;\r
+\r
+ image_points2->data.db[(i*innerCornersCount.width+j)]=(int)corners2[(i*innerCornersCount.width+j)].x;\r
+ image_points2->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=(int)corners2[(i*innerCornersCount.width+j)].y;\r
+\r
+ }\r
+\r
+ CvMat* R = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* T = cvCreateMat(3, 1, CV_64FC1);\r
+ CvMat* R1 = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* R2 = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* R_1 = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* R_2 = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* P1 = cvCreateMat(3, 4, CV_64FC1);\r
+ CvMat* P2 = cvCreateMat(3, 4, CV_64FC1);\r
+ CvMat* T1 = cvCreateMat(3, 1, CV_64FC1);\r
+ CvMat* T2 = cvCreateMat(3, 1, CV_64FC1);\r
+ CvMat* Q = cvCreateMat(4, 4, CV_64FC1);\r
+ CvMat* rotation_vector = cvCreateMat(3,1,CV_64FC1);\r
+ CvMat* new_camera1 = cvCreateMat(3, 3, CV_64FC1);\r
+ CvMat* new_camera2 = cvCreateMat(3, 3, CV_64FC1);\r
+\r
+\r
+ //Calculating Exrinsic camera parameters\r
+ cvFindExtrinsicCameraParams2(object_points,image_points1,intrinsic_matrix, distortion_coeffs,rotation_vector,T1);\r
+ cvRodrigues2(rotation_vector,R1);\r
+\r
+ cvFindExtrinsicCameraParams2(object_points,image_points2,intrinsic_matrix, distortion_coeffs,rotation_vector,T2);\r
+ cvRodrigues2(rotation_vector,R2);\r
+\r
+ //Finding rotation and translation vectors between two cameras\r
+ cvGEMM(R2,R1,1,NULL,0,R,CV_GEMM_B_T);\r
+ cvGEMM(R,T1,-1.0,T2,1,T);\r
+\r
+\r
+ //Rectifying cameras\r
+ cvStereoRectify( intrinsic_matrix, intrinsic_matrix,\r
+ distortion_coeffs, distortion_coeffs,\r
+ cvSize(img1->width,img1->height), R, T,\r
+ R_1, R_2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY);\r
+\r
+ for (int i=0;i<3;i++)\r
+ for (int j=0;j<3;j++)\r
+ {\r
+ new_camera1->data.db[i*new_camera1->cols+j]=P1->data.db[i*P1->cols+j];\r
+ new_camera2->data.db[i*new_camera2->cols+j]=P2->data.db[i*P2->cols+j];\r
+ }\r
+\r
+ CvMat* oldPoints1 = cvCreateMat( 5,1, CV_32FC2 );\r
+ CvMat* oldPoints2 = cvCreateMat( 5,1, CV_32FC2 );\r
+ CvMat* newPoints1 = cvCreateMat( 5,1, CV_32FC2 );\r
+ CvMat* newPoints2 = cvCreateMat( 5,1, CV_32FC2 );\r
+ CvMat* t = cvCreateMat(3,1,CV_64FC1);\r
+\r
+ CvPoint3D32f* xyd = new CvPoint3D32f[4];\r
+ CvPoint3D32f test_point;\r
+\r
+ //finding new x,y points and disparity\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ oldPoints1->data.fl[2*i]=(float)points1[i].x;\r
+ oldPoints1->data.fl[2*i+1]=(float)points1[i].y;\r
+ oldPoints2->data.fl[2*i]=(float)points2[i].x;\r
+ oldPoints2->data.fl[2*i+1]=(float)points2[i].y;\r
+ }\r
+ oldPoints1->data.fl[8]=(float)(image_points1->data.db[0]);\r
+ oldPoints1->data.fl[9]=(float)(image_points1->data.db[innerCornersCount.height*innerCornersCount.width]);\r
+ oldPoints2->data.fl[8]=(float)(image_points2->data.db[0]);\r
+ oldPoints2->data.fl[9]=(float)(image_points2->data.db[innerCornersCount.height*innerCornersCount.width]);\r
+ cvUndistortPoints(oldPoints1,newPoints1,intrinsic_matrix,distortion_coeffs,R_1,P1);\r
+ cvUndistortPoints(oldPoints2,newPoints2,intrinsic_matrix,distortion_coeffs,R_2,P2);\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ xyd[i].x=newPoints1->data.fl[2*i];\r
+ xyd[i].y=newPoints1->data.fl[2*i+1];\r
+ if (fabs(T->data.db[1]) < fabs(T->data.db[0])) \r
+ xyd[i].z=(newPoints2->data.fl[2*i])-(newPoints1->data.fl[2*i]);\r
+ else\r
+ xyd[i].z=(newPoints2->data.fl[2*i+1])-(newPoints1->data.fl[2*i+1]);\r
+ }\r
+ test_point.x = newPoints1->data.fl[8];\r
+ test_point.y = newPoints1->data.fl[9];\r
+ if (fabs(T->data.db[1]) < fabs(T->data.db[0])) \r
+ test_point.z=(newPoints2->data.fl[8])-(newPoints1->data.fl[8]);\r
+ else\r
+ test_point.z=(newPoints2->data.fl[9])-(newPoints1->data.fl[9]);\r
+\r
+ CvPoint3D32f* result = new CvPoint3D32f[8];\r
+ double x, y, z, w;\r
+\r
+ //calculating 3D points\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ float d = xyd[i].z;\r
+ x=(Q->data.db[0])*(xyd[i].x)+(Q->data.db[1])*(xyd[i].y)+(Q->data.db[2])*(d)+(Q->data.db[3]);\r
+ y=(Q->data.db[4])*(xyd[i].x)+(Q->data.db[5])*(xyd[i].y)+(Q->data.db[6])*(d)+(Q->data.db[7]);\r
+ z=(Q->data.db[8])*(xyd[i].x)+(Q->data.db[9])*(xyd[i].y)+(Q->data.db[10])*(d)+(Q->data.db[11]);\r
+ w=(Q->data.db[12])*(xyd[i].x)+(Q->data.db[13])*(xyd[i].y)+(Q->data.db[14])*(d)+(Q->data.db[15]);\r
+ if (w != 0)\r
+ {\r
+ result[i].x = (float)(x/w);\r
+ result[i].y = (float)(y/w);\r
+ result[i].z = (float)(z/w);\r
+ }\r
+ else\r
+ {\r
+ result[i].x = result[i].y = result[i].z = FLT_MAX;\r
+ isOK=false;\r
+ }\r
+\r
+ // Calculating points coordinates in chessboard coordinate system\r
+\r
+ t->data.db[0]=result[i].x;\r
+ t->data.db[1]=result[i].y;\r
+ t->data.db[2]=result[i].z;\r
+\r
+ cvGEMM(R_1,t,1.0,T1,-1.0,t,CV_GEMM_A_T);\r
+ cvGEMM(R1,t,1.0,NULL,0,t,CV_GEMM_A_T);\r
+\r
+\r
+ result[i].x = (float)t->data.db[0];\r
+ result[i].y = (float)t->data.db[1];\r
+ result[i].z = (float)t->data.db[2];\r
+ }\r
+\r
+ x=(Q->data.db[0])*(test_point.x)+(Q->data.db[1])*(test_point.y)+(Q->data.db[2])*(test_point.z)+(Q->data.db[3]);\r
+ y=(Q->data.db[4])*(test_point.x)+(Q->data.db[5])*(test_point.y)+(Q->data.db[6])*(test_point.z)+(Q->data.db[7]);\r
+ z=(Q->data.db[8])*(test_point.x)+(Q->data.db[9])*(test_point.y)+(Q->data.db[10])*(test_point.z)+(Q->data.db[11]);\r
+ w=(Q->data.db[12])*(test_point.x)+(Q->data.db[13])*(test_point.y)+(Q->data.db[14])*(test_point.z)+(Q->data.db[15]);\r
+ if (w != 0)\r
+ {\r
+ x/=w;\r
+ y/=w;\r
+ z/=w;\r
+ }\r
+ else\r
+ {\r
+ x = y = z = FLT_MAX;\r
+ isOK=false;\r
+ }\r
+ // Calculating test points coordinates in chessboard coordinate system\r
+\r
+ t->data.db[0]=x;\r
+ t->data.db[1]=y;\r
+ t->data.db[2]=z;\r
+\r
+ cvGEMM(R_1,t,1.0,T1,-1.0,t,CV_GEMM_A_T);\r
+ cvGEMM(R1,t,1.0,NULL,0,t,CV_GEMM_A_T);\r
+\r
+\r
+ x = t->data.db[0];\r
+ y = t->data.db[1];\r
+ z = t->data.db[2];\r
+\r
+ if ((abs(x) > maxRelativeError) || (abs(y) > maxRelativeError) || (abs(z) > maxRelativeError))\r
+ isOK = false;\r
+\r
+ cvReleaseMat(&t);\r
+\r
+ float maxSideSize = (fabs(result[0].x-result[1].x) > fabs(result[0].y-result[2].y)) ? fabs(result[0].x-result[1].x) : fabs(result[0].y-result[2].y);\r
+\r
+ for (int i=0;i<4;i++)\r
+ {\r
+ result[i+4].x = result[i].x;\r
+ result[i+4].y = result[i].y;\r
+ result[i+4].z = (result[i].z > 0)? (result[i].z - boxDepthCoeff*maxSideSize) : (result[i].z + boxDepthCoeff*maxSideSize);\r
+ }\r
+\r
+ //Memory Cleanup\r
+ delete[] xyd;\r
+ delete[] corners1;\r
+ delete[] corners2;\r
+ cvReleaseMat(&oldPoints1);\r
+ cvReleaseMat(&oldPoints2);\r
+ cvReleaseMat(&newPoints1);\r
+ cvReleaseMat(&newPoints2);\r
+ cvReleaseMat(&object_points);\r
+ cvReleaseMat(&T1);\r
+ cvReleaseMat(&T2);\r
+ cvReleaseMat(&rotation_vector);\r
+ cvReleaseMat(&R1);\r
+ cvReleaseMat(&R2);\r
+ cvReleaseMat(&R_1);\r
+ cvReleaseMat(&R_2);\r
+ cvReleaseMat(&P1);\r
+ cvReleaseMat(&P2);\r
+ cvReleaseMat(&R);\r
+ cvReleaseMat(&T);\r
+ cvReleaseMat(&Q);\r
+ cvReleaseMat(&new_camera1);\r
+ cvReleaseMat(&new_camera2);\r
+ cvReleaseImage(&img1);\r
+ cvReleaseImage(&img2);\r
+ cvReleaseMat(&distortion_coeffs);\r
+ cvReleaseMat(&image_points1);\r
+ cvReleaseMat(&image_points2);\r
+\r
+ if (isOK)\r
+ return result;\r
+ delete[] result;\r
+ return NULL;\r
+}\r
+\r
+CvPoint* Find3DObject(const IplImage* _img, const CvPoint3D32f* points, CvSize innerCornersCount,\r
+ const CvMat* intrinsic_matrix, const CvMat* _distortion_coeffs, bool undistortImage)\r
+{\r
+ IplImage* img;\r
+ CvMat* mx = cvCreateMat( _img->height,_img->width, CV_32F );\r
+ CvMat* my = cvCreateMat( _img->height,_img->width, CV_32F );\r
+ CvMat* invmx = NULL;\r
+ CvMat* invmy = NULL;\r
+\r
+\r
+ if (undistortImage)\r
+ {\r
+ img = cvCreateImage(cvSize(_img->width,_img->height),_img->depth,_img->nChannels);\r
+ cvInitUndistortMap(intrinsic_matrix,_distortion_coeffs,mx,my);\r
+ cvRemap(_img,img,mx,my);\r
+ InverseUndistortMap(mx,my,&invmx,&invmy,true);\r
+ }\r
+ else\r
+ {\r
+ img = cvCloneImage(_img);\r
+ }\r
+\r
+ CvMat* chessBoardPoints = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+\r
+ CvPoint2D32f* corners = new CvPoint2D32f[ innerCornersCount.height*innerCornersCount.width ];\r
+ int count = 0;\r
+\r
+\r
+ //Find chessboard corners\r
+ if (cvFindChessboardCorners(img,innerCornersCount,corners,&count)==0)\r
+ {\r
+ delete[] corners;\r
+ corners = NULL;\r
+ cvReleaseMat(&chessBoardPoints);\r
+ cvReleaseImage(&img);\r
+ cvReleaseMat(&mx);\r
+ cvReleaseMat(&my);\r
+ if (invmx)\r
+ cvReleaseMat(&invmx);\r
+ if (invmy)\r
+ cvReleaseMat(&invmy);\r
+ chessBoardPoints = NULL;\r
+\r
+ return NULL;\r
+ }\r
+\r
+ CvMat* distortion_coeffs = cvCloneMat(_distortion_coeffs);\r
+ cvZero(distortion_coeffs);\r
+\r
+ IplImage* view_gray = cvCreateImage( cvGetSize(img), 8, 1 );\r
+ cvCvtColor(img, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+\r
+ //assumes that chessboard squares are squared\r
+ float step = 1.0f;\r
+\r
+ // Sets object points and image points\r
+ CvMat* object_points = cvCreateMat(3,innerCornersCount.height*innerCornersCount.width,CV_64FC1);\r
+ for (int i=0; i< innerCornersCount.height;i++)\r
+ {\r
+ for (int j=0; j < innerCornersCount.width;j++)\r
+ {\r
+ object_points->data.db[(i*innerCornersCount.width+j)]=j*step;\r
+ object_points->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=i*step;\r
+ object_points->data.db[(i*innerCornersCount.width+j)+2*innerCornersCount.width*innerCornersCount.height]=0.0f;\r
+\r
+ chessBoardPoints->data.db[(i*innerCornersCount.width+j)]=corners[(i*innerCornersCount.width+j)].x;\r
+ chessBoardPoints->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=corners[(i*innerCornersCount.width+j)].y;\r
+\r
+ }\r
+ }\r
+\r
+ CvMat* T = cvCreateMat(3, 1, CV_64FC1);\r
+ CvMat* rotation_vector = cvCreateMat(3,1,CV_64FC1);\r
+\r
+ cvFindExtrinsicCameraParams2(object_points,chessBoardPoints,intrinsic_matrix, distortion_coeffs,rotation_vector,T);\r
+\r
+ CvMat* image_points3D = cvCreateMat(3,8,CV_64FC1);\r
+ CvMat* image_points2D = cvCreateMat(2,8,CV_64FC1);\r
+\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ image_points3D->data.db[i]=points[i].x;\r
+ image_points3D->data.db[i+8]=points[i].y;\r
+ image_points3D->data.db[i+16]=points[i].z;\r
+\r
+ }\r
+\r
+\r
+ cvProjectPoints2(image_points3D,rotation_vector,T,intrinsic_matrix,distortion_coeffs,image_points2D);\r
+\r
+ CvPoint* result = new CvPoint[8];\r
+\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ if (!undistortImage)\r
+ {\r
+ result[i].x=(int)(image_points2D->data.db[i]);\r
+ result[i].y=(int)(image_points2D->data.db[i+8]);\r
+ }\r
+ else\r
+ {\r
+ result[i].x=(int)(invmx->data.fl[(int)(image_points2D->data.db[i])+(invmx->cols)*(int)(image_points2D->data.db[i+8])]);\r
+ result[i].y=(int)(invmy->data.fl[(int)(image_points2D->data.db[i])+(invmx->cols)*(int)(image_points2D->data.db[i+8])]);\r
+ if ((result[i].x < 0)||(result[i].y < 0))\r
+ {\r
+ delete[] result;\r
+ result = NULL;\r
+ break;\r
+ }\r
+ }\r
+ }\r
+\r
+\r
+ cvReleaseMat(&image_points3D);\r
+ cvReleaseMat(&image_points2D);\r
+ cvReleaseMat(&rotation_vector);\r
+ cvReleaseMat(&T);\r
+ cvReleaseMat(&object_points);\r
+ cvReleaseMat(&chessBoardPoints);\r
+ cvReleaseMat(&distortion_coeffs);\r
+ cvReleaseImage(&img);\r
+ cvReleaseMat(&mx);\r
+ cvReleaseMat(&my);\r
+ cvReleaseMat(&invmx);\r
+ cvReleaseMat(&invmy);\r
+ delete[] corners;\r
+\r
+ return result;\r
+\r
+}\r
+//----------\r
+IplImage* GetSample3D(const IplImage* img, CvPoint* points)\r
+{\r
+ int minx = img->width;\r
+ int miny = img->height;\r
+ int maxx = 0;\r
+ int maxy = 0;\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ if (points[i].x < minx)\r
+ minx=points[i].x;\r
+ if (points[i].y < miny)\r
+ miny=points[i].y;\r
+ if (points[i].x > maxx)\r
+ maxx=points[i].x;\r
+ if (points[i].y > maxy)\r
+ maxy=points[i].y;\r
+ }\r
+ IplImage* result;\r
+ if ((maxx < img->width) && (maxy< img->height) && (maxx > minx) && (maxy > miny))\r
+ {\r
+ IplImage* workImage = cvCloneImage(img);\r
+ cvSetImageROI(workImage,cvRect(minx,miny,maxx-minx,maxy-miny));\r
+ result = cvCreateImage(cvSize(maxx-minx,maxy-miny),workImage->depth,workImage->nChannels);\r
+ cvConvert(workImage,result);\r
+ cvReleaseImage(&workImage);\r
+ }\r
+ else \r
+ result = NULL;\r
+ return result;\r
+}\r
+\r
+\r
+#if 0\r
+// test code\r
+int main( int argc, char** argv )\r
+{\r
+\r
+ CvMat* IntrinsicMatrix;\r
+ CvMat* DistortionCoeffs;\r
+ if (LoadCameraParams("f:\\_camera.yml",&IntrinsicMatrix,&DistortionCoeffs))\r
+ {\r
+ CvPoint* p1 = new CvPoint[4];\r
+ CvPoint* p2 = new CvPoint[4];\r
+\r
+ p1[0].x=481;\r
+ p1[0].y=251;\r
+ p1[1].x=630;\r
+ p1[1].y=283;\r
+ p1[2].x=487;\r
+ p1[2].y=308;\r
+ p1[3].x=619;\r
+ p1[3].y=354;\r
+\r
+ p2[0].x=504;\r
+ p2[0].y=242;\r
+ p2[1].x=632;\r
+ p2[1].y=299;\r
+ p2[2].x=496;\r
+ p2[2].y=297;\r
+ p2[3].x=621;\r
+ p2[3].y=359;\r
+\r
+\r
+ CvSize _innerCornersCount = cvSize(8,6);\r
+\r
+ IplImage* i2= cvLoadImage("f:\\451.jpg");\r
+ IplImage* i1= cvLoadImage("f:\\2.jpg");\r
+ CvCapture* capture = cvCreateFileCapture("f:\\DATA\\DoorHandle_xvid.avi");\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,200);\r
+\r
+ IplImage* itest;\r
+ IplImage* _itest = cvLoadImage("f:/2.jpg",1);\r
+ CvPoint3D32f* points = new CvPoint3D32f[8];\r
+ CvPoint* newPoints = new CvPoint[8];\r
+ points = calc3DPoints(i1,i2,p1,p2,_innerCornersCount,IntrinsicMatrix,DistortionCoeffs,false);\r
+ cvNamedWindow("res",1);\r
+ while (points && ((_itest = cvQueryFrame(capture))!=NULL))\r
+ {\r
+\r
+ itest = cvCloneImage(_itest);\r
+ newPoints = Find3DObject(itest,points,_innerCornersCount,IntrinsicMatrix,DistortionCoeffs,false);\r
+\r
+ if (newPoints)\r
+ {\r
+ for (int i=0;i<8;i++)\r
+ {\r
+ cvCircle(itest,newPoints[i],2,cvScalar(255,255,255));\r
+ cvCircle(itest,newPoints[i],3,cvScalar(0)); \r
+ }\r
+ cvLine(itest,newPoints[0],newPoints[1], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[0],newPoints[2], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[1],newPoints[3], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[2],newPoints[3], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[4],newPoints[5], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[5],newPoints[7], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[6],newPoints[7], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[4],newPoints[6], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[0],newPoints[4], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[1],newPoints[5], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[2],newPoints[6], cvScalar(100,255,100));\r
+ cvLine(itest,newPoints[3],newPoints[7], cvScalar(100,255,100));\r
+\r
+ }\r
+ cvShowImage("res",itest);\r
+ cvWaitKey(10);\r
+ cvReleaseImage(&itest);\r
+ delete[] newPoints;\r
+ newPoints=NULL;\r
+ }\r
+ }\r
+\r
+\r
+ return 0;\r
+}\r
+#endif\r
--- /dev/null
+#ifdef WIN32\r
+#include "cv.h"\r
+#include "highgui.h"\r
+#else\r
+#include "opencv/cv.h"\r
+#include "opencv/highgui.h"\r
+#endif\r
+#include <stdio.h>\r
+#include <string.h>\r
+#include <time.h>\r
+#include "tracker_calibration.h"\r
+// example command line (for copy-n-paste):\r
+// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>]\r
+\r
+/* The list of views may look as following (discard the starting and ending ------ separators):\r
+-------------------\r
+view000.png\r
+view001.png\r
+#view002.png\r
+view003.png\r
+view010.png\r
+one_extra_view.jpg\r
+-------------------\r
+that is, the file will contain 6 lines, view002.png will not be used for calibration,\r
+other ones will be (those, in which the chessboard pattern will be found)\r
+*/\r
+\r
+\r
+\r
+double compute_reprojection_error( const CvMat* object_points,\r
+ const CvMat* rot_vects, const CvMat* trans_vects,\r
+ const CvMat* camera_matrix, const CvMat* dist_coeffs,\r
+ const CvMat* image_points, const CvMat* point_counts,\r
+ CvMat* per_view_errors )\r
+{\r
+ CvMat* image_points2 = cvCreateMat( image_points->rows,\r
+ image_points->cols, image_points->type );\r
+ int i, image_count = rot_vects->rows, points_so_far = 0;\r
+ double total_err = 0, err;\r
+ \r
+ for( i = 0; i < image_count; i++ )\r
+ {\r
+ CvMat object_points_i, image_points_i, image_points2_i;\r
+ int point_count = point_counts->data.i[i];\r
+ CvMat rot_vect, trans_vect;\r
+\r
+ cvGetCols( object_points, &object_points_i,\r
+ points_so_far, points_so_far + point_count );\r
+ cvGetCols( image_points, &image_points_i,\r
+ points_so_far, points_so_far + point_count );\r
+ cvGetCols( image_points2, &image_points2_i,\r
+ points_so_far, points_so_far + point_count );\r
+ points_so_far += point_count;\r
+\r
+ cvGetRow( rot_vects, &rot_vect, i );\r
+ cvGetRow( trans_vects, &trans_vect, i );\r
+\r
+ cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,\r
+ camera_matrix, dist_coeffs, &image_points2_i,\r
+ 0, 0, 0, 0, 0 );\r
+ err = cvNorm( &image_points_i, &image_points2_i, CV_L1 );\r
+ if( per_view_errors )\r
+ per_view_errors->data.db[i] = err/point_count;\r
+ total_err += err;\r
+ }\r
+ \r
+ cvReleaseMat( &image_points2 );\r
+ return total_err/points_so_far;\r
+}\r
+\r
+\r
+int run_calibration( CvSeq* image_points_seq, CvSize img_size, CvSize board_size,\r
+ float square_size, float aspect_ratio, int flags,\r
+ CvMat* camera_matrix, CvMat* dist_coeffs, CvMat** extr_params,\r
+ CvMat** reproj_errs, double* avg_reproj_err )\r
+{\r
+ int code;\r
+ int image_count = image_points_seq->total;\r
+ int point_count = board_size.width*board_size.height;\r
+ CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2 );\r
+ CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3 );\r
+ CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1 );\r
+ CvMat rot_vects, trans_vects;\r
+ int i, j, k;\r
+ CvSeqReader reader;\r
+ cvStartReadSeq( image_points_seq, &reader );\r
+\r
+ // initialize arrays of points\r
+ for( i = 0; i < image_count; i++ )\r
+ {\r
+ CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;\r
+ CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i*point_count;\r
+ CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i*point_count;\r
+\r
+ for( j = 0; j < board_size.height; j++ )\r
+ for( k = 0; k < board_size.width; k++ )\r
+ {\r
+ *obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);\r
+ *dst_img_pt++ = *src_img_pt++;\r
+ }\r
+ CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader );\r
+ }\r
+\r
+ cvSet( point_counts, cvScalar(point_count) );\r
+\r
+ *extr_params = cvCreateMat( image_count, 6, CV_32FC1 );\r
+ cvGetCols( *extr_params, &rot_vects, 0, 3 );\r
+ cvGetCols( *extr_params, &trans_vects, 3, 6 );\r
+\r
+ cvZero( camera_matrix );\r
+ cvZero( dist_coeffs );\r
+\r
+ if( flags & CV_CALIB_FIX_ASPECT_RATIO )\r
+ {\r
+ camera_matrix->data.db[0] = aspect_ratio;\r
+ camera_matrix->data.db[4] = 1.;\r
+ }\r
+\r
+ cvCalibrateCamera2( object_points, image_points, point_counts,\r
+ img_size, camera_matrix, dist_coeffs,\r
+ &rot_vects, &trans_vects, flags );\r
+\r
+ code = cvCheckArr( camera_matrix, CV_CHECK_QUIET ) &&\r
+ cvCheckArr( dist_coeffs, CV_CHECK_QUIET ) &&\r
+ cvCheckArr( *extr_params, CV_CHECK_QUIET );\r
+\r
+ *reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 );\r
+ *avg_reproj_err =\r
+ compute_reprojection_error( object_points, &rot_vects, &trans_vects,\r
+ camera_matrix, dist_coeffs, image_points, point_counts, *reproj_errs );\r
+\r
+ cvReleaseMat( &object_points );\r
+ cvReleaseMat( &image_points );\r
+ cvReleaseMat( &point_counts );\r
+\r
+ return code;\r
+}\r
+\r
+\r
+void save_camera_params( const char* out_filename, int image_count, CvSize img_size,\r
+ CvSize board_size, float square_size,\r
+ float aspect_ratio, int flags,\r
+ const CvMat* camera_matrix, CvMat* dist_coeffs,\r
+ const CvMat* extr_params, const CvSeq* image_points_seq,\r
+ const CvMat* reproj_errs, double avg_reproj_err )\r
+{\r
+ CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE );\r
+ \r
+ time_t t;\r
+ time( &t );\r
+ struct tm *t2 = localtime( &t );\r
+ char buf[1024];\r
+ strftime( buf, sizeof(buf)-1, "%c", t2 );\r
+\r
+ cvWriteString( fs, "calibration_time", buf );\r
+ \r
+ cvWriteInt( fs, "image_count", image_count );\r
+ cvWriteInt( fs, "image_width", img_size.width );\r
+ cvWriteInt( fs, "image_height", img_size.height );\r
+ cvWriteInt( fs, "board_width", board_size.width );\r
+ cvWriteInt( fs, "board_height", board_size.height );\r
+ cvWriteReal( fs, "square_size", square_size );\r
+ \r
+ if( flags & CV_CALIB_FIX_ASPECT_RATIO )\r
+ cvWriteReal( fs, "aspect_ratio", aspect_ratio );\r
+\r
+ if( flags != 0 )\r
+ {\r
+ sprintf( buf, "flags: %s%s%s%s",\r
+ flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",\r
+ flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspect_ratio" : "",\r
+ flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",\r
+ flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" );\r
+ cvWriteComment( fs, buf, 0 );\r
+ }\r
+ \r
+ cvWriteInt( fs, "flags", flags );\r
+\r
+ cvWrite( fs, "camera_matrix", camera_matrix );\r
+ cvWrite( fs, "distortion_coefficients", dist_coeffs );\r
+\r
+ cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err );\r
+ if( reproj_errs )\r
+ cvWrite( fs, "per_view_reprojection_errors", reproj_errs );\r
+\r
+ if( extr_params )\r
+ {\r
+ cvWriteComment( fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );\r
+ cvWrite( fs, "extrinsic_parameters", extr_params );\r
+ }\r
+\r
+ if( image_points_seq )\r
+ {\r
+ cvWriteComment( fs, "the array of board corners projections used for calibration", 0 );\r
+ assert( image_points_seq->total == image_count );\r
+ CvMat* image_points = cvCreateMat( 1, image_count*board_size.width*board_size.height, CV_32FC2 );\r
+ cvCvtSeqToArray( image_points_seq, image_points->data.fl );\r
+\r
+ cvWrite( fs, "image_points", image_points );\r
+ cvReleaseMat( &image_points );\r
+ }\r
+\r
+ cvReleaseFileStorage( &fs );\r
+}\r
+\r
+\r
+int calibrate( int argc, char** argv )\r
+{\r
+ CvSize board_size = {0,0};\r
+ float square_size = 1.f, aspect_ratio = 1.f;\r
+ const char* out_filename = "out_camera_data.yml";\r
+ const char* input_filename = 0;\r
+ int i, image_count = 10;\r
+ int write_extrinsics = 0, write_points = 0;\r
+ int flags = 0;\r
+ CvCapture* capture = 0;\r
+ FILE* f = 0;\r
+ char imagename[1024];\r
+ CvMemStorage* storage;\r
+ CvSeq* image_points_seq = 0;\r
+ int elem_size, flip_vertical = 0;\r
+ int delay = 1000;\r
+ clock_t prev_timestamp = 0;\r
+ CvPoint2D32f* image_points_buf = 0;\r
+ CvFont font = cvFont( 1, 1 );\r
+ double _camera[9], _dist_coeffs[4];\r
+ CvMat camera = cvMat( 3, 3, CV_64F, _camera );\r
+ CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs );\r
+ CvMat *extr_params = 0, *reproj_errs = 0;\r
+ double avg_reproj_err = 0;\r
+ int mode = DETECTION;\r
+ int undistort_image = 0;\r
+ CvSize img_size = {0,0};\r
+ //const char* live_capture_help = \r
+ // "When the live video from camera is used as input, the following hot-keys may be used:\n"\r
+ // " <ESC>, 'q' - quit the calibration\n"\r
+ // " 'g' - start capturing images\n"\r
+ // " 'u' - switch undistortion on/off\n";\r
+ const char* live_capture_help = \r
+ "Press g to start calibrating the camera\n";\r
+\r
+ if( argc < 2 )\r
+ {\r
+ printf( "This is a camera calibration sample.\n"\r
+ "Usage: calibration\n"\r
+ " -w <board_width> # the number of inner corners per one of board dimension\n"\r
+ " -h <board_height> # the number of inner corners per another board dimension\n"\r
+ " [-n <number_of_frames>] # the number of frames to use for calibration\n"\r
+ " # (if not specified, it will be set to the number\n"\r
+ " # of board views actually available)\n"\r
+ " [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n"\r
+ " # (used only for video capturing)\n"\r
+ " [-s <square_size>] # square size in some user-defined units (1 by default)\n"\r
+ " [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"\r
+ " [-op] # write detected feature points\n"\r
+ " [-oe] # write extrinsic parameters\n"\r
+ " [-zt] # assume zero tangential distortion\n"\r
+ " [-a <aspect_ratio>] # fix aspect ratio (fx/fy)\n"\r
+ " [-p] # fix the principal point at the center\n"\r
+ " [-v] # flip the captured images around the horizontal axis\n"\r
+ " [input_data] # input data, one of the following:\n"\r
+ " # - text file with a list of the images of the board\n"\r
+ " # - name of video file with a video of the board\n"\r
+ " # if input_data not specified, a live view from the camera is used\n"\r
+ "\n" );\r
+ printf( "%s", live_capture_help );\r
+ return 0;\r
+ }\r
+\r
+ for( i = 1; i < argc; i++ )\r
+ {\r
+ const char* s = argv[i];\r
+ if( strcmp( s, "-w" ) == 0 )\r
+ {\r
+ if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 )\r
+ return fprintf( stderr, "Invalid board width\n" ), -1;\r
+ }\r
+ else if( strcmp( s, "-h" ) == 0 )\r
+ {\r
+ if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 )\r
+ return fprintf( stderr, "Invalid board height\n" ), -1;\r
+ }\r
+ else if( strcmp( s, "-s" ) == 0 )\r
+ {\r
+ if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 )\r
+ return fprintf( stderr, "Invalid board square width\n" ), -1;\r
+ }\r
+ else if( strcmp( s, "-n" ) == 0 )\r
+ {\r
+ if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )\r
+ return printf("Invalid number of images\n" ), -1;\r
+ }\r
+ else if( strcmp( s, "-a" ) == 0 )\r
+ {\r
+ if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )\r
+ return printf("Invalid aspect ratio\n" ), -1;\r
+ }\r
+ else if( strcmp( s, "-d" ) == 0 )\r
+ {\r
+ if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )\r
+ return printf("Invalid delay\n" ), -1;\r
+ }\r
+ else if( strcmp( s, "-op" ) == 0 )\r
+ {\r
+ write_points = 1;\r
+ }\r
+ else if( strcmp( s, "-oe" ) == 0 )\r
+ {\r
+ write_extrinsics = 1;\r
+ }\r
+ else if( strcmp( s, "-zt" ) == 0 )\r
+ {\r
+ flags |= CV_CALIB_ZERO_TANGENT_DIST;\r
+ }\r
+ else if( strcmp( s, "-p" ) == 0 )\r
+ {\r
+ flags |= CV_CALIB_FIX_PRINCIPAL_POINT;\r
+ }\r
+ else if( strcmp( s, "-v" ) == 0 )\r
+ {\r
+ flip_vertical = 1;\r
+ }\r
+ else if( strcmp( s, "-o" ) == 0 )\r
+ {\r
+ out_filename = argv[++i];\r
+ }\r
+ else if( s[0] != '-' )\r
+ input_filename = s;\r
+ else\r
+ return fprintf( stderr, "Unknown option %s", s ), -1;\r
+ }\r
+\r
+ if( input_filename )\r
+ {\r
+ capture = cvCreateFileCapture( input_filename );\r
+ if( !capture )\r
+ {\r
+ f = fopen( input_filename, "rt" );\r
+ if( !f )\r
+ return fprintf( stderr, "The input file could not be opened\n" ), -1;\r
+ image_count = -1;\r
+ }\r
+ mode = CAPTURING;\r
+ }\r
+ else\r
+ {\r
+ capture = cvCreateCameraCapture(0);\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,640);\r
+ cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,480);\r
+\r
+ }\r
+\r
+ if( !capture && !f )\r
+ return fprintf( stderr, "Could not initialize video capture\n" ), -2;\r
+\r
+ if( capture && !input_filename)\r
+ printf( "%s", live_capture_help );\r
+\r
+ elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]);\r
+ storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 ));\r
+ image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size );\r
+ image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );\r
+\r
+ cvNamedWindow( "Image View", 1 );\r
+\r
+ for(;;)\r
+ {\r
+ IplImage *view = 0, *view_gray = 0;\r
+ int count = 0, found, blink = 0;\r
+ CvPoint text_origin;\r
+ CvSize text_size = {0,0};\r
+ int base_line = 0;\r
+ char s[100];\r
+ int key;\r
+ \r
+ if( f && fgets( imagename, sizeof(imagename)-2, f ))\r
+ {\r
+ int l = strlen(imagename);\r
+ if( l > 0 && imagename[l-1] == '\n' )\r
+ imagename[--l] = '\0';\r
+ if( l > 0 )\r
+ {\r
+ if( imagename[0] == '#' )\r
+ continue;\r
+ view = cvLoadImage( imagename, 1 );\r
+ }\r
+ }\r
+ else if( capture )\r
+ {\r
+ IplImage* view0 = cvQueryFrame( capture );\r
+ if( view0 )\r
+ {\r
+ view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );\r
+ if( view0->origin == IPL_ORIGIN_BL )\r
+ cvFlip( view0, view, 0 );\r
+ else\r
+ cvCopy( view0, view );\r
+ }\r
+ }\r
+\r
+ if( !view )\r
+ {\r
+ if( image_points_seq->total > 0 )\r
+ {\r
+ image_count = image_points_seq->total;\r
+ goto calibrate;\r
+ }\r
+ break;\r
+ }\r
+\r
+ if( flip_vertical )\r
+ cvFlip( view, view, 0 );\r
+\r
+ img_size = cvGetSize(view);\r
+ found = cvFindChessboardCorners( view, board_size,\r
+ image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );\r
+\r
+#if 1\r
+ // improve the found corners' coordinate accuracy\r
+ view_gray = cvCreateImage( cvGetSize(view), 8, 1 );\r
+ cvCvtColor( view, view_gray, CV_BGR2GRAY );\r
+ cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),\r
+ cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));\r
+ cvReleaseImage( &view_gray );\r
+#endif\r
+\r
+ if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )\r
+ {\r
+ cvSeqPush( image_points_seq, image_points_buf );\r
+ prev_timestamp = clock();\r
+ blink = !f;\r
+#if 1\r
+ if( capture )\r
+ {\r
+ sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );\r
+ cvSaveImage( imagename, view );\r
+ }\r
+#endif\r
+ }\r
+\r
+ cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );\r
+\r
+ cvGetTextSize( "100/100", &font, &text_size, &base_line );\r
+ text_origin.x = view->width - text_size.width - 10;\r
+ text_origin.y = view->height - base_line - 10;\r
+\r
+ if( mode == CAPTURING )\r
+ {\r
+ if( image_count > 0 )\r
+ sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );\r
+ else\r
+ sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );\r
+ }\r
+ else if( mode == CALIBRATED )\r
+ sprintf( s, "Calibrated" );\r
+ else\r
+ sprintf( s, "Press 'g' to start" );\r
+\r
+ cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?\r
+ CV_RGB(255,0,0) : CV_RGB(0,255,0));\r
+\r
+ if( blink )\r
+ cvNot( view, view );\r
+\r
+ if( mode == CALIBRATED && undistort_image )\r
+ {\r
+ IplImage* t = cvCloneImage( view );\r
+ cvUndistort2( t, view, &camera, &dist_coeffs );\r
+ cvReleaseImage( &t );\r
+ }\r
+ if (mode == CALIBRATED/* && input_filename*/)\r
+ {\r
+ printf ("\n Calibration done\n");\r
+ break;\r
+ }\r
+\r
+ cvShowImage( "Image View", view );\r
+ key = cvWaitKey(capture ? 50 : 500);\r
+\r
+ if( key == 27 )\r
+ break;\r
+ \r
+ if( key == 'u' && mode == CALIBRATED )\r
+ undistort_image = !undistort_image;\r
+\r
+ if( capture && key == 'g' )\r
+ {\r
+ mode = CAPTURING;\r
+ cvClearMemStorage( storage );\r
+ image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );\r
+ }\r
+\r
+ if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )\r
+ {\r
+calibrate:\r
+ cvReleaseMat( &extr_params );\r
+ cvReleaseMat( &reproj_errs );\r
+ int code = run_calibration( image_points_seq, img_size, board_size,\r
+ square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params,\r
+ &reproj_errs, &avg_reproj_err );\r
+ // save camera parameters in any case, to catch Inf's/NaN's\r
+ save_camera_params( out_filename, image_count, img_size,\r
+ board_size, square_size, aspect_ratio, flags,\r
+ &camera, &dist_coeffs, write_extrinsics ? extr_params : 0,\r
+ write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err );\r
+ if( code )\r
+ mode = CALIBRATED;\r
+ else\r
+ mode = DETECTION;\r
+ }\r
+\r
+ if( !view )\r
+ break;\r
+ cvReleaseImage( &view );\r
+ }\r
+ cvDestroyAllWindows();\r
+ if( capture )\r
+ cvReleaseCapture( &capture );\r
+ if( storage )\r
+ cvReleaseMemStorage( &storage );\r
+ return 0;\r
+}\r
projects / opencv.git / commitdiff