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