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) 2009, 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 the copyright holders 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/imgproc/imgproc_c.h"
45 #include "distortion_model.hpp"
46 #include "calib3d_c_api.h"
47 #include <stdio.h>
48 #include <iterator>
49
50 /*
51 This is straight-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet
52 that is (in a large extent) based on the paper:
53 Z. Zhang. "A flexible new technique for camera calibration".
54 IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
55 The 1st initial port was done by Valery Mosyagin.
56 */
57
58 using namespace cv;
59
60 // reimplementation of dAB.m
cvCalcMatMulDeriv(const CvMat * A,const CvMat * B,CvMat * dABdA,CvMat * dABdB)61 CV_IMPL void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
62 {
63 int i, j, M, N, L;
64 int bstep;
65
66 CV_Assert( CV_IS_MAT(A) && CV_IS_MAT(B) );
67 CV_Assert( CV_ARE_TYPES_EQ(A, B) &&
68 (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) );
69 CV_Assert( A->cols == B->rows );
70
71 M = A->rows;
72 L = A->cols;
73 N = B->cols;
74 bstep = B->step/CV_ELEM_SIZE(B->type);
75
76 if( dABdA )
77 {
78 CV_Assert( CV_ARE_TYPES_EQ(A, dABdA) &&
79 dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols );
80 }
81
82 if( dABdB )
83 {
84 CV_Assert( CV_ARE_TYPES_EQ(A, dABdB) &&
85 dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols );
86 }
87
88 if( CV_MAT_TYPE(A->type) == CV_32F )
89 {
90 for( i = 0; i < M*N; i++ )
91 {
92 int i1 = i / N, i2 = i % N;
93
94 if( dABdA )
95 {
96 float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i);
97 const float* b = (const float*)B->data.ptr + i2;
98
99 for( j = 0; j < M*L; j++ )
100 dcda[j] = 0;
101 for( j = 0; j < L; j++ )
102 dcda[i1*L + j] = b[j*bstep];
103 }
104
105 if( dABdB )
106 {
107 float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i);
108 const float* a = (const float*)(A->data.ptr + A->step*i1);
109
110 for( j = 0; j < L*N; j++ )
111 dcdb[j] = 0;
112 for( j = 0; j < L; j++ )
113 dcdb[j*N + i2] = a[j];
114 }
115 }
116 }
117 else
118 {
119 for( i = 0; i < M*N; i++ )
120 {
121 int i1 = i / N, i2 = i % N;
122
123 if( dABdA )
124 {
125 double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i);
126 const double* b = (const double*)B->data.ptr + i2;
127
128 for( j = 0; j < M*L; j++ )
129 dcda[j] = 0;
130 for( j = 0; j < L; j++ )
131 dcda[i1*L + j] = b[j*bstep];
132 }
133
134 if( dABdB )
135 {
136 double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i);
137 const double* a = (const double*)(A->data.ptr + A->step*i1);
138
139 for( j = 0; j < L*N; j++ )
140 dcdb[j] = 0;
141 for( j = 0; j < L; j++ )
142 dcdb[j*N + i2] = a[j];
143 }
144 }
145 }
146 }
147
148 // reimplementation of compose_motion.m
cvComposeRT(const CvMat * _rvec1,const CvMat * _tvec1,const CvMat * _rvec2,const CvMat * _tvec2,CvMat * _rvec3,CvMat * _tvec3,CvMat * dr3dr1,CvMat * dr3dt1,CvMat * dr3dr2,CvMat * dr3dt2,CvMat * dt3dr1,CvMat * dt3dt1,CvMat * dt3dr2,CvMat * dt3dt2)149 CV_IMPL void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
150 const CvMat* _rvec2, const CvMat* _tvec2,
151 CvMat* _rvec3, CvMat* _tvec3,
152 CvMat* dr3dr1, CvMat* dr3dt1,
153 CvMat* dr3dr2, CvMat* dr3dt2,
154 CvMat* dt3dr1, CvMat* dt3dt1,
155 CvMat* dt3dr2, CvMat* dt3dt2 )
156 {
157 double _r1[3], _r2[3];
158 double _R1[9], _d1[9*3], _R2[9], _d2[9*3];
159 CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2);
160 CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2);
161 CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2);
162
163 CV_Assert( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) );
164
165 CV_Assert( CV_MAT_TYPE(_rvec1->type) == CV_32F ||
166 CV_MAT_TYPE(_rvec1->type) == CV_64F );
167
168 CV_Assert( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) );
169
170 cvConvert( _rvec1, &r1 );
171 cvConvert( _rvec2, &r2 );
172
173 cvRodrigues2( &r1, &R1, &dR1dr1 );
174 cvRodrigues2( &r2, &R2, &dR2dr2 );
175
176 if( _rvec3 || dr3dr1 || dr3dr2 )
177 {
178 double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3];
179 double _W1[9*3], _W2[3*3];
180 CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3);
181 CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2);
182 CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3);
183 CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2);
184
185 cvMatMul( &R2, &R1, &R3 );
186 cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 );
187
188 cvRodrigues2( &R3, &r3, &dr3dR3 );
189
190 if( _rvec3 )
191 cvConvert( &r3, _rvec3 );
192
193 if( dr3dr1 )
194 {
195 cvMatMul( &dr3dR3, &dR3dR1, &W1 );
196 cvMatMul( &W1, &dR1dr1, &W2 );
197 cvConvert( &W2, dr3dr1 );
198 }
199
200 if( dr3dr2 )
201 {
202 cvMatMul( &dr3dR3, &dR3dR2, &W1 );
203 cvMatMul( &W1, &dR2dr2, &W2 );
204 cvConvert( &W2, dr3dr2 );
205 }
206 }
207
208 if( dr3dt1 )
209 cvZero( dr3dt1 );
210 if( dr3dt2 )
211 cvZero( dr3dt2 );
212
213 if( _tvec3 || dt3dr2 || dt3dt1 )
214 {
215 double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3];
216 CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2);
217 CvMat t3 = cvMat(3,1,CV_64F,_t3);
218 CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2);
219 CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1);
220 CvMat W3 = cvMat(3, 3, CV_64F, _W3);
221
222 CV_Assert( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) );
223 CV_Assert( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) );
224
225 cvConvert( _tvec1, &t1 );
226 cvConvert( _tvec2, &t2 );
227 cvMatMulAdd( &R2, &t1, &t2, &t3 );
228
229 if( _tvec3 )
230 cvConvert( &t3, _tvec3 );
231
232 if( dt3dr2 || dt3dt1 )
233 {
234 cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 );
235 if( dt3dr2 )
236 {
237 cvMatMul( &dxdR2, &dR2dr2, &W3 );
238 cvConvert( &W3, dt3dr2 );
239 }
240 if( dt3dt1 )
241 cvConvert( &dxdt1, dt3dt1 );
242 }
243 }
244
245 if( dt3dt2 )
246 cvSetIdentity( dt3dt2 );
247 if( dt3dr1 )
248 cvZero( dt3dr1 );
249 }
250
cvRodrigues2(const CvMat * src,CvMat * dst,CvMat * jacobian)251 CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
252 {
253 double J[27] = {0};
254 CvMat matJ = cvMat( 3, 9, CV_64F, J );
255
256 if( !CV_IS_MAT(src) )
257 CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
258
259 if( !CV_IS_MAT(dst) )
260 CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
261 "The first output argument is not a valid matrix" );
262
263 int depth = CV_MAT_DEPTH(src->type);
264 int elem_size = CV_ELEM_SIZE(depth);
265
266 if( depth != CV_32F && depth != CV_64F )
267 CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
268
269 if( !CV_ARE_DEPTHS_EQ(src, dst) )
270 CV_Error( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
271
272 if( jacobian )
273 {
274 if( !CV_IS_MAT(jacobian) )
275 CV_Error( CV_StsBadArg, "Jacobian is not a valid matrix" );
276
277 if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
278 CV_Error( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );
279
280 if( (jacobian->rows != 9 || jacobian->cols != 3) &&
281 (jacobian->rows != 3 || jacobian->cols != 9))
282 CV_Error( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
283 }
284
285 if( src->cols == 1 || src->rows == 1 )
286 {
287 int step = src->rows > 1 ? src->step / elem_size : 1;
288
289 if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
290 CV_Error( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );
291
292 if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
293 CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
294
295 Point3d r;
296 if( depth == CV_32F )
297 {
298 r.x = src->data.fl[0];
299 r.y = src->data.fl[step];
300 r.z = src->data.fl[step*2];
301 }
302 else
303 {
304 r.x = src->data.db[0];
305 r.y = src->data.db[step];
306 r.z = src->data.db[step*2];
307 }
308
309 double theta = norm(r);
310
311 if( theta < DBL_EPSILON )
312 {
313 cvSetIdentity( dst );
314
315 if( jacobian )
316 {
317 memset( J, 0, sizeof(J) );
318 J[5] = J[15] = J[19] = -1;
319 J[7] = J[11] = J[21] = 1;
320 }
321 }
322 else
323 {
324 double c = cos(theta);
325 double s = sin(theta);
326 double c1 = 1. - c;
327 double itheta = theta ? 1./theta : 0.;
328
329 r *= itheta;
330
331 Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z );
332 Matx33d r_x( 0, -r.z, r.y,
333 r.z, 0, -r.x,
334 -r.y, r.x, 0 );
335
336 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
337 Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x;
338
339 Mat(R).convertTo(cvarrToMat(dst), dst->type);
340
341 if( jacobian )
342 {
343 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
344 double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0,
345 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0,
346 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z };
347 double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
348 0, 0, 1, 0, 0, 0, -1, 0, 0,
349 0, -1, 0, 1, 0, 0, 0, 0, 0 };
350 for( int i = 0; i < 3; i++ )
351 {
352 double ri = i == 0 ? r.x : i == 1 ? r.y : r.z;
353 double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
354 double a3 = (c - s*itheta)*ri, a4 = s*itheta;
355 for( int k = 0; k < 9; k++ )
356 J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] +
357 a3*r_x.val[k] + a4*d_r_x_[i*9+k];
358 }
359 }
360 }
361 }
362 else if( src->cols == 3 && src->rows == 3 )
363 {
364 Matx33d U, Vt;
365 Vec3d W;
366 double theta, s, c;
367 int step = dst->rows > 1 ? dst->step / elem_size : 1;
368
369 if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
370 (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
371 CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
372
373 Matx33d R = cvarrToMat(src);
374
375 if( !checkRange(R, true, NULL, -100, 100) )
376 {
377 cvZero(dst);
378 if( jacobian )
379 cvZero(jacobian);
380 return 0;
381 }
382
383 SVD::compute(R, W, U, Vt);
384 R = U*Vt;
385
386 Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
387
388 s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25);
389 c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5;
390 c = c > 1. ? 1. : c < -1. ? -1. : c;
391 theta = acos(c);
392
393 if( s < 1e-5 )
394 {
395 double t;
396
397 if( c > 0 )
398 r = Point3d(0, 0, 0);
399 else
400 {
401 t = (R(0, 0) + 1)*0.5;
402 r.x = std::sqrt(MAX(t,0.));
403 t = (R(1, 1) + 1)*0.5;
404 r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.);
405 t = (R(2, 2) + 1)*0.5;
406 r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.);
407 if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) )
408 r.z = -r.z;
409 theta /= norm(r);
410 r *= theta;
411 }
412
413 if( jacobian )
414 {
415 memset( J, 0, sizeof(J) );
416 if( c > 0 )
417 {
418 J[5] = J[15] = J[19] = -0.5;
419 J[7] = J[11] = J[21] = 0.5;
420 }
421 }
422 }
423 else
424 {
425 double vth = 1/(2*s);
426
427 if( jacobian )
428 {
429 double t, dtheta_dtr = -1./s;
430 // var1 = [vth;theta]
431 // var = [om1;var1] = [om1;vth;theta]
432 double dvth_dtheta = -vth*c/s;
433 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
434 double d2 = 0.5*dtheta_dtr;
435 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
436 double dvardR[5*9] =
437 {
438 0, 0, 0, 0, 0, 1, 0, -1, 0,
439 0, 0, -1, 0, 0, 0, 1, 0, 0,
440 0, 1, 0, -1, 0, 0, 0, 0, 0,
441 d1, 0, 0, 0, d1, 0, 0, 0, d1,
442 d2, 0, 0, 0, d2, 0, 0, 0, d2
443 };
444 // var2 = [om;theta]
445 double dvar2dvar[] =
446 {
447 vth, 0, 0, r.x, 0,
448 0, vth, 0, r.y, 0,
449 0, 0, vth, r.z, 0,
450 0, 0, 0, 0, 1
451 };
452 double domegadvar2[] =
453 {
454 theta, 0, 0, r.x*vth,
455 0, theta, 0, r.y*vth,
456 0, 0, theta, r.z*vth
457 };
458
459 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
460 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
461 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
462 double t0[3*5];
463 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
464
465 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
466 cvMatMul( &_t0, &_dvardR, &matJ );
467
468 // transpose every row of matJ (treat the rows as 3x3 matrices)
469 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
470 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
471 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
472 }
473
474 vth *= theta;
475 r *= vth;
476 }
477
478 if( depth == CV_32F )
479 {
480 dst->data.fl[0] = (float)r.x;
481 dst->data.fl[step] = (float)r.y;
482 dst->data.fl[step*2] = (float)r.z;
483 }
484 else
485 {
486 dst->data.db[0] = r.x;
487 dst->data.db[step] = r.y;
488 dst->data.db[step*2] = r.z;
489 }
490 }
491 else
492 {
493 CV_Error(CV_StsBadSize, "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix");
494 }
495
496 if( jacobian )
497 {
498 if( depth == CV_32F )
499 {
500 if( jacobian->rows == matJ.rows )
501 cvConvert( &matJ, jacobian );
502 else
503 {
504 float Jf[3*9];
505 CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf );
506 cvConvert( &matJ, &_Jf );
507 cvTranspose( &_Jf, jacobian );
508 }
509 }
510 else if( jacobian->rows == matJ.rows )
511 cvCopy( &matJ, jacobian );
512 else
513 cvTranspose( &matJ, jacobian );
514 }
515
516 return 1;
517 }
518
519
520 static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
521
522 static void cvProjectPoints2Internal( const CvMat* objectPoints,
523 const CvMat* r_vec,
524 const CvMat* t_vec,
525 const CvMat* A,
526 const CvMat* distCoeffs,
527 CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
528 CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
529 CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
530 CvMat* dpdo CV_DEFAULT(NULL),
531 double aspectRatio CV_DEFAULT(0) )
532 {
533 Ptr<CvMat> matM, _m;
534 Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
535 Ptr<CvMat> _dpdo;
536
537 int i, j, count;
538 int calc_derivatives;
539 const CvPoint3D64f* M;
540 CvPoint2D64f* m;
541 double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
542 Matx33d matTilt = Matx33d::eye();
543 Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
544 Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
545 CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
546 CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
547 double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
548 double* dpdo_p = 0;
549 int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
550 int dpdo_step = 0;
551 bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
552
553 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
554 !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
555 /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
556 CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
557
558 int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
559 if(total % 3 != 0)
560 {
561 //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
562 CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
563 }
564 count = total / 3;
565
566 if( CV_IS_CONT_MAT(objectPoints->type) &&
567 (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
568 ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
569 (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
570 (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
571 {
572 matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
573 cvConvert(objectPoints, matM);
574 }
575 else
576 {
577 // matM = cvCreateMat( 1, count, CV_64FC3 );
578 // cvConvertPointsHomogeneous( objectPoints, matM );
579 CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
580 }
581
582 if( CV_IS_CONT_MAT(imagePoints->type) &&
583 (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
584 ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
585 (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) ||
586 (imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
587 {
588 _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
589 cvConvert(imagePoints, _m);
590 }
591 else
592 {
593 // _m = cvCreateMat( 1, count, CV_64FC2 );
594 CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
595 }
596
597 M = (CvPoint3D64f*)matM->data.db;
598 m = (CvPoint2D64f*)_m->data.db;
599
600 if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
601 (((r_vec->rows != 1 && r_vec->cols != 1) ||
602 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
603 ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
604 CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
605 "floating-point rotation vector, or 3x3 rotation matrix" );
606
607 if( r_vec->rows == 3 && r_vec->cols == 3 )
608 {
609 _r = cvMat( 3, 1, CV_64FC1, r );
610 cvRodrigues2( r_vec, &_r );
611 cvRodrigues2( &_r, &matR, &_dRdr );
612 cvCopy( r_vec, &matR );
613 }
614 else
615 {
616 _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
617 cvConvert( r_vec, &_r );
618 cvRodrigues2( &_r, &matR, &_dRdr );
619 }
620
621 if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
622 (t_vec->rows != 1 && t_vec->cols != 1) ||
623 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
624 CV_Error( CV_StsBadArg,
625 "Translation vector must be 1x3 or 3x1 floating-point vector" );
626
627 _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
628 cvConvert( t_vec, &_t );
629
630 if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
631 A->rows != 3 || A->cols != 3 )
632 CV_Error( CV_StsBadArg, "Intrinsic parameters must be 3x3 floating-point matrix" );
633
634 cvConvert( A, &_a );
635 fx = a[0]; fy = a[4];
636 cx = a[2]; cy = a[5];
637
638 if( fixedAspectRatio )
639 fx = fy*aspectRatio;
640
641 if( distCoeffs )
642 {
643 if( !CV_IS_MAT(distCoeffs) ||
644 (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
645 CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
646 (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
647 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
648 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
649 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
650 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
651 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
652 CV_Error( CV_StsBadArg, cvDistCoeffErr );
653
654 _k = cvMat( distCoeffs->rows, distCoeffs->cols,
655 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
656 cvConvert( distCoeffs, &_k );
657 if(k[12] != 0 || k[13] != 0)
658 {
659 detail::computeTiltProjectionMatrix(k[12], k[13],
660 &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
661 }
662 }
663
664 if( dpdr )
665 {
666 if( !CV_IS_MAT(dpdr) ||
667 (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
668 CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
669 dpdr->rows != count*2 || dpdr->cols != 3 )
670 CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
671
672 if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
673 {
674 _dpdr.reset(cvCloneMat(dpdr));
675 }
676 else
677 _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
678 dpdr_p = _dpdr->data.db;
679 dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
680 }
681
682 if( dpdt )
683 {
684 if( !CV_IS_MAT(dpdt) ||
685 (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
686 CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
687 dpdt->rows != count*2 || dpdt->cols != 3 )
688 CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
689
690 if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
691 {
692 _dpdt.reset(cvCloneMat(dpdt));
693 }
694 else
695 _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
696 dpdt_p = _dpdt->data.db;
697 dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
698 }
699
700 if( dpdf )
701 {
702 if( !CV_IS_MAT(dpdf) ||
703 (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
704 dpdf->rows != count*2 || dpdf->cols != 2 )
705 CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
706
707 if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
708 {
709 _dpdf.reset(cvCloneMat(dpdf));
710 }
711 else
712 _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
713 dpdf_p = _dpdf->data.db;
714 dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
715 }
716
717 if( dpdc )
718 {
719 if( !CV_IS_MAT(dpdc) ||
720 (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
721 dpdc->rows != count*2 || dpdc->cols != 2 )
722 CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
723
724 if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
725 {
726 _dpdc.reset(cvCloneMat(dpdc));
727 }
728 else
729 _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
730 dpdc_p = _dpdc->data.db;
731 dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
732 }
733
734 if( dpdk )
735 {
736 if( !CV_IS_MAT(dpdk) ||
737 (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
738 dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
739 CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
740
741 if( !distCoeffs )
742 CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
743
744 if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
745 {
746 _dpdk.reset(cvCloneMat(dpdk));
747 }
748 else
749 _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
750 dpdk_p = _dpdk->data.db;
751 dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
752 }
753
754 if( dpdo )
755 {
756 if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
757 && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
758 || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
759 CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
760
761 if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
762 {
763 _dpdo.reset( cvCloneMat( dpdo ) );
764 }
765 else
766 _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
767 cvZero(_dpdo);
768 dpdo_p = _dpdo->data.db;
769 dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
770 }
771
772 calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
773
774 for( i = 0; i < count; i++ )
775 {
776 double X = M[i].x, Y = M[i].y, Z = M[i].z;
777 double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
778 double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
779 double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
780 double r2, r4, r6, a1, a2, a3, cdist, icdist2;
781 double xd, yd, xd0, yd0, invProj;
782 Vec3d vecTilt;
783 Vec3d dVecTilt;
784 Matx22d dMatTilt;
785 Vec2d dXdYd;
786
787 double z0 = z;
788 z = z ? 1./z : 1;
789 x *= z; y *= z;
790
791 r2 = x*x + y*y;
792 r4 = r2*r2;
793 r6 = r4*r2;
794 a1 = 2*x*y;
795 a2 = r2 + 2*x*x;
796 a3 = r2 + 2*y*y;
797 cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
798 icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
799 xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
800 yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
801
802 // additional distortion by projecting onto a tilt plane
803 vecTilt = matTilt*Vec3d(xd0, yd0, 1);
804 invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
805 xd = invProj * vecTilt(0);
806 yd = invProj * vecTilt(1);
807
808 m[i].x = xd*fx + cx;
809 m[i].y = yd*fy + cy;
810
811 if( calc_derivatives )
812 {
813 if( dpdc_p )
814 {
815 dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
816 dpdc_p[dpdc_step] = 0;
817 dpdc_p[dpdc_step+1] = 1;
818 dpdc_p += dpdc_step*2;
819 }
820
821 if( dpdf_p )
822 {
823 if( fixedAspectRatio )
824 {
825 dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
826 dpdf_p[dpdf_step] = 0;
827 dpdf_p[dpdf_step+1] = yd;
828 }
829 else
830 {
831 dpdf_p[0] = xd; dpdf_p[1] = 0;
832 dpdf_p[dpdf_step] = 0;
833 dpdf_p[dpdf_step+1] = yd;
834 }
835 dpdf_p += dpdf_step*2;
836 }
837 for (int row = 0; row < 2; ++row)
838 for (int col = 0; col < 2; ++col)
839 dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
840 - matTilt(2,col)*vecTilt(row);
841 double invProjSquare = (invProj*invProj);
842 dMatTilt *= invProjSquare;
843 if( dpdk_p )
844 {
845 dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
846 dpdk_p[0] = fx*dXdYd(0);
847 dpdk_p[dpdk_step] = fy*dXdYd(1);
848 dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
849 dpdk_p[1] = fx*dXdYd(0);
850 dpdk_p[dpdk_step+1] = fy*dXdYd(1);
851 if( _dpdk->cols > 2 )
852 {
853 dXdYd = dMatTilt*Vec2d(a1, a3);
854 dpdk_p[2] = fx*dXdYd(0);
855 dpdk_p[dpdk_step+2] = fy*dXdYd(1);
856 dXdYd = dMatTilt*Vec2d(a2, a1);
857 dpdk_p[3] = fx*dXdYd(0);
858 dpdk_p[dpdk_step+3] = fy*dXdYd(1);
859 if( _dpdk->cols > 4 )
860 {
861 dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
862 dpdk_p[4] = fx*dXdYd(0);
863 dpdk_p[dpdk_step+4] = fy*dXdYd(1);
864
865 if( _dpdk->cols > 5 )
866 {
867 dXdYd = dMatTilt*Vec2d(
868 x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
869 dpdk_p[5] = fx*dXdYd(0);
870 dpdk_p[dpdk_step+5] = fy*dXdYd(1);
871 dXdYd = dMatTilt*Vec2d(
872 x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
873 dpdk_p[6] = fx*dXdYd(0);
874 dpdk_p[dpdk_step+6] = fy*dXdYd(1);
875 dXdYd = dMatTilt*Vec2d(
876 x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
877 dpdk_p[7] = fx*dXdYd(0);
878 dpdk_p[dpdk_step+7] = fy*dXdYd(1);
879 if( _dpdk->cols > 8 )
880 {
881 dXdYd = dMatTilt*Vec2d(r2, 0);
882 dpdk_p[8] = fx*dXdYd(0); //s1
883 dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
884 dXdYd = dMatTilt*Vec2d(r4, 0);
885 dpdk_p[9] = fx*dXdYd(0); //s2
886 dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
887 dXdYd = dMatTilt*Vec2d(0, r2);
888 dpdk_p[10] = fx*dXdYd(0);//s3
889 dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
890 dXdYd = dMatTilt*Vec2d(0, r4);
891 dpdk_p[11] = fx*dXdYd(0);//s4
892 dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
893 if( _dpdk->cols > 12 )
894 {
895 dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
896 dpdk_p[12] = fx * invProjSquare * (
897 dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
898 dpdk_p[dpdk_step+12] = fy*invProjSquare * (
899 dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
900 dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
901 dpdk_p[13] = fx * invProjSquare * (
902 dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
903 dpdk_p[dpdk_step+13] = fy * invProjSquare * (
904 dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
905 }
906 }
907 }
908 }
909 }
910 dpdk_p += dpdk_step*2;
911 }
912
913 if( dpdt_p )
914 {
915 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
916 for( j = 0; j < 3; j++ )
917 {
918 double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
919 double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
920 double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
921 double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
922 double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
923 k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
924 double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
925 k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
926 dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
927 dpdt_p[j] = fx*dXdYd(0);
928 dpdt_p[dpdt_step+j] = fy*dXdYd(1);
929 }
930 dpdt_p += dpdt_step*2;
931 }
932
933 if( dpdr_p )
934 {
935 double dx0dr[] =
936 {
937 X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
938 X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
939 X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
940 };
941 double dy0dr[] =
942 {
943 X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
944 X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
945 X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
946 };
947 double dz0dr[] =
948 {
949 X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
950 X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
951 X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
952 };
953 for( j = 0; j < 3; j++ )
954 {
955 double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
956 double dydr = z*(dy0dr[j] - y*dz0dr[j]);
957 double dr2dr = 2*x*dxdr + 2*y*dydr;
958 double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
959 double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
960 double da1dr = 2*(x*dydr + y*dxdr);
961 double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
962 k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
963 double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
964 k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
965 dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
966 dpdr_p[j] = fx*dXdYd(0);
967 dpdr_p[dpdr_step+j] = fy*dXdYd(1);
968 }
969 dpdr_p += dpdr_step*2;
970 }
971
972 if( dpdo_p )
973 {
974 double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
975 z * ( R[1] - x * z * z0 * R[7] ),
976 z * ( R[2] - x * z * z0 * R[8] ) };
977 double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
978 z * ( R[4] - y * z * z0 * R[7] ),
979 z * ( R[5] - y * z * z0 * R[8] ) };
980 for( j = 0; j < 3; j++ )
981 {
982 double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
983 double dr4do = 2 * r2 * dr2do;
984 double dr6do = 3 * r4 * dr2do;
985 double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
986 double da2do = dr2do + 4 * x * dxdo[j];
987 double da3do = dr2do + 4 * y * dydo[j];
988 double dcdist_do
989 = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
990 double dicdist2_do = -icdist2 * icdist2
991 * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
992 double dxd0_do = cdist * icdist2 * dxdo[j]
993 + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
994 + k[2] * da1do + k[3] * da2do + k[8] * dr2do
995 + k[9] * dr4do;
996 double dyd0_do = cdist * icdist2 * dydo[j]
997 + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
998 + k[2] * da3do + k[3] * da1do + k[10] * dr2do
999 + k[11] * dr4do;
1000 dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
1001 dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
1002 dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
1003 }
1004 dpdo_p += dpdo_step * 2;
1005 }
1006 }
1007 }
1008
1009 if( _m != imagePoints )
1010 cvConvert( _m, imagePoints );
1011
1012 if( _dpdr != dpdr )
1013 cvConvert( _dpdr, dpdr );
1014
1015 if( _dpdt != dpdt )
1016 cvConvert( _dpdt, dpdt );
1017
1018 if( _dpdf != dpdf )
1019 cvConvert( _dpdf, dpdf );
1020
1021 if( _dpdc != dpdc )
1022 cvConvert( _dpdc, dpdc );
1023
1024 if( _dpdk != dpdk )
1025 cvConvert( _dpdk, dpdk );
1026
1027 if( _dpdo != dpdo )
1028 cvConvert( _dpdo, dpdo );
1029 }
1030
cvProjectPoints2(const CvMat * objectPoints,const CvMat * r_vec,const CvMat * t_vec,const CvMat * A,const CvMat * distCoeffs,CvMat * imagePoints,CvMat * dpdr,CvMat * dpdt,CvMat * dpdf,CvMat * dpdc,CvMat * dpdk,double aspectRatio)1031 CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
1032 const CvMat* r_vec,
1033 const CvMat* t_vec,
1034 const CvMat* A,
1035 const CvMat* distCoeffs,
1036 CvMat* imagePoints, CvMat* dpdr,
1037 CvMat* dpdt, CvMat* dpdf,
1038 CvMat* dpdc, CvMat* dpdk,
1039 double aspectRatio )
1040 {
1041 cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
1042 dpdf, dpdc, dpdk, NULL, aspectRatio );
1043 }
1044
cvFindExtrinsicCameraParams2(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * A,const CvMat * distCoeffs,CvMat * rvec,CvMat * tvec,int useExtrinsicGuess)1045 CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
1046 const CvMat* imagePoints, const CvMat* A,
1047 const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec,
1048 int useExtrinsicGuess )
1049 {
1050 const int max_iter = 20;
1051 Ptr<CvMat> matM, _Mxy, _m, _mn, matL;
1052
1053 int i, count;
1054 double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];
1055 double MM[9] = { 0 }, U[9] = { 0 }, V[9] = { 0 }, W[3] = { 0 };
1056 cv::Scalar Mc;
1057 double param[6] = { 0 };
1058 CvMat matA = cvMat( 3, 3, CV_64F, a );
1059 CvMat _Ar = cvMat( 3, 3, CV_64F, ar );
1060 CvMat matR = cvMat( 3, 3, CV_64F, R );
1061 CvMat _r = cvMat( 3, 1, CV_64F, param );
1062 CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
1063 CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val );
1064 CvMat _MM = cvMat( 3, 3, CV_64F, MM );
1065 CvMat matU = cvMat( 3, 3, CV_64F, U );
1066 CvMat matV = cvMat( 3, 3, CV_64F, V );
1067 CvMat matW = cvMat( 3, 1, CV_64F, W );
1068 CvMat _param = cvMat( 6, 1, CV_64F, param );
1069 CvMat _dpdr, _dpdt;
1070
1071 CV_Assert( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
1072 CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );
1073
1074 count = MAX(objectPoints->cols, objectPoints->rows);
1075 matM.reset(cvCreateMat( 1, count, CV_64FC3 ));
1076 _m.reset(cvCreateMat( 1, count, CV_64FC2 ));
1077
1078 cvConvertPointsHomogeneous( objectPoints, matM );
1079 cvConvertPointsHomogeneous( imagePoints, _m );
1080 cvConvert( A, &matA );
1081
1082 CV_Assert( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) &&
1083 (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 );
1084
1085 CV_Assert( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) &&
1086 (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 );
1087
1088 CV_Assert((count >= 4) || (count == 3 && useExtrinsicGuess)); // it is unsafe to call LM optimisation without an extrinsic guess in the case of 3 points. This is because there is no guarantee that it will converge on the correct solution.
1089
1090 _mn.reset(cvCreateMat( 1, count, CV_64FC2 ));
1091 _Mxy.reset(cvCreateMat( 1, count, CV_64FC2 ));
1092
1093 // normalize image points
1094 // (unapply the intrinsic matrix transformation and distortion)
1095 cvUndistortPoints( _m, _mn, &matA, distCoeffs, 0, &_Ar );
1096
1097 if( useExtrinsicGuess )
1098 {
1099 CvMat _r_temp = cvMat(rvec->rows, rvec->cols,
1100 CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1101 CvMat _t_temp = cvMat(tvec->rows, tvec->cols,
1102 CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3);
1103 cvConvert( rvec, &_r_temp );
1104 cvConvert( tvec, &_t_temp );
1105 }
1106 else
1107 {
1108 Mc = cvAvg(matM);
1109 cvReshape( matM, matM, 1, count );
1110 cvMulTransposed( matM, &_MM, 1, &_Mc );
1111 cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1112
1113 // initialize extrinsic parameters
1114 if( W[2]/W[1] < 1e-3)
1115 {
1116 // a planar structure case (all M's lie in the same plane)
1117 double tt[3], h[9], h1_norm, h2_norm;
1118 CvMat* R_transform = &matV;
1119 CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
1120 CvMat matH = cvMat( 3, 3, CV_64F, h );
1121 CvMat _h1, _h2, _h3;
1122
1123 if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
1124 cvSetIdentity( R_transform );
1125
1126 if( cvDet(R_transform) < 0 )
1127 cvScale( R_transform, R_transform, -1 );
1128
1129 cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
1130
1131 for( i = 0; i < count; i++ )
1132 {
1133 const double* Rp = R_transform->data.db;
1134 const double* Tp = T_transform.data.db;
1135 const double* src = matM->data.db + i*3;
1136 double* dst = _Mxy->data.db + i*2;
1137
1138 dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
1139 dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
1140 }
1141
1142 cvFindHomography( _Mxy, _mn, &matH );
1143
1144 if( cvCheckArr(&matH, CV_CHECK_QUIET) )
1145 {
1146 cvGetCol( &matH, &_h1, 0 );
1147 _h2 = _h1; _h2.data.db++;
1148 _h3 = _h2; _h3.data.db++;
1149 h1_norm = std::sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
1150 h2_norm = std::sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
1151
1152 cvScale( &_h1, &_h1, 1./MAX(h1_norm, DBL_EPSILON) );
1153 cvScale( &_h2, &_h2, 1./MAX(h2_norm, DBL_EPSILON) );
1154 cvScale( &_h3, &_t, 2./MAX(h1_norm + h2_norm, DBL_EPSILON));
1155 cvCrossProduct( &_h1, &_h2, &_h3 );
1156
1157 cvRodrigues2( &matH, &_r );
1158 cvRodrigues2( &_r, &matH );
1159 cvMatMulAdd( &matH, &T_transform, &_t, &_t );
1160 cvMatMul( &matH, R_transform, &matR );
1161 }
1162 else
1163 {
1164 cvSetIdentity( &matR );
1165 cvZero( &_t );
1166 }
1167
1168 cvRodrigues2( &matR, &_r );
1169 }
1170 else
1171 {
1172 // non-planar structure. Use DLT method
1173 CV_CheckGE(count, 6, "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences.");
1174 double* L;
1175 double LL[12*12], LW[12], LV[12*12], sc;
1176 CvMat _LL = cvMat( 12, 12, CV_64F, LL );
1177 CvMat _LW = cvMat( 12, 1, CV_64F, LW );
1178 CvMat _LV = cvMat( 12, 12, CV_64F, LV );
1179 CvMat _RRt, _RR, _tt;
1180 CvPoint3D64f* M = (CvPoint3D64f*)matM->data.db;
1181 CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db;
1182
1183 matL.reset(cvCreateMat( 2*count, 12, CV_64F ));
1184 L = matL->data.db;
1185
1186 for( i = 0; i < count; i++, L += 24 )
1187 {
1188 double x = -mn[i].x, y = -mn[i].y;
1189 L[0] = L[16] = M[i].x;
1190 L[1] = L[17] = M[i].y;
1191 L[2] = L[18] = M[i].z;
1192 L[3] = L[19] = 1.;
1193 L[4] = L[5] = L[6] = L[7] = 0.;
1194 L[12] = L[13] = L[14] = L[15] = 0.;
1195 L[8] = x*M[i].x;
1196 L[9] = x*M[i].y;
1197 L[10] = x*M[i].z;
1198 L[11] = x;
1199 L[20] = y*M[i].x;
1200 L[21] = y*M[i].y;
1201 L[22] = y*M[i].z;
1202 L[23] = y;
1203 }
1204
1205 cvMulTransposed( matL, &_LL, 1 );
1206 cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1207 _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
1208 cvGetCols( &_RRt, &_RR, 0, 3 );
1209 cvGetCol( &_RRt, &_tt, 3 );
1210 if( cvDet(&_RR) < 0 )
1211 cvScale( &_RRt, &_RRt, -1 );
1212 sc = cvNorm(&_RR);
1213 CV_Assert(fabs(sc) > DBL_EPSILON);
1214 cvSVD( &_RR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
1215 cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
1216 cvScale( &_tt, &_t, cvNorm(&matR)/sc );
1217 cvRodrigues2( &matR, &_r );
1218 }
1219 }
1220
1221 cvReshape( matM, matM, 3, 1 );
1222 cvReshape( _mn, _mn, 2, 1 );
1223
1224 // refine extrinsic parameters using iterative algorithm
1225 CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
1226 cvCopy( &_param, solver.param );
1227
1228 for(;;)
1229 {
1230 CvMat *matJ = 0, *_err = 0;
1231 const CvMat *__param = 0;
1232 bool proceed = solver.update( __param, matJ, _err );
1233 cvCopy( __param, &_param );
1234 if( !proceed || !_err )
1235 break;
1236 cvReshape( _err, _err, 2, 1 );
1237 if( matJ )
1238 {
1239 cvGetCols( matJ, &_dpdr, 0, 3 );
1240 cvGetCols( matJ, &_dpdt, 3, 6 );
1241 cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
1242 _err, &_dpdr, &_dpdt, 0, 0, 0 );
1243 }
1244 else
1245 {
1246 cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
1247 _err, 0, 0, 0, 0, 0 );
1248 }
1249 cvSub(_err, _m, _err);
1250 cvReshape( _err, _err, 1, 2*count );
1251 }
1252 cvCopy( solver.param, &_param );
1253
1254 _r = cvMat( rvec->rows, rvec->cols,
1255 CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1256 _t = cvMat( tvec->rows, tvec->cols,
1257 CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );
1258
1259 cvConvert( &_r, rvec );
1260 cvConvert( &_t, tvec );
1261 }
1262
cvInitIntrinsicParams2D(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * npoints,CvSize imageSize,CvMat * cameraMatrix,double aspectRatio)1263 CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints,
1264 const CvMat* imagePoints, const CvMat* npoints,
1265 CvSize imageSize, CvMat* cameraMatrix,
1266 double aspectRatio )
1267 {
1268 Ptr<CvMat> matA, _b, _allH;
1269
1270 int i, j, pos, nimages, ni = 0;
1271 double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
1272 double H[9] = {0}, f[2] = {0};
1273 CvMat _a = cvMat( 3, 3, CV_64F, a );
1274 CvMat matH = cvMat( 3, 3, CV_64F, H );
1275 CvMat _f = cvMat( 2, 1, CV_64F, f );
1276
1277 CV_Assert(npoints);
1278 CV_Assert(CV_MAT_TYPE(npoints->type) == CV_32SC1);
1279 CV_Assert(CV_IS_MAT_CONT(npoints->type));
1280 nimages = npoints->rows + npoints->cols - 1;
1281
1282 if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 &&
1283 CV_MAT_TYPE(objectPoints->type) != CV_64FC3) ||
1284 (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 &&
1285 CV_MAT_TYPE(imagePoints->type) != CV_64FC2) )
1286 CV_Error( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" );
1287
1288 if( objectPoints->rows != 1 || imagePoints->rows != 1 )
1289 CV_Error( CV_StsBadSize, "object points and image points must be a single-row matrices" );
1290
1291 matA.reset(cvCreateMat( 2*nimages, 2, CV_64F ));
1292 _b.reset(cvCreateMat( 2*nimages, 1, CV_64F ));
1293 a[2] = (!imageSize.width) ? 0.5 : (imageSize.width - 1)*0.5;
1294 a[5] = (!imageSize.height) ? 0.5 : (imageSize.height - 1)*0.5;
1295 _allH.reset(cvCreateMat( nimages, 9, CV_64F ));
1296
1297 // extract vanishing points in order to obtain initial value for the focal length
1298 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1299 {
1300 CV_DbgAssert(npoints->data.i);
1301 CV_DbgAssert(matA && matA->data.db);
1302 CV_DbgAssert(_b && _b->data.db);
1303 double* Ap = matA->data.db + i*4;
1304 double* bp = _b->data.db + i*2;
1305 ni = npoints->data.i[i];
1306 double h[3], v[3], d1[3], d2[3];
1307 double n[4] = {0,0,0,0};
1308 CvMat _m, matM;
1309 cvGetCols( objectPoints, &matM, pos, pos + ni );
1310 cvGetCols( imagePoints, &_m, pos, pos + ni );
1311
1312 cvFindHomography( &matM, &_m, &matH );
1313 CV_DbgAssert(_allH && _allH->data.db);
1314 memcpy( _allH->data.db + i*9, H, sizeof(H) );
1315
1316 H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
1317 H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
1318
1319 for( j = 0; j < 3; j++ )
1320 {
1321 double t0 = H[j*3], t1 = H[j*3+1];
1322 h[j] = t0; v[j] = t1;
1323 d1[j] = (t0 + t1)*0.5;
1324 d2[j] = (t0 - t1)*0.5;
1325 n[0] += t0*t0; n[1] += t1*t1;
1326 n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
1327 }
1328
1329 for( j = 0; j < 4; j++ )
1330 n[j] = 1./std::sqrt(n[j]);
1331
1332 for( j = 0; j < 3; j++ )
1333 {
1334 h[j] *= n[0]; v[j] *= n[1];
1335 d1[j] *= n[2]; d2[j] *= n[3];
1336 }
1337
1338 Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
1339 Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
1340 bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
1341 }
1342
1343 cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
1344 a[0] = std::sqrt(fabs(1./f[0]));
1345 a[4] = std::sqrt(fabs(1./f[1]));
1346 if( aspectRatio != 0 )
1347 {
1348 double tf = (a[0] + a[4])/(aspectRatio + 1.);
1349 a[0] = aspectRatio*tf;
1350 a[4] = tf;
1351 }
1352
1353 cvConvert( &_a, cameraMatrix );
1354 }
1355
subMatrix(const cv::Mat & src,cv::Mat & dst,const std::vector<uchar> & cols,const std::vector<uchar> & rows)1356 static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
1357 const std::vector<uchar>& rows) {
1358 int nonzeros_cols = cv::countNonZero(cols);
1359 cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
1360
1361 for (int i = 0, j = 0; i < (int)cols.size(); i++)
1362 {
1363 if (cols[i])
1364 {
1365 src.col(i).copyTo(tmp.col(j++));
1366 }
1367 }
1368
1369 int nonzeros_rows = cv::countNonZero(rows);
1370 dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
1371 for (int i = 0, j = 0; i < (int)rows.size(); i++)
1372 {
1373 if (rows[i])
1374 {
1375 tmp.row(i).copyTo(dst.row(j++));
1376 }
1377 }
1378 }
1379
cvCalibrateCamera2Internal(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * npoints,CvSize imageSize,int iFixedPoint,CvMat * cameraMatrix,CvMat * distCoeffs,CvMat * rvecs,CvMat * tvecs,CvMat * newObjPoints,CvMat * stdDevs,CvMat * perViewErrors,int flags,CvTermCriteria termCrit)1380 static double cvCalibrateCamera2Internal( const CvMat* objectPoints,
1381 const CvMat* imagePoints, const CvMat* npoints,
1382 CvSize imageSize, int iFixedPoint, CvMat* cameraMatrix, CvMat* distCoeffs,
1383 CvMat* rvecs, CvMat* tvecs, CvMat* newObjPoints, CvMat* stdDevs,
1384 CvMat* perViewErrors, int flags, CvTermCriteria termCrit )
1385 {
1386 const int NINTRINSIC = CV_CALIB_NINTRINSIC;
1387 double reprojErr = 0;
1388
1389 Matx33d A;
1390 double k[14] = {0};
1391 CvMat matA = cvMat(3, 3, CV_64F, A.val), _k;
1392 int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
1393 double aspectRatio = 0.;
1394
1395 // 0. check the parameters & allocate buffers
1396 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
1397 !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
1398 CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
1399
1400 if( imageSize.width <= 0 || imageSize.height <= 0 )
1401 CV_Error( CV_StsOutOfRange, "image width and height must be positive" );
1402
1403 if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
1404 (npoints->rows != 1 && npoints->cols != 1) )
1405 CV_Error( CV_StsUnsupportedFormat,
1406 "the array of point counters must be 1-dimensional integer vector" );
1407 if(flags & CALIB_TILTED_MODEL)
1408 {
1409 //when the tilted sensor model is used the distortion coefficients matrix must have 14 parameters
1410 if (distCoeffs->cols*distCoeffs->rows != 14)
1411 CV_Error( CV_StsBadArg, "The tilted sensor model must have 14 parameters in the distortion matrix" );
1412 }
1413 else
1414 {
1415 //when the thin prism model is used the distortion coefficients matrix must have 12 parameters
1416 if(flags & CALIB_THIN_PRISM_MODEL)
1417 if (distCoeffs->cols*distCoeffs->rows != 12)
1418 CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" );
1419 }
1420
1421 nimages = npoints->rows*npoints->cols;
1422 npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
1423
1424 if( rvecs )
1425 {
1426 cn = CV_MAT_CN(rvecs->type);
1427 if( !CV_IS_MAT(rvecs) ||
1428 (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
1429 ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
1430 (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
1431 CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
1432 "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
1433 }
1434
1435 if( tvecs )
1436 {
1437 cn = CV_MAT_CN(tvecs->type);
1438 if( !CV_IS_MAT(tvecs) ||
1439 (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
1440 ((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
1441 (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
1442 CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
1443 "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
1444 }
1445
1446 bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints->data.i[0] - 1;
1447
1448 if( stdDevs && !releaseObject )
1449 {
1450 cn = CV_MAT_CN(stdDevs->type);
1451 if( !CV_IS_MAT(stdDevs) ||
1452 (CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) ||
1453 ((stdDevs->rows != (nimages*6 + NINTRINSIC) || stdDevs->cols*cn != 1) &&
1454 (stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC) || cn != 1)) )
1455 #define STR__(x) #x
1456 #define STR_(x) STR__(x)
1457 CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel "
1458 "1x(n*6 + NINTRINSIC) or (n*6 + NINTRINSIC)x1 array, where n is the number of views,"
1459 " NINTRINSIC = " STR_(CV_CALIB_NINTRINSIC));
1460 }
1461
1462 if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
1463 CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
1464 cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
1465 CV_Error( CV_StsBadArg,
1466 "Intrinsic parameters must be 3x3 floating-point matrix" );
1467
1468 if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
1469 CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
1470 (distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
1471 (distCoeffs->cols*distCoeffs->rows != 4 &&
1472 distCoeffs->cols*distCoeffs->rows != 5 &&
1473 distCoeffs->cols*distCoeffs->rows != 8 &&
1474 distCoeffs->cols*distCoeffs->rows != 12 &&
1475 distCoeffs->cols*distCoeffs->rows != 14) )
1476 CV_Error( CV_StsBadArg, cvDistCoeffErr );
1477
1478 for( i = 0; i < nimages; i++ )
1479 {
1480 ni = npoints->data.i[i*npstep];
1481 if( ni < 4 )
1482 {
1483 CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i));
1484 }
1485 maxPoints = MAX( maxPoints, ni );
1486 total += ni;
1487 }
1488
1489 if( newObjPoints )
1490 {
1491 cn = CV_MAT_CN(newObjPoints->type);
1492 if( !CV_IS_MAT(newObjPoints) ||
1493 (CV_MAT_DEPTH(newObjPoints->type) != CV_32F && CV_MAT_DEPTH(newObjPoints->type) != CV_64F) ||
1494 ((newObjPoints->rows != maxPoints || newObjPoints->cols*cn != 3) &&
1495 (newObjPoints->rows != 1 || newObjPoints->cols != maxPoints || cn != 3)) )
1496 CV_Error( CV_StsBadArg, "the output array of refined object points must be 3-channel "
1497 "1xn or nx1 array or 1-channel nx3 array, where n is the number of object points per view" );
1498 }
1499
1500 if( stdDevs && releaseObject )
1501 {
1502 cn = CV_MAT_CN(stdDevs->type);
1503 if( !CV_IS_MAT(stdDevs) ||
1504 (CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) ||
1505 ((stdDevs->rows != (nimages*6 + NINTRINSIC + maxPoints*3) || stdDevs->cols*cn != 1) &&
1506 (stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC + maxPoints*3) || cn != 1)) )
1507 CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel "
1508 "1x(n*6 + NINTRINSIC + m*3) or (n*6 + NINTRINSIC + m*3)x1 array, where n is the number of views,"
1509 " NINTRINSIC = " STR_(CV_CALIB_NINTRINSIC) ", m is the number of object points per view");
1510 }
1511
1512 Mat matM( 1, total, CV_64FC3 );
1513 Mat _m( 1, total, CV_64FC2 );
1514 Mat allErrors(1, total, CV_64FC2);
1515
1516 if(CV_MAT_CN(objectPoints->type) == 3) {
1517 cvarrToMat(objectPoints).convertTo(matM, CV_64F);
1518 } else {
1519 convertPointsHomogeneous(cvarrToMat(objectPoints), matM);
1520 }
1521
1522 if(CV_MAT_CN(imagePoints->type) == 2) {
1523 cvarrToMat(imagePoints).convertTo(_m, CV_64F);
1524 } else {
1525 convertPointsHomogeneous(cvarrToMat(imagePoints), _m);
1526 }
1527
1528 nparams = NINTRINSIC + nimages*6;
1529 if( releaseObject )
1530 nparams += maxPoints * 3;
1531
1532 _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
1533 if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 8 )
1534 {
1535 if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 5 )
1536 flags |= CALIB_FIX_K3;
1537 flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6;
1538 }
1539 const double minValidAspectRatio = 0.01;
1540 const double maxValidAspectRatio = 100.0;
1541
1542 // 1. initialize intrinsic parameters & LM solver
1543 if( flags & CALIB_USE_INTRINSIC_GUESS )
1544 {
1545 cvConvert( cameraMatrix, &matA );
1546 if( A(0, 0) <= 0 || A(1, 1) <= 0 )
1547 CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
1548 if( A(0, 2) < 0 || A(0, 2) >= imageSize.width ||
1549 A(1, 2) < 0 || A(1, 2) >= imageSize.height )
1550 CV_Error( CV_StsOutOfRange, "Principal point must be within the image" );
1551 if( fabs(A(0, 1)) > 1e-5 )
1552 CV_Error( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
1553 if( fabs(A(1, 0)) > 1e-5 || fabs(A(2, 0)) > 1e-5 ||
1554 fabs(A(2, 1)) > 1e-5 || fabs(A(2,2)-1) > 1e-5 )
1555 CV_Error( CV_StsOutOfRange,
1556 "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
1557 A(0, 1) = A(1, 0) = A(2, 0) = A(2, 1) = 0.;
1558 A(2, 2) = 1.;
1559
1560 if( flags & CALIB_FIX_ASPECT_RATIO )
1561 {
1562 aspectRatio = A(0, 0)/A(1, 1);
1563
1564 if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio )
1565 CV_Error( CV_StsOutOfRange,
1566 "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" );
1567 }
1568 cvConvert( distCoeffs, &_k );
1569 }
1570 else
1571 {
1572 Scalar mean, sdv;
1573 meanStdDev(matM, mean, sdv);
1574 if( fabs(mean[2]) > 1e-5 || fabs(sdv[2]) > 1e-5 )
1575 CV_Error( CV_StsBadArg,
1576 "For non-planar calibration rigs the initial intrinsic matrix must be specified" );
1577 for( i = 0; i < total; i++ )
1578 matM.at<Point3d>(i).z = 0.;
1579
1580 if( flags & CALIB_FIX_ASPECT_RATIO )
1581 {
1582 aspectRatio = cvmGet(cameraMatrix,0,0);
1583 aspectRatio /= cvmGet(cameraMatrix,1,1);
1584 if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio )
1585 CV_Error( CV_StsOutOfRange,
1586 "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" );
1587 }
1588 CvMat _matM = cvMat(matM), m = cvMat(_m);
1589 cvInitIntrinsicParams2D( &_matM, &m, npoints, imageSize, &matA, aspectRatio );
1590 }
1591
1592 CvLevMarq solver( nparams, 0, termCrit );
1593
1594 Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0));
1595 Mat _Je( maxPoints*2, 6, CV_64FC1 );
1596 Mat _err( maxPoints*2, 1, CV_64FC1 );
1597
1598 const bool allocJo = (solver.state == CvLevMarq::CALC_J) || stdDevs || releaseObject;
1599 Mat _Jo = allocJo ? Mat( maxPoints*2, maxPoints*3, CV_64FC1, Scalar(0) ) : Mat();
1600
1601 if(flags & CALIB_USE_LU) {
1602 solver.solveMethod = DECOMP_LU;
1603 }
1604 else if(flags & CALIB_USE_QR) {
1605 solver.solveMethod = DECOMP_QR;
1606 }
1607
1608 {
1609 double* param = solver.param->data.db;
1610 uchar* mask = solver.mask->data.ptr;
1611
1612 param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2);
1613 std::copy(k, k + 14, param + 4);
1614
1615 if(flags & CALIB_FIX_ASPECT_RATIO)
1616 mask[0] = 0;
1617 if( flags & CALIB_FIX_FOCAL_LENGTH )
1618 mask[0] = mask[1] = 0;
1619 if( flags & CALIB_FIX_PRINCIPAL_POINT )
1620 mask[2] = mask[3] = 0;
1621 if( flags & CALIB_ZERO_TANGENT_DIST )
1622 {
1623 param[6] = param[7] = 0;
1624 mask[6] = mask[7] = 0;
1625 }
1626 if( !(flags & CALIB_RATIONAL_MODEL) )
1627 flags |= CALIB_FIX_K4 + CALIB_FIX_K5 + CALIB_FIX_K6;
1628 if( !(flags & CALIB_THIN_PRISM_MODEL))
1629 flags |= CALIB_FIX_S1_S2_S3_S4;
1630 if( !(flags & CALIB_TILTED_MODEL))
1631 flags |= CALIB_FIX_TAUX_TAUY;
1632
1633 mask[ 4] = !(flags & CALIB_FIX_K1);
1634 mask[ 5] = !(flags & CALIB_FIX_K2);
1635 if( flags & CALIB_FIX_TANGENT_DIST )
1636 {
1637 mask[6] = mask[7] = 0;
1638 }
1639 mask[ 8] = !(flags & CALIB_FIX_K3);
1640 mask[ 9] = !(flags & CALIB_FIX_K4);
1641 mask[10] = !(flags & CALIB_FIX_K5);
1642 mask[11] = !(flags & CALIB_FIX_K6);
1643
1644 if(flags & CALIB_FIX_S1_S2_S3_S4)
1645 {
1646 mask[12] = 0;
1647 mask[13] = 0;
1648 mask[14] = 0;
1649 mask[15] = 0;
1650 }
1651 if(flags & CALIB_FIX_TAUX_TAUY)
1652 {
1653 mask[16] = 0;
1654 mask[17] = 0;
1655 }
1656
1657 if(releaseObject)
1658 {
1659 // copy object points
1660 std::copy( matM.ptr<double>(), matM.ptr<double>( 0, maxPoints - 1 ) + 3,
1661 param + NINTRINSIC + nimages * 6 );
1662 // fix points
1663 mask[NINTRINSIC + nimages * 6] = 0;
1664 mask[NINTRINSIC + nimages * 6 + 1] = 0;
1665 mask[NINTRINSIC + nimages * 6 + 2] = 0;
1666 mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3] = 0;
1667 mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3 + 1] = 0;
1668 mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3 + 2] = 0;
1669 mask[nparams - 1] = 0;
1670 }
1671 }
1672
1673 // 2. initialize extrinsic parameters
1674 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1675 {
1676 CvMat _ri, _ti;
1677 ni = npoints->data.i[i*npstep];
1678
1679 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1680 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1681
1682 CvMat _Mi = cvMat(matM.colRange(pos, pos + ni));
1683 CvMat _mi = cvMat(_m.colRange(pos, pos + ni));
1684
1685 cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti );
1686 }
1687
1688 // 3. run the optimization
1689 for(;;)
1690 {
1691 const CvMat* _param = 0;
1692 CvMat *_JtJ = 0, *_JtErr = 0;
1693 double* _errNorm = 0;
1694 bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
1695 double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
1696 bool calcJ = solver.state == CvLevMarq::CALC_J || (!proceed && stdDevs);
1697
1698 if( flags & CALIB_FIX_ASPECT_RATIO )
1699 {
1700 param[0] = param[1]*aspectRatio;
1701 pparam[0] = pparam[1]*aspectRatio;
1702 }
1703
1704 A(0, 0) = param[0]; A(1, 1) = param[1]; A(0, 2) = param[2]; A(1, 2) = param[3];
1705 std::copy(param + 4, param + 4 + 14, k);
1706
1707 if ( !proceed && !stdDevs && !perViewErrors )
1708 break;
1709 else if ( !proceed && stdDevs )
1710 cvZero(_JtJ);
1711
1712 reprojErr = 0;
1713
1714 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1715 {
1716 CvMat _ri, _ti;
1717 ni = npoints->data.i[i*npstep];
1718
1719 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1720 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1721
1722 CvMat _Mi = cvMat(matM.colRange(pos, pos + ni));
1723 if( releaseObject )
1724 {
1725 cvGetRows( solver.param, &_Mi, NINTRINSIC + nimages * 6,
1726 NINTRINSIC + nimages * 6 + ni * 3 );
1727 cvReshape( &_Mi, &_Mi, 3, 1 );
1728 }
1729 CvMat _mi = cvMat(_m.colRange(pos, pos + ni));
1730 CvMat _me = cvMat(allErrors.colRange(pos, pos + ni));
1731
1732 _Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2);
1733 _Jo.resize(ni*2);
1734
1735 CvMat _mp = cvMat(_err.reshape(2, 1));
1736
1737 if( calcJ )
1738 {
1739 CvMat _dpdr = cvMat(_Je.colRange(0, 3));
1740 CvMat _dpdt = cvMat(_Je.colRange(3, 6));
1741 CvMat _dpdf = cvMat(_Ji.colRange(0, 2));
1742 CvMat _dpdc = cvMat(_Ji.colRange(2, 4));
1743 CvMat _dpdk = cvMat(_Ji.colRange(4, NINTRINSIC));
1744 CvMat _dpdo = _Jo.empty() ? CvMat() : cvMat(_Jo.colRange(0, ni * 3));
1745
1746 cvProjectPoints2Internal( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt,
1747 (flags & CALIB_FIX_FOCAL_LENGTH) ? nullptr : &_dpdf,
1748 (flags & CALIB_FIX_PRINCIPAL_POINT) ? nullptr : &_dpdc, &_dpdk,
1749 (_Jo.empty()) ? nullptr: &_dpdo,
1750 (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
1751 }
1752 else
1753 cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp );
1754
1755 cvSub( &_mp, &_mi, &_mp );
1756 if (perViewErrors || stdDevs)
1757 cvCopy(&_mp, &_me);
1758
1759 if( calcJ )
1760 {
1761 Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr));
1762
1763 // see HZ: (A6.14) for details on the structure of the Jacobian
1764 JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += _Ji.t() * _Ji;
1765 JtJ(Rect(NINTRINSIC + i * 6, NINTRINSIC + i * 6, 6, 6)) = _Je.t() * _Je;
1766 JtJ(Rect(NINTRINSIC + i * 6, 0, 6, NINTRINSIC)) = _Ji.t() * _Je;
1767 if( releaseObject )
1768 {
1769 JtJ(Rect(NINTRINSIC + nimages * 6, 0, maxPoints * 3, NINTRINSIC)) += _Ji.t() * _Jo;
1770 JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + i * 6, maxPoints * 3, 6))
1771 += _Je.t() * _Jo;
1772 JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + nimages * 6, maxPoints * 3, maxPoints * 3))
1773 += _Jo.t() * _Jo;
1774 }
1775
1776 JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err;
1777 JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err;
1778 if( releaseObject )
1779 {
1780 JtErr.rowRange(NINTRINSIC + nimages * 6, nparams) += _Jo.t() * _err;
1781 }
1782 }
1783
1784 double viewErr = norm(_err, NORM_L2SQR);
1785
1786 if( perViewErrors )
1787 perViewErrors->data.db[i] = std::sqrt(viewErr / ni);
1788
1789 reprojErr += viewErr;
1790 }
1791 if( _errNorm )
1792 *_errNorm = reprojErr;
1793
1794 if( !proceed )
1795 {
1796 if( stdDevs )
1797 {
1798 Mat mask = cvarrToMat(solver.mask);
1799 int nparams_nz = countNonZero(mask);
1800 Mat JtJinv, JtJN;
1801 JtJN.create(nparams_nz, nparams_nz, CV_64F);
1802 subMatrix(cvarrToMat(_JtJ), JtJN, mask, mask);
1803 completeSymm(JtJN, false);
1804 cv::invert(JtJN, JtJinv, DECOMP_SVD);
1805 //sigma2 is deviation of the noise
1806 //see any papers about variance of the least squares estimator for
1807 //detailed description of the variance estimation methods
1808 double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz);
1809 Mat stdDevsM = cvarrToMat(stdDevs);
1810 int j = 0;
1811 for ( int s = 0; s < nparams; s++ )
1812 if( mask.data[s] )
1813 {
1814 stdDevsM.at<double>(s) = std::sqrt(JtJinv.at<double>(j,j) * sigma2);
1815 j++;
1816 }
1817 else
1818 stdDevsM.at<double>(s) = 0.;
1819 }
1820 break;
1821 }
1822 }
1823
1824 // 4. store the results
1825 cvConvert( &matA, cameraMatrix );
1826 cvConvert( &_k, distCoeffs );
1827 if( newObjPoints && releaseObject )
1828 {
1829 CvMat _Mi;
1830 cvGetRows( solver.param, &_Mi, NINTRINSIC + nimages * 6,
1831 NINTRINSIC + nimages * 6 + maxPoints * 3 );
1832 cvReshape( &_Mi, &_Mi, 3, 1 );
1833 cvConvert( &_Mi, newObjPoints );
1834 }
1835
1836 for( i = 0, pos = 0; i < nimages; i++ )
1837 {
1838 CvMat src, dst;
1839
1840 if( rvecs )
1841 {
1842 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
1843 if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
1844 {
1845 dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
1846 rvecs->data.ptr + rvecs->step*i );
1847 cvRodrigues2( &src, &matA );
1848 cvConvert( &matA, &dst );
1849 }
1850 else
1851 {
1852 dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
1853 rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
1854 rvecs->data.ptr + rvecs->step*i );
1855 cvConvert( &src, &dst );
1856 }
1857 }
1858 if( tvecs )
1859 {
1860 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
1861 dst = cvMat( 3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ?
1862 tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
1863 tvecs->data.ptr + tvecs->step*i );
1864 cvConvert( &src, &dst );
1865 }
1866 }
1867
1868 return std::sqrt(reprojErr/total);
1869 }
1870
1871
1872 /* finds intrinsic and extrinsic camera parameters
1873 from a few views of known calibration pattern */
cvCalibrateCamera2(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * npoints,CvSize imageSize,CvMat * cameraMatrix,CvMat * distCoeffs,CvMat * rvecs,CvMat * tvecs,int flags,CvTermCriteria termCrit)1874 CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
1875 const CvMat* imagePoints, const CvMat* npoints,
1876 CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
1877 CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit )
1878 {
1879 return cvCalibrateCamera2Internal(objectPoints, imagePoints, npoints, imageSize, -1, cameraMatrix,
1880 distCoeffs, rvecs, tvecs, NULL, NULL, NULL, flags, termCrit);
1881 }
1882
cvCalibrateCamera4(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * npoints,CvSize imageSize,int iFixedPoint,CvMat * cameraMatrix,CvMat * distCoeffs,CvMat * rvecs,CvMat * tvecs,CvMat * newObjPoints,int flags,CvTermCriteria termCrit)1883 CV_IMPL double cvCalibrateCamera4( const CvMat* objectPoints,
1884 const CvMat* imagePoints, const CvMat* npoints,
1885 CvSize imageSize, int iFixedPoint, CvMat* cameraMatrix, CvMat* distCoeffs,
1886 CvMat* rvecs, CvMat* tvecs, CvMat* newObjPoints, int flags, CvTermCriteria termCrit )
1887 {
1888 if( !CV_IS_MAT(npoints) )
1889 CV_Error( CV_StsBadArg, "npoints is not a valid matrix" );
1890 if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
1891 (npoints->rows != 1 && npoints->cols != 1) )
1892 CV_Error( CV_StsUnsupportedFormat,
1893 "the array of point counters must be 1-dimensional integer vector" );
1894
1895 bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints->data.i[0] - 1;
1896 int nimages = npoints->rows * npoints->cols;
1897 int npstep = npoints->rows == 1 ? 1 : npoints->step / CV_ELEM_SIZE(npoints->type);
1898 int i, ni;
1899 // check object points. If not qualified, report errors.
1900 if( releaseObject )
1901 {
1902 if( !CV_IS_MAT(objectPoints) )
1903 CV_Error( CV_StsBadArg, "objectPoints is not a valid matrix" );
1904 Mat matM;
1905 if(CV_MAT_CN(objectPoints->type) == 3) {
1906 matM = cvarrToMat(objectPoints);
1907 } else {
1908 convertPointsHomogeneous(cvarrToMat(objectPoints), matM);
1909 }
1910
1911 matM = matM.reshape(3, 1);
1912 ni = npoints->data.i[0];
1913 for( i = 1; i < nimages; i++ )
1914 {
1915 if( npoints->data.i[i * npstep] != ni )
1916 {
1917 CV_Error( CV_StsBadArg, "All objectPoints[i].size() should be equal when "
1918 "object-releasing method is requested." );
1919 }
1920 Mat ocmp = matM.colRange(ni * i, ni * i + ni) != matM.colRange(0, ni);
1921 ocmp = ocmp.reshape(1);
1922 if( countNonZero(ocmp) )
1923 {
1924 CV_Error( CV_StsBadArg, "All objectPoints[i] should be identical when object-releasing"
1925 " method is requested." );
1926 }
1927 }
1928 }
1929
1930 return cvCalibrateCamera2Internal(objectPoints, imagePoints, npoints, imageSize, iFixedPoint,
1931 cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints, NULL,
1932 NULL, flags, termCrit);
1933 }
1934
cvCalibrationMatrixValues(const CvMat * calibMatr,CvSize imgSize,double apertureWidth,double apertureHeight,double * fovx,double * fovy,double * focalLength,CvPoint2D64f * principalPoint,double * pasp)1935 void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize,
1936 double apertureWidth, double apertureHeight, double *fovx, double *fovy,
1937 double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
1938 {
1939 /* Validate parameters. */
1940 if(calibMatr == 0)
1941 CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
1942
1943 if(!CV_IS_MAT(calibMatr))
1944 CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
1945
1946 double dummy = .0;
1947 Point2d pp;
1948 cv::calibrationMatrixValues(cvarrToMat(calibMatr), imgSize, apertureWidth, apertureHeight,
1949 fovx ? *fovx : dummy,
1950 fovy ? *fovy : dummy,
1951 focalLength ? *focalLength : dummy,
1952 pp,
1953 pasp ? *pasp : dummy);
1954
1955 if(principalPoint)
1956 *principalPoint = cvPoint2D64f(pp.x, pp.y);
1957 }
1958
1959
1960 //////////////////////////////// Stereo Calibration ///////////////////////////////////
1961
dbCmp(const void * _a,const void * _b)1962 static int dbCmp( const void* _a, const void* _b )
1963 {
1964 double a = *(const double*)_a;
1965 double b = *(const double*)_b;
1966
1967 return (a > b) - (a < b);
1968 }
1969
cvStereoCalibrateImpl(const CvMat * _objectPoints,const CvMat * _imagePoints1,const CvMat * _imagePoints2,const CvMat * _npoints,CvMat * _cameraMatrix1,CvMat * _distCoeffs1,CvMat * _cameraMatrix2,CvMat * _distCoeffs2,CvSize imageSize,CvMat * matR,CvMat * matT,CvMat * matE,CvMat * matF,CvMat * perViewErr,int flags,CvTermCriteria termCrit)1970 static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _imagePoints1,
1971 const CvMat* _imagePoints2, const CvMat* _npoints,
1972 CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
1973 CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
1974 CvSize imageSize, CvMat* matR, CvMat* matT,
1975 CvMat* matE, CvMat* matF,
1976 CvMat* perViewErr, int flags,
1977 CvTermCriteria termCrit )
1978 {
1979 const int NINTRINSIC = 18;
1980 Ptr<CvMat> npoints, imagePoints[2], objectPoints, RT0;
1981 double reprojErr = 0;
1982
1983 double A[2][9], dk[2][14]={{0}}, rlr[9];
1984 CvMat K[2], Dist[2], om_LR, T_LR;
1985 CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
1986 int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
1987 int nparams;
1988 bool recomputeIntrinsics = false;
1989 double aspectRatio[2] = {0};
1990
1991 CV_Assert( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) &&
1992 CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) &&
1993 CV_IS_MAT(matR) && CV_IS_MAT(matT) );
1994
1995 CV_Assert( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) &&
1996 CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) );
1997
1998 CV_Assert( (_npoints->cols == 1 || _npoints->rows == 1) &&
1999 CV_MAT_TYPE(_npoints->type) == CV_32SC1 );
2000
2001 nimages = _npoints->cols + _npoints->rows - 1;
2002 npoints.reset(cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type ));
2003 cvCopy( _npoints, npoints );
2004
2005 for( i = 0, pointsTotal = 0; i < nimages; i++ )
2006 {
2007 maxPoints = MAX(maxPoints, npoints->data.i[i]);
2008 pointsTotal += npoints->data.i[i];
2009 }
2010
2011 objectPoints.reset(cvCreateMat( _objectPoints->rows, _objectPoints->cols,
2012 CV_64FC(CV_MAT_CN(_objectPoints->type))));
2013 cvConvert( _objectPoints, objectPoints );
2014 cvReshape( objectPoints, objectPoints, 3, 1 );
2015
2016 for( k = 0; k < 2; k++ )
2017 {
2018 const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2;
2019 const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2020 const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
2021
2022 int cn = CV_MAT_CN(_imagePoints1->type);
2023 CV_Assert( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F ||
2024 CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) &&
2025 ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) ||
2026 (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) );
2027
2028 K[k] = cvMat(3,3,CV_64F,A[k]);
2029 Dist[k] = cvMat(1,14,CV_64F,dk[k]);
2030
2031 imagePoints[k].reset(cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type))));
2032 cvConvert( points, imagePoints[k] );
2033 cvReshape( imagePoints[k], imagePoints[k], 2, 1 );
2034
2035 if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS|
2036 CALIB_FIX_ASPECT_RATIO|CALIB_FIX_FOCAL_LENGTH) )
2037 cvConvert( cameraMatrix, &K[k] );
2038
2039 if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS|
2040 CALIB_FIX_K1|CALIB_FIX_K2|CALIB_FIX_K3|CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6|CALIB_FIX_TANGENT_DIST) )
2041 {
2042 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
2043 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
2044 cvConvert( distCoeffs, &tdist );
2045 }
2046
2047 if( !(flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS)))
2048 {
2049 cvCalibrateCamera2( objectPoints, imagePoints[k],
2050 npoints, imageSize, &K[k], &Dist[k], NULL, NULL, flags );
2051 }
2052 }
2053
2054 if( flags & CALIB_SAME_FOCAL_LENGTH )
2055 {
2056 static const int avg_idx[] = { 0, 4, 2, 5, -1 };
2057 for( k = 0; avg_idx[k] >= 0; k++ )
2058 A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5;
2059 }
2060
2061 if( flags & CALIB_FIX_ASPECT_RATIO )
2062 {
2063 for( k = 0; k < 2; k++ )
2064 aspectRatio[k] = A[k][0]/A[k][4];
2065 }
2066
2067 recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0;
2068
2069 Mat err( maxPoints*2, 1, CV_64F );
2070 Mat Je( maxPoints*2, 6, CV_64F );
2071 Mat J_LR( maxPoints*2, 6, CV_64F );
2072 Mat Ji( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
2073
2074 // we optimize for the inter-camera R(3),t(3), then, optionally,
2075 // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
2076 nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0);
2077
2078 CvLevMarq solver( nparams, 0, termCrit );
2079
2080 if(flags & CALIB_USE_LU) {
2081 solver.solveMethod = DECOMP_LU;
2082 }
2083
2084 if( recomputeIntrinsics )
2085 {
2086 uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2;
2087 if( !(flags & CALIB_RATIONAL_MODEL) )
2088 flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6;
2089 if( !(flags & CALIB_THIN_PRISM_MODEL) )
2090 flags |= CALIB_FIX_S1_S2_S3_S4;
2091 if( !(flags & CALIB_TILTED_MODEL) )
2092 flags |= CALIB_FIX_TAUX_TAUY;
2093 if( flags & CALIB_FIX_ASPECT_RATIO )
2094 imask[0] = imask[NINTRINSIC] = 0;
2095 if( flags & CALIB_FIX_FOCAL_LENGTH )
2096 imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0;
2097 if( flags & CALIB_FIX_PRINCIPAL_POINT )
2098 imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0;
2099 if( flags & (CALIB_ZERO_TANGENT_DIST|CALIB_FIX_TANGENT_DIST) )
2100 imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0;
2101 if( flags & CALIB_FIX_K1 )
2102 imask[4] = imask[NINTRINSIC+4] = 0;
2103 if( flags & CALIB_FIX_K2 )
2104 imask[5] = imask[NINTRINSIC+5] = 0;
2105 if( flags & CALIB_FIX_K3 )
2106 imask[8] = imask[NINTRINSIC+8] = 0;
2107 if( flags & CALIB_FIX_K4 )
2108 imask[9] = imask[NINTRINSIC+9] = 0;
2109 if( flags & CALIB_FIX_K5 )
2110 imask[10] = imask[NINTRINSIC+10] = 0;
2111 if( flags & CALIB_FIX_K6 )
2112 imask[11] = imask[NINTRINSIC+11] = 0;
2113 if( flags & CALIB_FIX_S1_S2_S3_S4 )
2114 {
2115 imask[12] = imask[NINTRINSIC+12] = 0;
2116 imask[13] = imask[NINTRINSIC+13] = 0;
2117 imask[14] = imask[NINTRINSIC+14] = 0;
2118 imask[15] = imask[NINTRINSIC+15] = 0;
2119 }
2120 if( flags & CALIB_FIX_TAUX_TAUY )
2121 {
2122 imask[16] = imask[NINTRINSIC+16] = 0;
2123 imask[17] = imask[NINTRINSIC+17] = 0;
2124 }
2125 }
2126
2127 // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
2128 RT0.reset(cvCreateMat( 6, nimages, CV_64F ));
2129 /*
2130 Compute initial estimate of pose
2131 For each image, compute:
2132 R(om) is the rotation matrix of om
2133 om(R) is the rotation vector of R
2134 R_ref = R(om_right) * R(om_left)'
2135 T_ref_list = [T_ref_list; T_right - R_ref * T_left]
2136 om_ref_list = {om_ref_list; om(R_ref)]
2137 om = median(om_ref_list)
2138 T = median(T_ref_list)
2139 */
2140 for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2141 {
2142 ni = npoints->data.i[i];
2143 CvMat objpt_i;
2144 double _om[2][3], r[2][9], t[2][3];
2145 CvMat om[2], R[2], T[2], imgpt_i[2];
2146
2147 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2148 for( k = 0; k < 2; k++ )
2149 {
2150 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2151 om[k] = cvMat(3, 1, CV_64F, _om[k]);
2152 R[k] = cvMat(3, 3, CV_64F, r[k]);
2153 T[k] = cvMat(3, 1, CV_64F, t[k]);
2154
2155 cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] );
2156 cvRodrigues2( &om[k], &R[k] );
2157 if( k == 0 )
2158 {
2159 // save initial om_left and T_left
2160 solver.param->data.db[(i+1)*6] = _om[0][0];
2161 solver.param->data.db[(i+1)*6 + 1] = _om[0][1];
2162 solver.param->data.db[(i+1)*6 + 2] = _om[0][2];
2163 solver.param->data.db[(i+1)*6 + 3] = t[0][0];
2164 solver.param->data.db[(i+1)*6 + 4] = t[0][1];
2165 solver.param->data.db[(i+1)*6 + 5] = t[0][2];
2166 }
2167 }
2168 cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T );
2169 cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] );
2170 cvRodrigues2( &R[0], &T[0] );
2171 RT0->data.db[i] = t[0][0];
2172 RT0->data.db[i + nimages] = t[0][1];
2173 RT0->data.db[i + nimages*2] = t[0][2];
2174 RT0->data.db[i + nimages*3] = t[1][0];
2175 RT0->data.db[i + nimages*4] = t[1][1];
2176 RT0->data.db[i + nimages*5] = t[1][2];
2177 }
2178
2179 if(flags & CALIB_USE_EXTRINSIC_GUESS)
2180 {
2181 Vec3d R, T;
2182 cvarrToMat(matT).convertTo(T, CV_64F);
2183
2184 if( matR->rows == 3 && matR->cols == 3 )
2185 Rodrigues(cvarrToMat(matR), R);
2186 else
2187 cvarrToMat(matR).convertTo(R, CV_64F);
2188
2189 solver.param->data.db[0] = R[0];
2190 solver.param->data.db[1] = R[1];
2191 solver.param->data.db[2] = R[2];
2192 solver.param->data.db[3] = T[0];
2193 solver.param->data.db[4] = T[1];
2194 solver.param->data.db[5] = T[2];
2195 }
2196 else
2197 {
2198 // find the medians and save the first 6 parameters
2199 for( i = 0; i < 6; i++ )
2200 {
2201 qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp );
2202 solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] :
2203 (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5;
2204 }
2205 }
2206
2207 if( recomputeIntrinsics )
2208 for( k = 0; k < 2; k++ )
2209 {
2210 double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC;
2211 if( flags & CALIB_ZERO_TANGENT_DIST )
2212 dk[k][2] = dk[k][3] = 0;
2213 iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5];
2214 iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2];
2215 iparam[7] = dk[k][3]; iparam[8] = dk[k][4]; iparam[9] = dk[k][5];
2216 iparam[10] = dk[k][6]; iparam[11] = dk[k][7];
2217 iparam[12] = dk[k][8];
2218 iparam[13] = dk[k][9];
2219 iparam[14] = dk[k][10];
2220 iparam[15] = dk[k][11];
2221 iparam[16] = dk[k][12];
2222 iparam[17] = dk[k][13];
2223 }
2224
2225 om_LR = cvMat(3, 1, CV_64F, solver.param->data.db);
2226 T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3);
2227
2228 for(;;)
2229 {
2230 const CvMat* param = 0;
2231 CvMat *JtJ = 0, *JtErr = 0;
2232 double *_errNorm = 0;
2233 double _omR[3], _tR[3];
2234 double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9];
2235 CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1);
2236 CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2);
2237 //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1);
2238 CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2);
2239 CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1);
2240 CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2);
2241 CvMat om[2], T[2], imgpt_i[2];
2242
2243 if( !solver.updateAlt( param, JtJ, JtErr, _errNorm ))
2244 break;
2245 reprojErr = 0;
2246
2247 cvRodrigues2( &om_LR, &R_LR );
2248 om[1] = cvMat(3,1,CV_64F,_omR);
2249 T[1] = cvMat(3,1,CV_64F,_tR);
2250
2251 if( recomputeIntrinsics )
2252 {
2253 double* iparam = solver.param->data.db + (nimages+1)*6;
2254 double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
2255
2256 if( flags & CALIB_SAME_FOCAL_LENGTH )
2257 {
2258 iparam[NINTRINSIC] = iparam[0];
2259 iparam[NINTRINSIC+1] = iparam[1];
2260 ipparam[NINTRINSIC] = ipparam[0];
2261 ipparam[NINTRINSIC+1] = ipparam[1];
2262 }
2263 if( flags & CALIB_FIX_ASPECT_RATIO )
2264 {
2265 iparam[0] = iparam[1]*aspectRatio[0];
2266 iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1];
2267 ipparam[0] = ipparam[1]*aspectRatio[0];
2268 ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1];
2269 }
2270 for( k = 0; k < 2; k++ )
2271 {
2272 A[k][0] = iparam[k*NINTRINSIC+0];
2273 A[k][4] = iparam[k*NINTRINSIC+1];
2274 A[k][2] = iparam[k*NINTRINSIC+2];
2275 A[k][5] = iparam[k*NINTRINSIC+3];
2276 dk[k][0] = iparam[k*NINTRINSIC+4];
2277 dk[k][1] = iparam[k*NINTRINSIC+5];
2278 dk[k][2] = iparam[k*NINTRINSIC+6];
2279 dk[k][3] = iparam[k*NINTRINSIC+7];
2280 dk[k][4] = iparam[k*NINTRINSIC+8];
2281 dk[k][5] = iparam[k*NINTRINSIC+9];
2282 dk[k][6] = iparam[k*NINTRINSIC+10];
2283 dk[k][7] = iparam[k*NINTRINSIC+11];
2284 dk[k][8] = iparam[k*NINTRINSIC+12];
2285 dk[k][9] = iparam[k*NINTRINSIC+13];
2286 dk[k][10] = iparam[k*NINTRINSIC+14];
2287 dk[k][11] = iparam[k*NINTRINSIC+15];
2288 dk[k][12] = iparam[k*NINTRINSIC+16];
2289 dk[k][13] = iparam[k*NINTRINSIC+17];
2290 }
2291 }
2292
2293 for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2294 {
2295 ni = npoints->data.i[i];
2296 CvMat objpt_i;
2297
2298 om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6);
2299 T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3);
2300
2301 if( JtJ || JtErr )
2302 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0,
2303 &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 );
2304 else
2305 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
2306
2307 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2308 err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2);
2309
2310 CvMat tmpimagePoints = cvMat(err.reshape(2, 1));
2311 CvMat dpdf = cvMat(Ji.colRange(0, 2));
2312 CvMat dpdc = cvMat(Ji.colRange(2, 4));
2313 CvMat dpdk = cvMat(Ji.colRange(4, NINTRINSIC));
2314 CvMat dpdrot = cvMat(Je.colRange(0, 3));
2315 CvMat dpdt = cvMat(Je.colRange(3, 6));
2316
2317 for( k = 0; k < 2; k++ )
2318 {
2319 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2320
2321 if( JtJ || JtErr )
2322 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k],
2323 &tmpimagePoints, &dpdrot, &dpdt, &dpdf, &dpdc, &dpdk,
2324 (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0);
2325 else
2326 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
2327 cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
2328
2329 if( solver.state == CvLevMarq::CALC_J )
2330 {
2331 int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
2332 assert( JtJ && JtErr );
2333
2334 Mat _JtJ(cvarrToMat(JtJ)), _JtErr(cvarrToMat(JtErr));
2335
2336 if( k == 1 )
2337 {
2338 // d(err_{x|y}R) ~ de3
2339 // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
2340 for( p = 0; p < ni*2; p++ )
2341 {
2342 CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je.ptr(p));
2343 CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 );
2344 CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR.ptr(p) );
2345 CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 );
2346 double _de3dr1[3], _de3dt1[3];
2347 CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 );
2348 CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 );
2349
2350 cvMatMul( &de3dr3, &dr3dr1, &de3dr1 );
2351 cvMatMul( &de3dt3, &dt3dt1, &de3dt1 );
2352
2353 cvMatMul( &de3dr3, &dr3dr2, &de3dr2 );
2354 cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 );
2355
2356 cvMatMul( &de3dt3, &dt3dt2, &de3dt2 );
2357
2358 cvCopy( &de3dr1, &de3dr3 );
2359 cvCopy( &de3dt1, &de3dt3 );
2360 }
2361
2362 _JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR;
2363 _JtJ(Rect(eofs, 0, 6, 6)) = J_LR.t()*Je;
2364 _JtErr.rowRange(0, 6) += J_LR.t()*err;
2365 }
2366
2367 _JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je;
2368 _JtErr.rowRange(eofs, eofs + 6) += Je.t()*err;
2369
2370 if( recomputeIntrinsics )
2371 {
2372 _JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji;
2373 _JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji;
2374 if( k == 1 )
2375 {
2376 _JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji;
2377 }
2378 _JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err;
2379 }
2380 }
2381
2382 double viewErr = norm(err, NORM_L2SQR);
2383
2384 if(perViewErr)
2385 perViewErr->data.db[i*2 + k] = std::sqrt(viewErr/ni);
2386
2387 reprojErr += viewErr;
2388 }
2389 }
2390 if(_errNorm)
2391 *_errNorm = reprojErr;
2392 }
2393
2394 cvRodrigues2( &om_LR, &R_LR );
2395 if( matR->rows == 1 || matR->cols == 1 )
2396 cvConvert( &om_LR, matR );
2397 else
2398 cvConvert( &R_LR, matR );
2399 cvConvert( &T_LR, matT );
2400
2401 if( recomputeIntrinsics )
2402 {
2403 cvConvert( &K[0], _cameraMatrix1 );
2404 cvConvert( &K[1], _cameraMatrix2 );
2405
2406 for( k = 0; k < 2; k++ )
2407 {
2408 CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
2409 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
2410 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
2411 cvConvert( &tdist, distCoeffs );
2412 }
2413 }
2414
2415 if( matE || matF )
2416 {
2417 double* t = T_LR.data.db;
2418 double tx[] =
2419 {
2420 0, -t[2], t[1],
2421 t[2], 0, -t[0],
2422 -t[1], t[0], 0
2423 };
2424 CvMat Tx = cvMat(3, 3, CV_64F, tx);
2425 double e[9], f[9];
2426 CvMat E = cvMat(3, 3, CV_64F, e);
2427 CvMat F = cvMat(3, 3, CV_64F, f);
2428 cvMatMul( &Tx, &R_LR, &E );
2429 if( matE )
2430 cvConvert( &E, matE );
2431 if( matF )
2432 {
2433 double ik[9];
2434 CvMat iK = cvMat(3, 3, CV_64F, ik);
2435 cvInvert(&K[1], &iK);
2436 cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );
2437 cvInvert(&K[0], &iK);
2438 cvMatMul(&E, &iK, &F);
2439 cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 );
2440 }
2441 }
2442
2443 return std::sqrt(reprojErr/(pointsTotal*2));
2444 }
cvStereoCalibrate(const CvMat * _objectPoints,const CvMat * _imagePoints1,const CvMat * _imagePoints2,const CvMat * _npoints,CvMat * _cameraMatrix1,CvMat * _distCoeffs1,CvMat * _cameraMatrix2,CvMat * _distCoeffs2,CvSize imageSize,CvMat * matR,CvMat * matT,CvMat * matE,CvMat * matF,int flags,CvTermCriteria termCrit)2445 double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
2446 const CvMat* _imagePoints2, const CvMat* _npoints,
2447 CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
2448 CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
2449 CvSize imageSize, CvMat* matR, CvMat* matT,
2450 CvMat* matE, CvMat* matF,
2451 int flags,
2452 CvTermCriteria termCrit )
2453 {
2454 return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1,
2455 _distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE,
2456 matF, NULL, flags, termCrit);
2457 }
2458
2459 static void
icvGetRectangles(const CvMat * cameraMatrix,const CvMat * distCoeffs,const CvMat * R,const CvMat * newCameraMatrix,CvSize imgSize,cv::Rect_<float> & inner,cv::Rect_<float> & outer)2460 icvGetRectangles( const CvMat* cameraMatrix, const CvMat* distCoeffs,
2461 const CvMat* R, const CvMat* newCameraMatrix, CvSize imgSize,
2462 cv::Rect_<float>& inner, cv::Rect_<float>& outer )
2463 {
2464 const int N = 9;
2465 int x, y, k;
2466 cv::Ptr<CvMat> _pts(cvCreateMat(1, N*N, CV_32FC2));
2467 CvPoint2D32f* pts = (CvPoint2D32f*)(_pts->data.ptr);
2468
2469 for( y = k = 0; y < N; y++ )
2470 for( x = 0; x < N; x++ )
2471 pts[k++] = cvPoint2D32f((float)x*imgSize.width/(N-1),
2472 (float)y*imgSize.height/(N-1));
2473
2474 cvUndistortPoints(_pts, _pts, cameraMatrix, distCoeffs, R, newCameraMatrix);
2475
2476 float iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX;
2477 float oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX;
2478 // find the inscribed rectangle.
2479 // the code will likely not work with extreme rotation matrices (R) (>45%)
2480 for( y = k = 0; y < N; y++ )
2481 for( x = 0; x < N; x++ )
2482 {
2483 CvPoint2D32f p = pts[k++];
2484 oX0 = MIN(oX0, p.x);
2485 oX1 = MAX(oX1, p.x);
2486 oY0 = MIN(oY0, p.y);
2487 oY1 = MAX(oY1, p.y);
2488
2489 if( x == 0 )
2490 iX0 = MAX(iX0, p.x);
2491 if( x == N-1 )
2492 iX1 = MIN(iX1, p.x);
2493 if( y == 0 )
2494 iY0 = MAX(iY0, p.y);
2495 if( y == N-1 )
2496 iY1 = MIN(iY1, p.y);
2497 }
2498 inner = cv::Rect_<float>(iX0, iY0, iX1-iX0, iY1-iY0);
2499 outer = cv::Rect_<float>(oX0, oY0, oX1-oX0, oY1-oY0);
2500 }
2501
2502
cvStereoRectify(const CvMat * _cameraMatrix1,const CvMat * _cameraMatrix2,const CvMat * _distCoeffs1,const CvMat * _distCoeffs2,CvSize imageSize,const CvMat * matR,const CvMat * matT,CvMat * _R1,CvMat * _R2,CvMat * _P1,CvMat * _P2,CvMat * matQ,int flags,double alpha,CvSize newImgSize,CvRect * roi1,CvRect * roi2)2503 void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
2504 const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
2505 CvSize imageSize, const CvMat* matR, const CvMat* matT,
2506 CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
2507 CvMat* matQ, int flags, double alpha, CvSize newImgSize,
2508 CvRect* roi1, CvRect* roi2 )
2509 {
2510 double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
2511 double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3];
2512 cv::Rect_<float> inner1, inner2, outer1, outer2;
2513
2514 CvMat om = cvMat(3, 1, CV_64F, _om);
2515 CvMat t = cvMat(3, 1, CV_64F, _t);
2516 CvMat uu = cvMat(3, 1, CV_64F, _uu);
2517 CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
2518 CvMat pp = cvMat(3, 4, CV_64F, _pp);
2519 CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps
2520 CvMat wR = cvMat(3, 3, CV_64F, _wr);
2521 CvMat Z = cvMat(3, 1, CV_64F, _z);
2522 CvMat Ri = cvMat(3, 3, CV_64F, _ri);
2523 double nx = imageSize.width, ny = imageSize.height;
2524 int i, k;
2525
2526 if( matR->rows == 3 && matR->cols == 3 )
2527 cvRodrigues2(matR, &om); // get vector rotation
2528 else
2529 cvConvert(matR, &om); // it's already a rotation vector
2530 cvConvertScale(&om, &om, -0.5); // get average rotation
2531 cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging
2532 cvMatMul(&r_r, matT, &t);
2533
2534 int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
2535 double c = _t[idx], nt = cvNorm(&t, 0, CV_L2);
2536 _uu[idx] = c > 0 ? 1 : -1;
2537
2538 CV_Assert(nt > 0.0);
2539
2540 // calculate global Z rotation
2541 cvCrossProduct(&t,&uu,&ww);
2542 double nw = cvNorm(&ww, 0, CV_L2);
2543 if (nw > 0.0)
2544 cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);
2545 cvRodrigues2(&ww, &wR);
2546
2547 // apply to both views
2548 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
2549 cvConvert( &Ri, _R1 );
2550 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
2551 cvConvert( &Ri, _R2 );
2552 cvMatMul(&Ri, matT, &t);
2553
2554 // calculate projection/camera matrices
2555 // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
2556 double fc_new = DBL_MAX;
2557 CvPoint2D64f cc_new[2] = {};
2558
2559 newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
2560 const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
2561 const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
2562 const double ratio = idx == 1 ? ratio_x : ratio_y;
2563 fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;
2564
2565 for( k = 0; k < 2; k++ )
2566 {
2567 const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2568 const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
2569 CvPoint2D32f _pts[4] = {};
2570 CvPoint3D32f _pts_3[4] = {};
2571 CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
2572 CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
2573
2574 for( i = 0; i < 4; i++ )
2575 {
2576 int j = (i<2) ? 0 : 1;
2577 _pts[i].x = (float)((i % 2)*(nx-1));
2578 _pts[i].y = (float)(j*(ny-1));
2579 }
2580 cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );
2581 cvConvertPointsHomogeneous( &pts, &pts_3 );
2582
2583 //Change camera matrix to have cc=[0,0] and fc = fc_new
2584 double _a_tmp[3][3];
2585 CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp);
2586 _a_tmp[0][0]=fc_new;
2587 _a_tmp[1][1]=fc_new;
2588 _a_tmp[0][2]=0.0;
2589 _a_tmp[1][2]=0.0;
2590 cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts );
2591 CvScalar avg = cvAvg(&pts);
2592 cc_new[k].x = (nx-1)/2 - avg.val[0];
2593 cc_new[k].y = (ny-1)/2 - avg.val[1];
2594 }
2595
2596 // vertical focal length must be the same for both images to keep the epipolar constraint
2597 // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)
2598 // use fy for fx also, for simplicity
2599
2600 // For simplicity, set the principal points for both cameras to be the average
2601 // of the two principal points (either one of or both x- and y- coordinates)
2602 if( flags & CALIB_ZERO_DISPARITY )
2603 {
2604 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2605 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2606 }
2607 else if( idx == 0 ) // horizontal stereo
2608 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2609 else // vertical stereo
2610 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2611
2612 cvZero( &pp );
2613 _pp[0][0] = _pp[1][1] = fc_new;
2614 _pp[0][2] = cc_new[0].x;
2615 _pp[1][2] = cc_new[0].y;
2616 _pp[2][2] = 1;
2617 cvConvert(&pp, _P1);
2618
2619 _pp[0][2] = cc_new[1].x;
2620 _pp[1][2] = cc_new[1].y;
2621 _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
2622 cvConvert(&pp, _P2);
2623
2624 alpha = MIN(alpha, 1.);
2625
2626 icvGetRectangles( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 );
2627 icvGetRectangles( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 );
2628
2629 {
2630 newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;
2631 double cx1_0 = cc_new[0].x;
2632 double cy1_0 = cc_new[0].y;
2633 double cx2_0 = cc_new[1].x;
2634 double cy2_0 = cc_new[1].y;
2635 double cx1 = newImgSize.width*cx1_0/imageSize.width;
2636 double cy1 = newImgSize.height*cy1_0/imageSize.height;
2637 double cx2 = newImgSize.width*cx2_0/imageSize.width;
2638 double cy2 = newImgSize.height*cy2_0/imageSize.height;
2639 double s = 1.;
2640
2641 if( alpha >= 0 )
2642 {
2643 double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)),
2644 (double)(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),
2645 (double)(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));
2646 s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)),
2647 (double)(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),
2648 (double)(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),
2649 s0);
2650
2651 double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)),
2652 (double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)),
2653 (double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0));
2654 s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)),
2655 (double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),
2656 (double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),
2657 s1);
2658
2659 s = s0*(1 - alpha) + s1*alpha;
2660 }
2661
2662 fc_new *= s;
2663 cc_new[0] = cvPoint2D64f(cx1, cy1);
2664 cc_new[1] = cvPoint2D64f(cx2, cy2);
2665
2666 cvmSet(_P1, 0, 0, fc_new);
2667 cvmSet(_P1, 1, 1, fc_new);
2668 cvmSet(_P1, 0, 2, cx1);
2669 cvmSet(_P1, 1, 2, cy1);
2670
2671 cvmSet(_P2, 0, 0, fc_new);
2672 cvmSet(_P2, 1, 1, fc_new);
2673 cvmSet(_P2, 0, 2, cx2);
2674 cvmSet(_P2, 1, 2, cy2);
2675 cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
2676
2677 if(roi1)
2678 {
2679 *roi1 = cvRect(
2680 cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1),
2681 cvCeil((inner1.y - cy1_0)*s + cy1),
2682 cvFloor(inner1.width*s), cvFloor(inner1.height*s))
2683 & cv::Rect(0, 0, newImgSize.width, newImgSize.height)
2684 );
2685 }
2686
2687 if(roi2)
2688 {
2689 *roi2 = cvRect(
2690 cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2),
2691 cvCeil((inner2.y - cy2_0)*s + cy2),
2692 cvFloor(inner2.width*s), cvFloor(inner2.height*s))
2693 & cv::Rect(0, 0, newImgSize.width, newImgSize.height)
2694 );
2695 }
2696 }
2697
2698 if( matQ )
2699 {
2700 double q[] =
2701 {
2702 1, 0, 0, -cc_new[0].x,
2703 0, 1, 0, -cc_new[0].y,
2704 0, 0, 0, fc_new,
2705 0, 0, -1./_t[idx],
2706 (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
2707 };
2708 CvMat Q = cvMat(4, 4, CV_64F, q);
2709 cvConvert( &Q, matQ );
2710 }
2711 }
2712
2713
cvGetOptimalNewCameraMatrix(const CvMat * cameraMatrix,const CvMat * distCoeffs,CvSize imgSize,double alpha,CvMat * newCameraMatrix,CvSize newImgSize,CvRect * validPixROI,int centerPrincipalPoint)2714 void cvGetOptimalNewCameraMatrix( const CvMat* cameraMatrix, const CvMat* distCoeffs,
2715 CvSize imgSize, double alpha,
2716 CvMat* newCameraMatrix, CvSize newImgSize,
2717 CvRect* validPixROI, int centerPrincipalPoint )
2718 {
2719 cv::Rect_<float> inner, outer;
2720 newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imgSize;
2721
2722 double M[3][3];
2723 CvMat matM = cvMat(3, 3, CV_64F, M);
2724 cvConvert(cameraMatrix, &matM);
2725
2726 if( centerPrincipalPoint )
2727 {
2728 double cx0 = M[0][2];
2729 double cy0 = M[1][2];
2730 double cx = (newImgSize.width-1)*0.5;
2731 double cy = (newImgSize.height-1)*0.5;
2732
2733 icvGetRectangles( cameraMatrix, distCoeffs, 0, cameraMatrix, imgSize, inner, outer );
2734 double s0 = std::max(std::max(std::max((double)cx/(cx0 - inner.x), (double)cy/(cy0 - inner.y)),
2735 (double)cx/(inner.x + inner.width - cx0)),
2736 (double)cy/(inner.y + inner.height - cy0));
2737 double s1 = std::min(std::min(std::min((double)cx/(cx0 - outer.x), (double)cy/(cy0 - outer.y)),
2738 (double)cx/(outer.x + outer.width - cx0)),
2739 (double)cy/(outer.y + outer.height - cy0));
2740 double s = s0*(1 - alpha) + s1*alpha;
2741
2742 M[0][0] *= s;
2743 M[1][1] *= s;
2744 M[0][2] = cx;
2745 M[1][2] = cy;
2746
2747 if( validPixROI )
2748 {
2749 inner = cv::Rect_<float>((float)((inner.x - cx0)*s + cx),
2750 (float)((inner.y - cy0)*s + cy),
2751 (float)(inner.width*s),
2752 (float)(inner.height*s));
2753 cv::Rect r(cvCeil(inner.x), cvCeil(inner.y), cvFloor(inner.width), cvFloor(inner.height));
2754 r &= cv::Rect(0, 0, newImgSize.width, newImgSize.height);
2755 *validPixROI = cvRect(r);
2756 }
2757 }
2758 else
2759 {
2760 // Get inscribed and circumscribed rectangles in normalized
2761 // (independent of camera matrix) coordinates
2762 icvGetRectangles( cameraMatrix, distCoeffs, 0, 0, imgSize, inner, outer );
2763
2764 // Projection mapping inner rectangle to viewport
2765 double fx0 = (newImgSize.width - 1) / inner.width;
2766 double fy0 = (newImgSize.height - 1) / inner.height;
2767 double cx0 = -fx0 * inner.x;
2768 double cy0 = -fy0 * inner.y;
2769
2770 // Projection mapping outer rectangle to viewport
2771 double fx1 = (newImgSize.width - 1) / outer.width;
2772 double fy1 = (newImgSize.height - 1) / outer.height;
2773 double cx1 = -fx1 * outer.x;
2774 double cy1 = -fy1 * outer.y;
2775
2776 // Interpolate between the two optimal projections
2777 M[0][0] = fx0*(1 - alpha) + fx1*alpha;
2778 M[1][1] = fy0*(1 - alpha) + fy1*alpha;
2779 M[0][2] = cx0*(1 - alpha) + cx1*alpha;
2780 M[1][2] = cy0*(1 - alpha) + cy1*alpha;
2781
2782 if( validPixROI )
2783 {
2784 icvGetRectangles( cameraMatrix, distCoeffs, 0, &matM, imgSize, inner, outer );
2785 cv::Rect r = inner;
2786 r &= cv::Rect(0, 0, newImgSize.width, newImgSize.height);
2787 *validPixROI = cvRect(r);
2788 }
2789 }
2790
2791 cvConvert(&matM, newCameraMatrix);
2792 }
2793
2794
cvStereoRectifyUncalibrated(const CvMat * _points1,const CvMat * _points2,const CvMat * F0,CvSize imgSize,CvMat * _H1,CvMat * _H2,double threshold)2795 CV_IMPL int cvStereoRectifyUncalibrated(
2796 const CvMat* _points1, const CvMat* _points2,
2797 const CvMat* F0, CvSize imgSize,
2798 CvMat* _H1, CvMat* _H2, double threshold )
2799 {
2800 Ptr<CvMat> _m1, _m2, _lines1, _lines2;
2801
2802 int i, j, npoints;
2803 double cx, cy;
2804 double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3] = {0};
2805 CvMat E2 = cvMat( 3, 1, CV_64F, e2 );
2806 CvMat U = cvMat( 3, 3, CV_64F, u );
2807 CvMat V = cvMat( 3, 3, CV_64F, v );
2808 CvMat W = cvMat( 3, 3, CV_64F, w );
2809 CvMat F = cvMat( 3, 3, CV_64F, f );
2810 CvMat H1 = cvMat( 3, 3, CV_64F, h1 );
2811 CvMat H2 = cvMat( 3, 3, CV_64F, h2 );
2812 CvMat H0 = cvMat( 3, 3, CV_64F, h0 );
2813
2814 CvPoint2D64f* m1;
2815 CvPoint2D64f* m2;
2816 CvPoint3D64f* lines1;
2817 CvPoint3D64f* lines2;
2818
2819 CV_Assert( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) &&
2820 CV_ARE_SIZES_EQ(_points1, _points2) );
2821
2822 npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2;
2823
2824 _m1.reset(cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) ));
2825 _m2.reset(cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) ));
2826 _lines1.reset(cvCreateMat( 1, npoints, CV_64FC3 ));
2827 _lines2.reset(cvCreateMat( 1, npoints, CV_64FC3 ));
2828
2829 cvConvert( F0, &F );
2830
2831 cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T );
2832 W.data.db[8] = 0.;
2833 cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T );
2834 cvMatMul( &W, &V, &F );
2835
2836 cx = cvRound( (imgSize.width-1)*0.5 );
2837 cy = cvRound( (imgSize.height-1)*0.5 );
2838
2839 cvZero( _H1 );
2840 cvZero( _H2 );
2841
2842 cvConvert( _points1, _m1 );
2843 cvConvert( _points2, _m2 );
2844 cvReshape( _m1, _m1, 2, 1 );
2845 cvReshape( _m2, _m2, 2, 1 );
2846
2847 m1 = (CvPoint2D64f*)_m1->data.ptr;
2848 m2 = (CvPoint2D64f*)_m2->data.ptr;
2849 lines1 = (CvPoint3D64f*)_lines1->data.ptr;
2850 lines2 = (CvPoint3D64f*)_lines2->data.ptr;
2851
2852 if( threshold > 0 )
2853 {
2854 cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 );
2855 cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 );
2856
2857 // measure distance from points to the corresponding epilines, mark outliers
2858 for( i = j = 0; i < npoints; i++ )
2859 {
2860 if( fabs(m1[i].x*lines2[i].x +
2861 m1[i].y*lines2[i].y +
2862 lines2[i].z) <= threshold &&
2863 fabs(m2[i].x*lines1[i].x +
2864 m2[i].y*lines1[i].y +
2865 lines1[i].z) <= threshold )
2866 {
2867 if( j < i )
2868 {
2869 m1[j] = m1[i];
2870 m2[j] = m2[i];
2871 }
2872 j++;
2873 }
2874 }
2875
2876 npoints = j;
2877 if( npoints == 0 )
2878 return 0;
2879 }
2880
2881 _m1->cols = _m2->cols = npoints;
2882 memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2883 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2884
2885 double t[] =
2886 {
2887 1, 0, -cx,
2888 0, 1, -cy,
2889 0, 0, 1
2890 };
2891 CvMat T = cvMat(3, 3, CV_64F, t);
2892 cvMatMul( &T, &E2, &E2 );
2893
2894 int mirror = e2[0] < 0;
2895 double d = MAX(std::sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON);
2896 double alpha = e2[0]/d;
2897 double beta = e2[1]/d;
2898 double r[] =
2899 {
2900 alpha, beta, 0,
2901 -beta, alpha, 0,
2902 0, 0, 1
2903 };
2904 CvMat R = cvMat(3, 3, CV_64F, r);
2905 cvMatMul( &R, &T, &T );
2906 cvMatMul( &R, &E2, &E2 );
2907 double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0];
2908 double k[] =
2909 {
2910 1, 0, 0,
2911 0, 1, 0,
2912 invf, 0, 1
2913 };
2914 CvMat K = cvMat(3, 3, CV_64F, k);
2915 cvMatMul( &K, &T, &H2 );
2916 cvMatMul( &K, &E2, &E2 );
2917
2918 double it[] =
2919 {
2920 1, 0, cx,
2921 0, 1, cy,
2922 0, 0, 1
2923 };
2924 CvMat iT = cvMat( 3, 3, CV_64F, it );
2925 cvMatMul( &iT, &H2, &H2 );
2926
2927 memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2928 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2929
2930 double e2_x[] =
2931 {
2932 0, -e2[2], e2[1],
2933 e2[2], 0, -e2[0],
2934 -e2[1], e2[0], 0
2935 };
2936 double e2_111[] =
2937 {
2938 e2[0], e2[0], e2[0],
2939 e2[1], e2[1], e2[1],
2940 e2[2], e2[2], e2[2],
2941 };
2942 CvMat E2_x = cvMat(3, 3, CV_64F, e2_x);
2943 CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111);
2944 cvMatMulAdd(&E2_x, &F, &E2_111, &H0 );
2945 cvMatMul(&H2, &H0, &H0);
2946 CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6);
2947 cvMatMul(&H0, &E1, &E1);
2948
2949 cvPerspectiveTransform( _m1, _m1, &H0 );
2950 cvPerspectiveTransform( _m2, _m2, &H2 );
2951 CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B;
2952 double x[3] = {0};
2953 CvMat X = cvMat( 3, 1, CV_64F, x );
2954 cvConvertPointsHomogeneous( _m1, &A );
2955 cvReshape( &A, &A, 1, npoints );
2956 cvReshape( _m2, &BxBy, 1, npoints );
2957 cvGetCol( &BxBy, &B, 0 );
2958 cvSolve( &A, &B, &X, CV_SVD );
2959
2960 double ha[] =
2961 {
2962 x[0], x[1], x[2],
2963 0, 1, 0,
2964 0, 0, 1
2965 };
2966 CvMat Ha = cvMat(3, 3, CV_64F, ha);
2967 cvMatMul( &Ha, &H0, &H1 );
2968 cvPerspectiveTransform( _m1, _m1, &Ha );
2969
2970 if( mirror )
2971 {
2972 double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 };
2973 CvMat MM = cvMat(3, 3, CV_64F, mm);
2974 cvMatMul( &MM, &H1, &H1 );
2975 cvMatMul( &MM, &H2, &H2 );
2976 }
2977
2978 cvConvert( &H1, _H1 );
2979 cvConvert( &H2, _H2 );
2980
2981 return 1;
2982 }
2983
2984
reprojectImageTo3D(InputArray _disparity,OutputArray __3dImage,InputArray _Qmat,bool handleMissingValues,int dtype)2985 void cv::reprojectImageTo3D( InputArray _disparity,
2986 OutputArray __3dImage, InputArray _Qmat,
2987 bool handleMissingValues, int dtype )
2988 {
2989 CV_INSTRUMENT_REGION();
2990
2991 Mat disparity = _disparity.getMat(), Q = _Qmat.getMat();
2992 int stype = disparity.type();
2993
2994 CV_Assert( stype == CV_8UC1 || stype == CV_16SC1 ||
2995 stype == CV_32SC1 || stype == CV_32FC1 );
2996 CV_Assert( Q.size() == Size(4,4) );
2997
2998 if( dtype >= 0 )
2999 dtype = CV_MAKETYPE(CV_MAT_DEPTH(dtype), 3);
3000
3001 if( __3dImage.fixedType() )
3002 {
3003 int dtype_ = __3dImage.type();
3004 CV_Assert( dtype == -1 || dtype == dtype_ );
3005 dtype = dtype_;
3006 }
3007
3008 if( dtype < 0 )
3009 dtype = CV_32FC3;
3010 else
3011 CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 );
3012
3013 __3dImage.create(disparity.size(), dtype);
3014 Mat _3dImage = __3dImage.getMat();
3015
3016 const float bigZ = 10000.f;
3017 Matx44d _Q;
3018 Q.convertTo(_Q, CV_64F);
3019
3020 int x, cols = disparity.cols;
3021 CV_Assert( cols >= 0 );
3022
3023 std::vector<float> _sbuf(cols);
3024 std::vector<Vec3f> _dbuf(cols);
3025 float* sbuf = &_sbuf[0];
3026 Vec3f* dbuf = &_dbuf[0];
3027 double minDisparity = FLT_MAX;
3028
3029 // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined.
3030 // and we set the corresponding Z's to some fixed big value.
3031 if( handleMissingValues )
3032 cv::minMaxIdx( disparity, &minDisparity, 0, 0, 0 );
3033
3034 for( int y = 0; y < disparity.rows; y++ )
3035 {
3036 float* sptr = sbuf;
3037 Vec3f* dptr = dbuf;
3038
3039 if( stype == CV_8UC1 )
3040 {
3041 const uchar* sptr0 = disparity.ptr<uchar>(y);
3042 for( x = 0; x < cols; x++ )
3043 sptr[x] = (float)sptr0[x];
3044 }
3045 else if( stype == CV_16SC1 )
3046 {
3047 const short* sptr0 = disparity.ptr<short>(y);
3048 for( x = 0; x < cols; x++ )
3049 sptr[x] = (float)sptr0[x];
3050 }
3051 else if( stype == CV_32SC1 )
3052 {
3053 const int* sptr0 = disparity.ptr<int>(y);
3054 for( x = 0; x < cols; x++ )
3055 sptr[x] = (float)sptr0[x];
3056 }
3057 else
3058 sptr = disparity.ptr<float>(y);
3059
3060 if( dtype == CV_32FC3 )
3061 dptr = _3dImage.ptr<Vec3f>(y);
3062
3063 for( x = 0; x < cols; x++)
3064 {
3065 double d = sptr[x];
3066 Vec4d homg_pt = _Q*Vec4d(x, y, d, 1.0);
3067 dptr[x] = Vec3d(homg_pt.val);
3068 dptr[x] /= homg_pt[3];
3069
3070 if( fabs(d-minDisparity) <= FLT_EPSILON )
3071 dptr[x][2] = bigZ;
3072 }
3073
3074 if( dtype == CV_16SC3 )
3075 {
3076 Vec3s* dptr0 = _3dImage.ptr<Vec3s>(y);
3077 for( x = 0; x < cols; x++ )
3078 {
3079 dptr0[x] = dptr[x];
3080 }
3081 }
3082 else if( dtype == CV_32SC3 )
3083 {
3084 Vec3i* dptr0 = _3dImage.ptr<Vec3i>(y);
3085 for( x = 0; x < cols; x++ )
3086 {
3087 dptr0[x] = dptr[x];
3088 }
3089 }
3090 }
3091 }
3092
3093
cvReprojectImageTo3D(const CvArr * disparityImage,CvArr * _3dImage,const CvMat * matQ,int handleMissingValues)3094 void cvReprojectImageTo3D( const CvArr* disparityImage,
3095 CvArr* _3dImage, const CvMat* matQ,
3096 int handleMissingValues )
3097 {
3098 cv::Mat disp = cv::cvarrToMat(disparityImage);
3099 cv::Mat _3dimg = cv::cvarrToMat(_3dImage);
3100 cv::Mat mq = cv::cvarrToMat(matQ);
3101 CV_Assert( disp.size() == _3dimg.size() );
3102 int dtype = _3dimg.type();
3103 CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 );
3104
3105 cv::reprojectImageTo3D(disp, _3dimg, mq, handleMissingValues != 0, dtype );
3106 }
3107
3108
3109 CV_IMPL void
cvRQDecomp3x3(const CvMat * matrixM,CvMat * matrixR,CvMat * matrixQ,CvMat * matrixQx,CvMat * matrixQy,CvMat * matrixQz,CvPoint3D64f * eulerAngles)3110 cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
3111 CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,
3112 CvPoint3D64f *eulerAngles)
3113 {
3114 double matM[3][3], matR[3][3], matQ[3][3];
3115 CvMat M = cvMat(3, 3, CV_64F, matM);
3116 CvMat R = cvMat(3, 3, CV_64F, matR);
3117 CvMat Q = cvMat(3, 3, CV_64F, matQ);
3118 double z, c, s;
3119
3120 /* Validate parameters. */
3121 CV_Assert( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&
3122 matrixM->cols == 3 && matrixM->rows == 3 &&
3123 CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));
3124
3125 cvConvert(matrixM, &M);
3126
3127 /* Find Givens rotation Q_x for x axis (left multiplication). */
3128 /*
3129 ( 1 0 0 )
3130 Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)
3131 ( 0 -s c )
3132 */
3133 s = matM[2][1];
3134 c = matM[2][2];
3135 z = 1./std::sqrt(c * c + s * s + DBL_EPSILON);
3136 c *= z;
3137 s *= z;
3138
3139 double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };
3140 CvMat Qx = cvMat(3, 3, CV_64F, _Qx);
3141
3142 cvMatMul(&M, &Qx, &R);
3143 assert(fabs(matR[2][1]) < FLT_EPSILON);
3144 matR[2][1] = 0;
3145
3146 /* Find Givens rotation for y axis. */
3147 /*
3148 ( c 0 -s )
3149 Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2)
3150 ( s 0 c )
3151 */
3152 s = -matR[2][0];
3153 c = matR[2][2];
3154 z = 1./std::sqrt(c * c + s * s + DBL_EPSILON);
3155 c *= z;
3156 s *= z;
3157
3158 double _Qy[3][3] = { {c, 0, -s}, {0, 1, 0}, {s, 0, c} };
3159 CvMat Qy = cvMat(3, 3, CV_64F, _Qy);
3160 cvMatMul(&R, &Qy, &M);
3161
3162 assert(fabs(matM[2][0]) < FLT_EPSILON);
3163 matM[2][0] = 0;
3164
3165 /* Find Givens rotation for z axis. */
3166 /*
3167 ( c s 0 )
3168 Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)
3169 ( 0 0 1 )
3170 */
3171
3172 s = matM[1][0];
3173 c = matM[1][1];
3174 z = 1./std::sqrt(c * c + s * s + DBL_EPSILON);
3175 c *= z;
3176 s *= z;
3177
3178 double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };
3179 CvMat Qz = cvMat(3, 3, CV_64F, _Qz);
3180
3181 cvMatMul(&M, &Qz, &R);
3182 assert(fabs(matR[1][0]) < FLT_EPSILON);
3183 matR[1][0] = 0;
3184
3185 // Solve the decomposition ambiguity.
3186 // Diagonal entries of R, except the last one, shall be positive.
3187 // Further rotate R by 180 degree if necessary
3188 if( matR[0][0] < 0 )
3189 {
3190 if( matR[1][1] < 0 )
3191 {
3192 // rotate around z for 180 degree, i.e. a rotation matrix of
3193 // [-1, 0, 0],
3194 // [ 0, -1, 0],
3195 // [ 0, 0, 1]
3196 matR[0][0] *= -1;
3197 matR[0][1] *= -1;
3198 matR[1][1] *= -1;
3199
3200 _Qz[0][0] *= -1;
3201 _Qz[0][1] *= -1;
3202 _Qz[1][0] *= -1;
3203 _Qz[1][1] *= -1;
3204 }
3205 else
3206 {
3207 // rotate around y for 180 degree, i.e. a rotation matrix of
3208 // [-1, 0, 0],
3209 // [ 0, 1, 0],
3210 // [ 0, 0, -1]
3211 matR[0][0] *= -1;
3212 matR[0][2] *= -1;
3213 matR[1][2] *= -1;
3214 matR[2][2] *= -1;
3215
3216 cvTranspose( &Qz, &Qz );
3217
3218 _Qy[0][0] *= -1;
3219 _Qy[0][2] *= -1;
3220 _Qy[2][0] *= -1;
3221 _Qy[2][2] *= -1;
3222 }
3223 }
3224 else if( matR[1][1] < 0 )
3225 {
3226 // ??? for some reason, we never get here ???
3227
3228 // rotate around x for 180 degree, i.e. a rotation matrix of
3229 // [ 1, 0, 0],
3230 // [ 0, -1, 0],
3231 // [ 0, 0, -1]
3232 matR[0][1] *= -1;
3233 matR[0][2] *= -1;
3234 matR[1][1] *= -1;
3235 matR[1][2] *= -1;
3236 matR[2][2] *= -1;
3237
3238 cvTranspose( &Qz, &Qz );
3239 cvTranspose( &Qy, &Qy );
3240
3241 _Qx[1][1] *= -1;
3242 _Qx[1][2] *= -1;
3243 _Qx[2][1] *= -1;
3244 _Qx[2][2] *= -1;
3245 }
3246
3247 // calculate the euler angle
3248 if( eulerAngles )
3249 {
3250 eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
3251 eulerAngles->y = acos(_Qy[0][0]) * (_Qy[2][0] >= 0 ? 1 : -1) * (180.0 / CV_PI);
3252 eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
3253 }
3254
3255 /* Calculate orthogonal matrix. */
3256 /*
3257 Q = QzT * QyT * QxT
3258 */
3259 cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
3260 cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
3261
3262 /* Save R and Q matrices. */
3263 cvConvert( &R, matrixR );
3264 cvConvert( &Q, matrixQ );
3265
3266 if( matrixQx )
3267 cvConvert(&Qx, matrixQx);
3268 if( matrixQy )
3269 cvConvert(&Qy, matrixQy);
3270 if( matrixQz )
3271 cvConvert(&Qz, matrixQz);
3272 }
3273
3274
3275 CV_IMPL void
cvDecomposeProjectionMatrix(const CvMat * projMatr,CvMat * calibMatr,CvMat * rotMatr,CvMat * posVect,CvMat * rotMatrX,CvMat * rotMatrY,CvMat * rotMatrZ,CvPoint3D64f * eulerAngles)3276 cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
3277 CvMat *rotMatr, CvMat *posVect,
3278 CvMat *rotMatrX, CvMat *rotMatrY,
3279 CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
3280 {
3281 double tmpProjMatrData[16], tmpMatrixDData[16], tmpMatrixVData[16];
3282 CvMat tmpProjMatr = cvMat(4, 4, CV_64F, tmpProjMatrData);
3283 CvMat tmpMatrixD = cvMat(4, 4, CV_64F, tmpMatrixDData);
3284 CvMat tmpMatrixV = cvMat(4, 4, CV_64F, tmpMatrixVData);
3285 CvMat tmpMatrixM;
3286
3287 /* Validate parameters. */
3288 if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
3289 CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
3290
3291 if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
3292 CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
3293
3294 if(projMatr->cols != 4 || projMatr->rows != 3)
3295 CV_Error(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
3296
3297 if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
3298 CV_Error(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
3299
3300 if(posVect->cols != 1 || posVect->rows != 4)
3301 CV_Error(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
3302
3303 /* Compute position vector. */
3304 cvSetZero(&tmpProjMatr); // Add zero row to make matrix square.
3305 int i, k;
3306 for(i = 0; i < 3; i++)
3307 for(k = 0; k < 4; k++)
3308 cvmSet(&tmpProjMatr, i, k, cvmGet(projMatr, i, k));
3309
3310 cvSVD(&tmpProjMatr, &tmpMatrixD, NULL, &tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T);
3311
3312 /* Save position vector. */
3313 for(i = 0; i < 4; i++)
3314 cvmSet(posVect, i, 0, cvmGet(&tmpMatrixV, 3, i)); // Solution is last row of V.
3315
3316 /* Compute calibration and rotation matrices via RQ decomposition. */
3317 cvGetCols(projMatr, &tmpMatrixM, 0, 3); // M is first square matrix of P.
3318
3319 CV_Assert(cvDet(&tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
3320
3321 cvRQDecomp3x3(&tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles);
3322 }
3323
3324
3325
3326 namespace cv
3327 {
3328
collectCalibrationData(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints1,InputArrayOfArrays imagePoints2,int iFixedPoint,Mat & objPtMat,Mat & imgPtMat1,Mat * imgPtMat2,Mat & npoints)3329 static void collectCalibrationData( InputArrayOfArrays objectPoints,
3330 InputArrayOfArrays imagePoints1,
3331 InputArrayOfArrays imagePoints2,
3332 int iFixedPoint,
3333 Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
3334 Mat& npoints )
3335 {
3336 int nimages = (int)objectPoints.total();
3337 int total = 0;
3338 CV_Assert(nimages > 0);
3339 CV_CheckEQ(nimages, (int)imagePoints1.total(), "");
3340 if (imgPtMat2)
3341 CV_CheckEQ(nimages, (int)imagePoints2.total(), "");
3342
3343 for (int i = 0; i < nimages; i++)
3344 {
3345 Mat objectPoint = objectPoints.getMat(i);
3346 if (objectPoint.empty())
3347 CV_Error(CV_StsBadSize, "objectPoints should not contain empty vector of vectors of points");
3348 int numberOfObjectPoints = objectPoint.checkVector(3, CV_32F);
3349 if (numberOfObjectPoints <= 0)
3350 CV_Error(CV_StsUnsupportedFormat, "objectPoints should contain vector of vectors of points of type Point3f");
3351
3352 Mat imagePoint1 = imagePoints1.getMat(i);
3353 if (imagePoint1.empty())
3354 CV_Error(CV_StsBadSize, "imagePoints1 should not contain empty vector of vectors of points");
3355 int numberOfImagePoints = imagePoint1.checkVector(2, CV_32F);
3356 if (numberOfImagePoints <= 0)
3357 CV_Error(CV_StsUnsupportedFormat, "imagePoints1 should contain vector of vectors of points of type Point2f");
3358 CV_CheckEQ(numberOfObjectPoints, numberOfImagePoints, "Number of object and image points must be equal");
3359
3360 total += numberOfObjectPoints;
3361 }
3362
3363 npoints.create(1, (int)nimages, CV_32S);
3364 objPtMat.create(1, (int)total, CV_32FC3);
3365 imgPtMat1.create(1, (int)total, CV_32FC2);
3366 Point2f* imgPtData2 = 0;
3367
3368 if (imgPtMat2)
3369 {
3370 imgPtMat2->create(1, (int)total, CV_32FC2);
3371 imgPtData2 = imgPtMat2->ptr<Point2f>();
3372 }
3373
3374 Point3f* objPtData = objPtMat.ptr<Point3f>();
3375 Point2f* imgPtData1 = imgPtMat1.ptr<Point2f>();
3376
3377 for (int i = 0, j = 0; i < nimages; i++)
3378 {
3379 Mat objpt = objectPoints.getMat(i);
3380 Mat imgpt1 = imagePoints1.getMat(i);
3381 int numberOfObjectPoints = objpt.checkVector(3, CV_32F);
3382 npoints.at<int>(i) = numberOfObjectPoints;
3383 for (int n = 0; n < numberOfObjectPoints; ++n)
3384 {
3385 objPtData[j + n] = objpt.ptr<Point3f>()[n];
3386 imgPtData1[j + n] = imgpt1.ptr<Point2f>()[n];
3387 }
3388
3389 if (imgPtData2)
3390 {
3391 Mat imgpt2 = imagePoints2.getMat(i);
3392 int numberOfImage2Points = imgpt2.checkVector(2, CV_32F);
3393 CV_CheckEQ(numberOfObjectPoints, numberOfImage2Points, "Number of object and image(2) points must be equal");
3394 for (int n = 0; n < numberOfImage2Points; ++n)
3395 {
3396 imgPtData2[j + n] = imgpt2.ptr<Point2f>()[n];
3397 }
3398 }
3399
3400 j += numberOfObjectPoints;
3401 }
3402
3403 int ni = npoints.at<int>(0);
3404 bool releaseObject = iFixedPoint > 0 && iFixedPoint < ni - 1;
3405 // check object points. If not qualified, report errors.
3406 if( releaseObject )
3407 {
3408 for (int i = 1; i < nimages; i++)
3409 {
3410 if( npoints.at<int>(i) != ni )
3411 {
3412 CV_Error( CV_StsBadArg, "All objectPoints[i].size() should be equal when "
3413 "object-releasing method is requested." );
3414 }
3415 Mat ocmp = objPtMat.colRange(ni * i, ni * i + ni) != objPtMat.colRange(0, ni);
3416 ocmp = ocmp.reshape(1);
3417 if( countNonZero(ocmp) )
3418 {
3419 CV_Error( CV_StsBadArg, "All objectPoints[i] should be identical when object-releasing"
3420 " method is requested." );
3421 }
3422 }
3423 }
3424 }
3425
collectCalibrationData(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints1,InputArrayOfArrays imagePoints2,Mat & objPtMat,Mat & imgPtMat1,Mat * imgPtMat2,Mat & npoints)3426 static void collectCalibrationData( InputArrayOfArrays objectPoints,
3427 InputArrayOfArrays imagePoints1,
3428 InputArrayOfArrays imagePoints2,
3429 Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
3430 Mat& npoints )
3431 {
3432 collectCalibrationData( objectPoints, imagePoints1, imagePoints2, -1, objPtMat, imgPtMat1,
3433 imgPtMat2, npoints );
3434 }
3435
prepareCameraMatrix(Mat & cameraMatrix0,int rtype,int flags)3436 static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype, int flags)
3437 {
3438 Mat cameraMatrix = Mat::eye(3, 3, rtype);
3439 if( cameraMatrix0.size() == cameraMatrix.size() )
3440 cameraMatrix0.convertTo(cameraMatrix, rtype);
3441 else if( flags & CALIB_USE_INTRINSIC_GUESS )
3442 CV_Error(Error::StsBadArg, "CALIB_USE_INTRINSIC_GUESS flag is set, but the camera matrix is not 3x3");
3443 return cameraMatrix;
3444 }
3445
prepareDistCoeffs(Mat & distCoeffs0,int rtype,int outputSize=14)3446 static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype, int outputSize = 14)
3447 {
3448 CV_Assert((int)distCoeffs0.total() <= outputSize);
3449 Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, outputSize) : Size(outputSize, 1), rtype);
3450 if( distCoeffs0.size() == Size(1, 4) ||
3451 distCoeffs0.size() == Size(1, 5) ||
3452 distCoeffs0.size() == Size(1, 8) ||
3453 distCoeffs0.size() == Size(1, 12) ||
3454 distCoeffs0.size() == Size(1, 14) ||
3455 distCoeffs0.size() == Size(4, 1) ||
3456 distCoeffs0.size() == Size(5, 1) ||
3457 distCoeffs0.size() == Size(8, 1) ||
3458 distCoeffs0.size() == Size(12, 1) ||
3459 distCoeffs0.size() == Size(14, 1) )
3460 {
3461 Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
3462 distCoeffs0.convertTo(dstCoeffs, rtype);
3463 }
3464 return distCoeffs;
3465 }
3466
3467 } // namespace cv
3468
3469
Rodrigues(InputArray _src,OutputArray _dst,OutputArray _jacobian)3470 void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian)
3471 {
3472 CV_INSTRUMENT_REGION();
3473
3474 Mat src = _src.getMat();
3475 const Size srcSz = src.size();
3476 CV_Check(srcSz, srcSz == Size(3, 1) || srcSz == Size(1, 3) ||
3477 (srcSz == Size(1, 1) && src.channels() == 3) ||
3478 srcSz == Size(3, 3),
3479 "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix");
3480
3481 bool v2m = src.cols == 1 || src.rows == 1;
3482 _dst.create(3, v2m ? 3 : 1, src.depth());
3483 Mat dst = _dst.getMat();
3484 CvMat _csrc = cvMat(src), _cdst = cvMat(dst), _cjacobian;
3485 if( _jacobian.needed() )
3486 {
3487 _jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.depth());
3488 _cjacobian = cvMat(_jacobian.getMat());
3489 }
3490 bool ok = cvRodrigues2(&_csrc, &_cdst, _jacobian.needed() ? &_cjacobian : 0) > 0;
3491 if( !ok )
3492 dst = Scalar(0);
3493 }
3494
matMulDeriv(InputArray _Amat,InputArray _Bmat,OutputArray _dABdA,OutputArray _dABdB)3495 void cv::matMulDeriv( InputArray _Amat, InputArray _Bmat,
3496 OutputArray _dABdA, OutputArray _dABdB )
3497 {
3498 CV_INSTRUMENT_REGION();
3499
3500 Mat A = _Amat.getMat(), B = _Bmat.getMat();
3501 _dABdA.create(A.rows*B.cols, A.rows*A.cols, A.type());
3502 _dABdB.create(A.rows*B.cols, B.rows*B.cols, A.type());
3503 Mat dABdA = _dABdA.getMat(), dABdB = _dABdB.getMat();
3504 CvMat matA = cvMat(A), matB = cvMat(B), c_dABdA = cvMat(dABdA), c_dABdB = cvMat(dABdB);
3505 cvCalcMatMulDeriv(&matA, &matB, &c_dABdA, &c_dABdB);
3506 }
3507
3508
composeRT(InputArray _rvec1,InputArray _tvec1,InputArray _rvec2,InputArray _tvec2,OutputArray _rvec3,OutputArray _tvec3,OutputArray _dr3dr1,OutputArray _dr3dt1,OutputArray _dr3dr2,OutputArray _dr3dt2,OutputArray _dt3dr1,OutputArray _dt3dt1,OutputArray _dt3dr2,OutputArray _dt3dt2)3509 void cv::composeRT( InputArray _rvec1, InputArray _tvec1,
3510 InputArray _rvec2, InputArray _tvec2,
3511 OutputArray _rvec3, OutputArray _tvec3,
3512 OutputArray _dr3dr1, OutputArray _dr3dt1,
3513 OutputArray _dr3dr2, OutputArray _dr3dt2,
3514 OutputArray _dt3dr1, OutputArray _dt3dt1,
3515 OutputArray _dt3dr2, OutputArray _dt3dt2 )
3516 {
3517 Mat rvec1 = _rvec1.getMat(), tvec1 = _tvec1.getMat();
3518 Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat();
3519 int rtype = rvec1.type();
3520 _rvec3.create(rvec1.size(), rtype);
3521 _tvec3.create(tvec1.size(), rtype);
3522 Mat rvec3 = _rvec3.getMat(), tvec3 = _tvec3.getMat();
3523
3524 CvMat c_rvec1 = cvMat(rvec1), c_tvec1 = cvMat(tvec1), c_rvec2 = cvMat(rvec2),
3525 c_tvec2 = cvMat(tvec2), c_rvec3 = cvMat(rvec3), c_tvec3 = cvMat(tvec3);
3526 CvMat c_dr3dr1, c_dr3dt1, c_dr3dr2, c_dr3dt2, c_dt3dr1, c_dt3dt1, c_dt3dr2, c_dt3dt2;
3527 CvMat *p_dr3dr1=0, *p_dr3dt1=0, *p_dr3dr2=0, *p_dr3dt2=0, *p_dt3dr1=0, *p_dt3dt1=0, *p_dt3dr2=0, *p_dt3dt2=0;
3528 #define CV_COMPOSE_RT_PARAM(name) \
3529 Mat name; \
3530 if (_ ## name.needed())\
3531 { \
3532 _ ## name.create(3, 3, rtype); \
3533 name = _ ## name.getMat(); \
3534 p_ ## name = &(c_ ## name = cvMat(name)); \
3535 }
3536
3537 CV_COMPOSE_RT_PARAM(dr3dr1); CV_COMPOSE_RT_PARAM(dr3dt1);
3538 CV_COMPOSE_RT_PARAM(dr3dr2); CV_COMPOSE_RT_PARAM(dr3dt2);
3539 CV_COMPOSE_RT_PARAM(dt3dr1); CV_COMPOSE_RT_PARAM(dt3dt1);
3540 CV_COMPOSE_RT_PARAM(dt3dr2); CV_COMPOSE_RT_PARAM(dt3dt2);
3541 #undef CV_COMPOSE_RT_PARAM
3542
3543 cvComposeRT(&c_rvec1, &c_tvec1, &c_rvec2, &c_tvec2, &c_rvec3, &c_tvec3,
3544 p_dr3dr1, p_dr3dt1, p_dr3dr2, p_dr3dt2,
3545 p_dt3dr1, p_dt3dt1, p_dt3dr2, p_dt3dt2);
3546 }
3547
3548
projectPoints(InputArray _opoints,InputArray _rvec,InputArray _tvec,InputArray _cameraMatrix,InputArray _distCoeffs,OutputArray _ipoints,OutputArray _jacobian,double aspectRatio)3549 void cv::projectPoints( InputArray _opoints,
3550 InputArray _rvec,
3551 InputArray _tvec,
3552 InputArray _cameraMatrix,
3553 InputArray _distCoeffs,
3554 OutputArray _ipoints,
3555 OutputArray _jacobian,
3556 double aspectRatio )
3557 {
3558 Mat opoints = _opoints.getMat();
3559 int npoints = opoints.checkVector(3), depth = opoints.depth();
3560 if (npoints < 0)
3561 opoints = opoints.t();
3562 npoints = opoints.checkVector(3);
3563 CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
3564
3565 if (opoints.cols == 3)
3566 opoints = opoints.reshape(3);
3567
3568 CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
3569 CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;
3570
3571 CV_Assert( _ipoints.needed() );
3572
3573 _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true);
3574 Mat imagePoints = _ipoints.getMat();
3575 CvMat c_imagePoints = cvMat(imagePoints);
3576 CvMat c_objectPoints = cvMat(opoints);
3577 Mat cameraMatrix = _cameraMatrix.getMat();
3578
3579 Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
3580 CvMat c_cameraMatrix = cvMat(cameraMatrix);
3581 CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);
3582
3583 double dc0buf[5]={0};
3584 Mat dc0(5,1,CV_64F,dc0buf);
3585 Mat distCoeffs = _distCoeffs.getMat();
3586 if( distCoeffs.empty() )
3587 distCoeffs = dc0;
3588 CvMat c_distCoeffs = cvMat(distCoeffs);
3589 int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
3590
3591 Mat jacobian;
3592 if( _jacobian.needed() )
3593 {
3594 _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
3595 jacobian = _jacobian.getMat();
3596 pdpdrot = &(dpdrot = cvMat(jacobian.colRange(0, 3)));
3597 pdpdt = &(dpdt = cvMat(jacobian.colRange(3, 6)));
3598 pdpdf = &(dpdf = cvMat(jacobian.colRange(6, 8)));
3599 pdpdc = &(dpdc = cvMat(jacobian.colRange(8, 10)));
3600 pdpddist = &(dpddist = cvMat(jacobian.colRange(10, 10+ndistCoeffs)));
3601 }
3602
3603 cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
3604 &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
3605 }
3606
initCameraMatrix2D(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints,Size imageSize,double aspectRatio)3607 cv::Mat cv::initCameraMatrix2D( InputArrayOfArrays objectPoints,
3608 InputArrayOfArrays imagePoints,
3609 Size imageSize, double aspectRatio )
3610 {
3611 CV_INSTRUMENT_REGION();
3612
3613 Mat objPt, imgPt, npoints, cameraMatrix(3, 3, CV_64F);
3614 collectCalibrationData( objectPoints, imagePoints, noArray(),
3615 objPt, imgPt, 0, npoints );
3616 CvMat _objPt = cvMat(objPt), _imgPt = cvMat(imgPt), _npoints = cvMat(npoints), _cameraMatrix = cvMat(cameraMatrix);
3617 cvInitIntrinsicParams2D( &_objPt, &_imgPt, &_npoints,
3618 cvSize(imageSize), &_cameraMatrix, aspectRatio );
3619 return cameraMatrix;
3620 }
3621
3622
3623
calibrateCamera(InputArrayOfArrays _objectPoints,InputArrayOfArrays _imagePoints,Size imageSize,InputOutputArray _cameraMatrix,InputOutputArray _distCoeffs,OutputArrayOfArrays _rvecs,OutputArrayOfArrays _tvecs,int flags,TermCriteria criteria)3624 double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
3625 InputArrayOfArrays _imagePoints,
3626 Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
3627 OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags, TermCriteria criteria )
3628 {
3629 CV_INSTRUMENT_REGION();
3630
3631 return calibrateCamera(_objectPoints, _imagePoints, imageSize, _cameraMatrix, _distCoeffs,
3632 _rvecs, _tvecs, noArray(), noArray(), noArray(), flags, criteria);
3633 }
3634
calibrateCamera(InputArrayOfArrays _objectPoints,InputArrayOfArrays _imagePoints,Size imageSize,InputOutputArray _cameraMatrix,InputOutputArray _distCoeffs,OutputArrayOfArrays _rvecs,OutputArrayOfArrays _tvecs,OutputArray stdDeviationsIntrinsics,OutputArray stdDeviationsExtrinsics,OutputArray _perViewErrors,int flags,TermCriteria criteria)3635 double cv::calibrateCamera(InputArrayOfArrays _objectPoints,
3636 InputArrayOfArrays _imagePoints,
3637 Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
3638 OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
3639 OutputArray stdDeviationsIntrinsics,
3640 OutputArray stdDeviationsExtrinsics,
3641 OutputArray _perViewErrors, int flags, TermCriteria criteria )
3642 {
3643 CV_INSTRUMENT_REGION();
3644
3645 return calibrateCameraRO(_objectPoints, _imagePoints, imageSize, -1, _cameraMatrix, _distCoeffs,
3646 _rvecs, _tvecs, noArray(), stdDeviationsIntrinsics, stdDeviationsExtrinsics,
3647 noArray(), _perViewErrors, flags, criteria);
3648 }
3649
calibrateCameraRO(InputArrayOfArrays _objectPoints,InputArrayOfArrays _imagePoints,Size imageSize,int iFixedPoint,InputOutputArray _cameraMatrix,InputOutputArray _distCoeffs,OutputArrayOfArrays _rvecs,OutputArrayOfArrays _tvecs,OutputArray newObjPoints,int flags,TermCriteria criteria)3650 double cv::calibrateCameraRO(InputArrayOfArrays _objectPoints,
3651 InputArrayOfArrays _imagePoints,
3652 Size imageSize, int iFixedPoint, InputOutputArray _cameraMatrix,
3653 InputOutputArray _distCoeffs,
3654 OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
3655 OutputArray newObjPoints,
3656 int flags, TermCriteria criteria)
3657 {
3658 CV_INSTRUMENT_REGION();
3659
3660 return calibrateCameraRO(_objectPoints, _imagePoints, imageSize, iFixedPoint, _cameraMatrix,
3661 _distCoeffs, _rvecs, _tvecs, newObjPoints, noArray(), noArray(),
3662 noArray(), noArray(), flags, criteria);
3663 }
3664
calibrateCameraRO(InputArrayOfArrays _objectPoints,InputArrayOfArrays _imagePoints,Size imageSize,int iFixedPoint,InputOutputArray _cameraMatrix,InputOutputArray _distCoeffs,OutputArrayOfArrays _rvecs,OutputArrayOfArrays _tvecs,OutputArray newObjPoints,OutputArray stdDeviationsIntrinsics,OutputArray stdDeviationsExtrinsics,OutputArray stdDeviationsObjPoints,OutputArray _perViewErrors,int flags,TermCriteria criteria)3665 double cv::calibrateCameraRO(InputArrayOfArrays _objectPoints,
3666 InputArrayOfArrays _imagePoints,
3667 Size imageSize, int iFixedPoint, InputOutputArray _cameraMatrix,
3668 InputOutputArray _distCoeffs,
3669 OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
3670 OutputArray newObjPoints,
3671 OutputArray stdDeviationsIntrinsics,
3672 OutputArray stdDeviationsExtrinsics,
3673 OutputArray stdDeviationsObjPoints,
3674 OutputArray _perViewErrors, int flags, TermCriteria criteria )
3675 {
3676 CV_INSTRUMENT_REGION();
3677
3678 int rtype = CV_64F;
3679
3680 CV_Assert( _cameraMatrix.needed() );
3681 CV_Assert( _distCoeffs.needed() );
3682
3683 Mat cameraMatrix = _cameraMatrix.getMat();
3684 cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype, flags);
3685 Mat distCoeffs = _distCoeffs.getMat();
3686 distCoeffs = (flags & CALIB_THIN_PRISM_MODEL) && !(flags & CALIB_TILTED_MODEL) ? prepareDistCoeffs(distCoeffs, rtype, 12) :
3687 prepareDistCoeffs(distCoeffs, rtype);
3688 if( !(flags & CALIB_RATIONAL_MODEL) &&
3689 (!(flags & CALIB_THIN_PRISM_MODEL)) &&
3690 (!(flags & CALIB_TILTED_MODEL)))
3691 distCoeffs = distCoeffs.rows == 1 ? distCoeffs.colRange(0, 5) : distCoeffs.rowRange(0, 5);
3692
3693 int nimages = int(_objectPoints.total());
3694 CV_Assert( nimages > 0 );
3695 Mat objPt, imgPt, npoints, rvecM, tvecM, stdDeviationsM, errorsM;
3696
3697 bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(),
3698 stddev_needed = stdDeviationsIntrinsics.needed(), errors_needed = _perViewErrors.needed(),
3699 stddev_ext_needed = stdDeviationsExtrinsics.needed();
3700 bool newobj_needed = newObjPoints.needed();
3701 bool stddev_obj_needed = stdDeviationsObjPoints.needed();
3702
3703 bool rvecs_mat_vec = _rvecs.isMatVector();
3704 bool tvecs_mat_vec = _tvecs.isMatVector();
3705
3706 if( rvecs_needed )
3707 {
3708 _rvecs.create(nimages, 1, CV_64FC3);
3709
3710 if(rvecs_mat_vec)
3711 rvecM.create(nimages, 3, CV_64F);
3712 else
3713 rvecM = _rvecs.getMat();
3714 }
3715
3716 if( tvecs_needed )
3717 {
3718 _tvecs.create(nimages, 1, CV_64FC3);
3719
3720 if(tvecs_mat_vec)
3721 tvecM.create(nimages, 3, CV_64F);
3722 else
3723 tvecM = _tvecs.getMat();
3724 }
3725
3726 collectCalibrationData( _objectPoints, _imagePoints, noArray(), iFixedPoint,
3727 objPt, imgPt, 0, npoints );
3728 bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints.at<int>(0) - 1;
3729
3730 newobj_needed = newobj_needed && releaseObject;
3731 int np = npoints.at<int>( 0 );
3732 Mat newObjPt;
3733 if( newobj_needed ) {
3734 newObjPoints.create( 1, np, CV_32FC3 );
3735 newObjPt = newObjPoints.getMat();
3736 }
3737
3738 stddev_obj_needed = stddev_obj_needed && releaseObject;
3739 bool stddev_any_needed = stddev_needed || stddev_ext_needed || stddev_obj_needed;
3740 if( stddev_any_needed )
3741 {
3742 if( releaseObject )
3743 stdDeviationsM.create(nimages*6 + CV_CALIB_NINTRINSIC + np * 3, 1, CV_64F);
3744 else
3745 stdDeviationsM.create(nimages*6 + CV_CALIB_NINTRINSIC, 1, CV_64F);
3746 }
3747
3748 if( errors_needed )
3749 {
3750 _perViewErrors.create(nimages, 1, CV_64F);
3751 errorsM = _perViewErrors.getMat();
3752 }
3753
3754 CvMat c_objPt = cvMat(objPt), c_imgPt = cvMat(imgPt), c_npoints = cvMat(npoints);
3755 CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs);
3756 CvMat c_rvecM = cvMat(rvecM), c_tvecM = cvMat(tvecM), c_stdDev = cvMat(stdDeviationsM), c_errors = cvMat(errorsM);
3757 CvMat c_newObjPt = cvMat( newObjPt );
3758
3759 double reprojErr = cvCalibrateCamera2Internal(&c_objPt, &c_imgPt, &c_npoints, cvSize(imageSize),
3760 iFixedPoint,
3761 &c_cameraMatrix, &c_distCoeffs,
3762 rvecs_needed ? &c_rvecM : NULL,
3763 tvecs_needed ? &c_tvecM : NULL,
3764 newobj_needed ? &c_newObjPt : NULL,
3765 stddev_any_needed ? &c_stdDev : NULL,
3766 errors_needed ? &c_errors : NULL, flags, cvTermCriteria(criteria));
3767
3768 if( newobj_needed )
3769 newObjPt.copyTo(newObjPoints);
3770
3771 if( stddev_needed )
3772 {
3773 stdDeviationsIntrinsics.create(CV_CALIB_NINTRINSIC, 1, CV_64F);
3774 Mat stdDeviationsIntrinsicsMat = stdDeviationsIntrinsics.getMat();
3775 std::memcpy(stdDeviationsIntrinsicsMat.ptr(), stdDeviationsM.ptr(),
3776 CV_CALIB_NINTRINSIC*sizeof(double));
3777 }
3778
3779 if ( stddev_ext_needed )
3780 {
3781 stdDeviationsExtrinsics.create(nimages*6, 1, CV_64F);
3782 Mat stdDeviationsExtrinsicsMat = stdDeviationsExtrinsics.getMat();
3783 std::memcpy(stdDeviationsExtrinsicsMat.ptr(),
3784 stdDeviationsM.ptr() + CV_CALIB_NINTRINSIC*sizeof(double),
3785 nimages*6*sizeof(double));
3786 }
3787
3788 if( stddev_obj_needed )
3789 {
3790 stdDeviationsObjPoints.create( np * 3, 1, CV_64F );
3791 Mat stdDeviationsObjPointsMat = stdDeviationsObjPoints.getMat();
3792 std::memcpy( stdDeviationsObjPointsMat.ptr(), stdDeviationsM.ptr()
3793 + ( CV_CALIB_NINTRINSIC + nimages * 6 ) * sizeof( double ),
3794 np * 3 * sizeof( double ) );
3795 }
3796
3797 // overly complicated and inefficient rvec/ tvec handling to support vector<Mat>
3798 for(int i = 0; i < nimages; i++ )
3799 {
3800 if( rvecs_needed && rvecs_mat_vec)
3801 {
3802 _rvecs.create(3, 1, CV_64F, i, true);
3803 Mat rv = _rvecs.getMat(i);
3804 memcpy(rv.ptr(), rvecM.ptr(i), 3*sizeof(double));
3805 }
3806 if( tvecs_needed && tvecs_mat_vec)
3807 {
3808 _tvecs.create(3, 1, CV_64F, i, true);
3809 Mat tv = _tvecs.getMat(i);
3810 memcpy(tv.ptr(), tvecM.ptr(i), 3*sizeof(double));
3811 }
3812 }
3813
3814 cameraMatrix.copyTo(_cameraMatrix);
3815 distCoeffs.copyTo(_distCoeffs);
3816
3817 return reprojErr;
3818 }
3819
3820
calibrationMatrixValues(InputArray _cameraMatrix,Size imageSize,double apertureWidth,double apertureHeight,double & fovx,double & fovy,double & focalLength,Point2d & principalPoint,double & aspectRatio)3821 void cv::calibrationMatrixValues( InputArray _cameraMatrix, Size imageSize,
3822 double apertureWidth, double apertureHeight,
3823 double& fovx, double& fovy, double& focalLength,
3824 Point2d& principalPoint, double& aspectRatio )
3825 {
3826 CV_INSTRUMENT_REGION();
3827
3828 if(_cameraMatrix.size() != Size(3, 3))
3829 CV_Error(CV_StsUnmatchedSizes, "Size of cameraMatrix must be 3x3!");
3830
3831 Matx33d K = _cameraMatrix.getMat();
3832
3833 CV_DbgAssert(imageSize.width != 0 && imageSize.height != 0 && K(0, 0) != 0.0 && K(1, 1) != 0.0);
3834
3835 /* Calculate pixel aspect ratio. */
3836 aspectRatio = K(1, 1) / K(0, 0);
3837
3838 /* Calculate number of pixel per realworld unit. */
3839 double mx, my;
3840 if(apertureWidth != 0.0 && apertureHeight != 0.0) {
3841 mx = imageSize.width / apertureWidth;
3842 my = imageSize.height / apertureHeight;
3843 } else {
3844 mx = 1.0;
3845 my = aspectRatio;
3846 }
3847
3848 /* Calculate fovx and fovy. */
3849 fovx = atan2(K(0, 2), K(0, 0)) + atan2(imageSize.width - K(0, 2), K(0, 0));
3850 fovy = atan2(K(1, 2), K(1, 1)) + atan2(imageSize.height - K(1, 2), K(1, 1));
3851 fovx *= 180.0 / CV_PI;
3852 fovy *= 180.0 / CV_PI;
3853
3854 /* Calculate focal length. */
3855 focalLength = K(0, 0) / mx;
3856
3857 /* Calculate principle point. */
3858 principalPoint = Point2d(K(0, 2) / mx, K(1, 2) / my);
3859 }
3860
stereoCalibrate(InputArrayOfArrays _objectPoints,InputArrayOfArrays _imagePoints1,InputArrayOfArrays _imagePoints2,InputOutputArray _cameraMatrix1,InputOutputArray _distCoeffs1,InputOutputArray _cameraMatrix2,InputOutputArray _distCoeffs2,Size imageSize,OutputArray _Rmat,OutputArray _Tmat,OutputArray _Emat,OutputArray _Fmat,int flags,TermCriteria criteria)3861 double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
3862 InputArrayOfArrays _imagePoints1,
3863 InputArrayOfArrays _imagePoints2,
3864 InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1,
3865 InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2,
3866 Size imageSize, OutputArray _Rmat, OutputArray _Tmat,
3867 OutputArray _Emat, OutputArray _Fmat, int flags,
3868 TermCriteria criteria)
3869 {
3870 if (flags & CALIB_USE_EXTRINSIC_GUESS)
3871 CV_Error(Error::StsBadFlag, "stereoCalibrate does not support CALIB_USE_EXTRINSIC_GUESS.");
3872
3873 Mat Rmat, Tmat;
3874 double ret = stereoCalibrate(_objectPoints, _imagePoints1, _imagePoints2, _cameraMatrix1, _distCoeffs1,
3875 _cameraMatrix2, _distCoeffs2, imageSize, Rmat, Tmat, _Emat, _Fmat,
3876 noArray(), flags, criteria);
3877 Rmat.copyTo(_Rmat);
3878 Tmat.copyTo(_Tmat);
3879 return ret;
3880 }
3881
stereoCalibrate(InputArrayOfArrays _objectPoints,InputArrayOfArrays _imagePoints1,InputArrayOfArrays _imagePoints2,InputOutputArray _cameraMatrix1,InputOutputArray _distCoeffs1,InputOutputArray _cameraMatrix2,InputOutputArray _distCoeffs2,Size imageSize,InputOutputArray _Rmat,InputOutputArray _Tmat,OutputArray _Emat,OutputArray _Fmat,OutputArray _perViewErrors,int flags,TermCriteria criteria)3882 double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
3883 InputArrayOfArrays _imagePoints1,
3884 InputArrayOfArrays _imagePoints2,
3885 InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1,
3886 InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2,
3887 Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat,
3888 OutputArray _Emat, OutputArray _Fmat,
3889 OutputArray _perViewErrors, int flags ,
3890 TermCriteria criteria)
3891 {
3892 int rtype = CV_64F;
3893 Mat cameraMatrix1 = _cameraMatrix1.getMat();
3894 Mat cameraMatrix2 = _cameraMatrix2.getMat();
3895 Mat distCoeffs1 = _distCoeffs1.getMat();
3896 Mat distCoeffs2 = _distCoeffs2.getMat();
3897 cameraMatrix1 = prepareCameraMatrix(cameraMatrix1, rtype, flags);
3898 cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype, flags);
3899 distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype);
3900 distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype);
3901
3902 if( !(flags & CALIB_RATIONAL_MODEL) &&
3903 (!(flags & CALIB_THIN_PRISM_MODEL)) &&
3904 (!(flags & CALIB_TILTED_MODEL)))
3905 {
3906 distCoeffs1 = distCoeffs1.rows == 1 ? distCoeffs1.colRange(0, 5) : distCoeffs1.rowRange(0, 5);
3907 distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5);
3908 }
3909
3910 if((flags & CALIB_USE_EXTRINSIC_GUESS) == 0)
3911 {
3912 _Rmat.create(3, 3, rtype);
3913 _Tmat.create(3, 1, rtype);
3914 }
3915
3916 Mat objPt, imgPt, imgPt2, npoints;
3917
3918 collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2,
3919 objPt, imgPt, &imgPt2, npoints );
3920 CvMat c_objPt = cvMat(objPt), c_imgPt = cvMat(imgPt), c_imgPt2 = cvMat(imgPt2), c_npoints = cvMat(npoints);
3921 CvMat c_cameraMatrix1 = cvMat(cameraMatrix1), c_distCoeffs1 = cvMat(distCoeffs1);
3922 CvMat c_cameraMatrix2 = cvMat(cameraMatrix2), c_distCoeffs2 = cvMat(distCoeffs2);
3923 Mat matR_ = _Rmat.getMat(), matT_ = _Tmat.getMat();
3924 CvMat c_matR = cvMat(matR_), c_matT = cvMat(matT_), c_matE, c_matF, c_matErr;
3925
3926 bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
3927
3928 Mat matE_, matF_, matErr_;
3929 if( E_needed )
3930 {
3931 _Emat.create(3, 3, rtype);
3932 matE_ = _Emat.getMat();
3933 c_matE = cvMat(matE_);
3934 }
3935 if( F_needed )
3936 {
3937 _Fmat.create(3, 3, rtype);
3938 matF_ = _Fmat.getMat();
3939 c_matF = cvMat(matF_);
3940 }
3941
3942 if( errors_needed )
3943 {
3944 int nimages = int(_objectPoints.total());
3945 _perViewErrors.create(nimages, 2, CV_64F);
3946 matErr_ = _perViewErrors.getMat();
3947 c_matErr = cvMat(matErr_);
3948 }
3949
3950 double err = cvStereoCalibrateImpl(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1,
3951 &c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, cvSize(imageSize), &c_matR,
3952 &c_matT, E_needed ? &c_matE : NULL, F_needed ? &c_matF : NULL,
3953 errors_needed ? &c_matErr : NULL, flags, cvTermCriteria(criteria));
3954
3955 cameraMatrix1.copyTo(_cameraMatrix1);
3956 cameraMatrix2.copyTo(_cameraMatrix2);
3957 distCoeffs1.copyTo(_distCoeffs1);
3958 distCoeffs2.copyTo(_distCoeffs2);
3959
3960 return err;
3961 }
3962
3963
stereoRectify(InputArray _cameraMatrix1,InputArray _distCoeffs1,InputArray _cameraMatrix2,InputArray _distCoeffs2,Size imageSize,InputArray _Rmat,InputArray _Tmat,OutputArray _Rmat1,OutputArray _Rmat2,OutputArray _Pmat1,OutputArray _Pmat2,OutputArray _Qmat,int flags,double alpha,Size newImageSize,Rect * validPixROI1,Rect * validPixROI2)3964 void cv::stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1,
3965 InputArray _cameraMatrix2, InputArray _distCoeffs2,
3966 Size imageSize, InputArray _Rmat, InputArray _Tmat,
3967 OutputArray _Rmat1, OutputArray _Rmat2,
3968 OutputArray _Pmat1, OutputArray _Pmat2,
3969 OutputArray _Qmat, int flags,
3970 double alpha, Size newImageSize,
3971 Rect* validPixROI1, Rect* validPixROI2 )
3972 {
3973 Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
3974 Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
3975 Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
3976 CvMat c_cameraMatrix1 = cvMat(cameraMatrix1);
3977 CvMat c_cameraMatrix2 = cvMat(cameraMatrix2);
3978 CvMat c_distCoeffs1 = cvMat(distCoeffs1);
3979 CvMat c_distCoeffs2 = cvMat(distCoeffs2);
3980 CvMat c_R = cvMat(Rmat), c_T = cvMat(Tmat);
3981
3982 int rtype = CV_64F;
3983 _Rmat1.create(3, 3, rtype);
3984 _Rmat2.create(3, 3, rtype);
3985 _Pmat1.create(3, 4, rtype);
3986 _Pmat2.create(3, 4, rtype);
3987 Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q;
3988 CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2);
3989 CvMat c_Q, *p_Q = 0;
3990
3991 if( _Qmat.needed() )
3992 {
3993 _Qmat.create(4, 4, rtype);
3994 p_Q = &(c_Q = cvMat(Q = _Qmat.getMat()));
3995 }
3996
3997 CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1;
3998 CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2;
3999 cvStereoRectify( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2,
4000 cvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
4001 cvSize(newImageSize), (CvRect*)validPixROI1, (CvRect*)validPixROI2);
4002 }
4003
stereoRectifyUncalibrated(InputArray _points1,InputArray _points2,InputArray _Fmat,Size imgSize,OutputArray _Hmat1,OutputArray _Hmat2,double threshold)4004 bool cv::stereoRectifyUncalibrated( InputArray _points1, InputArray _points2,
4005 InputArray _Fmat, Size imgSize,
4006 OutputArray _Hmat1, OutputArray _Hmat2, double threshold )
4007 {
4008 CV_INSTRUMENT_REGION();
4009
4010 int rtype = CV_64F;
4011 _Hmat1.create(3, 3, rtype);
4012 _Hmat2.create(3, 3, rtype);
4013 Mat F = _Fmat.getMat();
4014 Mat points1 = _points1.getMat(), points2 = _points2.getMat();
4015 CvMat c_pt1 = cvMat(points1), c_pt2 = cvMat(points2);
4016 Mat H1 = _Hmat1.getMat(), H2 = _Hmat2.getMat();
4017 CvMat c_F, *p_F=0, c_H1 = cvMat(H1), c_H2 = cvMat(H2);
4018 if( F.size() == Size(3, 3) )
4019 p_F = &(c_F = cvMat(F));
4020 return cvStereoRectifyUncalibrated(&c_pt1, &c_pt2, p_F, cvSize(imgSize), &c_H1, &c_H2, threshold) > 0;
4021 }
4022
getOptimalNewCameraMatrix(InputArray _cameraMatrix,InputArray _distCoeffs,Size imgSize,double alpha,Size newImgSize,Rect * validPixROI,bool centerPrincipalPoint)4023 cv::Mat cv::getOptimalNewCameraMatrix( InputArray _cameraMatrix,
4024 InputArray _distCoeffs,
4025 Size imgSize, double alpha, Size newImgSize,
4026 Rect* validPixROI, bool centerPrincipalPoint )
4027 {
4028 CV_INSTRUMENT_REGION();
4029
4030 Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
4031 CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs);
4032
4033 Mat newCameraMatrix(3, 3, CV_MAT_TYPE(c_cameraMatrix.type));
4034 CvMat c_newCameraMatrix = cvMat(newCameraMatrix);
4035
4036 cvGetOptimalNewCameraMatrix(&c_cameraMatrix, &c_distCoeffs, cvSize(imgSize),
4037 alpha, &c_newCameraMatrix,
4038 cvSize(newImgSize), (CvRect*)validPixROI, (int)centerPrincipalPoint);
4039 return newCameraMatrix;
4040 }
4041
4042
RQDecomp3x3(InputArray _Mmat,OutputArray _Rmat,OutputArray _Qmat,OutputArray _Qx,OutputArray _Qy,OutputArray _Qz)4043 cv::Vec3d cv::RQDecomp3x3( InputArray _Mmat,
4044 OutputArray _Rmat,
4045 OutputArray _Qmat,
4046 OutputArray _Qx,
4047 OutputArray _Qy,
4048 OutputArray _Qz )
4049 {
4050 CV_INSTRUMENT_REGION();
4051
4052 Mat M = _Mmat.getMat();
4053 _Rmat.create(3, 3, M.type());
4054 _Qmat.create(3, 3, M.type());
4055 Mat Rmat = _Rmat.getMat();
4056 Mat Qmat = _Qmat.getMat();
4057 Vec3d eulerAngles;
4058
4059 CvMat matM = cvMat(M), matR = cvMat(Rmat), matQ = cvMat(Qmat);
4060 #define CV_RQDecomp3x3_PARAM(name) \
4061 Mat name; \
4062 CvMat c_ ## name, *p ## name = NULL; \
4063 if( _ ## name.needed() ) \
4064 { \
4065 _ ## name.create(3, 3, M.type()); \
4066 name = _ ## name.getMat(); \
4067 c_ ## name = cvMat(name); p ## name = &c_ ## name; \
4068 }
4069
4070 CV_RQDecomp3x3_PARAM(Qx);
4071 CV_RQDecomp3x3_PARAM(Qy);
4072 CV_RQDecomp3x3_PARAM(Qz);
4073 #undef CV_RQDecomp3x3_PARAM
4074 cvRQDecomp3x3(&matM, &matR, &matQ, pQx, pQy, pQz, (CvPoint3D64f*)&eulerAngles[0]);
4075 return eulerAngles;
4076 }
4077
4078
decomposeProjectionMatrix(InputArray _projMatrix,OutputArray _cameraMatrix,OutputArray _rotMatrix,OutputArray _transVect,OutputArray _rotMatrixX,OutputArray _rotMatrixY,OutputArray _rotMatrixZ,OutputArray _eulerAngles)4079 void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraMatrix,
4080 OutputArray _rotMatrix, OutputArray _transVect,
4081 OutputArray _rotMatrixX, OutputArray _rotMatrixY,
4082 OutputArray _rotMatrixZ, OutputArray _eulerAngles )
4083 {
4084 CV_INSTRUMENT_REGION();
4085
4086 Mat projMatrix = _projMatrix.getMat();
4087 int type = projMatrix.type();
4088 _cameraMatrix.create(3, 3, type);
4089 _rotMatrix.create(3, 3, type);
4090 _transVect.create(4, 1, type);
4091 Mat cameraMatrix = _cameraMatrix.getMat();
4092 Mat rotMatrix = _rotMatrix.getMat();
4093 Mat transVect = _transVect.getMat();
4094 CvMat c_projMatrix = cvMat(projMatrix), c_cameraMatrix = cvMat(cameraMatrix);
4095 CvMat c_rotMatrix = cvMat(rotMatrix), c_transVect = cvMat(transVect);
4096 CvPoint3D64f *p_eulerAngles = 0;
4097
4098 #define CV_decomposeProjectionMatrix_PARAM(name) \
4099 Mat name; \
4100 CvMat c_ ## name, *p_ ## name = NULL; \
4101 if( _ ## name.needed() ) \
4102 { \
4103 _ ## name.create(3, 3, type); \
4104 name = _ ## name.getMat(); \
4105 c_ ## name = cvMat(name); p_ ## name = &c_ ## name; \
4106 }
4107
4108 CV_decomposeProjectionMatrix_PARAM(rotMatrixX);
4109 CV_decomposeProjectionMatrix_PARAM(rotMatrixY);
4110 CV_decomposeProjectionMatrix_PARAM(rotMatrixZ);
4111 #undef CV_decomposeProjectionMatrix_PARAM
4112
4113 if( _eulerAngles.needed() )
4114 {
4115 _eulerAngles.create(3, 1, CV_64F, -1, true);
4116 p_eulerAngles = _eulerAngles.getMat().ptr<CvPoint3D64f>();
4117 }
4118
4119 cvDecomposeProjectionMatrix(&c_projMatrix, &c_cameraMatrix, &c_rotMatrix,
4120 &c_transVect, p_rotMatrixX, p_rotMatrixY,
4121 p_rotMatrixZ, p_eulerAngles);
4122 }
4123
4124
4125 namespace cv
4126 {
4127
adjust3rdMatrix(InputArrayOfArrays _imgpt1_0,InputArrayOfArrays _imgpt3_0,const Mat & cameraMatrix1,const Mat & distCoeffs1,const Mat & cameraMatrix3,const Mat & distCoeffs3,const Mat & R1,const Mat & R3,const Mat & P1,Mat & P3)4128 static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0,
4129 InputArrayOfArrays _imgpt3_0,
4130 const Mat& cameraMatrix1, const Mat& distCoeffs1,
4131 const Mat& cameraMatrix3, const Mat& distCoeffs3,
4132 const Mat& R1, const Mat& R3, const Mat& P1, Mat& P3 )
4133 {
4134 size_t n1 = _imgpt1_0.total(), n3 = _imgpt3_0.total();
4135 std::vector<Point2f> imgpt1, imgpt3;
4136
4137 for( int i = 0; i < (int)std::min(n1, n3); i++ )
4138 {
4139 Mat pt1 = _imgpt1_0.getMat(i), pt3 = _imgpt3_0.getMat(i);
4140 int ni1 = pt1.checkVector(2, CV_32F), ni3 = pt3.checkVector(2, CV_32F);
4141 CV_Assert( ni1 > 0 && ni1 == ni3 );
4142 const Point2f* pt1data = pt1.ptr<Point2f>();
4143 const Point2f* pt3data = pt3.ptr<Point2f>();
4144 std::copy(pt1data, pt1data + ni1, std::back_inserter(imgpt1));
4145 std::copy(pt3data, pt3data + ni3, std::back_inserter(imgpt3));
4146 }
4147
4148 undistortPoints(imgpt1, imgpt1, cameraMatrix1, distCoeffs1, R1, P1);
4149 undistortPoints(imgpt3, imgpt3, cameraMatrix3, distCoeffs3, R3, P3);
4150
4151 double y1_ = 0, y2_ = 0, y1y1_ = 0, y1y2_ = 0;
4152 size_t n = imgpt1.size();
4153 CV_DbgAssert(n > 0);
4154
4155 for( size_t i = 0; i < n; i++ )
4156 {
4157 double y1 = imgpt3[i].y, y2 = imgpt1[i].y;
4158
4159 y1_ += y1; y2_ += y2;
4160 y1y1_ += y1*y1; y1y2_ += y1*y2;
4161 }
4162
4163 y1_ /= n;
4164 y2_ /= n;
4165 y1y1_ /= n;
4166 y1y2_ /= n;
4167
4168 double a = (y1y2_ - y1_*y2_)/(y1y1_ - y1_*y1_);
4169 double b = y2_ - a*y1_;
4170
4171 P3.at<double>(0,0) *= a;
4172 P3.at<double>(1,1) *= a;
4173 P3.at<double>(0,2) = P3.at<double>(0,2)*a;
4174 P3.at<double>(1,2) = P3.at<double>(1,2)*a + b;
4175 P3.at<double>(0,3) *= a;
4176 P3.at<double>(1,3) *= a;
4177 }
4178
4179 }
4180
rectify3Collinear(InputArray _cameraMatrix1,InputArray _distCoeffs1,InputArray _cameraMatrix2,InputArray _distCoeffs2,InputArray _cameraMatrix3,InputArray _distCoeffs3,InputArrayOfArrays _imgpt1,InputArrayOfArrays _imgpt3,Size imageSize,InputArray _Rmat12,InputArray _Tmat12,InputArray _Rmat13,InputArray _Tmat13,OutputArray _Rmat1,OutputArray _Rmat2,OutputArray _Rmat3,OutputArray _Pmat1,OutputArray _Pmat2,OutputArray _Pmat3,OutputArray _Qmat,double alpha,Size newImgSize,Rect * roi1,Rect * roi2,int flags)4181 float cv::rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1,
4182 InputArray _cameraMatrix2, InputArray _distCoeffs2,
4183 InputArray _cameraMatrix3, InputArray _distCoeffs3,
4184 InputArrayOfArrays _imgpt1,
4185 InputArrayOfArrays _imgpt3,
4186 Size imageSize, InputArray _Rmat12, InputArray _Tmat12,
4187 InputArray _Rmat13, InputArray _Tmat13,
4188 OutputArray _Rmat1, OutputArray _Rmat2, OutputArray _Rmat3,
4189 OutputArray _Pmat1, OutputArray _Pmat2, OutputArray _Pmat3,
4190 OutputArray _Qmat,
4191 double alpha, Size newImgSize,
4192 Rect* roi1, Rect* roi2, int flags )
4193 {
4194 // first, rectify the 1-2 stereo pair
4195 stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2,
4196 imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat,
4197 flags, alpha, newImgSize, roi1, roi2 );
4198
4199 Mat R12 = _Rmat12.getMat(), R13 = _Rmat13.getMat(), T12 = _Tmat12.getMat(), T13 = _Tmat13.getMat();
4200
4201 _Rmat3.create(3, 3, CV_64F);
4202 _Pmat3.create(3, 4, CV_64F);
4203
4204 Mat P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat();
4205 Mat R3 = _Rmat3.getMat(), P3 = _Pmat3.getMat();
4206
4207 // recompute rectification transforms for cameras 1 & 2.
4208 Mat om, r_r, r_r13;
4209
4210 if( R13.size() != Size(3,3) )
4211 Rodrigues(R13, r_r13);
4212 else
4213 R13.copyTo(r_r13);
4214
4215 if( R12.size() == Size(3,3) )
4216 Rodrigues(R12, om);
4217 else
4218 R12.copyTo(om);
4219
4220 om *= -0.5;
4221 Rodrigues(om, r_r); // rotate cameras to same orientation by averaging
4222 Mat_<double> t12 = r_r * T12;
4223
4224 int idx = fabs(t12(0,0)) > fabs(t12(1,0)) ? 0 : 1;
4225 double c = t12(idx,0), nt = norm(t12, CV_L2);
4226 CV_Assert(fabs(nt) > 0);
4227 Mat_<double> uu = Mat_<double>::zeros(3,1);
4228 uu(idx, 0) = c > 0 ? 1 : -1;
4229
4230 // calculate global Z rotation
4231 Mat_<double> ww = t12.cross(uu), wR;
4232 double nw = norm(ww, CV_L2);
4233 CV_Assert(fabs(nw) > 0);
4234 ww *= acos(fabs(c)/nt)/nw;
4235 Rodrigues(ww, wR);
4236
4237 // now rotate camera 3 to make its optical axis parallel to cameras 1 and 2.
4238 R3 = wR*r_r.t()*r_r13.t();
4239 Mat_<double> t13 = R3 * T13;
4240
4241 P2.copyTo(P3);
4242 Mat t = P3.col(3);
4243 t13.copyTo(t);
4244 P3.at<double>(0,3) *= P3.at<double>(0,0);
4245 P3.at<double>(1,3) *= P3.at<double>(1,1);
4246
4247 if( !_imgpt1.empty() && !_imgpt3.empty() )
4248 adjust3rdMatrix(_imgpt1, _imgpt3, _cameraMatrix1.getMat(), _distCoeffs1.getMat(),
4249 _cameraMatrix3.getMat(), _distCoeffs3.getMat(), _Rmat1.getMat(), R3, P1, P3);
4250
4251 return (float)((P3.at<double>(idx,3)/P3.at<double>(idx,idx))/
4252 (P2.at<double>(idx,3)/P2.at<double>(idx,idx)));
4253 }
4254
4255
4256 /* End of file. */
4257