1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
45 This is stright forward port v2 of Matlab calibration engine by Jean-Yves Bouguet
46 that is (in a large extent) based on the paper:
47 Z. Zhang. "A flexible new technique for camera calibration".
48 IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
50 The 1st initial port was done by Valery Mosyagin.
54 icvGaussNewton( const CvMat* J, const CvMat* err, CvMat* delta,
55 CvMat* JtJ=0, CvMat* JtErr=0, CvMat* JtJW=0, CvMat* JtJV=0 )
58 CvMat* _temp_JtErr = 0;
59 CvMat* _temp_JtJW = 0;
60 CvMat* _temp_JtJV = 0;
62 CV_FUNCNAME( "icvGaussNewton" );
66 if( !CV_IS_MAT(J) || !CV_IS_MAT(err) || !CV_IS_MAT(delta) )
67 CV_ERROR( CV_StsBadArg, "Some of required arguments is not a valid matrix" );
71 CV_CALL( _temp_JtJ = cvCreateMat( J->cols, J->cols, J->type ));
74 else if( !CV_IS_MAT(JtJ) )
75 CV_ERROR( CV_StsBadArg, "JtJ is not a valid matrix" );
79 CV_CALL( _temp_JtErr = cvCreateMat( J->cols, 1, J->type ));
82 else if( !CV_IS_MAT(JtErr) )
83 CV_ERROR( CV_StsBadArg, "JtErr is not a valid matrix" );
87 CV_CALL( _temp_JtJW = cvCreateMat( J->cols, 1, J->type ));
90 else if( !CV_IS_MAT(JtJW) )
91 CV_ERROR( CV_StsBadArg, "JtJW is not a valid matrix" );
95 CV_CALL( _temp_JtJV = cvCreateMat( J->cols, J->cols, J->type ));
98 else if( !CV_IS_MAT(JtJV) )
99 CV_ERROR( CV_StsBadArg, "JtJV is not a valid matrix" );
101 cvMulTransposed( J, JtJ, 1 );
102 cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
103 cvSVD( JtJ, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
104 cvSVBkSb( JtJW, JtJV, JtJV, JtErr, delta, CV_SVD_U_T + CV_SVD_V_T );
108 if( _temp_JtJ || _temp_JtErr || _temp_JtJW || _temp_JtJV )
110 cvReleaseMat( &_temp_JtJ );
111 cvReleaseMat( &_temp_JtErr );
112 cvReleaseMat( &_temp_JtJW );
113 cvReleaseMat( &_temp_JtJV );
118 /*static double calc_repr_err( const double* object_points, int o_step,
119 const double* image_points,
120 const double* h, int count )
123 for( int i = 0; i < count; i++ )
125 double X = object_points[i*o_step], Y = object_points[i*o_step + 1];
126 double x0 = image_points[i*2], y0 = image_points[i*2 + 1];
127 double d = 1./(h[6]*X + h[7]*Y + h[8]);
128 double x = (h[0]*X + h[1]*Y + h[2])*d;
129 double y = (h[3]*X + h[4]*Y + h[5])*d;
130 err += fabs(x - x0) + fabs(y - y0);
136 // finds perspective transformation H between the object plane and image plane,
137 // so that (sxi,syi,s) ~ H*(Xi,Yi,1)
139 cvFindHomography( const CvMat* object_points, const CvMat* image_points, CvMat* __H )
141 CvMat *_m = 0, *_M = 0;
144 CV_FUNCNAME( "cvFindHomography" );
149 int i, k, count, count2;
151 CvPoint2D64f cm = {0,0}, sm = {0,0}, cM = {0,0}, sM = {0,0};
152 double inv_Hnorm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
153 double Hnorm2[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
155 CvMat _inv_Hnorm = cvMat( 3, 3, CV_64FC1, inv_Hnorm );
156 CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
157 CvMat _H = cvMat( 3, 3, CV_64FC1, H );
158 double LtL[9*9], LW[9], LV[9*9];
161 CvMat _LtL = cvMat( 9, 9, CV_64FC1, LtL );
162 CvMat _LW = cvMat( 9, 1, CV_64FC1, LW );
163 CvMat _LV = cvMat( 9, 9, CV_64FC1, LV );
164 CvMat _Hrem = cvMat( 3, 3, CV_64FC1, LV + 8*9 );
165 CvMat _Htemp = cvMat( 3, 3, CV_64FC1, LV + 7*9 );
167 if( !CV_IS_MAT(image_points) || !CV_IS_MAT(object_points) || !CV_IS_MAT(__H) )
168 CV_ERROR( CV_StsBadArg, "one of arguments is not a valid matrix" );
170 h_type = CV_MAT_TYPE(__H->type);
171 if( h_type != CV_32FC1 && h_type != CV_64FC1 )
172 CV_ERROR( CV_StsUnsupportedFormat, "Homography matrix must have 32fC1 or 64fC1 type" );
173 if( __H->rows != 3 || __H->cols != 3 )
174 CV_ERROR( CV_StsBadSize, "Homography matrix must be 3x3" );
176 count = MAX(image_points->cols, image_points->rows);
177 count2 = MAX(object_points->cols, object_points->rows);
178 if( count != count2 )
179 CV_ERROR( CV_StsUnmatchedSizes, "Numbers of image and object points do not match" );
181 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
182 CV_CALL( cvConvertPointsHomogenious( image_points, _m ));
183 m = (CvPoint2D64f*)_m->data.ptr;
185 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC2 ));
186 CV_CALL( cvConvertPointsHomogenious( object_points, _M ));
187 M = (CvPoint2D64f*)_M->data.ptr;
189 // calculate the normalization transformations Hnorm, Hnorm2.
190 for( i = 0; i < count; i++ )
192 cm.x += m[i].x; cm.y += m[i].y;
193 cM.x += M[i].x; cM.y += M[i].y;
196 cm.x /= count; cm.y /= count;
197 cM.x /= count; cM.y /= count;
199 for( i = 0; i < count; i++ )
201 sm.x += fabs(m[i].x - cm.x);
202 sm.y += fabs(m[i].y - cm.y);
203 sM.x += fabs(M[i].x - cM.x);
204 sM.y += fabs(M[i].y - cM.y);
207 sm.x /= count; sm.y /= count;
208 sM.x /= count; sM.y /= count;
228 Hnorm2[2] = -cM.x*sM.x;
229 Hnorm2[5] = -cM.y*sM.y;
231 CV_CALL( _Lp = _L = cvCreateMat( 2*count, 9, CV_64FC1 ) );
234 for( i = 0; i < count; i++, L += 18 )
236 double x = -(m[i].x - cm.x)*sm.x, y = -(m[i].y - cm.y)*sm.y;
237 double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
241 L[9 + 0] = L[9 + 1] = L[9 + 2] = L[3] = L[4] = L[5] = 0;
252 cvMulTransposed( _L, &_LtL, 1 );
256 _LW.rows = MIN(count*2, 9);
257 cvSVD( _Lp, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
259 cvMatMul( &_inv_Hnorm, &_Hrem, &_Htemp );
260 cvMatMul( &_Htemp, &_Hnorm2, &_H );
261 cvScale( &_H, &_H, 1./_H.data.db[8] );
265 // reuse the available storage for jacobian and other vars
266 CvMat _J = cvMat( 2*count, 8, CV_64FC1, _L->data.db );
267 CvMat _err = cvMat( 2*count, 1, CV_64FC1, _L->data.db + 2*count*8 );
268 CvMat _JtJ = cvMat( 8, 8, CV_64FC1, LtL );
269 CvMat _JtErr = cvMat( 8, 1, CV_64FC1, LtL + 8*8 );
270 CvMat _JtJW = cvMat( 8, 1, CV_64FC1, LW );
271 CvMat _JtJV = cvMat( 8, 8, CV_64FC1, LV );
272 CvMat _Hinnov = cvMat( 8, 1, CV_64FC1, LV + 8*8 );
274 for( k = 0; k < 10; k++ )
276 double* J = _J.data.db, *err = _err.data.db;
278 for( i = 0; i < count; i++, J += 16, err += 2 )
280 double di = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
281 double _xi = (H[0]*M[i].x + H[1]*M[i].y + H[2])*di;
282 double _yi = (H[3]*M[i].x + H[4]*M[i].y + H[5])*di;
283 err[0] = m[i].x - _xi;
284 err[1] = m[i].y - _yi;
293 J[8+6] = -J[8+3]*_yi;
294 J[8+7] = -J[8+4]*_yi;
295 J[3] = J[4] = J[5] = J[8+0] = J[8+1] = J[8+2] = 0.;
298 icvGaussNewton( &_J, &_err, &_Hinnov, &_JtJ, &_JtErr, &_JtJW, &_JtJV );
300 for( i = 0; i < 8; i++ )
301 H[i] += _Hinnov.data.db[i];
305 cvConvert( &_H, __H );
316 cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
320 CV_FUNCNAME( "cvRogrigues2" );
324 int depth, elem_size;
327 CvMat _J = cvMat( 3, 9, CV_64F, J );
329 if( !CV_IS_MAT(src) )
330 CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
332 if( !CV_IS_MAT(dst) )
333 CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
334 "The first output argument is not a valid matrix" );
336 depth = CV_MAT_DEPTH(src->type);
337 elem_size = CV_ELEM_SIZE(depth);
339 if( depth != CV_32F && depth != CV_64F )
340 CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
342 if( !CV_ARE_DEPTHS_EQ(src, dst) )
343 CV_ERROR( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
347 if( !CV_IS_MAT(jacobian) )
348 CV_ERROR( CV_StsBadArg, "Jacobian is not a valid matrix" );
350 if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
351 CV_ERROR( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );
353 if( (jacobian->rows != 9 || jacobian->cols != 3) &&
354 (jacobian->rows != 3 || jacobian->cols != 9))
355 CV_ERROR( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
358 if( src->cols == 1 || src->rows == 1 )
360 double rx, ry, rz, theta;
361 int step = src->rows > 1 ? src->step / elem_size : 1;
363 if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
364 CV_ERROR( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );
366 if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
367 CV_ERROR( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
369 if( depth == CV_32F )
371 rx = src->data.fl[0];
372 ry = src->data.fl[step];
373 rz = src->data.fl[step*2];
377 rx = src->data.db[0];
378 ry = src->data.db[step];
379 rz = src->data.db[step*2];
381 theta = sqrt(rx*rx + ry*ry + rz*rz);
383 if( theta < DBL_EPSILON )
385 cvSetIdentity( dst );
389 memset( J, 0, sizeof(J) );
390 J[5] = J[15] = J[19] = -1;
391 J[7] = J[11] = J[21] = 1;
396 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
398 double c = cos(theta);
399 double s = sin(theta);
401 double itheta = theta ? 1./theta : 0.;
403 rx *= itheta; ry *= itheta; rz *= itheta;
405 double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
406 double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
408 CvMat _R = cvMat( 3, 3, CV_64F, R );
410 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
411 // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
412 for( k = 0; k < 9; k++ )
413 R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k];
415 cvConvert( &_R, dst );
419 double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0,
420 0, rx, 0, rx, ry+ry, rz, 0, rz, 0,
421 0, 0, rx, 0, 0, ry, rx, ry, rz+rz };
422 double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
423 0, 0, 1, 0, 0, 0, -1, 0, 0,
424 0, -1, 0, 1, 0, 0, 0, 0, 0 };
425 for( i = 0; i < 3; i++ )
427 double ri = i == 0 ? rx : i == 1 ? ry : rz;
428 double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
429 double a3 = (c - s*itheta)*ri, a4 = s*itheta;
430 for( k = 0; k < 9; k++ )
431 J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] +
432 a3*_r_x_[k] + a4*d_r_x_[i*9+k];
437 else if( src->cols == 3 && src->rows == 3 )
439 double R[9], U[9], V[9], W[3], rx, ry, rz;
440 CvMat _R = cvMat( 3, 3, CV_64F, R );
441 CvMat _U = cvMat( 3, 3, CV_64F, U );
442 CvMat _V = cvMat( 3, 3, CV_64F, V );
443 CvMat _W = cvMat( 3, 1, CV_64F, W );
445 int step = dst->rows > 1 ? dst->step / elem_size : 1;
447 if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
448 (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
449 CV_ERROR( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
451 cvConvert( src, &_R );
452 if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
460 cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
461 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
467 s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
468 c = (R[0] + R[4] + R[8] - 1)*0.5;
469 c = c > 1. ? 1. : c < -1. ? -1. : c;
481 rx = theta*sqrt(MAX(t,0.));
483 ry = theta*sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.);
485 rz = theta*sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.);
490 memset( J, 0, sizeof(J) );
493 J[5] = J[15] = J[19] = -0.5;
494 J[7] = J[11] = J[21] = 0.5;
500 double vth = 1/(2*s);
504 double t, dtheta_dtr = -1./s;
505 // var1 = [vth;theta]
506 // var = [om1;var1] = [om1;vth;theta]
507 double dvth_dtheta = -vth*c/s;
508 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
509 double d2 = 0.5*dtheta_dtr;
510 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
513 0, 0, 0, 0, 0, 1, 0, -1, 0,
514 0, 0, -1, 0, 0, 0, 1, 0, 0,
515 0, 1, 0, -1, 0, 0, 0, 0, 0,
516 d1, 0, 0, 0, d1, 0, 0, 0, d1,
517 d2, 0, 0, 0, d2, 0, 0, 0, d2
527 double domegadvar2[] =
534 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
535 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
536 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
538 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
540 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
541 cvMatMul( &_t0, &_dvardR, &_J );
543 // transpose every row of _J (treat the rows as 3x3 matrices)
544 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
545 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
546 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
550 rx *= vth; ry *= vth; rz *= vth;
553 if( depth == CV_32F )
555 dst->data.fl[0] = (float)rx;
556 dst->data.fl[step] = (float)ry;
557 dst->data.fl[step*2] = (float)rz;
561 dst->data.db[0] = rx;
562 dst->data.db[step] = ry;
563 dst->data.db[step*2] = rz;
569 if( depth == CV_32F )
571 if( jacobian->rows == _J.rows )
572 cvConvert( &_J, jacobian );
576 CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf );
577 cvConvert( &_J, &_Jf );
578 cvTranspose( &_Jf, jacobian );
581 else if( jacobian->rows == _J.rows )
582 cvCopy( &_J, jacobian );
584 cvTranspose( &_J, jacobian );
596 cvProjectPoints2( const CvMat* obj_points,
600 const CvMat* dist_coeffs,
601 CvMat* img_points, CvMat* dpdr,
602 CvMat* dpdt, CvMat* dpdf,
603 CvMat* dpdc, CvMat* dpdk )
605 CvMat *_M = 0, *_m = 0;
606 CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0;
608 CV_FUNCNAME( "cvProjectPoints2" );
613 int calc_derivatives;
614 const CvPoint3D64f* M;
616 double r[3], R[9], dRdr[27], t[3], a[9], k[4] = {0,0,0,0}, fx, fy, cx, cy;
617 CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
618 CvMat _R = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
619 double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
620 int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
622 if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(r_vec) ||
623 !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
624 /*!CV_IS_MAT(dist_coeffs) ||*/ !CV_IS_MAT(img_points) )
625 CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
627 count = MAX(obj_points->rows, obj_points->cols);
629 if( CV_IS_CONT_MAT(obj_points->type) && CV_MAT_DEPTH(obj_points->type) == CV_64F &&
630 (obj_points->rows == 1 && CV_MAT_CN(obj_points->type) == 3 ||
631 obj_points->rows == count && CV_MAT_CN(obj_points->type)*obj_points->cols == 3))
632 _M = (CvMat*)obj_points;
635 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
636 CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
639 if( CV_IS_CONT_MAT(img_points->type) && CV_MAT_DEPTH(img_points->type) == CV_64F &&
640 (img_points->rows == 1 && CV_MAT_CN(img_points->type) == 2 ||
641 img_points->rows == count && CV_MAT_CN(img_points->type)*img_points->cols == 2))
644 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
646 M = (CvPoint3D64f*)_M->data.db;
647 m = (CvPoint2D64f*)_m->data.db;
649 if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
650 (r_vec->rows != 1 && r_vec->cols != 1 ||
651 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
652 (r_vec->rows != 3 && r_vec->cols != 3 || CV_MAT_CN(r_vec->type) != 1))
653 CV_ERROR( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
654 "floating-point rotation vector, or 3x3 rotation matrix" );
656 if( r_vec->rows == 3 && r_vec->cols == 3 )
658 _r = cvMat( 3, 1, CV_64FC1, r );
659 CV_CALL( cvRodrigues2( r_vec, &_r ));
660 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ));
661 cvCopy( r_vec, &_R );
665 _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
666 CV_CALL( cvConvert( r_vec, &_r ));
667 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ) );
670 if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
671 t_vec->rows != 1 && t_vec->cols != 1 ||
672 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
673 CV_ERROR( CV_StsBadArg,
674 "Translation vector must be 1x3 or 3x1 floating-point vector" );
676 _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
677 CV_CALL( cvConvert( t_vec, &_t ));
679 if( CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1 ||
680 A->rows != 3 || A->cols != 3 )
681 CV_ERROR( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
683 CV_CALL( cvConvert( A, &_a ));
684 fx = a[0]; fy = a[4];
685 cx = a[2]; cy = a[5];
689 if( !CV_IS_MAT(dist_coeffs) ||
690 CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
691 CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
692 dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
693 dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
694 CV_ERROR( CV_StsBadArg,
695 "Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
697 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
698 CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
699 CV_CALL( cvConvert( dist_coeffs, &_k ));
704 if( !CV_IS_MAT(dpdr) ||
705 CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
706 CV_MAT_TYPE(dpdr->type) != CV_64FC1 ||
707 dpdr->rows != count*2 || dpdr->cols != 3 )
708 CV_ERROR( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
710 if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
713 CV_CALL( _dpdr = cvCreateMat( 2*count, 3, CV_64FC1 ));
714 dpdr_p = _dpdr->data.db;
715 dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
720 if( !CV_IS_MAT(dpdt) ||
721 CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
722 CV_MAT_TYPE(dpdt->type) != CV_64FC1 ||
723 dpdt->rows != count*2 || dpdt->cols != 3 )
724 CV_ERROR( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
726 if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
729 CV_CALL( _dpdt = cvCreateMat( 2*count, 3, CV_64FC1 ));
730 dpdt_p = _dpdt->data.db;
731 dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
736 if( !CV_IS_MAT(dpdf) ||
737 CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1 ||
738 dpdf->rows != count*2 || dpdf->cols != 2 )
739 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
741 if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
744 CV_CALL( _dpdf = cvCreateMat( 2*count, 2, CV_64FC1 ));
745 dpdf_p = _dpdf->data.db;
746 dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
751 if( !CV_IS_MAT(dpdc) ||
752 CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1 ||
753 dpdc->rows != count*2 || dpdc->cols != 2 )
754 CV_ERROR( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
756 if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
759 CV_CALL( _dpdc = cvCreateMat( 2*count, 2, CV_64FC1 ));
760 dpdc_p = _dpdc->data.db;
761 dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
766 if( !CV_IS_MAT(dpdk) ||
767 CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1 ||
768 dpdk->rows != count*2 || (dpdk->cols != 4 && dpdk->cols != 2) )
769 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx4 or 2Nx2 floating-point matrix" );
772 CV_ERROR( CV_StsNullPtr, "dist_coeffs is NULL while dpdk is not" );
774 if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
777 CV_CALL( _dpdk = cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
778 dpdk_p = _dpdk->data.db;
779 dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
782 calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
784 for( i = 0; i < count; i++ )
786 double X = M[i].x, Y = M[i].y, Z = M[i].z;
787 double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
788 double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
789 double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
790 double r2, r4, a1, a2, a3, cdist;
801 cdist = 1 + k[0]*r2 + k[1]*r4;
802 xd = x*cdist + k[2]*a1 + k[3]*a2;
803 yd = y*cdist + k[2]*a3 + k[3]*a1;
808 if( calc_derivatives )
812 dpdc_p[0] = 1; dpdc_p[1] = 0;
813 dpdc_p[dpdc_step] = 0;
814 dpdc_p[dpdc_step+1] = 1;
815 dpdc_p += dpdc_step*2;
820 dpdf_p[0] = xd; dpdf_p[1] = 0;
821 dpdf_p[dpdf_step] = 0;
822 dpdf_p[dpdf_step+1] = yd;
823 dpdf_p += dpdf_step*2;
830 dpdk_p[dpdk_step] = fy*y*r2;
831 dpdk_p[dpdk_step+1] = fy*y*r4;
832 if( _dpdk->cols > 2 )
836 dpdk_p[dpdk_step+2] = fy*a3;
837 dpdk_p[dpdk_step+3] = fy*a1;
839 dpdk_p += dpdk_step*2;
844 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
845 for( j = 0; j < 3; j++ )
847 double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
848 double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt;
849 double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
850 double dmxdt = fx*(dxdt[j]*cdist + x*dcdist_dt +
851 k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j]));
852 double dmydt = fy*(dydt[j]*cdist + y*dcdist_dt +
853 k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt);
855 dpdt_p[dpdt_step+j] = dmydt;
857 dpdt_p += dpdt_step*2;
864 X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
865 X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
866 X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
870 X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
871 X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
872 X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
876 X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
877 X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
878 X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
880 for( j = 0; j < 3; j++ )
882 double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
883 double dydr = z*(dy0dr[j] - y*dz0dr[j]);
884 double dr2dr = 2*x*dxdr + 2*y*dydr;
885 double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr;
886 double da1dr = 2*(x*dydr + y*dxdr);
887 double dmxdr = fx*(dxdr*cdist + x*dcdist_dr +
888 k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr));
889 double dmydr = fy*(dydr*cdist + y*dcdist_dr +
890 k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr);
892 dpdr_p[dpdr_step+j] = dmydr;
894 dpdr_p += dpdr_step*2;
899 if( _m != img_points )
900 cvConvertPointsHomogenious( _m, img_points );
902 cvConvert( _dpdr, dpdr );
904 cvConvert( _dpdt, dpdt );
906 cvConvert( _dpdf, dpdf );
908 cvConvert( _dpdc, dpdc );
910 cvConvert( _dpdk, dpdk );
914 if( _M != obj_points )
916 if( _m != img_points )
919 cvReleaseMat( &_dpdr );
921 cvReleaseMat( &_dpdt );
923 cvReleaseMat( &_dpdf );
925 cvReleaseMat( &_dpdc );
927 cvReleaseMat( &_dpdk );
932 cvFindExtrinsicCameraParams2( const CvMat* obj_points,
933 const CvMat* img_points, const CvMat* A,
934 const CvMat* dist_coeffs,
935 CvMat* r_vec, CvMat* t_vec )
937 const int max_iter = 20;
938 CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
940 CV_FUNCNAME( "cvFindExtrinsicCameraParams2" );
945 double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
946 double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
947 double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
949 CvPoint2D64f *m = 0, *mn = 0;
950 CvMat _a = cvMat( 3, 3, CV_64F, a );
951 CvMat _R = cvMat( 3, 3, CV_64F, R );
952 CvMat _r = cvMat( 3, 1, CV_64F, param );
953 CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
954 CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
955 CvMat _MM = cvMat( 3, 3, CV_64F, MM );
956 CvMat _U = cvMat( 3, 3, CV_64F, U );
957 CvMat _V = cvMat( 3, 3, CV_64F, V );
958 CvMat _W = cvMat( 3, 1, CV_64F, W );
959 CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
960 CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
961 CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
962 CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
963 CvMat _delta = cvMat( 6, 1, CV_64F, delta );
964 CvMat _param = cvMat( 6, 1, CV_64F, param );
967 if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
968 !CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
969 CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
971 count = MAX(obj_points->cols, obj_points->rows);
972 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
973 CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
974 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
975 CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
976 M = (CvPoint3D64f*)_M->data.db;
977 m = (CvPoint2D64f*)_m->data.db;
978 mn = (CvPoint2D64f*)_mn->data.db;
980 CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
981 CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
982 CV_CALL( cvConvert( A, &_a ));
987 if( !CV_IS_MAT(dist_coeffs) ||
988 CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
989 CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
990 dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
991 dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
992 CV_ERROR( CV_StsBadArg,
993 "Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
995 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
996 CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
997 CV_CALL( cvConvert( dist_coeffs, &_k ));
1000 if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
1001 r_vec->rows != 1 && r_vec->cols != 1 ||
1002 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
1003 CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
1005 if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
1006 t_vec->rows != 1 && t_vec->cols != 1 ||
1007 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
1008 CV_ERROR( CV_StsBadArg,
1009 "Translation vector must be 1x3 or 3x1 floating-point vector" );
1011 ifx = 1./a[0]; ify = 1./a[4];
1012 cx = a[2]; cy = a[5];
1014 // normalize image points
1015 // (unapply the intrinsic matrix transformation and distortion)
1016 for( i = 0; i < count; i++ )
1018 double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
1020 // compensate distortion iteratively
1022 for( j = 0; j < 5; j++ )
1024 double r2 = x*x + y*y;
1025 double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
1026 double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
1027 double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
1028 x = (x0 - delta_x)*icdist;
1029 y = (y0 - delta_y)*icdist;
1031 mn[i].x = x; mn[i].y = y;
1043 cvReshape( _M, _M, 1, count );
1044 cvMulTransposed( _M, &_MM, 1, &_Mc );
1045 cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
1047 // initialize extrinsic parameters
1048 if( W[2]/W[1] < 1e-3 || count < 4 )
1050 // a planar structure case (all M's lie in the same plane)
1051 double tt[3], h[9], h1_norm, h2_norm;
1052 CvMat* R_transform = &_V;
1053 CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
1054 CvMat _H = cvMat( 3, 3, CV_64F, h );
1055 CvMat _h1, _h2, _h3;
1057 if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
1058 cvSetIdentity( R_transform );
1060 if( cvDet(R_transform) < 0 )
1061 cvScale( R_transform, R_transform, -1 );
1063 cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
1065 for( i = 0; i < count; i++ )
1067 const double* Rp = R_transform->data.db;
1068 const double* Tp = T_transform.data.db;
1069 const double* src = _M->data.db + i*3;
1070 double* dst = _Mxy->data.db + i*2;
1072 dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
1073 dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
1076 cvFindHomography( _Mxy, _mn, &_H );
1078 cvGetCol( &_H, &_h1, 0 );
1079 _h2 = _h1; _h2.data.db++;
1080 _h3 = _h2; _h3.data.db++;
1081 h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
1082 h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
1084 cvScale( &_h1, &_h1, 1./h1_norm );
1085 cvScale( &_h2, &_h2, 1./h2_norm );
1086 cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
1087 cvCrossProduct( &_h1, &_h2, &_h3 );
1089 cvRodrigues2( &_H, &_r );
1090 cvRodrigues2( &_r, &_H );
1091 cvMatMulAdd( &_H, &T_transform, &_t, &_t );
1092 cvMatMul( &_H, R_transform, &_R );
1093 cvRodrigues2( &_R, &_r );
1097 // non-planar structure. Use DLT method
1099 double LL[12*12], LW[12], LV[12*12], sc;
1100 CvMat _LL = cvMat( 12, 12, CV_64F, LL );
1101 CvMat _LW = cvMat( 12, 1, CV_64F, LW );
1102 CvMat _LV = cvMat( 12, 12, CV_64F, LV );
1103 CvMat _RRt, _RR, _tt;
1105 CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
1108 for( i = 0; i < count; i++, L += 24 )
1110 double x = -mn[i].x, y = -mn[i].y;
1111 L[0] = L[16] = M[i].x;
1112 L[1] = L[17] = M[i].y;
1113 L[2] = L[18] = M[i].z;
1115 L[4] = L[5] = L[6] = L[7] = 0.;
1116 L[12] = L[13] = L[14] = L[15] = 0.;
1127 cvMulTransposed( _L, &_LL, 1 );
1128 cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1129 _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
1130 cvGetCols( &_RRt, &_RR, 0, 3 );
1131 cvGetCol( &_RRt, &_tt, 3 );
1132 if( cvDet(&_RR) < 0 )
1133 cvScale( &_RRt, &_RRt, -1 );
1135 cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
1136 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
1137 cvScale( &_tt, &_t, cvNorm(&_R)/sc );
1138 cvRodrigues2( &_R, &_r );
1139 cvReleaseMat( &_L );
1142 CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
1143 cvGetCols( _J, &_dpdr, 0, 3 );
1144 cvGetCols( _J, &_dpdt, 3, 6 );
1146 // refine extrinsic parameters using iterative algorithm
1147 for( i = 0; i < max_iter; i++ )
1150 cvReshape( _mn, _mn, 2, 1 );
1151 cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
1152 _mn, &_dpdr, &_dpdt, 0, 0, 0 );
1153 cvSub( _m, _mn, _mn );
1154 cvReshape( _mn, _mn, 1, 2*count );
1156 cvMulTransposed( _J, &_JtJ, 1 );
1157 cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
1158 cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1159 if( JtJW[5]/JtJW[0] < 1e-12 )
1161 cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
1162 &_delta, CV_SVD_U_T + CV_SVD_V_T );
1163 cvAdd( &_delta, &_param, &_param );
1164 n1 = cvNorm( &_delta );
1165 n2 = cvNorm( &_param );
1170 _r = cvMat( r_vec->rows, r_vec->cols,
1171 CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
1172 _t = cvMat( t_vec->rows, t_vec->cols,
1173 CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
1175 cvConvert( &_r, r_vec );
1176 cvConvert( &_t, t_vec );
1180 cvReleaseMat( &_M );
1181 cvReleaseMat( &_Mxy );
1182 cvReleaseMat( &_m );
1183 cvReleaseMat( &_mn );
1184 cvReleaseMat( &_L );
1185 cvReleaseMat( &_J );
1190 icvInitIntrinsicParams2D( const CvMat* obj_points,
1191 const CvMat* img_points,
1192 const CvMat* point_counts,
1194 CvMat* intrinsic_matrix,
1195 double aspect_ratio )
1197 CvMat *_A = 0, *_b = 0;
1199 CV_FUNCNAME( "icvInitIntrinsicParams2D" );
1203 int i, j, pos, img_count;
1204 double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
1205 double H[9], AtA[4], AtAW[2], AtAV[4], Atb[2], f[2];
1206 CvMat _a = cvMat( 3, 3, CV_64F, a );
1207 CvMat _H = cvMat( 3, 3, CV_64F, H );
1208 CvMat _AtA = cvMat( 2, 2, CV_64F, AtA );
1209 CvMat _AtAW = cvMat( 2, 1, CV_64F, AtAW );
1210 CvMat _AtAV = cvMat( 2, 2, CV_64F, AtAV );
1211 CvMat _Atb = cvMat( 2, 1, CV_64F, Atb );
1212 CvMat _f = cvMat( 2, 1, CV_64F, f );
1214 assert( CV_MAT_TYPE(point_counts->type) == CV_32SC1 &&
1215 CV_IS_MAT_CONT(point_counts->type) );
1216 img_count = point_counts->rows + point_counts->cols - 1;
1218 if( CV_MAT_TYPE(obj_points->type) != CV_32FC3 &&
1219 CV_MAT_TYPE(obj_points->type) != CV_64FC3 ||
1220 CV_MAT_TYPE(img_points->type) != CV_32FC2 &&
1221 CV_MAT_TYPE(img_points->type) != CV_64FC2 )
1222 CV_ERROR( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" );
1224 if( obj_points->rows != 1 || img_points->rows != 1 )
1225 CV_ERROR( CV_StsBadSize, "object points and image points must be a single-row matrices" );
1227 CV_CALL( _A = cvCreateMat( 2*img_count, 2, CV_64F ));
1228 CV_CALL( _b = cvCreateMat( 2*img_count, 1, CV_64F ));
1229 a[2] = (image_size.width - 1)*0.5;
1230 a[5] = (image_size.height - 1)*0.5;
1232 // extract vanishing points in order to obtain initial value for the focal length
1233 for( i = 0, pos = 0; i < img_count; i++ )
1235 double* Ap = _A->data.db + i*4;
1236 double* bp = _b->data.db + i*2;
1237 int count = point_counts->data.i[i];
1238 double h[3], v[3], d1[3], d2[3];
1239 double n[4] = {0,0,0,0};
1241 cvGetCols( obj_points, &_M, pos, pos + count );
1242 cvGetCols( img_points, &_m, pos, pos + count );
1245 CV_CALL( cvFindHomography( &_M, &_m, &_H ));
1247 H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
1248 H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
1250 for( j = 0; j < 3; j++ )
1252 double t0 = H[j*3], t1 = H[j*3+1];
1253 h[j] = t0; v[j] = t1;
1254 d1[j] = (t0 + t1)*0.5;
1255 d2[j] = (t0 - t1)*0.5;
1256 n[0] += t0*t0; n[1] += t1*t1;
1257 n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
1260 for( j = 0; j < 4; j++ )
1261 n[j] = 1./sqrt(n[j]);
1263 for( j = 0; j < 3; j++ )
1265 h[j] *= n[0]; v[j] *= n[1];
1266 d1[j] *= n[2]; d2[j] *= n[3];
1269 Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
1270 Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
1271 bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
1274 // while it is not about gradient descent search,
1275 // the formula is the same: f = inv(At*A)*At*b
1276 icvGaussNewton( _A, _b, &_f, &_AtA, &_Atb, &_AtAW, &_AtAV );
1277 a[0] = sqrt(fabs(1./f[0]));
1278 a[4] = sqrt(fabs(1./f[1]));
1279 if( aspect_ratio != 0 )
1281 double tf = (a[0] + a[4])/(aspect_ratio + 1.);
1282 a[0] = aspect_ratio*tf;
1286 cvConvert( &_a, intrinsic_matrix );
1290 cvReleaseMat( &_A );
1291 cvReleaseMat( &_b );
1295 /* finds intrinsic and extrinsic camera parameters
1296 from a few views of known calibration pattern */
1298 cvCalibrateCamera2( const CvMat* obj_points,
1299 const CvMat* img_points,
1300 const CvMat* point_counts,
1302 CvMat* A, CvMat* dist_coeffs,
1303 CvMat* r_vecs, CvMat* t_vecs,
1306 double alpha_smooth = 0.4;
1308 CvMat *counts = 0, *_M = 0, *_m = 0;
1309 CvMat *_Ji = 0, *_Je = 0, *_JtJ = 0, *_JtErr = 0, *_JtJW = 0, *_JtJV = 0;
1310 CvMat *_param = 0, *_param_innov = 0, *_err = 0;
1312 CV_FUNCNAME( "cvCalibrateCamera2" );
1317 CvMat _a = cvMat( 3, 3, CV_64F, a ), _k;
1318 CvMat _Mi, _mi, _ri, _ti, _part;
1319 CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk;
1320 CvMat _sr_part = cvMat( 1, 3, CV_64F ), _st_part = cvMat( 1, 3, CV_64F ), _r_part, _t_part;
1321 int i, j, pos, iter, img_count, count = 0, max_count = 0, total = 0, param_count;
1322 int r_depth = 0, t_depth = 0, r_step = 0, t_step = 0, cn, dims;
1323 int output_r_matrices = 0;
1324 double aspect_ratio = 0.;
1326 if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
1327 !CV_IS_MAT(point_counts) || !CV_IS_MAT(A) || !CV_IS_MAT(dist_coeffs) )
1328 CV_ERROR( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
1330 if( image_size.width <= 0 || image_size.height <= 0 )
1331 CV_ERROR( CV_StsOutOfRange, "image width and height must be positive" );
1333 if( CV_MAT_TYPE(point_counts->type) != CV_32SC1 ||
1334 point_counts->rows != 1 && point_counts->cols != 1 )
1335 CV_ERROR( CV_StsUnsupportedFormat,
1336 "the array of point counters must be 1-dimensional integer vector" );
1338 CV_CALL( counts = cvCreateMat( point_counts->rows, point_counts->width, CV_32SC1 ));
1339 cvCopy( point_counts, counts );
1341 img_count = counts->rows + counts->cols - 1;
1345 r_depth = CV_MAT_DEPTH(r_vecs->type);
1346 r_step = r_vecs->rows == 1 ? 3*CV_ELEM_SIZE(r_depth) : r_vecs->step;
1347 cn = CV_MAT_CN(r_vecs->type);
1348 if( !CV_IS_MAT(r_vecs) || r_depth != CV_32F && r_depth != CV_64F ||
1349 (r_vecs->rows != img_count || r_vecs->cols*cn != 3 && r_vecs->cols*cn != 9) &&
1350 (r_vecs->rows != 1 || r_vecs->cols != img_count || cn != 3) )
1351 CV_ERROR( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
1352 "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
1353 output_r_matrices = r_vecs->rows == img_count && r_vecs->cols*cn == 9;
1358 t_depth = CV_MAT_DEPTH(t_vecs->type);
1359 t_step = t_vecs->rows == 1 ? 3*CV_ELEM_SIZE(t_depth) : t_vecs->step;
1360 cn = CV_MAT_CN(t_vecs->type);
1361 if( !CV_IS_MAT(t_vecs) || t_depth != CV_32F && t_depth != CV_64F ||
1362 (t_vecs->rows != img_count || t_vecs->cols*cn != 3) &&
1363 (t_vecs->rows != 1 || t_vecs->cols != img_count || cn != 3) )
1364 CV_ERROR( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
1365 "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
1368 if( CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 ||
1369 A->rows != 3 || A->cols != 3 )
1370 CV_ERROR( CV_StsBadArg,
1371 "Intrinsic parameters must be 3x3 floating-point matrix" );
1373 if( CV_MAT_TYPE(dist_coeffs->type) != CV_32FC1 &&
1374 CV_MAT_TYPE(dist_coeffs->type) != CV_64FC1 ||
1375 (dist_coeffs->rows != 4 || dist_coeffs->cols != 1) &&
1376 (dist_coeffs->cols != 4 || dist_coeffs->rows != 1))
1377 CV_ERROR( CV_StsBadArg,
1378 "Distortion coefficients must be 4x1 or 1x4 floating-point matrix" );
1380 for( i = 0; i < img_count; i++ )
1382 int temp_count = counts->data.i[i];
1383 if( temp_count < 4 )
1386 sprintf( buf, "The number of points in the view #%d is <4", i );
1387 CV_ERROR( CV_StsOutOfRange, buf );
1389 max_count = MAX( max_count, temp_count );
1390 total += temp_count;
1393 dims = CV_MAT_CN(obj_points->type)*(obj_points->rows == 1 ? 1 : obj_points->cols);
1395 if( CV_MAT_DEPTH(obj_points->type) != CV_32F &&
1396 CV_MAT_DEPTH(obj_points->type) != CV_64F ||
1397 (obj_points->rows != total || dims != 3 && dims != 2) &&
1398 (obj_points->rows != 1 || obj_points->cols != total || (dims != 3 && dims != 2)) )
1399 CV_ERROR( CV_StsBadArg, "Object points must be 1xn or nx1, 2- or 3-channel matrix, "
1400 "or nx3 or nx2 single-channel matrix" );
1402 cn = CV_MAT_CN(img_points->type);
1403 if( CV_MAT_DEPTH(img_points->type) != CV_32F &&
1404 CV_MAT_DEPTH(img_points->type) != CV_64F ||
1405 (img_points->rows != total || img_points->cols*cn != 2) &&
1406 (img_points->rows != 1 || img_points->cols != total || cn != 2) )
1407 CV_ERROR( CV_StsBadArg, "Image points must be 1xn or nx1, 2-channel matrix, "
1408 "or nx2 single-channel matrix" );
1410 CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 ));
1411 CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 ));
1413 CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
1414 CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
1416 param_count = 8 + img_count*6;
1417 CV_CALL( _param = cvCreateMat( param_count, 1, CV_64FC1 ));
1418 CV_CALL( _param_innov = cvCreateMat( param_count, 1, CV_64FC1 ));
1419 CV_CALL( _JtJ = cvCreateMat( param_count, param_count, CV_64FC1 ));
1420 CV_CALL( _JtErr = cvCreateMat( param_count, 1, CV_64FC1 ));
1421 CV_CALL( _JtJW = cvCreateMat( param_count, 1, CV_64FC1 ));
1422 CV_CALL( _JtJV = cvCreateMat( param_count, param_count, CV_64FC1 ));
1423 CV_CALL( _Ji = cvCreateMat( max_count*2, 8, CV_64FC1 ));
1424 CV_CALL( _Je = cvCreateMat( max_count*2, 6, CV_64FC1 ));
1425 CV_CALL( _err = cvCreateMat( max_count*2, 1, CV_64FC1 ));
1427 cvGetCols( _Je, &_dpdr, 0, 3 );
1428 cvGetCols( _Je, &_dpdt, 3, 6 );
1429 cvGetCols( _Ji, &_dpdf, 0, 2 );
1430 cvGetCols( _Ji, &_dpdc, 2, 4 );
1431 cvGetCols( _Ji, &_dpdk, 4, flags & CV_CALIB_ZERO_TANGENT_DIST ? 6 : 8 );
1434 // 1. initialize intrinsic parameters
1435 if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
1437 cvConvert( A, &_a );
1438 if( a[0] <= 0 || a[4] <= 0 )
1439 CV_ERROR( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
1440 if( a[2] < 0 || a[2] >= image_size.width ||
1441 a[5] < 0 || a[5] >= image_size.height )
1442 CV_ERROR( CV_StsOutOfRange, "Principal point must be within the image" );
1443 if( fabs(a[1]) > 1e-5 )
1444 CV_ERROR( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
1445 if( fabs(a[3]) > 1e-5 || fabs(a[6]) > 1e-5 ||
1446 fabs(a[7]) > 1e-5 || fabs(a[8]-1) > 1e-5 )
1447 CV_ERROR( CV_StsOutOfRange,
1448 "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
1449 a[1] = a[3] = a[6] = a[7] = 0.;
1452 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1453 aspect_ratio = a[0]/a[4];
1460 cvAvgSdv( _M, &mean, &sdv );
1461 if( fabs(mean.val[2]) > 1e-5 && fabs(mean.val[2] - 1) > 1e-5 ||
1462 fabs(sdv.val[2]) > 1e-5 )
1463 CV_ERROR( CV_StsBadArg,
1464 "For non-planar calibration rigs the initial intrinsic matrix must be specified" );
1466 for( i = 0; i < total; i++ )
1467 ((CvPoint3D64f*)(_M->data.db + i*3))->z = 0.;
1469 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1471 aspect_ratio = cvmGet(A,0,0);
1472 aspect_ratio /= cvmGet(A,1,1);
1473 if( aspect_ratio < 0.01 || aspect_ratio > 100 )
1474 CV_ERROR( CV_StsOutOfRange,
1475 "The specified aspect ratio (=a(0,0)/a(1,1)) is incorrect" );
1477 icvInitIntrinsicParams2D( _M, _m, counts, image_size, &_a, aspect_ratio );
1480 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, CV_64FC1, _param->data.db + 4 );
1483 // 2. initialize extrinsic parameters
1484 for( i = 0, pos = 0; i < img_count; i++, pos += count )
1486 count = counts->data.i[i];
1487 _ri = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6 );
1488 _ti = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6 + 3 );
1490 cvGetCols( _M, &_Mi, pos, pos + count );
1491 cvGetCols( _m, &_mi, pos, pos + count );
1492 cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_a, &_k, &_ri, &_ti );
1495 _param->data.db[0] = a[0];
1496 _param->data.db[1] = a[4];
1497 _param->data.db[2] = a[2];
1498 _param->data.db[3] = a[5];
1500 // 3. run the optimization
1501 for( iter = 0; iter < 30; iter++ )
1503 double* jj = _JtJ->data.db;
1506 for( i = 0, pos = 0; i < img_count; i++, pos += count )
1508 count = counts->data.i[i];
1509 _ri = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6);
1510 _ti = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6 + 3);
1512 cvGetCols( _M, &_Mi, pos, pos + count );
1513 _mi = cvMat( count*2, 1, CV_64FC1, _m->data.db + pos*2 );
1515 _dpdr.rows = _dpdt.rows = _dpdf.rows = _dpdc.rows = _dpdk.rows = count*2;
1518 _err->rows = count*2;
1519 cvReshape( _err, _err, 2, count );
1520 cvProjectPoints2( &_Mi, &_ri, &_ti, &_a, &_k, _err, &_dpdr, &_dpdt, &_dpdf,
1521 flags & CV_CALIB_FIX_PRINCIPAL_POINT ? 0 : &_dpdc, &_dpdk );
1523 // alter dpdf in case if only one of the focal
1524 // parameters (fy) is independent variable
1525 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1526 for( j = 0; j < count; j++ )
1528 double* dpdf_j = (double*)(_dpdf.data.ptr + j*_dpdf.step*2);
1529 dpdf_j[1] = dpdf_j[0]*aspect_ratio;
1533 cvReshape( _err, _err, 1, count*2 );
1534 cvSub( &_mi, _err, _err );
1536 _Je->rows = _Ji->rows = count*2;
1538 cvGetSubRect( _JtJ, &_part, cvRect(0,0,8,8) );
1539 cvGEMM( _Ji, _Ji, 1, &_part, i > 0, &_part, CV_GEMM_A_T );
1541 cvGetSubRect( _JtJ, &_part, cvRect(8+i*6,8+i*6,6,6) );
1542 cvMulTransposed( _Je, &_part, 1 );
1544 cvGetSubRect( _JtJ, &_part, cvRect(8+i*6,0,6,8) );
1545 cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1547 cvGetRows( _JtErr, &_part, 0, 8 );
1548 cvGEMM( _Ji, _err, 1, &_part, i > 0, &_part, CV_GEMM_A_T );
1550 cvGetRows( _JtErr, &_part, 8 + i*6, 8 + (i+1)*6 );
1551 cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
1554 // make the matrix JtJ exactly symmetrical and add missing zeros
1555 for( i = 0; i < param_count; i++ )
1557 int mj = i < 8 ? param_count : ((i - 8)/6)*6 + 14;
1558 for( j = i+1; j < mj; j++ )
1559 jj[j*param_count + i] = jj[i*param_count + j];
1560 for( ; j < param_count; j++ )
1561 jj[j*param_count + i] = jj[i*param_count + j] = 0;
1564 cvSVD( _JtJ, _JtJW, 0, _JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1565 cvSVBkSb( _JtJW, _JtJV, _JtJV, _JtErr, _param_innov, CV_SVD_U_T + CV_SVD_V_T );
1567 cvScale( _param_innov, _param_innov, 1. - pow(1. - alpha_smooth, iter + 1.) );
1568 cvGetRows( _param_innov, &_part, 0, 4 );
1569 change = cvNorm( &_part );
1570 cvGetRows( _param, &_part, 0, 4 );
1571 change /= cvNorm( &_part );
1573 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1574 _param_innov->data.db[2] = _param_innov->data.db[3] = 0.;
1576 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1577 _param_innov->data.db[6] = _param_innov->data.db[7] = 0.;
1579 cvAdd( _param, _param_innov, _param );
1581 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1582 _param->data.db[0] = _param->data.db[1]*aspect_ratio;
1584 a[0] = _param->data.db[0];
1585 a[4] = _param->data.db[1];
1586 a[2] = _param->data.db[2];
1587 a[5] = _param->data.db[3];
1589 if( change < FLT_EPSILON )
1593 cvConvert( &_a, A );
1594 cvConvert( &_k, dist_coeffs );
1596 _r_part = cvMat( output_r_matrices ? 3 : 1, 3, r_depth );
1597 _t_part = cvMat( 1, 3, t_depth );
1598 for( i = 0; i < img_count; i++ )
1602 _sr_part.data.db = _param->data.db + 8 + i*6;
1603 _r_part.data.ptr = r_vecs->data.ptr + i*r_step;
1604 if( !output_r_matrices )
1605 cvConvert( &_sr_part, &_r_part );
1608 cvRodrigues2( &_sr_part, &_a );
1609 cvConvert( &_a, &_r_part );
1614 _st_part.data.db = _param->data.db + 8 + i*6 + 3;
1615 _t_part.data.ptr = t_vecs->data.ptr + i*t_step;
1616 cvConvert( &_st_part, &_t_part );
1622 cvReleaseMat( &counts );
1623 cvReleaseMat( &_M );
1624 cvReleaseMat( &_m );
1625 cvReleaseMat( &_param );
1626 cvReleaseMat( &_param_innov );
1627 cvReleaseMat( &_JtJ );
1628 cvReleaseMat( &_JtErr );
1629 cvReleaseMat( &_JtJW );
1630 cvReleaseMat( &_JtJV );
1631 cvReleaseMat( &_Ji );
1632 cvReleaseMat( &_Je );
1633 cvReleaseMat( &_err );
1637 void cvCalibrationMatrixValues( const CvMat *calibMatr, int imgWidth, int imgHeight,
1638 double apertureWidth, double apertureHeight, double *fovx, double *fovy,
1639 double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
1641 double alphax, alphay, mx, my;
1643 CV_FUNCNAME("cvCalibrationMatrixValues");
1646 /* Validate parameters. */
1649 CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
1651 if(!CV_IS_MAT(calibMatr))
1652 CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
1654 if(calibMatr->cols != 3 || calibMatr->rows != 3)
1655 CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
1657 alphax = cvmGet(calibMatr, 0, 0);
1658 alphay = cvmGet(calibMatr, 1, 1);
1659 assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0);
1661 /* Calculate pixel aspect ratio. */
1663 *pasp = alphay / alphax;
1665 /* Calculate number of pixel per realworld unit. */
1667 if(apertureWidth != 0.0 && apertureHeight != 0.0) {
1668 mx = imgWidth / apertureWidth;
1669 my = imgHeight / apertureHeight;
1675 /* Calculate fovx and fovy. */
1678 *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI;
1681 *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI;
1683 /* Calculate focal length. */
1686 *focalLength = alphax / mx;
1688 /* Calculate principle point. */
1691 *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my);