opencv_extra/3d/createsamples3d/createsamplesfromvideo.cpp

   1 /*
   2  * A Demo of automatic samples capturing
   3  * Author: Alexey Latyshev
   4  */
   5
   6
   7 #ifdef WIN32
   8 #include "cv.h"
   9 #include "highgui.h"
  10 #else
  11 #include "opencv/cv.h"
  12 #include "opencv/highgui.h"
  13 #endif
  14 #include <vector>
  15 #include <string>
  16 #include <algorithm>
  17 #include <stdio.h>
  18 #include <ctype.h>
  19 #include "calibration.h"
  20 #include "createsamples3d.h"
  21 #include "createsamplesfromvideo.h"
  22
  23
  24 int VIDEO_CALIBRATION_DELAY = 1000;
  25 bool AUTOFIND_CHESSBOARD=true;
  26 bool USE_DEINTERLACING=false;
  27 bool DRAW_CORNERS = true;
  28 bool DRAW_CHESSBOARD = true;
  29 bool USE_UNDISTORTION = false;
  30 bool CALIBRATE_CAMERA = false;
  31 bool IS_VIDEO_CAPTURE = false;
  32 bool SAVE_DRAWINGS = false;
  33 bool SAVE_SAMPLES = false;
  34 bool SAVE_ALL_FRAMES = false;
  35 bool SHOW_TEST_SQUARE = false;
  36 bool USE_3D = false;
  37 char* SAMPLES_PATH;
  38 char* INPUT_VIDEO;
  39 char* OUTPUT_DIRECTORY;
  40 char* CAMERA_PARAMETERS_PATH;
  41
  42 CvCapture* capture;
  43 IplImage* frame;
  44 IplImage* workImage;
  45 CvSize innerCornersCount;
  46 CvMat* IntrinsicMatrix;
  47 CvMat* DistortionCoeffs;
  48
  49
  50
  51 // Image deinterlacing
  52 IplImage* Deinterlace(IplImage* src)
  53 {
  54         IplImage* res = cvCloneImage(src);
  55         uchar* linea;
  56         uchar* lineb;
  57         uchar* linec;
  58
  59         for (int i = 1; i < res->height-1; i+=2)
  60         {
  61                 linea = (uchar*)res->imageData + ((i-1) * res->widthStep);
  62                 lineb = (uchar*)res->imageData + ((i) * res->widthStep);
  63                 linec = (uchar*)res->imageData + ((i+1) * res->widthStep);
  64
  65                 for (int j = 0; j < res->width * res->nChannels; j++)
  66                 {
  67                         lineb[j] = (uchar)((linea[j] + linec[j])/2);
  68                 }
  69         }
  70
  71         if (res->height > 1 && res->height % 2 == 0)
  72         {
  73                 linea = (uchar*)res->imageData + ((res->height-2) * res->widthStep);
  74                 lineb = (uchar*)res->imageData + ((res->height-1) * res->widthStep);
  75                 memcpy(lineb, linea, res->width);
  76         }
  77         return res;
  78
  79 }
  80
  81 void InverseUndistortMap(const CvMat* mapx,const CvMat* mapy, CvMat** invmapx, CvMat** invmapy, bool interpolate)
  82 {
  83         *invmapx=cvCreateMat(mapx->rows,mapx->cols,mapx->type);
  84         *invmapy=cvCreateMat(mapy->rows,mapy->cols,mapy->type);
  85         int x,y;
  86         for (int i=0;i<mapx->cols;i++)
  87                 for (int j=0;j<mapy->rows;j++)
  88                 {
  89                         (*invmapx)->data.fl[i+(*invmapx)->cols*j]=-1e20f;
  90                         (*invmapy)->data.fl[i+(*invmapy)->cols*j]=-1e20f;
  91                 }
  92
  93                 for (int i=0;i<mapx->cols;i++)
  94                         for (int j=0;j<mapy->rows;j++)
  95                         {
  96                                 x = (int) mapx->data.fl[i+mapx->cols*j];
  97                                 y = (int) mapy->data.fl[i+mapy->cols*j];
  98                                 if ((x>=0) && (x<(*invmapx)->cols) && (y>=0) && (y < (*invmapy)->rows))
  99                                 {
 100                                         (*invmapx)->data.fl[x+(*invmapx)->cols*y]=(float)i;
 101                                         (*invmapy)->data.fl[x+(*invmapy)->cols*y]=(float)j;
 102                                 }
 103                         }
 104                         if (interpolate)
 105                         {
 106                                 for (int i=1;i<mapx->cols-1;i++)
 107                                         for (int j=1;j<mapy->rows-1;j++)
 108                                         {
 109                                                 if ((*invmapx)->data.fl[i+(*invmapx)->cols*j]==-1e20)
 110                                                 {
 111                                                         (*invmapx)->data.fl[i+(*invmapx)->cols*j] = ((*invmapx)->data.fl[i-1+(*invmapx)->cols*j]+(*invmapx)->data.fl[i+1+(*invmapx)->cols*j]
 112                                                         + (*invmapx)->data.fl[i+(*invmapx)->cols*(j-1)]+(*invmapx)->data.fl[i+(*invmapx)->cols*(j+1)])/4;
 113                                                 }
 114                                                 if ((*invmapy)->data.fl[i+(*invmapy)->cols*j]==-1e20)
 115                                                 {
 116                                                         (*invmapy)->data.fl[i+(*invmapy)->cols*j] = ((*invmapy)->data.fl[i-1+(*invmapy)->cols*j]+(*invmapy)->data.fl[i+1+(*invmapy)->cols*j]
 117                                                         + (*invmapy)->data.fl[i+(*invmapy)->cols*(j-1)]+(*invmapy)->data.fl[i+(*invmapy)->cols*(j+1)])/4;
 118                                                 }
 119                                         }
 120                         }
 121 }
 122
 123 // left button click sets new point;
 124 // (!!!removed!!!)right button click removes last point
 125 void on_mouse( int event, int x, int y, int flags, void* param )
 126 {
 127         switch( event )
 128         {
 129         case CV_EVENT_LBUTTONUP:
 130                 {
 131
 132                         int n=0;
 133                         //for (n=0;(n<4) && ((*((CvPoint**)param))[n].x != -1);n++);
 134                         for (n=0;(n<4) && (((CvPoint*)param)[n].x != -1);n++);
 135                         if (n<4)
 136                         {
 137                                 ((CvPoint*)param)[n].x = x;
 138                                 ((CvPoint*)param)[n].y = y;
 139                         }
 140                 }
 141                 break;
 142         }
 143 }
 144
 145
 146 //Load camera params from yaml file
 147 int LoadCameraParams(char* filename, CvMat** intrinsic_matrix, CvMat** distortion_coeffs)
 148 {
 149         CvFileStorage* fs = cvOpenFileStorage( filename, 0, CV_STORAGE_READ );
 150         if (fs==NULL) return 0;
 151
 152         *intrinsic_matrix = (CvMat*)cvReadByName( fs,0,"camera_matrix");
 153         *distortion_coeffs = (CvMat*)cvReadByName( fs,0,"distortion_coefficients");
 154
 155         return 1;
 156 }
 157
 158
 159 int LoadApplicationParams(char* filename)
 160 {
 161         CvFileStorage* fs = cvOpenFileStorage( filename, 0, CV_STORAGE_READ );
 162         if (fs==NULL) return 0;
 163
 164         USE_DEINTERLACING = cvReadIntByName( fs,0,"USE_DEINTERLACING",0) != 0;
 165         USE_UNDISTORTION = cvReadIntByName( fs,0,"USE_UNDISTORTION",0) != 0;
 166         CALIBRATE_CAMERA = cvReadIntByName( fs,0,"CALIBRATE_CAMERA",0) != 0;
 167         DRAW_CORNERS = cvReadIntByName( fs,0,"DRAW_CORNERS",1) != 0;
 168         DRAW_CHESSBOARD = cvReadIntByName( fs,0,"DRAW_CHESSBOARD",1) != 0;
 169         AUTOFIND_CHESSBOARD = cvReadIntByName( fs,0,"AUTOFIND_CHESSBOARD",1) != 0;
 170         IS_VIDEO_CAPTURE = cvReadIntByName( fs,0,"IS_VIDEO_CAPTURE",0) != 0;
 171         SAVE_DRAWINGS = cvReadIntByName( fs,0,"SAVE_DRAWINGS",0) != 0;
 172         SAVE_SAMPLES = cvReadIntByName( fs,0,"SAVE_SAMPLES",0) != 0;
 173         SAVE_ALL_FRAMES = cvReadIntByName( fs,0,"SAVE_ALL_FRAMES",0) != 0;
 174         SHOW_TEST_SQUARE = cvReadIntByName( fs,0,"SHOW_TEST_SQUARE",0) != 0;
 175         USE_3D = cvReadIntByName( fs,0,"USE_3D",0) != 0;
 176         if (SAVE_SAMPLES)
 177         {
 178                 SAMPLES_PATH = new char[500];
 179                 if (cvReadStringByName( fs,0,"SAMPLES_PATH"))
 180                         strcpy(SAMPLES_PATH, cvReadStringByName( fs,0,"SAMPLES_PATH"));
 181                 else return 0;
 182         }
 183         if (IS_VIDEO_CAPTURE)
 184         {
 185                 INPUT_VIDEO = new char[500];
 186                 if (cvReadStringByName( fs,0,"INPUT_VIDEO"))
 187                         strcpy(INPUT_VIDEO,cvReadStringByName( fs,0,"INPUT_VIDEO") );//input video filename
 188                 else return 0;
 189         }
 190
 191         OUTPUT_DIRECTORY = new char[500];
 192         if (cvReadStringByName( fs,0,"OUTPUT_DIRECTORY"))
 193                 strcpy(OUTPUT_DIRECTORY,cvReadStringByName( fs,0,"OUTPUT_DIRECTORY"));//output directory for images and selected regions path
 194         else return 0;
 195         //chessboard inner corners count (width x height)
 196         int CHESSBOARD_WIDTH=cvReadIntByName( fs,0,"CHESSBOARD_WIDTH");
 197         int CHESSBOARD_HEIGHT=cvReadIntByName( fs,0,"CHESSBOARD_HEIGHT");
 198         innerCornersCount=cvSize(CHESSBOARD_WIDTH,CHESSBOARD_HEIGHT);
 199
 200
 201         CAMERA_PARAMETERS_PATH = new char[500];
 202         if (cvReadStringByName( fs,0,"CAMERA_PARAMETERS_PATH"))
 203                 strcpy(CAMERA_PARAMETERS_PATH, cvReadStringByName( fs,0,"CAMERA_PARAMETERS_PATH"));//camera parameters filename
 204         else
 205                 return 0;
 206         if (LoadCameraParams(CAMERA_PARAMETERS_PATH,&IntrinsicMatrix,&DistortionCoeffs))
 207         {
 208                 printf("\nCamera parameters loaded successfully\n");
 209         }
 210         else
 211         {
 212                 printf("\nUnable to load parameters\n");
 213                 if (USE_UNDISTORTION && (!CALIBRATE_CAMERA))
 214                         return 0;
 215         }
 216
 217
 218         //cvReleaseFileStorage(&fs);
 219         return 1;
 220 }
 221
 222
 223 IplImage* Undistort(IplImage* src,const CvMat* intrinsic_matrix,const CvMat* distortion_coeffs)
 224 {
 225         IplImage* undistortImg;
 226         undistortImg = cvCreateImage(cvSize(src->width,src->height),src->depth,src->nChannels);
 227         cvUndistort2(src,undistortImg,intrinsic_matrix,distortion_coeffs);
 228
 229         return undistortImg;
 230 }
 231
 232 int ShowFrame(int pos)
 233 {
 234         cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,pos);
 235         frame = cvQueryFrame(capture);
 236         if (frame)
 237         {
 238                 IplImage* undistortImg = 0;
 239                 cvReleaseImage(&workImage);
 240                 if (USE_DEINTERLACING)
 241                         workImage =  Deinterlace(frame);
 242                 else
 243                         workImage = cvCloneImage(frame);
 244                 if (USE_UNDISTORTION)
 245                 {
 246
 247                         undistortImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
 248                         workImage = cvCloneImage(undistortImg);
 249                         cvReleaseImage(&undistortImg);
 250                 }
 251
 252                 cvShowImage(FRAME_WINDOW,workImage);
 253                 return 1;
 254         }
 255         return 0;
 256 }
 257
 258
 259 //Calculates selected points coordinates (by x and y) in coordinate system connected with chessboard
 260 //(zero point is in the upper-left corner of the chessboard, axises are horizontal and vertical chessboard edges)
 261 // in: nFrame (number of frame on which points were selected), points (CvPoint[4] array with selected points)
 262 // out: relCoords (pointer to the array[4] with (x1,y1) coordinate of every point)
 263 // Function returns 0 if there is no chessboard found and array 2xN with corners otherwise
 264 // Camera Matrix is for 3D relative position estimation with z start coordinate
 265 // We also can give found Chessboard to the method instead of running chessboard finding algorithm
 266 CvMat* CalcRelativePosition(IplImage* workImage, CvPoint* points, CvPoint2D32f** relCoords, CvMat* cameraMatrix=NULL, float z = 0, CvMat* Chessboard=NULL)
 267 {
 268         CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
 269         int count;
 270
 271         if (!Chessboard)
 272         {
 273                 if (cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0)
 274                 {
 275                         delete[] corners;
 276                         return NULL;
 277                 }
 278
 279                 IplImage* view_gray = cvCreateImage( cvGetSize(workImage), 8, 1 );
 280                 cvCvtColor(workImage, view_gray, CV_BGR2GRAY );
 281                 cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
 282                 cvReleaseImage( &view_gray );
 283         }
 284         else
 285         {
 286                 for (int i=0;i<Chessboard->cols;i++)
 287                 {
 288                         corners[i].x = (float)Chessboard->data.db[i];
 289                         corners[i].y = (float)Chessboard->data.db[Chessboard->cols+i];
 290                 }
 291         }
 292
 293         if (!cameraMatrix)
 294         {
 295                 CvPoint2D32f src[3];
 296                 CvPoint2D32f dst[3];
 297                 src[0].x = corners[0].x;
 298                 src[0].y = corners[0].y;
 299                 src[1].x = corners[innerCornersCount.width-1].x;
 300                 src[1].y = corners[innerCornersCount.width-1].y;
 301                 src[2].x = corners[innerCornersCount.width*(innerCornersCount.height-1)].x;
 302                 src[2].y = corners[innerCornersCount.width*(innerCornersCount.height-1)].y;
 303                 dst[0].x = 0;
 304                 dst[0].y = 0;
 305                 dst[1].x = innerCornersCount.width-1.f;
 306                 dst[1].y = 0;
 307                 dst[2].x = 0;
 308                 dst[2].y = innerCornersCount.height-1.f;
 309
 310                 CvMat* map_matrix = cvCreateMat(2,3,CV_64F);
 311                 cvGetAffineTransform(src,dst,map_matrix);
 312
 313                 for (int i=0;i<4;i++)
 314                 {
 315                         (*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]));
 316                         (*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]));
 317                 }
 318                 cvReleaseMat(&map_matrix);
 319         }
 320         else
 321         {
 322                 CvMat* image_points = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);
 323                 CvMat* object_points = cvCreateMat(3,innerCornersCount.height*innerCornersCount.width,CV_64FC1);
 324
 325                 // Sets object points and image points
 326                 for (int i=0; i< innerCornersCount.height;i++)
 327                         for (int j=0; j < innerCornersCount.width;j++)
 328                         {
 329                                 object_points->data.db[(i*innerCornersCount.width+j)]=j;
 330                                 object_points->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=i;
 331                                 object_points->data.db[(i*innerCornersCount.width+j)+2*innerCornersCount.width*innerCornersCount.height]=0.0f;
 332
 333                                 image_points->data.db[(i*innerCornersCount.width+j)]=corners[(i*innerCornersCount.width+j)].x;
 334                                 image_points->data.db[(i*innerCornersCount.width+j)+innerCornersCount.width*innerCornersCount.height]=corners[(i*innerCornersCount.width+j)].y;
 335                         }
 336
 337                 CvMat* R = cvCreateMat(3, 3, CV_64FC1);
 338                 CvMat* T = cvCreateMat(3, 1, CV_64FC1);
 339                 CvMat* rotation_vector = cvCreateMat(3,1,CV_64FC1);
 340                 //Calculating Exrinsic camera parameters
 341                 CvMat* distCoeffs = cvCreateMat(5,1,CV_64FC1);
 342                 cvZero(distCoeffs);
 343                 cvFindExtrinsicCameraParams2(object_points,image_points,cameraMatrix, distCoeffs,rotation_vector,T);
 344                 cvReleaseMat(&distCoeffs);
 345                 cvRodrigues2(rotation_vector,R);
 346                 CvMat* M = cvCreateMat(3, 4, CV_64FC1);
 347                 CvMat* A = cvCreateMat(3, 3, CV_64FC1);
 348                 CvMat* invA = cvCreateMat(3, 3, CV_64FC1);
 349                 CvMat* point3D = cvCreateMat(3, 1, CV_64FC1);
 350                 CvMat* t = cvCreateMat(3, 1, CV_64FC1);
 351
 352                 cvGEMM(cameraMatrix,R,1.0,NULL,0,A);
 353                 cvGEMM(cameraMatrix,T,1.0,NULL,0,t);
 354
 355                 t->data.db[0]+=z*A->data.db[2];
 356                 t->data.db[1]+=z*A->data.db[5];
 357                 t->data.db[2]+=z*A->data.db[8];
 358
 359                 t->data.db[0]=-t->data.db[0];
 360                 t->data.db[1]=-t->data.db[1];
 361                 t->data.db[2]=-t->data.db[2];
 362
 363
 364                 for (int i=0;i<4;i++)
 365                 {
 366                         A->data.db[2]=-points[i].x;
 367                         A->data.db[5]=-points[i].y;
 368                         A->data.db[8]=-1.0f;
 369
 370                         cvInvert(A,invA);
 371                         cvGEMM(invA,t,1.0,NULL,0,point3D);
 372                         (*relCoords)[i].x = (float)(point3D->data.db[0]);
 373                         (*relCoords)[i].y = (float)(point3D->data.db[1]);
 374
 375                         //TEST
 376                         CvMat* d3 = cvCreateMat(1,1,CV_64FC3);
 377                         d3->data.db[0] = point3D->data.db[0];
 378                         d3->data.db[1] = point3D->data.db[1];
 379                         d3->data.db[2] = z;
 380                         distCoeffs = cvCreateMat(5,1,CV_64FC1);
 381                         CvMat* imP = cvCreateMat(1,1,CV_64FC2);
 382                         cvZero(distCoeffs);
 383
 384                         cvProjectPoints2(d3,rotation_vector,T,cameraMatrix,distCoeffs,imP);
 385                         cvReleaseMat(&imP);
 386                         cvReleaseMat(&d3);
 387                         cvReleaseMat(&distCoeffs);
 388                         //END OF
 389                 }
 390
 391                 cvReleaseMat(&T);
 392                 cvReleaseMat(&t);
 393                 cvReleaseMat(&R);
 394                 cvReleaseMat(&rotation_vector);
 395                 cvReleaseMat(&M);
 396                 cvReleaseMat(&A);
 397                 cvReleaseMat(&invA);
 398                 cvReleaseMat(&point3D);
 399
 400         }
 401         if (!Chessboard)
 402         {
 403                 CvMat* result = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);
 404                 for (int i=0;i<result->cols;i++)
 405                 {
 406                         result->data.db[i]=corners[i].x;
 407                         result->data.db[result->cols+i]=corners[i].y;
 408                 }
 409
 410                 delete[] corners;
 411                 return result;
 412         }
 413         else
 414         {
 415                 return NULL;
 416         }
 417
 418 }
 419
 420 // if chessboardPoints = NULL using simle affine transform otherwise we must have correct oldPoints pointer
 421 // chessBoardPoints is array 2xN
 422 //  returns new points location
 423 CvPoint* GetCurrentPointsPosition(IplImage* workImage, CvPoint2D32f* relCoords, CvMat* chessboardPoints, CvPoint* oldPoints, CvPoint2D32f* outCorners)
 424 {
 425         CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
 426         int count;
 427
 428         if (cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0)
 429         {
 430                 delete[] corners;
 431                 corners = NULL;
 432                 return NULL;
 433         }
 434
 435         IplImage* view_gray = cvCreateImage( cvGetSize(workImage), 8, 1 );
 436         cvCvtColor(workImage, view_gray, CV_BGR2GRAY );
 437         cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
 438         cvReleaseImage( &view_gray );
 439
 440         CvPoint* result = new CvPoint[4];
 441
 442         if (chessboardPoints && oldPoints)
 443         {
 444                 CvMat* chessboardPoints2 = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);
 445                 for (int i=0;i<(chessboardPoints2)->cols;i++)
 446                 {
 447                         chessboardPoints2->data.db[i]=(corners)[i].x;
 448                         chessboardPoints2->data.db[chessboardPoints2->cols+i]=(corners)[i].y;
 449                 }
 450                 CvMat* homography = cvCreateMat(3,3,CV_64FC1);
 451
 452                 cvFindHomography(chessboardPoints,chessboardPoints2,homography);
 453
 454                 for (int i=0;i<4;i++)
 455                 {
 456                         double t = oldPoints[i].x*(homography->data.db[6])+oldPoints[i].y*(homography->data.db[7])+(homography->data.db[8]);
 457                         result[i].x = (int)((oldPoints[i].x*(homography->data.db[0])+oldPoints[i].y*(homography->data.db[1])+(homography->data.db[2]))/t);
 458                         result[i].y = (int)((oldPoints[i].x*(homography->data.db[3])+oldPoints[i].y*(homography->data.db[4])+(homography->data.db[5]))/t);
 459
 460                 }
 461                 cvReleaseMat(&homography);
 462         }
 463
 464         else
 465         {
 466                 CvPoint2D32f src[3];
 467                 CvPoint2D32f dst[3];
 468                 dst[0].x = (corners)[0].x;
 469                 dst[0].y = (corners)[0].y;
 470                 dst[1].x = (corners)[innerCornersCount.width-1].x;
 471                 dst[1].y = (corners)[innerCornersCount.width-1].y;
 472                 dst[2].x = (corners)[innerCornersCount.width*(innerCornersCount.height-1)].x;
 473                 dst[2].y = (corners)[innerCornersCount.width*(innerCornersCount.height-1)].y;
 474                 src[0].x = 0;
 475                 src[0].y = 0;
 476                 src[1].x = innerCornersCount.width-1.f;
 477                 src[1].y = 0;
 478                 src[2].x = 0;
 479                 src[2].y = innerCornersCount.height-1.f;
 480
 481                 CvMat* map_matrix = cvCreateMat(2,3,CV_64F);
 482                 cvGetAffineTransform(src,dst,map_matrix);
 483
 484                 for (int i=0;i<4;i++)
 485                 {
 486                         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]));
 487                         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]));
 488                 }
 489                 cvReleaseMat(&map_matrix);
 490         }
 491
 492         if (outCorners)
 493         {
 494                 for (int i=0;i<innerCornersCount.height*innerCornersCount.width;i++)
 495                 {
 496                         outCorners[i].x=corners[i].x;
 497                         outCorners[i].y=corners[i].y;
 498                 }
 499         }
 500
 501         delete[] corners;
 502
 503         return result;
 504 }
 505
 506
 507
 508
 509 IplImage* GetSample(const IplImage* src,CvSize innerCornersCount, const CvPoint* points, CvPoint2D32f* chessboardCorners)
 510 {
 511         CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
 512         int count;
 513
 514         if (!chessboardCorners)
 515         {
 516                 if (cvFindChessboardCorners(src,innerCornersCount,corners,&count)==0)
 517                 {
 518                         delete[] corners;
 519                         return NULL;
 520                 }
 521
 522                 IplImage* view_gray = cvCreateImage( cvGetSize(src), 8, 1 );
 523                 cvCvtColor(src, view_gray, CV_BGR2GRAY );
 524                 cvFindCornerSubPix( view_gray, corners, count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
 525                 cvReleaseImage( &view_gray );
 526         }
 527         else
 528         {
 529                 for (int i=0;i<innerCornersCount.height*innerCornersCount.width;i++)
 530                 {
 531                         corners[i]=chessboardCorners[i];
 532                 }
 533
 534         }
 535
 536         CvPoint result[4];
 537         CvPoint2D32f top;
 538         CvPoint2D32f bot;
 539         top.x = corners[0].x < corners[(innerCornersCount.height-1)*innerCornersCount.width].x ? corners[0].x : corners[(innerCornersCount.height-1)*innerCornersCount.width].x;
 540         top.y = corners[0].y < corners[innerCornersCount.width-1].y ? corners[0].y : corners[innerCornersCount.width-1].y;
 541         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;
 542         bot.y = corners[innerCornersCount.height*innerCornersCount.width-1].y < corners[(innerCornersCount.height-1)*innerCornersCount.width].y ?
 543                 corners[(innerCornersCount.height-1)*innerCornersCount.width].y : corners[innerCornersCount.height*innerCornersCount.width-1].y;
 544
 545         CvMat* chessboardPoints = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);
 546         CvMat* chessboardPoints2 = cvCreateMat(2,innerCornersCount.height*innerCornersCount.width,CV_64FC1);
 547
 548         //assumes that chessboard squares are squared
 549         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);
 550         for (int i=0;i<innerCornersCount.width;i++)
 551                 for (int j=0;j<innerCornersCount.height;j++)
 552                 {
 553                         chessboardPoints2->data.db[i+j*innerCornersCount.width]=top.x+i*step;
 554                         chessboardPoints2->data.db[i+j*(innerCornersCount.width)+innerCornersCount.width*innerCornersCount.height]=
 555                                 top.y+j*step;
 556                 }
 557
 558                 for (int i=0;i<(chessboardPoints)->cols;i++)
 559                 {
 560                         chessboardPoints->data.db[i]=(corners)[i].x;
 561                         chessboardPoints->data.db[chessboardPoints->cols+i]=(corners)[i].y;
 562                 }
 563                 CvMat* homography = cvCreateMat(3,3,CV_64FC1);
 564
 565                 cvFindHomography(chessboardPoints,chessboardPoints2,homography);
 566
 567                 for (int i=0;i<4;i++)
 568                 {
 569                         double t = points[i].x*(homography->data.db[6])+points[i].y*(homography->data.db[7])+(homography->data.db[8]);
 570                         result[i].x = (int)((points[i].x*(homography->data.db[0])+points[i].y*(homography->data.db[1])+(homography->data.db[2]))/t);
 571                         result[i].y = (int)((points[i].x*(homography->data.db[3])+points[i].y*(homography->data.db[4])+(homography->data.db[5]))/t);
 572                 }
 573
 574                 IplImage* resImage= cvCloneImage(src);
 575                 cvWarpPerspective( src, resImage, homography);
 576                 CvRect rect;
 577                 rect.x = result[0].x<result[2].x ? result[0].x : result[2].x;
 578                 rect.y = result[0].y<result[1].y ? result[0].y : result[1].y;
 579                 rect.width = result[1].x<result[3].x ? (result[3].x-rect.x) : (result[1].x-rect.x);
 580                 rect.height = result[2].y<result[3].y ? (result[3].y-rect.y) : (result[2].y-rect.y);
 581                 if ((rect.x>=0) && (rect.y >=0) && ((rect.x+rect.width)<resImage->width)&&((rect.y+rect.height)<resImage->height))
 582                         cvSetImageROI(resImage,rect);
 583                 else
 584                         return NULL;
 585
 586                 cvReleaseMat(&homography);
 587                 return resImage;
 588                 delete[] corners;
 589 }
 590
 591 void createSamples2DObject(int argc, char** argv)
 592 {
 593         CvPoint points[4];
 594         CvPoint pointsChessboardSquare[4];
 595
 596         CvPoint2D32f* relCoords = new CvPoint2D32f[4];
 597         CvPoint2D32f* relCoordsChessboardSquare = new CvPoint2D32f[4];
 598         CvPoint * newPoints;
 599         CvPoint * newPointsChessboardSquare;
 600         int currentFrame=0;
 601         int key;
 602         IplImage* undistortedImg;
 603
 604         if (CALIBRATE_CAMERA)
 605         {
 606                 int v = IS_VIDEO_CAPTURE ? 10 : 7;
 607                 char** c = new char*[v];
 608                 c[0]=new char[11]; c[0]="calibration";
 609                 c[1]=new char[2]; c[1]="-w";
 610                 c[2] = new char[2]; sprintf(c[2],"%d",innerCornersCount.width);
 611                 c[3]=new char[2]; c[3]="-h";
 612                 c[4] = new char[2];  sprintf(c[4],"%d",innerCornersCount.height);
 613                 c[5] = new char[2]; c[5]="-d";
 614                 c[6] = new char[5];  sprintf(c[6],"%d",VIDEO_CALIBRATION_DELAY);
 615                 c[7]=new char[2]; c[7]="-o";
 616                 c[8]=new char[100];
 617                 sprintf(c[8],"%scamera.yml",OUTPUT_DIRECTORY);
 618                 if (IS_VIDEO_CAPTURE)
 619                 {
 620                         c[9]=new char[strlen(INPUT_VIDEO)+1];
 621                         strcpy(c[9],INPUT_VIDEO);
 622                 }
 623
 624                 calibrate(v,c);
 625                 LoadCameraParams(c[8],&IntrinsicMatrix, &DistortionCoeffs);
 626
 627                 // Add functionality for camera calibration
 628         }
 629
 630         cvNamedWindow(FRAME_WINDOW,1);
 631         capture = IS_VIDEO_CAPTURE ? cvCreateFileCapture(INPUT_VIDEO) : cvCreateCameraCapture(0);
 632
 633         if (!IS_VIDEO_CAPTURE)
 634         {
 635                 cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,640);
 636                 cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,480);
 637         }
 638
 639         cvResizeWindow(FRAME_WINDOW,(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH),(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT));
 640
 641         CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
 642         int count;
 643
 644         do
 645         {
 646                 if (IS_VIDEO_CAPTURE)
 647                         cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,++currentFrame);
 648                 if (workImage) cvReleaseImage(&workImage);
 649                 frame = cvQueryFrame(capture);
 650                 if (frame)
 651                 {
 652                         if (USE_DEINTERLACING)
 653                                 workImage =  Deinterlace(frame);
 654                         else
 655                                 workImage = cvCloneImage(frame);
 656
 657                         if (USE_UNDISTORTION)
 658                         {
 659
 660                                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
 661                                 cvReleaseImage(&workImage);
 662                                 workImage = cvCloneImage(undistortedImg);
 663                                 cvReleaseImage(&undistortedImg);
 664                         }
 665                         //if (!IS_VIDEO_CAPTURE)
 666                         cvShowImage(FRAME_WINDOW,workImage);
 667                         key = cvWaitKey(30);
 668                         if (key==27) return;
 669                 }
 670
 671         }
 672         while ( frame && (((AUTOFIND_CHESSBOARD && IS_VIDEO_CAPTURE)|| (!IS_VIDEO_CAPTURE)) && cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0));
 673
 674         if (frame == NULL)
 675         {
 676                 printf("\n Unable to load video with chessboard or connect to the camera");
 677                 return;
 678         }
 679
 680         delete[] corners;
 681         currentFrame--;
 682
 683         IplImage* firstImage=cvCloneImage(workImage);
 684
 685         cvShowImage(FRAME_WINDOW,workImage);
 686
 687         points[0].x =-1;
 688         points[1].x =-1;
 689         points[2].x =-1;
 690         points[3].x =-1;
 691
 692         printf("\n Select the quadrangle region by selectig its vertices on the frame with a mouse\n");
 693         cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points);
 694
 695         // Getting initial points position
 696         for (int currPoint=0;currPoint<4;)
 697         {
 698                 if (points[currPoint].x != -1)
 699                 {
 700                         cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),2,cvScalar(255,255,255));
 701                         cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),3,cvScalar(0));
 702                         cvShowImage(FRAME_WINDOW,workImage);
 703                         currPoint++;
 704                 }
 705                 key = cvWaitKey(30);
 706                 if (IS_VIDEO_CAPTURE)
 707                 {
 708                         switch (key)
 709                         {
 710                         case 32: // Space symbol
 711                                 if (ShowFrame(++currentFrame))
 712                                 {
 713                                         points[0].x =-1;
 714                                         points[1].x =-1;
 715                                         points[2].x =-1;
 716                                         points[3].x =-1;
 717                                         currPoint = 0;
 718                                 }
 719                                 else
 720                                 {
 721                                         currentFrame--;
 722                                 }
 723                                 break;
 724                         case 8: // Backspace symbol
 725                                 if (currentFrame>0)
 726                                 {
 727                                         if (ShowFrame(--currentFrame))
 728                                         {
 729                                                 points[0].x =-1;
 730                                                 points[1].x =-1;
 731                                                 points[2].x =-1;
 732                                                 points[3].x =-1;
 733                                                 currPoint = 0;
 734                                         }
 735                                         else
 736                                         {
 737                                                 currentFrame++;
 738                                         }
 739                                 }
 740                                 break;
 741                         }
 742                 }
 743         }
 744
 745         // sorting points
 746
 747         for (int i=1;i<4;i++)
 748         {
 749
 750                 if (points[i].y<points[0].y)
 751                 {
 752                         CvPoint temp;
 753                         temp = points[0];
 754                         points[0]=points[i];
 755                         points[i]=temp;
 756                 }
 757         }
 758         if (points[1].x<points[0].x)
 759         {
 760                 CvPoint temp;
 761                 temp = points[0];
 762                 points[0]=points[1];
 763                 points[1]=temp;
 764         }
 765
 766         if (points[3].x<points[2].x)
 767         {
 768                 CvPoint temp;
 769                 temp = points[3];
 770                 points[3]=points[2];
 771                 points[2]=temp;
 772         }
 773
 774         //end of sorting
 775
 776         if (workImage) cvReleaseImage(&workImage);
 777         workImage = cvCloneImage(firstImage);
 778         if (USE_DEINTERLACING)
 779                 workImage =  Deinterlace(frame);
 780         else
 781                 workImage = cvCloneImage(frame);
 782         if (USE_UNDISTORTION)
 783         {
 784                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
 785                 cvReleaseImage(&workImage);
 786                 workImage = cvCloneImage(undistortedImg);
 787                 cvReleaseImage(&undistortedImg);
 788         }
 789         cvReleaseImage(&firstImage);
 790
 791
 792         CvMat* chessboardPointsMat = CalcRelativePosition(workImage, points,&relCoords);
 793         if (SHOW_TEST_SQUARE && chessboardPointsMat)
 794         {
 795                 pointsChessboardSquare[0].x = (int)chessboardPointsMat->data.db[(innerCornersCount.height-1)*innerCornersCount.width-2];
 796                 pointsChessboardSquare[1].x = (int)chessboardPointsMat->data.db[(innerCornersCount.height-1)*innerCornersCount.width-1];
 797                 pointsChessboardSquare[3].x = (int)chessboardPointsMat->data.db[innerCornersCount.height*innerCornersCount.width-2];
 798                 pointsChessboardSquare[2].x = (int)chessboardPointsMat->data.db[innerCornersCount.height*innerCornersCount.width-1];
 799
 800                 pointsChessboardSquare[0].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+(innerCornersCount.height-1)*innerCornersCount.width-2];
 801                 pointsChessboardSquare[1].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+(innerCornersCount.height-1)*innerCornersCount.width-1];
 802                 pointsChessboardSquare[3].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+innerCornersCount.height*innerCornersCount.width-2];
 803                 pointsChessboardSquare[2].y = (int)chessboardPointsMat->data.db[chessboardPointsMat->cols+innerCornersCount.height*innerCornersCount.width-1];
 804                 CalcRelativePosition(workImage,pointsChessboardSquare,&relCoordsChessboardSquare);
 805         }
 806
 807         char* PATH = OUTPUT_DIRECTORY;
 808         FILE* f;
 809         char path[100];
 810         char path_frames[100];
 811         sprintf(path_frames,"%sframes.txt",PATH);
 812         remove(path_frames);
 813         char cmd[100];
 814 #ifdef WIN32
 815         sprintf(cmd,"mkdir %s",PATH);
 816         system(cmd);
 817         sprintf(cmd,"del %s*.* /q",PATH);
 818         system(cmd);
 819         if (SAVE_SAMPLES)
 820         {
 821                 sprintf(cmd,"mkdir %s",SAMPLES_PATH);
 822                 system(cmd);
 823                 sprintf(cmd,"del %s*.* /q",SAMPLES_PATH);
 824                 system(cmd);
 825         }
 826 #else
 827
 828         sprintf(cmd,"mkdir %s",PATH);
 829         system(cmd);
 830         sprintf(cmd,"rm -f %s*.*",PATH);
 831         system(cmd);
 832         if (SAVE_SAMPLES)
 833         {
 834                 sprintf(cmd,"mkdir %s",SAMPLES_PATH);
 835                 system(cmd);
 836                 sprintf(cmd,"rm -f %s*.*",SAMPLES_PATH);
 837                 system(cmd);
 838         }
 839 #endif
 840
 841         do
 842         {
 843                 if (workImage) cvReleaseImage(&workImage);
 844                 frame = cvQueryFrame(capture);
 845                 if (frame)
 846                 {
 847                         if (USE_DEINTERLACING)
 848                                 workImage =  Deinterlace(frame);
 849                         else
 850                                 workImage = cvCloneImage(frame);
 851
 852                         if (USE_UNDISTORTION)
 853                         {
 854                                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
 855                                 cvReleaseImage(&workImage);
 856                                 workImage = cvCloneImage(undistortedImg);
 857                                 cvReleaseImage(&undistortedImg);
 858                         }
 859
 860                         if (chessboardPointsMat)
 861                         {
 862                                 CvPoint2D32f* ChessboardCorners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
 863                                 newPoints = GetCurrentPointsPosition(workImage,relCoords,chessboardPointsMat,points,ChessboardCorners/*&newPointsMat*/);
 864
 865                                 if (newPoints)
 866                                 {
 867                                         if (SHOW_TEST_SQUARE)
 868                                         {
 869                                                 newPointsChessboardSquare = GetCurrentPointsPosition(workImage,relCoordsChessboardSquare,chessboardPointsMat,pointsChessboardSquare,ChessboardCorners);
 870                                                 cvFillConvexPoly(workImage,newPointsChessboardSquare,4,cvScalar(10,10,250));
 871                                         }
 872                                         bool areOnFrame=true; // are points on frame or not
 873                                         for (int i=0;i<4;i++)
 874                                         {
 875                                                 if ((newPoints[i].x>workImage->width)||(newPoints[i].x<0)||(newPoints[i].y>workImage->height)||(newPoints[i].y<0))
 876                                                         areOnFrame=false;
 877                                         }
 878
 879                                         if (areOnFrame)
 880                                         {
 881                                                 f = fopen(path_frames,"a");
 882                                                 sprintf(path,"%s%d.jpg",PATH,currentFrame+1);
 883                                                 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);
 884                                                 fclose(f);
 885
 886                                                 if (DRAW_CHESSBOARD)
 887                                                         cvDrawChessboardCorners(workImage,innerCornersCount,ChessboardCorners,innerCornersCount.height*innerCornersCount.width,1);
 888                                                 if (DRAW_CORNERS)
 889                                                 {
 890                                                         cvCircle(workImage,newPoints[0],2,cvScalar(255,255,255));
 891                                                         cvCircle(workImage,newPoints[0],3,cvScalar(0));
 892                                                         cvCircle(workImage,newPoints[1],2,cvScalar(255,255,255));
 893                                                         cvCircle(workImage,newPoints[1],3,cvScalar(0));
 894                                                         cvCircle(workImage,newPoints[2],2,cvScalar(255,255,255));
 895                                                         cvCircle(workImage,newPoints[2],3,cvScalar(0));
 896                                                         cvCircle(workImage,newPoints[3],2,cvScalar(255,255,255));
 897                                                         cvCircle(workImage,newPoints[3],3,cvScalar(0));
 898                                                 }
 899                                                 if (SAVE_DRAWINGS)
 900                                                         cvSaveImage(path,workImage);
 901                                                 else
 902                                                 {
 903                                                         if (USE_DEINTERLACING)
 904                                                         {
 905                                                                 IplImage* tmp = Deinterlace(frame);
 906                                                                 if (USE_UNDISTORTION)
 907                                                                 {
 908                                                                         undistortedImg = Undistort(tmp,IntrinsicMatrix,DistortionCoeffs);
 909                                                                         cvReleaseImage(&tmp);
 910                                                                         tmp = cvCloneImage(undistortedImg);
 911                                                                         cvReleaseImage(&undistortedImg);
 912                                                                 }
 913                                                                 cvSaveImage(path,tmp);
 914                                                                 cvReleaseImage(&tmp);
 915                                                         }
 916                                                         else
 917                                                         {
 918                                                                 if (!USE_UNDISTORTION)
 919                                                                         cvSaveImage(path,frame);
 920                                                                 else
 921                                                                 {
 922                                                                         undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);
 923                                                                         cvSaveImage(path,undistortedImg);
 924                                                                         cvReleaseImage(&undistortedImg);
 925                                                                 }
 926                                                         }
 927                                                 }
 928                                                 if (SAVE_SAMPLES)
 929                                                 {
 930                                                         sprintf(path,"%s%d.jpg",SAMPLES_PATH,currentFrame+1);
 931                                                         IplImage* tmp;
 932                                                         if (!USE_UNDISTORTION)
 933                                                                 tmp = GetSample(frame,innerCornersCount,newPoints,ChessboardCorners);
 934                                                         else
 935                                                         {
 936                                                                 undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);
 937                                                                 tmp = GetSample(undistortedImg,innerCornersCount,newPoints,ChessboardCorners);
 938                                                                 cvReleaseImage(&undistortedImg);
 939                                                         }
 940                                                         if (tmp)
 941                                                         {
 942                                                                 cvSaveImage(path,tmp);
 943                                                                 cvReleaseImage(&tmp);
 944                                                         }
 945                                                 }
 946                                                 printf("Frame %d successfully saved to %s\n",currentFrame+1,path);
 947                                                 delete[] ChessboardCorners;
 948                                                 ChessboardCorners = NULL;
 949                                         }
 950
 951                                         delete[] newPoints;
 952                                         newPoints = NULL;
 953                                 }
 954                                 else
 955                                         if (SAVE_ALL_FRAMES)
 956                                         {
 957                                                 sprintf(path,"%s%d.jpg",PATH,currentFrame+1);
 958                                                 cvSaveImage(path,workImage);
 959                                         }
 960
 961                         }
 962
 963
 964                         cvShowImage(FRAME_WINDOW,workImage);
 965                         key = cvWaitKey(30);
 966                 }
 967                 currentFrame++;
 968         }
 969         while (frame && (key!=27));
 970
 971         if (capture)
 972                 cvReleaseCapture(&capture);
 973 }
 974 //--------------------------
 975
 976 void createSamples3DObject(int argc, char** argv)
 977 {
 978         CvPoint points1[4];
 979         CvPoint points2[4];
 980
 981         IplImage* i1;
 982         IplImage* i2;
 983         IplImage* undistortedImg;
 984
 985         CvPoint3D32f* points3D = new CvPoint3D32f[8];
 986         CvPoint* newPoints = new CvPoint[8];
 987
 988         int currentFrame=0;
 989         int key;
 990         int boxDepthValue=10;
 991         int maxErrorValue=10;
 992
 993
 994         if (CALIBRATE_CAMERA)
 995         {
 996                 int v = 10;/*IS_VIDEO_CAPTURE ? 10 : 7*/;
 997                 char** c = new char*[v];
 998                 c[0] = new char[11]; c[0]="calibration";
 999                 c[1] = new char[2]; c[1]="-w";
1000                 c[2] = new char[2]; sprintf(c[2],"%d",innerCornersCount.width);
1001                 c[3] = new char[2]; c[3]="-h";
1002                 c[4] = new char[2];  sprintf(c[4],"%d",innerCornersCount.height);
1003                 c[5] = new char[2]; c[5]="-d";
1004                 c[6] = new char[5];  sprintf(c[6],"%d",VIDEO_CALIBRATION_DELAY);
1005                 c[7] = new char[2]; c[7]="-o";
1006                 c[8] = new char[100];
1007                 sprintf(c[8],"%scamera.yml",OUTPUT_DIRECTORY);
1008                 c[9]=new char[strlen(INPUT_VIDEO)+1];
1009                 strcpy(c[9],INPUT_VIDEO);
1010
1011                 calibrate(v,c);
1012                 LoadCameraParams(c[8],&IntrinsicMatrix, &DistortionCoeffs);
1013
1014                 // Add functionality for camera calibration
1015         }
1016
1017         cvNamedWindow(FRAME_WINDOW,1);
1018         cvCreateTrackbar(DEPTH_TRACKBAR,FRAME_WINDOW,&boxDepthValue,MAX_DEPTH_VALUE,NULL);
1019         cvCreateTrackbar(ERROR_TRACKBAR,FRAME_WINDOW,&maxErrorValue,MAX_ERROR_VALUE,NULL);
1020         capture =cvCreateFileCapture(INPUT_VIDEO);
1021
1022         cvResizeWindow(FRAME_WINDOW,(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH),(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT));
1023
1024         CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
1025
1026         if (capture == NULL)
1027         {
1028                 printf("\n Unable to load video with chessboard or connect to the camera");
1029                 return;
1030         }
1031
1032         delete[] corners;
1033
1034         if (workImage) cvReleaseImage(&workImage);
1035         frame = cvQueryFrame(capture);
1036         if (frame)
1037         {
1038                 if (USE_DEINTERLACING)
1039                         workImage =  Deinterlace(frame);
1040                 else
1041                         workImage = cvCloneImage(frame);
1042
1043                 if (USE_UNDISTORTION)
1044                 {
1045
1046                         undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1047                         cvReleaseImage(&workImage);
1048                         workImage = cvCloneImage(undistortedImg);
1049                         cvReleaseImage(&undistortedImg);
1050                 }
1051         }
1052
1053         IplImage* firstImage=cvCloneImage(workImage);
1054
1055         cvShowImage(FRAME_WINDOW,workImage);
1056
1057         do
1058         {
1059                 points1[0].x =-1;
1060                 points1[1].x =-1;
1061                 points1[2].x =-1;
1062                 points1[3].x =-1;
1063                 points2[0].x =-1;
1064                 points2[1].x =-1;
1065                 points2[2].x =-1;
1066                 points2[3].x =-1;
1067
1068                 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");
1069
1070                 cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points1);
1071
1072                 // Getting initial points position
1073                 for (int currPoint=0;currPoint<4;)
1074                 {
1075                         if (points1[currPoint].x != -1)
1076                         {
1077                                 cvCircle(workImage,cvPoint(points1[currPoint].x,points1[currPoint].y),2,cvScalar(255,255,255));
1078                                 cvCircle(workImage,cvPoint(points1[currPoint].x,points1[currPoint].y),3,cvScalar(0));
1079                                 cvShowImage(FRAME_WINDOW,workImage);
1080                                 currPoint++;
1081                         }
1082                         key = cvWaitKey(30);
1083                         switch (key)
1084                         {
1085                         case 32: // Space symbol
1086                                 if (ShowFrame(++currentFrame))
1087                                 {
1088                                         points1[0].x =-1;
1089                                         points1[1].x =-1;
1090                                         points1[2].x =-1;
1091                                         points1[3].x =-1;
1092                                         currPoint = 0;
1093                                 }
1094                                 else
1095                                 {
1096                                         currentFrame--;
1097                                 }
1098                                 break;
1099                         case 8: // Backspace symbol
1100                                 if (currentFrame>0)
1101                                 {
1102                                         if (ShowFrame(--currentFrame))
1103                                         {
1104                                                 points1[0].x =-1;
1105                                                 points1[1].x =-1;
1106                                                 points1[2].x =-1;
1107                                                 points1[3].x =-1;
1108
1109                                                 currPoint = 0;
1110                                         }
1111                                         else
1112                                         {
1113                                                 currentFrame++;
1114                                         }
1115                                 }
1116                                 break;
1117                         }
1118                 }
1119
1120                 i1 = cvCloneImage(workImage);
1121
1122                 // sorting points
1123
1124                 for (int i=1;i<4;i++)
1125                 {
1126
1127                         if (points1[i].y<points1[0].y)
1128                         {
1129                                 CvPoint temp;
1130                                 temp = points1[0];
1131                                 points1[0]=points1[i];
1132                                 points1[i]=temp;
1133                         }
1134                 }
1135                 if (points1[1].x<points1[0].x)
1136                 {
1137                         CvPoint temp;
1138                         temp = points1[0];
1139                         points1[0]=points1[1];
1140                         points1[1]=temp;
1141                 }
1142
1143                 if (points1[3].x<points1[2].x)
1144                 {
1145                         CvPoint temp;
1146                         temp = points1[3];
1147                         points1[3]=points1[2];
1148                         points1[2]=temp;
1149                 }
1150
1151                 //end of sorting
1152
1153
1154                 ShowFrame(currentFrame);
1155                 //second frame
1156                 cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points2);
1157
1158                 printf("\nSelect one more time on another frame\n");
1159                 // Getting initial points position
1160                 for (int currPoint=0;currPoint<4;)
1161                 {
1162                         if (points2[currPoint].x != -1)
1163                         {
1164                                 cvCircle(workImage,cvPoint(points2[currPoint].x,points2[currPoint].y),2,cvScalar(255,255,255));
1165                                 cvCircle(workImage,cvPoint(points2[currPoint].x,points2[currPoint].y),3,cvScalar(0));
1166                                 cvShowImage(FRAME_WINDOW,workImage);
1167                                 currPoint++;
1168                         }
1169                         key = cvWaitKey(30);
1170                         switch (key)
1171                         {
1172                         case 32: // Space symbol
1173                                 if (ShowFrame(++currentFrame))
1174                                 {
1175                                         points2[0].x =-1;
1176                                         points2[1].x =-1;
1177                                         points2[2].x =-1;
1178                                         points2[3].x =-1;
1179                                         currPoint = 0;
1180                                 }
1181                                 else
1182                                 {
1183                                         currentFrame--;
1184                                 }
1185                                 break;
1186                         case 8: // Backspace symbol
1187                                 if (currentFrame>0)
1188                                 {
1189                                         if (ShowFrame(--currentFrame))
1190                                         {
1191                                                 points2[0].x =-1;
1192                                                 points2[1].x =-1;
1193                                                 points2[2].x =-1;
1194                                                 points2[3].x =-1;
1195                                                 currPoint = 0;
1196                                         }
1197                                         else
1198                                         {
1199                                                 currentFrame++;
1200                                         }
1201                                 }
1202                                 break;
1203                         }
1204                 }
1205
1206                 i2 = cvCloneImage(workImage);
1207                 // sorting points
1208
1209                 for (int i=1;i<4;i++)
1210                 {
1211
1212                         if (points2[i].y<points2[0].y)
1213                         {
1214                                 CvPoint temp;
1215                                 temp = points2[0];
1216                                 points2[0]=points2[i];
1217                                 points2[i]=temp;
1218                         }
1219                 }
1220                 if (points2[1].x<points2[0].x)
1221                 {
1222                         CvPoint temp;
1223                         temp = points2[0];
1224                         points2[0]=points2[1];
1225                         points2[1]=temp;
1226                 }
1227
1228                 if (points2[3].x<points2[2].x)
1229                 {
1230                         CvPoint temp;
1231                         temp = points2[3];
1232                         points2[3]=points2[2];
1233                         points2[2]=temp;
1234                 }
1235
1236                 //end of sorting
1237
1238
1239
1240                 if (workImage) cvReleaseImage(&workImage);
1241                 if (USE_DEINTERLACING)
1242                 {
1243                         workImage =  Deinterlace(i1);
1244                         cvReleaseImage(&i1);
1245                         i1=cvCloneImage(workImage);
1246                         cvReleaseImage(&workImage);
1247                         workImage =  Deinterlace(i2);
1248                         cvReleaseImage(&i2);
1249                         i2=cvCloneImage(workImage);
1250                         cvReleaseImage(&workImage);
1251                 }
1252
1253                 if (USE_UNDISTORTION)
1254                 {
1255                         workImage =  Undistort(i1,IntrinsicMatrix,DistortionCoeffs);
1256                         cvReleaseImage(&i1);
1257                         i1=cvCloneImage(workImage);
1258                         cvReleaseImage(&workImage);
1259                         workImage =  Undistort(i2,IntrinsicMatrix,DistortionCoeffs);
1260                         cvReleaseImage(&i2);
1261                         i2=cvCloneImage(workImage);
1262                         cvReleaseImage(&workImage);
1263                 }
1264
1265                 points3D = calc3DPoints(i1,i2,points1,points2,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,
1266                         !USE_UNDISTORTION,(float)(cvGetTrackbarPos(DEPTH_TRACKBAR,FRAME_WINDOW))/IDENTITY_DEPTH_VALUE,(float)(cvGetTrackbarPos(ERROR_TRACKBAR,FRAME_WINDOW))/IDENTITY_ERROR_VALUE);
1267                 if (!points3D)
1268                 {
1269                         printf("\nUnable to find correct stereo correspondence, please try again\n");
1270                         ShowFrame(currentFrame);
1271                 }
1272         }
1273         while (!points3D);
1274
1275
1276
1277         workImage = cvCloneImage(firstImage);
1278         if (USE_DEINTERLACING)
1279                 workImage =  Deinterlace(frame);
1280         else
1281                 workImage = cvCloneImage(frame);
1282         if (USE_UNDISTORTION)
1283         {
1284                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1285                 cvReleaseImage(&workImage);
1286                 workImage = cvCloneImage(undistortedImg);
1287                 cvReleaseImage(&undistortedImg);
1288         }
1289         cvReleaseImage(&firstImage);
1290
1291
1292         char* PATH = OUTPUT_DIRECTORY;
1293         FILE* f;
1294         char path[100];
1295         char path_frames[100];
1296         sprintf(path_frames,"%sframes.txt",PATH);
1297         remove(path_frames);
1298         char cmd[100];
1299 #ifdef WIN32
1300         sprintf(cmd,"mkdir %s",PATH);
1301         system(cmd);
1302         sprintf(cmd,"del %s*.* /q",PATH);
1303         system(cmd);
1304         if (SAVE_SAMPLES)
1305         {
1306                 sprintf(cmd,"mkdir %s",SAMPLES_PATH);
1307                 system(cmd);
1308                 sprintf(cmd,"del %s*.* /q",SAMPLES_PATH);
1309                 system(cmd);
1310         }
1311 #else
1312
1313         sprintf(cmd,"mkdir %s",PATH);
1314         system(cmd);
1315         sprintf(cmd,"rm -f %s*.*",PATH);
1316         system(cmd);
1317         if (SAVE_SAMPLES)
1318         {
1319                 sprintf(cmd,"mkdir %s",SAMPLES_PATH);
1320                 system(cmd);
1321                 sprintf(cmd,"rm -f %s*.*",SAMPLES_PATH);
1322                 system(cmd);
1323         }
1324 #endif
1325
1326         do
1327         {
1328                 if (workImage) cvReleaseImage(&workImage);
1329                 frame = cvQueryFrame(capture);
1330                 if (frame)
1331                 {
1332                         if (USE_DEINTERLACING)
1333                                 workImage =  Deinterlace(frame);
1334                         else
1335                                 workImage = cvCloneImage(frame);
1336
1337                         if (USE_UNDISTORTION)
1338                         {
1339                                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1340                                 cvReleaseImage(&workImage);
1341                                 workImage = cvCloneImage(undistortedImg);
1342                                 cvReleaseImage(&undistortedImg);
1343                         }
1344
1345                         if (points3D)
1346                         {
1347
1348                                 newPoints = Find3DObject(workImage,points3D,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,!USE_UNDISTORTION);
1349
1350                                 if (newPoints)
1351                                 {
1352                                         bool areOnFrame=true; // are points on frame or not
1353                                         if (areOnFrame=true)
1354                                         {
1355                                                 f = fopen(path_frames,"a");
1356                                                 sprintf(path,"%s%d.jpg",PATH,currentFrame+1);
1357                                                 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);
1358                                                 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);
1359                                                 fclose(f);
1360
1361                                                 if (DRAW_CHESSBOARD)
1362                                                 {
1363                                                         CvPoint2D32f* ChessboardCorners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
1364                                                         cvFindChessboardCorners(workImage,innerCornersCount,ChessboardCorners);
1365                                                         cvDrawChessboardCorners(workImage,innerCornersCount,ChessboardCorners,innerCornersCount.height*innerCornersCount.width,1);
1366                                                         delete[] ChessboardCorners;
1367                                                 }
1368                                                 if (DRAW_CORNERS)
1369                                                 {
1370                                                         for (int i=0;i<8;i++)
1371                                                         {
1372                                                                 cvCircle(workImage,newPoints[i],2,cvScalar(255,255,255));
1373                                                                 cvCircle(workImage,newPoints[i],3,cvScalar(0));
1374                                                         }
1375                                                         cvLine(workImage,newPoints[0],newPoints[1], cvScalar(100,255,100));
1376                                                         cvLine(workImage,newPoints[0],newPoints[2], cvScalar(100,255,100));
1377                                                         cvLine(workImage,newPoints[1],newPoints[3], cvScalar(100,255,100));
1378                                                         cvLine(workImage,newPoints[2],newPoints[3], cvScalar(100,255,100));
1379                                                         cvLine(workImage,newPoints[4],newPoints[5], cvScalar(100,255,100));
1380                                                         cvLine(workImage,newPoints[5],newPoints[7], cvScalar(100,255,100));
1381                                                         cvLine(workImage,newPoints[6],newPoints[7], cvScalar(100,255,100));
1382                                                         cvLine(workImage,newPoints[4],newPoints[6], cvScalar(100,255,100));
1383                                                         cvLine(workImage,newPoints[0],newPoints[4], cvScalar(100,255,100));
1384                                                         cvLine(workImage,newPoints[1],newPoints[5], cvScalar(100,255,100));
1385                                                         cvLine(workImage,newPoints[2],newPoints[6], cvScalar(100,255,100));
1386                                                         cvLine(workImage,newPoints[3],newPoints[7], cvScalar(100,255,100));
1387                                                 }
1388                                                 if (SAVE_DRAWINGS)
1389                                                         cvSaveImage(path,workImage);
1390                                                 else
1391                                                 {
1392                                                         if (USE_DEINTERLACING)
1393                                                         {
1394                                                                 IplImage* tmp = Deinterlace(frame);
1395                                                                 if (USE_UNDISTORTION)
1396                                                                 {
1397                                                                         undistortedImg = Undistort(tmp,IntrinsicMatrix,DistortionCoeffs);
1398                                                                         cvReleaseImage(&tmp);
1399                                                                         tmp = cvCloneImage(undistortedImg);
1400                                                                         cvReleaseImage(&undistortedImg);
1401                                                                 }
1402                                                                 cvSaveImage(path,tmp);
1403                                                                 cvReleaseImage(&tmp);
1404                                                         }
1405                                                         else
1406                                                         {
1407                                                                 if (!USE_UNDISTORTION)
1408                                                                         cvSaveImage(path,frame);
1409                                                                 else
1410                                                                 {
1411                                                                         undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);
1412                                                                         cvSaveImage(path,undistortedImg);
1413                                                                         cvReleaseImage(&undistortedImg);
1414                                                                 }
1415                                                         }
1416                                                 }
1417                                                 if (SAVE_SAMPLES)
1418                                                 {
1419                                                         sprintf(path,"%s%d.jpg",SAMPLES_PATH,currentFrame+1);
1420                                                         IplImage* tmp;
1421                                                         if (!USE_UNDISTORTION)
1422                                                                 tmp = GetSample3D(frame,newPoints);
1423                                                         else
1424                                                         {
1425                                                                 undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);
1426                                                                 tmp = GetSample3D(undistortedImg,newPoints);
1427                                                                 cvReleaseImage(&undistortedImg);
1428                                                         }
1429                                                         if (tmp)
1430                                                         {
1431                                                                 cvSaveImage(path,tmp);
1432                                                                 cvReleaseImage(&tmp);
1433                                                         }
1434                                                 }
1435                                                 printf("Frame %d successfully saved to %s\n",currentFrame+1,path);
1436                                         }
1437
1438                                         delete[] newPoints;
1439                                         newPoints = NULL;
1440                                 }
1441                                 else
1442                                         if (SAVE_ALL_FRAMES)
1443                                         {
1444                                                 sprintf(path,"%s%d.jpg",PATH,currentFrame+1);
1445                                                 cvSaveImage(path,workImage);
1446                                         }
1447
1448                         }
1449
1450
1451                         cvShowImage(FRAME_WINDOW,workImage);
1452                         key = cvWaitKey(30);
1453                 }
1454                 currentFrame++;
1455         }
1456         while (frame && (key!=27));
1457
1458         if (capture)
1459                 cvReleaseCapture(&capture);
1460 }
1461
1462 //-------------
1463
1464 void createSamples3DObject2(int argc, char** argv)
1465 {
1466         CvPoint points[4];
1467
1468         CvPoint2D32f* relCoords = new CvPoint2D32f[4];
1469         CvPoint3D32f* objectPoints = new CvPoint3D32f[8];
1470
1471         CvPoint * newPoints;
1472         int currentFrame=0;
1473         int key;
1474         IplImage* undistortedImg;
1475
1476         if (CALIBRATE_CAMERA)
1477         {
1478                 int v = IS_VIDEO_CAPTURE ? 10 : 7;
1479                 char** c = new char*[v];
1480                 c[0]=new char[11]; c[0]="calibration";
1481                 c[1]=new char[2]; c[1]="-w";
1482                 c[2] = new char[2]; sprintf(c[2],"%d",innerCornersCount.width);
1483                 c[3]=new char[2]; c[3]="-h";
1484                 c[4] = new char[2];  sprintf(c[4],"%d",innerCornersCount.height);
1485                 c[5] = new char[2]; c[5]="-d";
1486                 c[6] = new char[5];  sprintf(c[6],"%d",VIDEO_CALIBRATION_DELAY);
1487                 c[7]=new char[2]; c[7]="-o";
1488                 c[8]=new char[100];
1489                 sprintf(c[8],"%scamera.yml",OUTPUT_DIRECTORY);
1490                 if (IS_VIDEO_CAPTURE)
1491                 {
1492                         c[9]=new char[strlen(INPUT_VIDEO)+1];
1493                         strcpy(c[9],INPUT_VIDEO);
1494                 }
1495
1496                 calibrate(v,c);
1497                 LoadCameraParams(c[8],&IntrinsicMatrix, &DistortionCoeffs);
1498
1499                 // Add functionality for camera calibration
1500         }
1501
1502         int boxDepthValue = IDENTITY_DEPTH_VALUE;
1503         int initialBoxDepthValue = (MAX_INITAL_DEPTH_VALUE - MIN_INITAL_DEPTH_VALUE)/2;
1504         cvNamedWindow(FRAME_WINDOW,1);
1505         cvCreateTrackbar(DEPTH_TRACKBAR,FRAME_WINDOW,&boxDepthValue,MAX_DEPTH_VALUE,NULL);
1506         cvCreateTrackbar(INITIAL_DEPTH_TRACKBAR,FRAME_WINDOW,&initialBoxDepthValue,MAX_INITAL_DEPTH_VALUE-MIN_INITAL_DEPTH_VALUE,NULL);
1507
1508
1509         capture = IS_VIDEO_CAPTURE ? cvCreateFileCapture(INPUT_VIDEO) : cvCreateCameraCapture(0);
1510
1511         if (!IS_VIDEO_CAPTURE)
1512         {
1513                 cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,640);
1514                 cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,480);
1515         }
1516
1517         cvResizeWindow(FRAME_WINDOW,(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH),(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT));
1518
1519         CvPoint2D32f* corners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
1520         int count;
1521
1522         do
1523         {
1524                 if (IS_VIDEO_CAPTURE)
1525                         cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,++currentFrame);
1526                 if (workImage) cvReleaseImage(&workImage);
1527                 frame = cvQueryFrame(capture);
1528                 if (frame)
1529                 {
1530                         if (USE_DEINTERLACING)
1531                                 workImage =  Deinterlace(frame);
1532                         else
1533                                 workImage = cvCloneImage(frame);
1534
1535
1536                         if (USE_UNDISTORTION)
1537                         {
1538
1539                                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1540                                 cvReleaseImage(&workImage);
1541                                 workImage = cvCloneImage(undistortedImg);
1542                                 cvReleaseImage(&undistortedImg);
1543                         }
1544                         cvShowImage(FRAME_WINDOW,workImage);
1545                         key = cvWaitKey(30);
1546                         if (key==27) return;
1547                 }
1548
1549         }
1550         while ( frame && (((AUTOFIND_CHESSBOARD && IS_VIDEO_CAPTURE)|| (!IS_VIDEO_CAPTURE)) && cvFindChessboardCorners(workImage,innerCornersCount,corners,&count)==0));
1551
1552         if (frame == NULL)
1553         {
1554                 printf("\n Unable to load video with chessboard or connect to the camera");
1555                 return;
1556         }
1557
1558         delete[] corners;
1559         currentFrame--;
1560
1561         IplImage* firstImage=cvCloneImage(workImage);
1562
1563         cvShowImage(FRAME_WINDOW,workImage);
1564
1565         points[0].x =-1;
1566         points[1].x =-1;
1567         points[2].x =-1;
1568         points[3].x =-1;
1569
1570         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");
1571         cvSetMouseCallback(FRAME_WINDOW, on_mouse, &points);
1572
1573         // Getting initial points position
1574         for (int currPoint=0;currPoint<4;)
1575         {
1576                 if (points[currPoint].x != -1)
1577                 {
1578                         cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),2,cvScalar(255,255,255));
1579                         cvCircle(workImage,cvPoint(points[currPoint].x,points[currPoint].y),3,cvScalar(0));
1580                         cvShowImage(FRAME_WINDOW,workImage);
1581                         currPoint++;
1582                 }
1583                 key = cvWaitKey(30);
1584                 if (IS_VIDEO_CAPTURE)
1585                 {
1586                         switch (key)
1587                         {
1588                         case 32: // Space symbol
1589                                 if (ShowFrame(++currentFrame))
1590                                 {
1591                                         points[0].x =-1;
1592                                         points[1].x =-1;
1593                                         points[2].x =-1;
1594                                         points[3].x =-1;
1595                                         currPoint = 0;
1596                                 }
1597                                 else
1598                                 {
1599                                         currentFrame--;
1600                                 }
1601                                 break;
1602                         case 8: // Backspace symbol
1603                                 if (currentFrame>0)
1604                                 {
1605                                         if (ShowFrame(--currentFrame))
1606                                         {
1607                                                 points[0].x =-1;
1608                                                 points[1].x =-1;
1609                                                 points[2].x =-1;
1610                                                 points[3].x =-1;
1611                                                 currPoint = 0;
1612                                         }
1613                                         else
1614                                         {
1615                                                 currentFrame++;
1616                                         }
1617                                 }
1618                                 break;
1619                         }
1620                 }
1621         }
1622
1623         // sorting points
1624
1625         for (int i=1;i<4;i++)
1626         {
1627
1628                 if (points[i].y<points[0].y)
1629                 {
1630                         CvPoint temp;
1631                         temp = points[0];
1632                         points[0]=points[i];
1633                         points[i]=temp;
1634                 }
1635         }
1636         if (points[1].x<points[0].x)
1637         {
1638                 CvPoint temp;
1639                 temp = points[0];
1640                 points[0]=points[1];
1641                 points[1]=temp;
1642         }
1643
1644         if (points[3].x<points[2].x)
1645         {
1646                 CvPoint temp;
1647                 temp = points[3];
1648                 points[3]=points[2];
1649                 points[2]=temp;
1650         }
1651
1652         //end of sorting
1653
1654
1655
1656         if (workImage) cvReleaseImage(&workImage);
1657         workImage = cvCloneImage(firstImage);
1658         if (USE_DEINTERLACING)
1659                 workImage =  Deinterlace(frame);
1660         else
1661                 workImage = cvCloneImage(frame);
1662         if (USE_UNDISTORTION)
1663         {
1664                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1665                 cvReleaseImage(&workImage);
1666                 workImage = cvCloneImage(undistortedImg);
1667                 cvReleaseImage(&undistortedImg);
1668         }
1669         cvReleaseImage(&firstImage);
1670
1671
1672         CvMat* Chessboard = CalcRelativePosition(workImage,points,&relCoords,IntrinsicMatrix, (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE);
1673
1674         if (relCoords)
1675         {
1676                 for (int i=0;i<4;i++)
1677                 {
1678                         objectPoints[i+4].x = objectPoints[i].x = relCoords[i].x;
1679                         objectPoints[i+4].y = objectPoints[i].y = relCoords[i].y;
1680                         objectPoints[i].z = (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE;
1681                         objectPoints[i+4].z = objectPoints[i].z + (float)(boxDepthValue)/IDENTITY_DEPTH_VALUE;
1682                 }
1683         }
1684         else
1685         {
1686                 delete[] objectPoints;
1687                 objectPoints = NULL;
1688                 return;
1689         }
1690
1691
1692         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");
1693         if (IS_VIDEO_CAPTURE)
1694                 currentFrame--;
1695         //waiting for box depth approving
1696         IplImage* startImage = cvCloneImage(workImage);
1697         key=-1;
1698         while (key != 13)
1699         {
1700                 key = cvWaitKey(30);
1701                 if (key==27)
1702                         return;
1703                 if (IS_VIDEO_CAPTURE)
1704                 {
1705                         switch (key)
1706                         {
1707                         case 32: // Space symbol
1708                                 if (ShowFrame(++currentFrame))
1709                                 {
1710
1711                                 }
1712                                 else
1713                                 {
1714                                         currentFrame--;
1715                                 }
1716                                 break;
1717                         case 8: // Backspace symbol
1718                                 if (currentFrame>0)
1719                                 {
1720                                         if (ShowFrame(--currentFrame))
1721                                         {
1722
1723                                         }
1724                                         else
1725                                         {
1726                                                 currentFrame++;
1727                                         }
1728                                 }
1729                                 break;
1730                         default:
1731                                 ShowFrame(currentFrame);
1732                                 break;
1733                         }
1734                 }
1735                 else
1736                 {
1737                         frame = cvQueryFrame(capture);
1738                         if (frame)
1739                         {
1740                                 if (USE_DEINTERLACING)
1741                                         workImage =  Deinterlace(frame);
1742                                 else
1743                                         workImage = cvCloneImage(frame);
1744
1745                                 if (USE_UNDISTORTION)
1746                                 {
1747                                         undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1748                                         cvReleaseImage(&workImage);
1749                                         workImage = cvCloneImage(undistortedImg);
1750                                         cvReleaseImage(&undistortedImg);
1751                                 }
1752
1753
1754                         }
1755                         else
1756                                 return;
1757                 }
1758
1759
1760                 CalcRelativePosition(workImage,points,&relCoords,IntrinsicMatrix, (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE,Chessboard);
1761
1762
1763                 for (int i=0;i<4;i++)
1764                 {
1765                         objectPoints[i+4].x = objectPoints[i].x = relCoords[i].x;
1766                         objectPoints[i+4].y = objectPoints[i].y = relCoords[i].y;
1767                         objectPoints[i].z = (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE;
1768                         objectPoints[i+4].z = objectPoints[i].z + (float)(boxDepthValue)/IDENTITY_DEPTH_VALUE;
1769                 }
1770
1771
1772
1773                 for (int i=0;i<4;i++)
1774                 {
1775                         objectPoints[i].z = (float)(initialBoxDepthValue+MIN_INITAL_DEPTH_VALUE)/IDENTITY_INITIAL_DEPTH_VALUE;
1776                         objectPoints[i+4].z = objectPoints[i].z +(float)(boxDepthValue)/IDENTITY_DEPTH_VALUE;
1777                 }
1778                 newPoints = Find3DObject(workImage,objectPoints,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,false);
1779                 if (newPoints)
1780                 {
1781                         for (int i=0;i<8;i++)
1782                         {
1783                                 cvCircle(workImage,newPoints[i],2,cvScalar(255,255,255));
1784                                 cvCircle(workImage,newPoints[i],3,cvScalar(0));
1785                         }
1786                         cvLine(workImage,newPoints[0],newPoints[1], cvScalar(100,255,100));
1787                         cvLine(workImage,newPoints[0],newPoints[2], cvScalar(100,255,100));
1788                         cvLine(workImage,newPoints[1],newPoints[3], cvScalar(100,255,100));
1789                         cvLine(workImage,newPoints[2],newPoints[3], cvScalar(100,255,100));
1790
1791                         cvLine(workImage,newPoints[4],newPoints[5], cvScalar(50,150,50));
1792                         cvLine(workImage,newPoints[5],newPoints[7], cvScalar(50,150,50));
1793                         cvLine(workImage,newPoints[6],newPoints[7], cvScalar(50,150,50));
1794                         cvLine(workImage,newPoints[4],newPoints[6], cvScalar(50,150,50));
1795
1796                         cvLine(workImage,newPoints[0],newPoints[4], cvScalar(50,150,50));
1797                         cvLine(workImage,newPoints[1],newPoints[5], cvScalar(50,150,50));
1798                         cvLine(workImage,newPoints[2],newPoints[6], cvScalar(50,150,50));
1799                         cvLine(workImage,newPoints[3],newPoints[7], cvScalar(50,150,50));
1800                 }
1801                 cvShowImage(FRAME_WINDOW,workImage);
1802         }
1803         cvReleaseImage(&startImage);
1804
1805
1806         char* PATH = OUTPUT_DIRECTORY;
1807         FILE* f;
1808         char path[100];
1809         char path_frames[100];
1810         sprintf(path_frames,"%sframes.txt",PATH);
1811         remove(path_frames);
1812         char cmd[100];
1813 #ifdef WIN32
1814         sprintf(cmd,"mkdir %s",PATH);
1815         system(cmd);
1816         sprintf(cmd,"del %s*.* /q",PATH);
1817         system(cmd);
1818         if (SAVE_SAMPLES)
1819         {
1820                 sprintf(cmd,"mkdir %s",SAMPLES_PATH);
1821                 system(cmd);
1822                 sprintf(cmd,"del %s*.* /q",SAMPLES_PATH);
1823                 system(cmd);
1824         }
1825 #else
1826
1827         sprintf(cmd,"mkdir %s",PATH);
1828         system(cmd);
1829         sprintf(cmd,"rm -f %s*.*",PATH);
1830         system(cmd);
1831         if (SAVE_SAMPLES)
1832         {
1833                 sprintf(cmd,"mkdir %s",SAMPLES_PATH);
1834                 system(cmd);
1835                 sprintf(cmd,"rm -f %s*.*",SAMPLES_PATH);
1836                 system(cmd);
1837         }
1838 #endif
1839         if (IS_VIDEO_CAPTURE)
1840         {
1841                 currentFrame = 0;
1842                 cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,currentFrame);
1843         }
1844         do
1845         {
1846                 if (workImage) cvReleaseImage(&workImage);
1847                 frame = cvQueryFrame(capture);
1848                 if (frame)
1849                 {
1850                         if (USE_DEINTERLACING)
1851                                 workImage =  Deinterlace(frame);
1852                         else
1853                                 workImage = cvCloneImage(frame);
1854
1855                         if (USE_UNDISTORTION)
1856                         {
1857                                 undistortedImg = Undistort(workImage,IntrinsicMatrix,DistortionCoeffs);
1858                                 cvReleaseImage(&workImage);
1859                                 workImage = cvCloneImage(undistortedImg);
1860                                 cvReleaseImage(&undistortedImg);
1861                         }
1862
1863                         if (objectPoints)
1864                         {
1865                                 newPoints = Find3DObject(workImage,objectPoints,innerCornersCount,IntrinsicMatrix,DistortionCoeffs,false);
1866
1867                                 if (newPoints)
1868                                 {
1869                                         bool areOnFrame=true; // are points on frame or not
1870                                         if (areOnFrame=true)
1871                                         {
1872                                                 f = fopen(path_frames,"a");
1873                                                 sprintf(path,"%s%d.jpg",PATH,currentFrame+1);
1874                                                 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);
1875                                                 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);
1876                                                 fclose(f);
1877
1878                                                 if (DRAW_CHESSBOARD)
1879                                                 {
1880                                                         CvPoint2D32f* ChessboardCorners = new CvPoint2D32f[innerCornersCount.height*innerCornersCount.width];
1881                                                         cvFindChessboardCorners(workImage,innerCornersCount,ChessboardCorners);
1882                                                         cvDrawChessboardCorners(workImage,innerCornersCount,ChessboardCorners,innerCornersCount.height*innerCornersCount.width,1);
1883                                                         delete[] ChessboardCorners;
1884                                                 }
1885                                                 if (DRAW_CORNERS)
1886                                                 {
1887                                                         for (int i=0;i<8;i++)
1888                                                         {
1889                                                                 cvCircle(workImage,newPoints[i],2,cvScalar(255,255,255));
1890                                                                 cvCircle(workImage,newPoints[i],3,cvScalar(0));
1891                                                         }
1892                                                         cvLine(workImage,newPoints[0],newPoints[1], cvScalar(100,255,100));
1893                                                         cvLine(workImage,newPoints[0],newPoints[2], cvScalar(100,255,100));
1894                                                         cvLine(workImage,newPoints[1],newPoints[3], cvScalar(100,255,100));
1895                                                         cvLine(workImage,newPoints[2],newPoints[3], cvScalar(100,255,100));
1896
1897                                                         cvLine(workImage,newPoints[4],newPoints[5], cvScalar(50,150,50));
1898                                                         cvLine(workImage,newPoints[5],newPoints[7], cvScalar(50,150,50));
1899                                                         cvLine(workImage,newPoints[6],newPoints[7], cvScalar(50,150,50));
1900                                                         cvLine(workImage,newPoints[4],newPoints[6], cvScalar(50,150,50));
1901
1902                                                         cvLine(workImage,newPoints[0],newPoints[4], cvScalar(50,150,50));
1903                                                         cvLine(workImage,newPoints[1],newPoints[5], cvScalar(50,150,50));
1904                                                         cvLine(workImage,newPoints[2],newPoints[6], cvScalar(50,150,50));
1905                                                         cvLine(workImage,newPoints[3],newPoints[7], cvScalar(50,150,50));
1906                                                 }
1907                                                 if (SAVE_DRAWINGS)
1908                                                         cvSaveImage(path,workImage);
1909                                                 else
1910                                                 {
1911                                                         if (USE_DEINTERLACING)
1912                                                         {
1913                                                                 IplImage* tmp = Deinterlace(frame);
1914                                                                 if (USE_UNDISTORTION)
1915                                                                 {
1916                                                                         undistortedImg = Undistort(tmp,IntrinsicMatrix,DistortionCoeffs);
1917                                                                         cvReleaseImage(&tmp);
1918                                                                         tmp = cvCloneImage(undistortedImg);
1919                                                                         cvReleaseImage(&undistortedImg);
1920                                                                 }
1921                                                                 cvSaveImage(path,tmp);
1922                                                                 cvReleaseImage(&tmp);
1923                                                         }
1924                                                         else
1925                                                         {
1926                                                                 if (!USE_UNDISTORTION)
1927                                                                         cvSaveImage(path,frame);
1928                                                                 else
1929                                                                 {
1930                                                                         undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);
1931                                                                         cvSaveImage(path,undistortedImg);
1932                                                                         cvReleaseImage(&undistortedImg);
1933                                                                 }
1934                                                         }
1935                                                 }
1936                                                 if (SAVE_SAMPLES)
1937                                                 {
1938                                                         sprintf(path,"%s%d.jpg",SAMPLES_PATH,currentFrame+1);
1939                                                         IplImage* tmp;
1940                                                         if (!USE_UNDISTORTION)
1941                                                                 tmp = GetSample3D(frame,newPoints);
1942                                                         else
1943                                                         {
1944                                                                 undistortedImg = Undistort(frame,IntrinsicMatrix,DistortionCoeffs);
1945                                                                 tmp = GetSample3D(undistortedImg,newPoints);
1946                                                                 cvReleaseImage(&undistortedImg);
1947                                                         }
1948                                                         if (tmp)
1949                                                         {
1950                                                                 cvSaveImage(path,tmp);
1951                                                                 cvReleaseImage(&tmp);
1952                                                         }
1953                                                 }
1954                                                 printf("Frame %d successfully saved to %s\n",currentFrame+1,path);
1955                                         }
1956
1957                                         delete[] newPoints;
1958                                         newPoints = NULL;
1959                                 }
1960                                 else
1961                                         if (SAVE_ALL_FRAMES)
1962                                         {
1963                                                 sprintf(path,"%s%d.jpg",PATH,currentFrame+1);
1964                                                 cvSaveImage(path,workImage);
1965                                         }
1966
1967                         }
1968
1969
1970                         cvShowImage(FRAME_WINDOW,workImage);
1971                         key = cvWaitKey(30);
1972                 }
1973                 currentFrame++;
1974         }
1975         while (frame && (key!=27));
1976         if (capture)
1977                 cvReleaseCapture(&capture);
1978 }
1979 //-------------
1980
1981 // Command line arguments: <config_file>
1982 int main( int argc, char** argv )
1983 {
1984         if(argc != 2)
1985         {
1986                 printf("Usage: createsamples arguments.yml\n");
1987         }
1988         if (LoadApplicationParams(argv[1]))
1989         {
1990                 if (USE_3D)
1991                         createSamples3DObject2(argc,argv);
1992                 else
1993                         createSamples2DObject(argc,argv);
1994         }
1995         else
1996         {
1997                 printf("\nUnable to load configuration file from %s\nClosing application...\n",argv[1]);
1998         }
1999
2000         return 0;
2001 }