modules/legacy/src/compat.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2008-2010, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of Intel Corporation may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44 #include "opencv2/calib3d/calib3d_c.h"
  45
  46 CvMat cvMatArray( int rows, int cols, int type,
  47                   int count, void* data)
  48 {
  49     return cvMat( rows*count, cols, type, data );
  50 }
  51
  52
  53 double cvMean( const CvArr* image, const CvArr* mask )
  54 {
  55     CvScalar mean = cvAvg( image, mask );
  56     return mean.val[0];
  57 }
  58
  59
  60 double  cvSumPixels( const CvArr* image )
  61 {
  62     CvScalar scalar = cvSum( image );
  63     return scalar.val[0];
  64 }
  65
  66 void  cvMean_StdDev( const CvArr* image, double* mean, double* sdv, const CvArr* mask)
  67 {
  68     CvScalar _mean, _sdv;
  69     cvAvgSdv( image, &_mean, &_sdv, mask );
  70
  71     if( mean )
  72         *mean = _mean.val[0];
  73
  74     if( sdv )
  75         *sdv = _sdv.val[0];
  76 }
  77
  78
  79 void cvmPerspectiveProject( const CvMat* mat, const CvArr* src, CvArr* dst )
  80 {
  81     CvMat tsrc, tdst;
  82
  83     cvReshape( src, &tsrc, 3, 0 );
  84     cvReshape( dst, &tdst, 3, 0 );
  85
  86     cvPerspectiveTransform( &tsrc, &tdst, mat );
  87 }
  88
  89
  90 void cvFillImage( CvArr* mat, double color )
  91 {
  92     cvSet( mat, cvColorToScalar(color, cvGetElemType(mat)), 0 );
  93 }
  94
  95
  96 /* Changes RNG range while preserving RNG state */
  97 void  cvRandSetRange( CvRandState* state, double param1, double param2, int index)
  98 {
  99     if( !state )
 100     {
 101         cvError( CV_StsNullPtr, "cvRandSetRange", "Null pointer to RNG state", "cvcompat.h", 0 );
 102         return;
 103     }
 104
 105     if( (unsigned)(index + 1) > 4 )
 106     {
 107         cvError( CV_StsOutOfRange, "cvRandSetRange", "index is not in -1..3", "cvcompat.h", 0 );
 108         return;
 109     }
 110
 111     if( index < 0 )
 112     {
 113         state->param[0].val[0] = state->param[0].val[1] =
 114         state->param[0].val[2] = state->param[0].val[3] = param1;
 115         state->param[1].val[0] = state->param[1].val[1] =
 116         state->param[1].val[2] = state->param[1].val[3] = param2;
 117     }
 118     else
 119     {
 120         state->param[0].val[index] = param1;
 121         state->param[1].val[index] = param2;
 122     }
 123 }
 124
 125
 126 void  cvRandInit( CvRandState* state, double param1, double param2,
 127                   int seed, int disttype)
 128 {
 129     if( !state )
 130     {
 131         cvError( CV_StsNullPtr, "cvRandInit", "Null pointer to RNG state", "cvcompat.h", 0 );
 132         return;
 133     }
 134
 135     if( disttype != CV_RAND_UNI && disttype != CV_RAND_NORMAL )
 136     {
 137         cvError( CV_StsBadFlag, "cvRandInit", "Unknown distribution type", "cvcompat.h", 0 );
 138         return;
 139     }
 140
 141     state->state = (uint64)(seed ? seed : -1);
 142     state->disttype = disttype;
 143     cvRandSetRange( state, param1, param2, -1 );
 144 }
 145
 146
 147 /* Fills array with random numbers */
 148 void cvRand( CvRandState* state, CvArr* arr )
 149 {
 150     if( !state )
 151     {
 152         cvError( CV_StsNullPtr, "cvRand", "Null pointer to RNG state", "cvcompat.h", 0 );
 153         return;
 154     }
 155     cvRandArr( &state->state, arr, state->disttype, state->param[0], state->param[1] );
 156 }
 157
 158 void cvbRand( CvRandState* state, float* dst, int len )
 159 {
 160     CvMat mat = cvMat( 1, len, CV_32F, (void*)dst );
 161     cvRand( state, &mat );
 162 }
 163
 164
 165 void  cvbCartToPolar( const float* y, const float* x, float* magnitude, float* angle, int len )
 166 {
 167     CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
 168     CvMat my = mx;
 169     CvMat mm = mx;
 170     CvMat ma = mx;
 171
 172     my.data.fl = (float*)y;
 173     mm.data.fl = (float*)magnitude;
 174     ma.data.fl = (float*)angle;
 175
 176     cvCartToPolar( &mx, &my, &mm, angle ? &ma : NULL, 1 );
 177 }
 178
 179
 180 void  cvbFastArctan( const float* y, const float* x, float* angle, int len )
 181 {
 182     CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
 183     CvMat my = mx;
 184     CvMat ma = mx;
 185
 186     my.data.fl = (float*)y;
 187     ma.data.fl = (float*)angle;
 188
 189     cvCartToPolar( &mx, &my, NULL, &ma, 1 );
 190 }
 191
 192
 193  void  cvbSqrt( const float* x, float* y, int len )
 194 {
 195     CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
 196     CvMat my = mx;
 197     my.data.fl = (float*)y;
 198
 199     cvPow( &mx, &my, 0.5 );
 200 }
 201
 202
 203  void  cvbInvSqrt( const float* x, float* y, int len )
 204 {
 205     CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
 206     CvMat my = mx;
 207     my.data.fl = (float*)y;
 208
 209     cvPow( &mx, &my, -0.5 );
 210 }
 211
 212
 213  void  cvbReciprocal( const float* x, float* y, int len )
 214 {
 215     CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
 216     CvMat my = mx;
 217     my.data.fl = (float*)y;
 218
 219     cvPow( &mx, &my, -1 );
 220 }
 221
 222
 223  void  cvbFastExp( const float* x, double* y, int len )
 224 {
 225     int i;
 226     for( i = 0; i < len; i++ )
 227         y[i] = exp((double)x[i]);
 228 }
 229
 230
 231  void  cvbFastLog( const double* x, float* y, int len )
 232 {
 233     int i;
 234     for( i = 0; i < len; i++ )
 235         y[i] = (float)log(x[i]);
 236 }
 237
 238
 239 CvRect  cvContourBoundingRect( void* point_set, int update)
 240 {
 241     return cvBoundingRect( point_set, update );
 242 }
 243
 244
 245 double cvPseudoInverse( const CvArr* src, CvArr* dst )
 246 {
 247     return cvInvert( src, dst, CV_SVD );
 248 }
 249
 250
 251 /* Calculates exact convex hull of 2d point set */
 252 void cvConvexHull( CvPoint* points, int num_points, CvRect*,
 253                    int orientation, int* hull, int* hullsize )
 254 {
 255     CvMat points1 = cvMat( 1, num_points, CV_32SC2, points );
 256     CvMat hull1 = cvMat( 1, num_points, CV_32SC1, hull );
 257
 258     cvConvexHull2( &points1, &hull1, orientation, 0 );
 259     *hullsize = hull1.cols;
 260 }
 261
 262 void cvMinAreaRect( CvPoint* points, int n, int, int, int, int,
 263                     CvPoint2D32f* anchor, CvPoint2D32f* vect1, CvPoint2D32f* vect2 )
 264 {
 265     CvMat mat = cvMat( 1, n, CV_32SC2, points );
 266     CvBox2D box = cvMinAreaRect2( &mat, 0 );
 267     CvPoint2D32f pt[4];
 268
 269     cvBoxPoints( box, pt );
 270     *anchor = pt[0];
 271     vect1->x = pt[1].x - pt[0].x;
 272     vect1->y = pt[1].y - pt[0].y;
 273     vect2->x = pt[3].x - pt[0].x;
 274     vect2->y = pt[3].y - pt[0].y;
 275 }
 276
 277 void  cvFitLine3D( CvPoint3D32f* points, int count, int dist,
 278                    void *param, float reps, float aeps, float* line )
 279 {
 280     CvMat mat = cvMat( 1, count, CV_32FC3, points );
 281     float _param = param != NULL ? *(float*)param : 0.f;
 282     assert( dist != CV_DIST_USER );
 283     cvFitLine( &mat, dist, _param, reps, aeps, line );
 284 }
 285
 286 /* Fits a line into set of 2d points in a robust way (M-estimator technique) */
 287 void  cvFitLine2D( CvPoint2D32f* points, int count, int dist,
 288                    void *param, float reps, float aeps, float* line )
 289 {
 290     CvMat mat = cvMat( 1, count, CV_32FC2, points );
 291     float _param = param != NULL ? *(float*)param : 0.f;
 292     assert( dist != CV_DIST_USER );
 293     cvFitLine( &mat, dist, _param, reps, aeps, line );
 294 }
 295
 296
 297 void cvFitEllipse( const CvPoint2D32f* points, int count, CvBox2D* box )
 298 {
 299     CvMat mat = cvMat( 1, count, CV_32FC2, (void*)points );
 300     *box = cvFitEllipse2( &mat );
 301 }
 302
 303 /* Projects 2d points to one of standard coordinate planes
 304    (i.e. removes one of coordinates) */
 305 void  cvProject3D( CvPoint3D32f* points3D, int count,
 306                    CvPoint2D32f* points2D, int xIndx, int yIndx)
 307 {
 308     CvMat src = cvMat( 1, count, CV_32FC3, points3D );
 309     CvMat dst = cvMat( 1, count, CV_32FC2, points2D );
 310     float m[6] = {0,0,0,0,0,0};
 311     CvMat M = cvMat( 2, 3, CV_32F, m );
 312
 313     assert( (unsigned)xIndx < 3 && (unsigned)yIndx < 3 );
 314     m[xIndx] = m[yIndx+3] = 1.f;
 315
 316     cvTransform( &src, &dst, &M, NULL );
 317 }
 318
 319
 320 int  cvHoughLines( CvArr* image, double rho,
 321                    double theta, int threshold,
 322                    float* lines, int linesNumber )
 323 {
 324     CvMat linesMat = cvMat( 1, linesNumber, CV_32FC2, lines );
 325     cvHoughLines2( image, &linesMat, CV_HOUGH_STANDARD,
 326                    rho, theta, threshold, 0, 0 );
 327
 328     return linesMat.cols;
 329 }
 330
 331
 332 int  cvHoughLinesP( CvArr* image, double rho,
 333                     double theta, int threshold,
 334                     int lineLength, int lineGap,
 335                     int* lines, int linesNumber )
 336 {
 337     CvMat linesMat = cvMat( 1, linesNumber, CV_32SC4, lines );
 338     cvHoughLines2( image, &linesMat, CV_HOUGH_PROBABILISTIC,
 339                    rho, theta, threshold, lineLength, lineGap );
 340
 341     return linesMat.cols;
 342 }
 343
 344
 345 int  cvHoughLinesSDiv( CvArr* image, double rho, int srn,
 346                        double theta, int stn, int threshold,
 347                        float* lines, int linesNumber )
 348 {
 349     CvMat linesMat = cvMat( 1, linesNumber, CV_32FC2, lines );
 350     cvHoughLines2( image, &linesMat, CV_HOUGH_MULTI_SCALE,
 351                    rho, theta, threshold, srn, stn );
 352
 353     return linesMat.cols;
 354 }
 355
 356
 357 float  cvCalcEMD( const float* signature1, int size1, const float* signature2, int size2,
 358                   int dims, int dist_type, CvDistanceFunction dist_func,
 359                   float* lower_bound, void* user_param)
 360 {
 361     CvMat sign1 = cvMat( size1, dims + 1, CV_32FC1, (void*)signature1 );
 362     CvMat sign2 = cvMat( size2, dims + 1, CV_32FC1, (void*)signature2 );
 363
 364     return cvCalcEMD2( &sign1, &sign2, dist_type, dist_func, 0, 0, lower_bound, user_param );
 365 }
 366
 367
 368 void  cvKMeans( int num_clusters, float** samples,
 369                 int num_samples, int vec_size,
 370                 CvTermCriteria termcrit, int* cluster_idx )
 371 {
 372     CvMat* samples_mat = cvCreateMat( num_samples, vec_size, CV_32FC1 );
 373     CvMat cluster_idx_mat = cvMat( num_samples, 1, CV_32SC1, cluster_idx );
 374     int i;
 375     for( i = 0; i < num_samples; i++ )
 376         memcpy( samples_mat->data.fl + i*vec_size, samples[i], vec_size*sizeof(float));
 377     cvKMeans2( samples_mat, num_clusters, &cluster_idx_mat, termcrit, 1, 0, 0, 0, 0 );
 378     cvReleaseMat( &samples_mat );
 379 }
 380
 381
 382 void  cvStartScanGraph( CvGraph* graph, CvGraphScanner* scanner,
 383                         CvGraphVtx* vtx, int mask)
 384 {
 385     CvGraphScanner* temp_scanner;
 386
 387     if( !scanner )
 388         cvError( CV_StsNullPtr, "cvStartScanGraph", "Null scanner pointer", "cvcompat.h", 0 );
 389
 390     temp_scanner = cvCreateGraphScanner( graph, vtx, mask );
 391     *scanner = *temp_scanner;
 392     cvFree( &temp_scanner );
 393 }
 394
 395
 396 void  cvEndScanGraph( CvGraphScanner* scanner )
 397 {
 398     if( !scanner )
 399         cvError( CV_StsNullPtr, "cvEndScanGraph", "Null scanner pointer", "cvcompat.h", 0 );
 400
 401     if( scanner->stack )
 402     {
 403         CvGraphScanner* temp_scanner = (CvGraphScanner*)cvAlloc( sizeof(*temp_scanner) );
 404         *temp_scanner = *scanner;
 405         cvReleaseGraphScanner( &temp_scanner );
 406         memset( scanner, 0, sizeof(*scanner) );
 407     }
 408 }
 409
 410
 411 /* old drawing functions */
 412 void  cvLineAA( CvArr* img, CvPoint pt1, CvPoint pt2, double color, int scale)
 413 {
 414     cvLine( img, pt1, pt2, cvColorToScalar(color, cvGetElemType(img)), 1, CV_AA, scale );
 415 }
 416
 417 void  cvCircleAA( CvArr* img, CvPoint center, int radius, double color, int scale)
 418 {
 419     cvCircle( img, center, radius, cvColorToScalar(color, cvGetElemType(img)), 1, CV_AA, scale );
 420 }
 421
 422 void  cvEllipseAA( CvArr* img, CvPoint center, CvSize axes,
 423                    double angle, double start_angle,
 424                    double end_angle, double color,
 425                    int scale)
 426 {
 427     cvEllipse( img, center, axes, angle, start_angle, end_angle,
 428                cvColorToScalar(color, cvGetElemType(img)), 1, CV_AA, scale );
 429 }
 430
 431 void  cvPolyLineAA( CvArr* img, CvPoint** pts, int* npts, int contours,
 432                     int is_closed, double color, int scale )
 433 {
 434     cvPolyLine( img, pts, npts, contours, is_closed,
 435                 cvColorToScalar(color, cvGetElemType(img)),
 436                 1, CV_AA, scale );
 437 }
 438
 439
 440 void cvUnDistortOnce( const CvArr* src, CvArr* dst,
 441                       const float* intrinsic_matrix,
 442                       const float* distortion_coeffs,
 443                       int )
 444 {
 445     CvMat _a = cvMat( 3, 3, CV_32F, (void*)intrinsic_matrix );
 446     CvMat _k = cvMat( 4, 1, CV_32F, (void*)distortion_coeffs );
 447     cvUndistort2( src, dst, &_a, &_k, 0 );
 448 }
 449
 450
 451 /* the two functions below have quite hackerish implementations, use with care
 452    (or, which is better, switch to cvUndistortInitMap and cvRemap instead */
 453 void cvUnDistortInit( const CvArr*,
 454                       CvArr* undistortion_map,
 455                       const float* A, const float* k,
 456                       int)
 457 {
 458     union { uchar* ptr; float* fl; } data;
 459     CvSize sz;
 460     cvGetRawData( undistortion_map, &data.ptr, 0, &sz );
 461     assert( sz.width >= 8 );
 462     /* just save the intrinsic parameters to the map */
 463     data.fl[0] = A[0]; data.fl[1] = A[4];
 464     data.fl[2] = A[2]; data.fl[3] = A[5];
 465     data.fl[4] = k[0]; data.fl[5] = k[1];
 466     data.fl[6] = k[2]; data.fl[7] = k[3];
 467 }
 468
 469 void  cvUnDistort( const CvArr* src, CvArr* dst,
 470                    const CvArr* undistortion_map, int )
 471 {
 472     union { uchar* ptr; float* fl; } data;
 473     float a[] = {0,0,0,0,0,0,0,0,1};
 474     CvSize sz;
 475     cvGetRawData( undistortion_map, &data.ptr, 0, &sz );
 476     assert( sz.width >= 8 );
 477     a[0] = data.fl[0]; a[4] = data.fl[1];
 478     a[2] = data.fl[2]; a[5] = data.fl[3];
 479     cvUnDistortOnce( src, dst, a, data.fl + 4, 1 );
 480 }
 481
 482
 483 /* Find fundamental matrix */
 484 void  cvFindFundamentalMatrix( int* points1, int* points2, int numpoints, int, float* matrix )
 485 {
 486     CvMat* pointsMat1;
 487     CvMat* pointsMat2;
 488     CvMat fundMatr = cvMat(3,3,CV_32F,matrix);
 489     int i, curr = 0;
 490
 491     pointsMat1 = cvCreateMat(3,numpoints,CV_64F);
 492     pointsMat2 = cvCreateMat(3,numpoints,CV_64F);
 493
 494     for( i = 0; i < numpoints; i++ )
 495     {
 496         cvmSet(pointsMat1,0,i,points1[curr]);//x
 497         cvmSet(pointsMat1,1,i,points1[curr+1]);//y
 498         cvmSet(pointsMat1,2,i,1.0);
 499
 500         cvmSet(pointsMat2,0,i,points2[curr]);//x
 501         cvmSet(pointsMat2,1,i,points2[curr+1]);//y
 502         cvmSet(pointsMat2,2,i,1.0);
 503         curr += 2;
 504     }
 505
 506     cvFindFundamentalMat(pointsMat1,pointsMat2,&fundMatr,CV_FM_RANSAC,1,0.99,0);
 507
 508     cvReleaseMat(&pointsMat1);
 509     cvReleaseMat(&pointsMat2);
 510 }
 511
 512
 513 int cvFindChessBoardCornerGuesses( const void* arr, void*,
 514                                    CvMemStorage*, CvSize pattern_size,
 515                                    CvPoint2D32f* corners, int* corner_count )
 516 {
 517     return cvFindChessboardCorners( arr, pattern_size, corners,
 518                                     corner_count, CV_CALIB_CB_ADAPTIVE_THRESH );
 519 }
 520
 521
 522 /* Calibrates camera using multiple views of calibration pattern */
 523 void cvCalibrateCamera( int image_count, int* _point_counts,
 524     CvSize image_size, CvPoint2D32f* _image_points, CvPoint3D32f* _object_points,
 525     float* _distortion_coeffs, float* _camera_matrix, float* _translation_vectors,
 526     float* _rotation_matrices, int flags )
 527 {
 528     int i, total = 0;
 529     CvMat point_counts = cvMat( image_count, 1, CV_32SC1, _point_counts );
 530     CvMat image_points, object_points;
 531     CvMat dist_coeffs = cvMat( 4, 1, CV_32FC1, _distortion_coeffs );
 532     CvMat camera_matrix = cvMat( 3, 3, CV_32FC1, _camera_matrix );
 533     CvMat rotation_matrices = cvMat( image_count, 9, CV_32FC1, _rotation_matrices );
 534     CvMat translation_vectors = cvMat( image_count, 3, CV_32FC1, _translation_vectors );
 535
 536     for( i = 0; i < image_count; i++ )
 537         total += _point_counts[i];
 538
 539     image_points = cvMat( total, 1, CV_32FC2, _image_points );
 540     object_points = cvMat( total, 1, CV_32FC3, _object_points );
 541
 542     cvCalibrateCamera2( &object_points, &image_points, &point_counts, image_size,
 543         &camera_matrix, &dist_coeffs, &rotation_matrices, &translation_vectors,
 544         flags );
 545 }
 546
 547
 548 void cvCalibrateCamera_64d( int image_count, int* _point_counts,
 549     CvSize image_size, CvPoint2D64f* _image_points, CvPoint3D64f* _object_points,
 550     double* _distortion_coeffs, double* _camera_matrix, double* _translation_vectors,
 551     double* _rotation_matrices, int flags )
 552 {
 553     int i, total = 0;
 554     CvMat point_counts = cvMat( image_count, 1, CV_32SC1, _point_counts );
 555     CvMat image_points, object_points;
 556     CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion_coeffs );
 557     CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, _camera_matrix );
 558     CvMat rotation_matrices = cvMat( image_count, 9, CV_64FC1, _rotation_matrices );
 559     CvMat translation_vectors = cvMat( image_count, 3, CV_64FC1, _translation_vectors );
 560
 561     for( i = 0; i < image_count; i++ )
 562         total += _point_counts[i];
 563
 564     image_points = cvMat( total, 1, CV_64FC2, _image_points );
 565     object_points = cvMat( total, 1, CV_64FC3, _object_points );
 566
 567     cvCalibrateCamera2( &object_points, &image_points, &point_counts, image_size,
 568         &camera_matrix, &dist_coeffs, &rotation_matrices, &translation_vectors,
 569         flags );
 570 }
 571
 572
 573
 574 /* Find 3d position of object given intrinsic camera parameters,
 575    3d model of the object and projection of the object into view plane */
 576 void cvFindExtrinsicCameraParams( int point_count,
 577     CvSize, CvPoint2D32f* _image_points,
 578     CvPoint3D32f* _object_points, float* focal_length,
 579     CvPoint2D32f principal_point, float* _distortion_coeffs,
 580     float* _rotation_vector, float* _translation_vector )
 581 {
 582     CvMat image_points = cvMat( point_count, 1, CV_32FC2, _image_points );
 583     CvMat object_points = cvMat( point_count, 1, CV_32FC3, _object_points );
 584     CvMat dist_coeffs = cvMat( 4, 1, CV_32FC1, _distortion_coeffs );
 585     float a[9];
 586     CvMat camera_matrix = cvMat( 3, 3, CV_32FC1, a );
 587     CvMat rotation_vector = cvMat( 1, 1, CV_32FC3, _rotation_vector );
 588     CvMat translation_vector = cvMat( 1, 1, CV_32FC3, _translation_vector );
 589
 590     a[0] = focal_length[0]; a[4] = focal_length[1];
 591     a[2] = principal_point.x; a[5] = principal_point.y;
 592     a[1] = a[3] = a[6] = a[7] = 0.f;
 593     a[8] = 1.f;
 594
 595     cvFindExtrinsicCameraParams2( &object_points, &image_points, &camera_matrix,
 596         &dist_coeffs, &rotation_vector, &translation_vector, 0 );
 597 }
 598
 599
 600 /* Variant of the previous function that takes double-precision parameters */
 601 void cvFindExtrinsicCameraParams_64d( int point_count,
 602     CvSize, CvPoint2D64f* _image_points,
 603     CvPoint3D64f* _object_points, double* focal_length,
 604     CvPoint2D64f principal_point, double* _distortion_coeffs,
 605     double* _rotation_vector, double* _translation_vector )
 606 {
 607     CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
 608     CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
 609     CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion_coeffs );
 610     double a[9];
 611     CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, a );
 612     CvMat rotation_vector = cvMat( 1, 1, CV_64FC3, _rotation_vector );
 613     CvMat translation_vector = cvMat( 1, 1, CV_64FC3, _translation_vector );
 614
 615     a[0] = focal_length[0]; a[4] = focal_length[1];
 616     a[2] = principal_point.x; a[5] = principal_point.y;
 617     a[1] = a[3] = a[6] = a[7] = 0.;
 618     a[8] = 1.;
 619
 620     cvFindExtrinsicCameraParams2( &object_points, &image_points, &camera_matrix,
 621         &dist_coeffs, &rotation_vector, &translation_vector, 0 );
 622 }
 623
 624 /* Converts rotation_matrix matrix to rotation_matrix vector or vice versa */
 625 void  cvRodrigues( CvMat* rotation_matrix, CvMat* rotation_vector,
 626                    CvMat* jacobian, int conv_type )
 627 {
 628     if( conv_type == CV_RODRIGUES_V2M )
 629         cvRodrigues2( rotation_vector, rotation_matrix, jacobian );
 630     else
 631         cvRodrigues2( rotation_matrix, rotation_vector, jacobian );
 632 }
 633
 634
 635 /* Does reprojection of 3d object points to the view plane */
 636 void  cvProjectPoints( int point_count, CvPoint3D64f* _object_points,
 637     double* _rotation_vector, double*  _translation_vector,
 638     double* focal_length, CvPoint2D64f principal_point,
 639     double* _distortion, CvPoint2D64f* _image_points,
 640     double* _deriv_points_rotation_matrix,
 641     double* _deriv_points_translation_vect,
 642     double* _deriv_points_focal,
 643     double* _deriv_points_principal_point,
 644     double* _deriv_points_distortion_coeffs )
 645 {
 646     CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
 647     CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
 648     CvMat rotation_vector = cvMat( 3, 1, CV_64FC1, _rotation_vector );
 649     CvMat translation_vector = cvMat( 3, 1, CV_64FC1, _translation_vector );
 650     double a[9];
 651     CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, a );
 652     CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion );
 653     CvMat dpdr = cvMat( 2*point_count, 3, CV_64FC1, _deriv_points_rotation_matrix );
 654     CvMat dpdt = cvMat( 2*point_count, 3, CV_64FC1, _deriv_points_translation_vect );
 655     CvMat dpdf = cvMat( 2*point_count, 2, CV_64FC1, _deriv_points_focal );
 656     CvMat dpdc = cvMat( 2*point_count, 2, CV_64FC1, _deriv_points_principal_point );
 657     CvMat dpdk = cvMat( 2*point_count, 4, CV_64FC1, _deriv_points_distortion_coeffs );
 658
 659     a[0] = focal_length[0]; a[4] = focal_length[1];
 660     a[2] = principal_point.x; a[5] = principal_point.y;
 661     a[1] = a[3] = a[6] = a[7] = 0.;
 662     a[8] = 1.;
 663
 664     cvProjectPoints2( &object_points, &rotation_vector, &translation_vector,
 665                       &camera_matrix, &dist_coeffs, &image_points,
 666                       &dpdr, &dpdt, &dpdf, &dpdc, &dpdk, 0 );
 667 }
 668
 669
 670 /* Simpler version of the previous function */
 671 void  cvProjectPointsSimple( int point_count, CvPoint3D64f* _object_points,
 672     double* _rotation_matrix, double*  _translation_vector,
 673     double* _camera_matrix, double* _distortion, CvPoint2D64f* _image_points )
 674 {
 675     CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
 676     CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
 677     CvMat rotation_matrix = cvMat( 3, 3, CV_64FC1, _rotation_matrix );
 678     CvMat translation_vector = cvMat( 3, 1, CV_64FC1, _translation_vector );
 679     CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, _camera_matrix );
 680     CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion );
 681
 682     cvProjectPoints2( &object_points, &rotation_matrix, &translation_vector,
 683                       &camera_matrix, &dist_coeffs, &image_points,
 684                       0, 0, 0, 0, 0, 0 );
 685 }