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