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 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 #include "_cv.h"
43
44 /*
45 This is stright-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet
46 that is (in a large extent) based on the paper:
47 Z. Zhang. "A flexible new technique for camera calibration".
48 IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
49
50 The 1st initial port was done by Valery Mosyagin.
51 */
52
CvLevMarq()53 CvLevMarq::CvLevMarq()
54 {
55 mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0;
56 lambdaLg10 = 0; state = DONE;
57 criteria = cvTermCriteria(0,0,0);
58 iters = 0;
59 completeSymmFlag = false;
60 }
61
CvLevMarq(int nparams,int nerrs,CvTermCriteria criteria0,bool _completeSymmFlag)62 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
63 {
64 mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0;
65 init(nparams, nerrs, criteria0, _completeSymmFlag);
66 }
67
clear()68 void CvLevMarq::clear()
69 {
70 cvReleaseMat(&mask);
71 cvReleaseMat(&prevParam);
72 cvReleaseMat(¶m);
73 cvReleaseMat(&J);
74 cvReleaseMat(&err);
75 cvReleaseMat(&JtJ);
76 cvReleaseMat(&JtJN);
77 cvReleaseMat(&JtErr);
78 cvReleaseMat(&JtJV);
79 cvReleaseMat(&JtJW);
80 }
81
~CvLevMarq()82 CvLevMarq::~CvLevMarq()
83 {
84 clear();
85 }
86
init(int nparams,int nerrs,CvTermCriteria criteria0,bool _completeSymmFlag)87 void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
88 {
89 if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
90 clear();
91 mask = cvCreateMat( nparams, 1, CV_8U );
92 cvSet(mask, cvScalarAll(1));
93 prevParam = cvCreateMat( nparams, 1, CV_64F );
94 param = cvCreateMat( nparams, 1, CV_64F );
95 JtJ = cvCreateMat( nparams, nparams, CV_64F );
96 JtJN = cvCreateMat( nparams, nparams, CV_64F );
97 JtJV = cvCreateMat( nparams, nparams, CV_64F );
98 JtJW = cvCreateMat( nparams, 1, CV_64F );
99 JtErr = cvCreateMat( nparams, 1, CV_64F );
100 if( nerrs > 0 )
101 {
102 J = cvCreateMat( nerrs, nparams, CV_64F );
103 err = cvCreateMat( nerrs, 1, CV_64F );
104 }
105 prevErrNorm = DBL_MAX;
106 lambdaLg10 = -3;
107 criteria = criteria0;
108 if( criteria.type & CV_TERMCRIT_ITER )
109 criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
110 else
111 criteria.max_iter = 30;
112 if( criteria.type & CV_TERMCRIT_EPS )
113 criteria.epsilon = MAX(criteria.epsilon, 0);
114 else
115 criteria.epsilon = DBL_EPSILON;
116 state = STARTED;
117 iters = 0;
118 completeSymmFlag = _completeSymmFlag;
119 }
120
update(const CvMat * & _param,CvMat * & _J,CvMat * & _err)121 bool CvLevMarq::update( const CvMat*& _param, CvMat*& _J, CvMat*& _err )
122 {
123 double change;
124
125 assert( err != 0 );
126 if( state == DONE )
127 {
128 _param = param;
129 return false;
130 }
131
132 if( state == STARTED )
133 {
134 _param = param;
135 cvZero( J );
136 cvZero( err );
137 _J = J;
138 _err = err;
139 state = CALC_J;
140 return true;
141 }
142
143 if( state == CALC_J )
144 {
145 cvMulTransposed( J, JtJ, 1 );
146 cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
147 cvCopy( param, prevParam );
148 step();
149 if( iters == 0 )
150 prevErrNorm = cvNorm(err, 0, CV_L2);
151 _param = param;
152 cvZero( err );
153 _err = err;
154 state = CHECK_ERR;
155 return true;
156 }
157
158 assert( state == CHECK_ERR );
159 errNorm = cvNorm( err, 0, CV_L2 );
160 if( errNorm > prevErrNorm )
161 {
162 lambdaLg10++;
163 step();
164 _param = param;
165 cvZero( err );
166 _err = err;
167 state = CHECK_ERR;
168 return true;
169 }
170
171 lambdaLg10 = MAX(lambdaLg10-1, -16);
172 if( ++iters >= criteria.max_iter ||
173 (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
174 {
175 _param = param;
176 state = DONE;
177 return true;
178 }
179
180 prevErrNorm = errNorm;
181 _param = param;
182 cvZero(J);
183 _J = J;
184 state = CALC_J;
185 return false;
186 }
187
188
updateAlt(const CvMat * & _param,CvMat * & _JtJ,CvMat * & _JtErr,double * & _errNorm)189 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
190 {
191 double change;
192
193 assert( err == 0 );
194 if( state == DONE )
195 {
196 _param = param;
197 return false;
198 }
199
200 if( state == STARTED )
201 {
202 _param = param;
203 cvZero( JtJ );
204 cvZero( JtErr );
205 errNorm = 0;
206 _JtJ = JtJ;
207 _JtErr = JtErr;
208 _errNorm = &errNorm;
209 state = CALC_J;
210 return true;
211 }
212
213 if( state == CALC_J )
214 {
215 cvCopy( param, prevParam );
216 step();
217 _param = param;
218 prevErrNorm = errNorm;
219 errNorm = 0;
220 _errNorm = &errNorm;
221 state = CHECK_ERR;
222 return true;
223 }
224
225 assert( state == CHECK_ERR );
226 if( errNorm > prevErrNorm )
227 {
228 lambdaLg10++;
229 step();
230 _param = param;
231 errNorm = 0;
232 _errNorm = &errNorm;
233 state = CHECK_ERR;
234 return true;
235 }
236
237 lambdaLg10 = MAX(lambdaLg10-1, -16);
238 if( ++iters >= criteria.max_iter ||
239 (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
240 {
241 _param = param;
242 state = DONE;
243 return false;
244 }
245
246 prevErrNorm = errNorm;
247 cvZero( JtJ );
248 cvZero( JtErr );
249 _param = param;
250 _JtJ = JtJ;
251 _JtErr = JtErr;
252 state = CALC_J;
253 return true;
254 }
255
step()256 void CvLevMarq::step()
257 {
258 const double LOG10 = log(10.);
259 double lambda = exp(lambdaLg10*LOG10);
260 int i, j, nparams = param->rows;
261
262 for( i = 0; i < nparams; i++ )
263 if( mask->data.ptr[i] == 0 )
264 {
265 double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
266 for( j = 0; j < nparams; j++ )
267 row[j] = col[j*nparams] = 0;
268 JtErr->data.db[i] = 0;
269 }
270
271 if( !err )
272 cvCompleteSymm( JtJ, completeSymmFlag );
273 cvSetIdentity( JtJN, cvRealScalar(lambda) );
274 cvAdd( JtJ, JtJN, JtJN );
275 cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
276 cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
277 for( i = 0; i < nparams; i++ )
278 param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
279 }
280
281 // reimplementation of dAB.m
282 CV_IMPL void
cvCalcMatMulDeriv(const CvMat * A,const CvMat * B,CvMat * dABdA,CvMat * dABdB)283 cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
284 {
285 CV_FUNCNAME( "cvCalcMatMulDeriv" );
286
287 __BEGIN__;
288
289 int i, j, M, N, L;
290 int bstep;
291
292 CV_ASSERT( CV_IS_MAT(A) && CV_IS_MAT(B) );
293 CV_ASSERT( CV_ARE_TYPES_EQ(A, B) &&
294 (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) );
295 CV_ASSERT( A->cols == B->rows );
296
297 M = A->rows;
298 L = A->cols;
299 N = B->cols;
300 bstep = B->step/CV_ELEM_SIZE(B->type);
301
302 if( dABdA )
303 {
304 CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdA) &&
305 dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols );
306 }
307
308 if( dABdB )
309 {
310 CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdB) &&
311 dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols );
312 }
313
314 if( CV_MAT_TYPE(A->type) == CV_32F )
315 {
316 for( i = 0; i < M*N; i++ )
317 {
318 int i1 = i / N, i2 = i % N;
319
320 if( dABdA )
321 {
322 float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i);
323 const float* b = (const float*)B->data.ptr + i2;
324
325 for( j = 0; j < M*L; j++ )
326 dcda[j] = 0;
327 for( j = 0; j < L; j++ )
328 dcda[i1*L + j] = b[j*bstep];
329 }
330
331 if( dABdB )
332 {
333 float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i);
334 const float* a = (const float*)(A->data.ptr + A->step*i1);
335
336 for( j = 0; j < L*N; j++ )
337 dcdb[j] = 0;
338 for( j = 0; j < L; j++ )
339 dcdb[j*N + i2] = a[j];
340 }
341 }
342 }
343 else
344 {
345 for( i = 0; i < M*N; i++ )
346 {
347 int i1 = i / N, i2 = i % N;
348
349 if( dABdA )
350 {
351 double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i);
352 const double* b = (const double*)B->data.ptr + i2;
353
354 for( j = 0; j < M*L; j++ )
355 dcda[j] = 0;
356 for( j = 0; j < L; j++ )
357 dcda[i1*L + j] = b[j*bstep];
358 }
359
360 if( dABdB )
361 {
362 double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i);
363 const double* a = (const double*)(A->data.ptr + A->step*i1);
364
365 for( j = 0; j < L*N; j++ )
366 dcdb[j] = 0;
367 for( j = 0; j < L; j++ )
368 dcdb[j*N + i2] = a[j];
369 }
370 }
371 }
372
373 __END__;
374 }
375
376 // reimplementation of compose_motion.m
377 CV_IMPL void
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)378 cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
379 const CvMat* _rvec2, const CvMat* _tvec2,
380 CvMat* _rvec3, CvMat* _tvec3,
381 CvMat* dr3dr1, CvMat* dr3dt1,
382 CvMat* dr3dr2, CvMat* dr3dt2,
383 CvMat* dt3dr1, CvMat* dt3dt1,
384 CvMat* dt3dr2, CvMat* dt3dt2 )
385 {
386 CV_FUNCNAME( "cvComposeRT" );
387
388 __BEGIN__;
389
390 double _r1[3], _r2[3];
391 double _R1[9], _d1[9*3], _R2[9], _d2[9*3];
392 CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2);
393 CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2);
394 CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2);
395
396 CV_ASSERT( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) );
397
398 CV_ASSERT( CV_MAT_TYPE(_rvec1->type) == CV_32F ||
399 CV_MAT_TYPE(_rvec1->type) == CV_64F );
400
401 CV_ASSERT( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) );
402
403 cvConvert( _rvec1, &r1 );
404 cvConvert( _rvec2, &r2 );
405
406 cvRodrigues2( &r1, &R1, &dR1dr1 );
407 cvRodrigues2( &r2, &R2, &dR2dr2 );
408
409 if( _rvec3 || dr3dr1 || dr3dr1 )
410 {
411 double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3];
412 double _W1[9*3], _W2[3*3];
413 CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3);
414 CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2);
415 CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3);
416 CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2);
417
418 cvMatMul( &R2, &R1, &R3 );
419 cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 );
420
421 cvRodrigues2( &R3, &r3, &dr3dR3 );
422
423 if( _rvec3 )
424 cvConvert( &r3, _rvec3 );
425
426 if( dr3dr1 )
427 {
428 cvMatMul( &dr3dR3, &dR3dR1, &W1 );
429 cvMatMul( &W1, &dR1dr1, &W2 );
430 cvConvert( &W2, dr3dr1 );
431 }
432
433 if( dr3dr2 )
434 {
435 cvMatMul( &dr3dR3, &dR3dR2, &W1 );
436 cvMatMul( &W1, &dR2dr2, &W2 );
437 cvConvert( &W2, dr3dr2 );
438 }
439 }
440
441 if( dr3dt1 )
442 cvZero( dr3dt1 );
443 if( dr3dt2 )
444 cvZero( dr3dt2 );
445
446 if( _tvec3 || dt3dr2 || dt3dt1 )
447 {
448 double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3];
449 CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2);
450 CvMat t3 = cvMat(3,1,CV_64F,_t3);
451 CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2);
452 CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1);
453 CvMat W3 = cvMat(3, 3, CV_64F, _W3);
454
455 CV_ASSERT( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) );
456 CV_ASSERT( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) );
457
458 cvConvert( _tvec1, &t1 );
459 cvConvert( _tvec2, &t2 );
460 cvMatMulAdd( &R2, &t1, &t2, &t3 );
461
462 if( _tvec3 )
463 cvConvert( &t3, _tvec3 );
464
465 if( dt3dr2 || dt3dt1 )
466 {
467 cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 );
468 if( dt3dr2 )
469 {
470 cvMatMul( &dxdR2, &dR2dr2, &W3 );
471 cvConvert( &W3, dt3dr2 );
472 }
473 if( dt3dt1 )
474 cvConvert( &dxdt1, dt3dt1 );
475 }
476 }
477
478 if( dt3dt2 )
479 cvSetIdentity( dt3dt2 );
480 if( dt3dr1 )
481 cvZero( dt3dr1 );
482
483 __END__;
484 }
485
486 CV_IMPL int
cvRodrigues2(const CvMat * src,CvMat * dst,CvMat * jacobian)487 cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
488 {
489 int result = 0;
490
491 CV_FUNCNAME( "cvRogrigues2" );
492
493 __BEGIN__;
494
495 int depth, elem_size;
496 int i, k;
497 double J[27];
498 CvMat _J = cvMat( 3, 9, CV_64F, J );
499
500 if( !CV_IS_MAT(src) )
501 CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
502
503 if( !CV_IS_MAT(dst) )
504 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 CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
512
513 if( !CV_ARE_DEPTHS_EQ(src, dst) )
514 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 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 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 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 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 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 _R = 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( &_R, 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 _R = cvMat( 3, 3, CV_64F, R );
612 CvMat _U = cvMat( 3, 3, CV_64F, U );
613 CvMat _V = cvMat( 3, 3, CV_64F, V );
614 CvMat _W = 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 CV_ERROR( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
621
622 cvConvert( src, &_R );
623 if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
624 {
625 cvZero(dst);
626 if( jacobian )
627 cvZero(jacobian);
628 EXIT;
629 }
630
631 cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
632 cvGEMM( &_U, &_V, 1, 0, 0, &_R, 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 = theta*sqrt(MAX(t,0.));
653 t = (R[4] + 1)*0.5;
654 ry = theta*sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.);
655 t = (R[8] + 1)*0.5;
656 rz = theta*sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.);
657 }
658
659 if( jacobian )
660 {
661 memset( J, 0, sizeof(J) );
662 if( c > 0 )
663 {
664 J[5] = J[15] = J[19] = -0.5;
665 J[7] = J[11] = J[21] = 0.5;
666 }
667 }
668 }
669 else
670 {
671 double vth = 1/(2*s);
672
673 if( jacobian )
674 {
675 double t, dtheta_dtr = -1./s;
676 // var1 = [vth;theta]
677 // var = [om1;var1] = [om1;vth;theta]
678 double dvth_dtheta = -vth*c/s;
679 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
680 double d2 = 0.5*dtheta_dtr;
681 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
682 double dvardR[5*9] =
683 {
684 0, 0, 0, 0, 0, 1, 0, -1, 0,
685 0, 0, -1, 0, 0, 0, 1, 0, 0,
686 0, 1, 0, -1, 0, 0, 0, 0, 0,
687 d1, 0, 0, 0, d1, 0, 0, 0, d1,
688 d2, 0, 0, 0, d2, 0, 0, 0, d2
689 };
690 // var2 = [om;theta]
691 double dvar2dvar[] =
692 {
693 vth, 0, 0, rx, 0,
694 0, vth, 0, ry, 0,
695 0, 0, vth, rz, 0,
696 0, 0, 0, 0, 1
697 };
698 double domegadvar2[] =
699 {
700 theta, 0, 0, rx*vth,
701 0, theta, 0, ry*vth,
702 0, 0, theta, rz*vth
703 };
704
705 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
706 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
707 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
708 double t0[3*5];
709 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
710
711 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
712 cvMatMul( &_t0, &_dvardR, &_J );
713
714 // transpose every row of _J (treat the rows as 3x3 matrices)
715 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
716 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
717 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
718 }
719
720 vth *= theta;
721 rx *= vth; ry *= vth; rz *= vth;
722 }
723
724 if( depth == CV_32F )
725 {
726 dst->data.fl[0] = (float)rx;
727 dst->data.fl[step] = (float)ry;
728 dst->data.fl[step*2] = (float)rz;
729 }
730 else
731 {
732 dst->data.db[0] = rx;
733 dst->data.db[step] = ry;
734 dst->data.db[step*2] = rz;
735 }
736 }
737
738 if( jacobian )
739 {
740 if( depth == CV_32F )
741 {
742 if( jacobian->rows == _J.rows )
743 cvConvert( &_J, jacobian );
744 else
745 {
746 float Jf[3*9];
747 CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf );
748 cvConvert( &_J, &_Jf );
749 cvTranspose( &_Jf, jacobian );
750 }
751 }
752 else if( jacobian->rows == _J.rows )
753 cvCopy( &_J, jacobian );
754 else
755 cvTranspose( &_J, jacobian );
756 }
757
758 result = 1;
759
760 __END__;
761
762 return result;
763 }
764
765
766 CV_IMPL void
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)767 cvProjectPoints2( const CvMat* objectPoints,
768 const CvMat* r_vec,
769 const CvMat* t_vec,
770 const CvMat* A,
771 const CvMat* distCoeffs,
772 CvMat* imagePoints, CvMat* dpdr,
773 CvMat* dpdt, CvMat* dpdf,
774 CvMat* dpdc, CvMat* dpdk,
775 double aspectRatio )
776 {
777 CvMat *_M = 0, *_m = 0;
778 CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0;
779
780 CV_FUNCNAME( "cvProjectPoints2" );
781
782 __BEGIN__;
783
784 int i, j, count;
785 int calc_derivatives;
786 const CvPoint3D64f* M;
787 CvPoint2D64f* m;
788 double r[3], R[9], dRdr[27], t[3], a[9], k[5] = {0,0,0,0,0}, fx, fy, cx, cy;
789 CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
790 CvMat _R = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
791 double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
792 int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
793 bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
794
795 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
796 !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
797 /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
798 CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
799
800 count = MAX(objectPoints->rows, objectPoints->cols);
801
802 if( CV_IS_CONT_MAT(objectPoints->type) && CV_MAT_DEPTH(objectPoints->type) == CV_64F &&
803 ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
804 (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3)))
805 _M = (CvMat*)objectPoints;
806 else
807 {
808 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
809 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
810 }
811
812 if( CV_IS_CONT_MAT(imagePoints->type) && CV_MAT_DEPTH(imagePoints->type) == CV_64F &&
813 ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
814 (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2)))
815 _m = imagePoints;
816 else
817 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
818
819 M = (CvPoint3D64f*)_M->data.db;
820 m = (CvPoint2D64f*)_m->data.db;
821
822 if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
823 (((r_vec->rows != 1 && r_vec->cols != 1) ||
824 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
825 ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
826 CV_ERROR( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
827 "floating-point rotation vector, or 3x3 rotation matrix" );
828
829 if( r_vec->rows == 3 && r_vec->cols == 3 )
830 {
831 _r = cvMat( 3, 1, CV_64FC1, r );
832 CV_CALL( cvRodrigues2( r_vec, &_r ));
833 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ));
834 cvCopy( r_vec, &_R );
835 }
836 else
837 {
838 _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
839 CV_CALL( cvConvert( r_vec, &_r ));
840 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ) );
841 }
842
843 if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
844 (t_vec->rows != 1 && t_vec->cols != 1) ||
845 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
846 CV_ERROR( CV_StsBadArg,
847 "Translation vector must be 1x3 or 3x1 floating-point vector" );
848
849 _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
850 CV_CALL( cvConvert( t_vec, &_t ));
851
852 if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
853 A->rows != 3 || A->cols != 3 )
854 CV_ERROR( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
855
856 CV_CALL( cvConvert( A, &_a ));
857 fx = a[0]; fy = a[4];
858 cx = a[2]; cy = a[5];
859
860 if( fixedAspectRatio )
861 fx = fy*aspectRatio;
862
863 if( distCoeffs )
864 {
865 if( !CV_IS_MAT(distCoeffs) ||
866 (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
867 CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
868 (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
869 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
870 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5) )
871 CV_ERROR( CV_StsBadArg,
872 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" );
873
874 _k = cvMat( distCoeffs->rows, distCoeffs->cols,
875 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
876 CV_CALL( cvConvert( distCoeffs, &_k ));
877 }
878
879 if( dpdr )
880 {
881 if( !CV_IS_MAT(dpdr) ||
882 (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
883 CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
884 dpdr->rows != count*2 || dpdr->cols != 3 )
885 CV_ERROR( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
886
887 if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
888 _dpdr = dpdr;
889 else
890 CV_CALL( _dpdr = cvCreateMat( 2*count, 3, CV_64FC1 ));
891 dpdr_p = _dpdr->data.db;
892 dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
893 }
894
895 if( dpdt )
896 {
897 if( !CV_IS_MAT(dpdt) ||
898 (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
899 CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
900 dpdt->rows != count*2 || dpdt->cols != 3 )
901 CV_ERROR( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
902
903 if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
904 _dpdt = dpdt;
905 else
906 CV_CALL( _dpdt = cvCreateMat( 2*count, 3, CV_64FC1 ));
907 dpdt_p = _dpdt->data.db;
908 dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
909 }
910
911 if( dpdf )
912 {
913 if( !CV_IS_MAT(dpdf) ||
914 (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
915 dpdf->rows != count*2 || dpdf->cols != 2 )
916 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
917
918 if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
919 _dpdf = dpdf;
920 else
921 CV_CALL( _dpdf = cvCreateMat( 2*count, 2, CV_64FC1 ));
922 dpdf_p = _dpdf->data.db;
923 dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
924 }
925
926 if( dpdc )
927 {
928 if( !CV_IS_MAT(dpdc) ||
929 (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
930 dpdc->rows != count*2 || dpdc->cols != 2 )
931 CV_ERROR( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
932
933 if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
934 _dpdc = dpdc;
935 else
936 CV_CALL( _dpdc = cvCreateMat( 2*count, 2, CV_64FC1 ));
937 dpdc_p = _dpdc->data.db;
938 dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
939 }
940
941 if( dpdk )
942 {
943 if( !CV_IS_MAT(dpdk) ||
944 (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
945 dpdk->rows != count*2 || (dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
946 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
947
948 if( !distCoeffs )
949 CV_ERROR( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
950
951 if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
952 _dpdk = dpdk;
953 else
954 CV_CALL( _dpdk = cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
955 dpdk_p = _dpdk->data.db;
956 dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
957 }
958
959 calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
960
961 for( i = 0; i < count; i++ )
962 {
963 double X = M[i].x, Y = M[i].y, Z = M[i].z;
964 double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
965 double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
966 double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
967 double r2, r4, r6, a1, a2, a3, cdist;
968 double xd, yd;
969
970 z = z ? 1./z : 1;
971 x *= z; y *= z;
972
973 r2 = x*x + y*y;
974 r4 = r2*r2;
975 r6 = r4*r2;
976 a1 = 2*x*y;
977 a2 = r2 + 2*x*x;
978 a3 = r2 + 2*y*y;
979 cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
980 xd = x*cdist + k[2]*a1 + k[3]*a2;
981 yd = y*cdist + k[2]*a3 + k[3]*a1;
982
983 m[i].x = xd*fx + cx;
984 m[i].y = yd*fy + cy;
985
986 if( calc_derivatives )
987 {
988 if( dpdc_p )
989 {
990 dpdc_p[0] = 1; dpdc_p[1] = 0;
991 dpdc_p[dpdc_step] = 0;
992 dpdc_p[dpdc_step+1] = 1;
993 dpdc_p += dpdc_step*2;
994 }
995
996 if( dpdf_p )
997 {
998 if( fixedAspectRatio )
999 {
1000 dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio;
1001 dpdf_p[dpdf_step] = 0;
1002 dpdf_p[dpdf_step+1] = yd;
1003 }
1004 else
1005 {
1006 dpdf_p[0] = xd; dpdf_p[1] = 0;
1007 dpdf_p[dpdf_step] = 0;
1008 dpdf_p[dpdf_step+1] = yd;
1009 }
1010 dpdf_p += dpdf_step*2;
1011 }
1012
1013 if( dpdk_p )
1014 {
1015 dpdk_p[0] = fx*x*r2;
1016 dpdk_p[1] = fx*x*r4;
1017 dpdk_p[dpdk_step] = fy*y*r2;
1018 dpdk_p[dpdk_step+1] = fy*y*r4;
1019 if( _dpdk->cols > 2 )
1020 {
1021 dpdk_p[2] = fx*a1;
1022 dpdk_p[3] = fx*a2;
1023 dpdk_p[dpdk_step+2] = fy*a3;
1024 dpdk_p[dpdk_step+3] = fy*a1;
1025 if( _dpdk->cols > 4 )
1026 {
1027 dpdk_p[4] = fx*x*r6;
1028 dpdk_p[dpdk_step+4] = fy*y*r6;
1029 }
1030 }
1031 dpdk_p += dpdk_step*2;
1032 }
1033
1034 if( dpdt_p )
1035 {
1036 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
1037 for( j = 0; j < 3; j++ )
1038 {
1039 double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
1040 double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
1041 double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
1042 double dmxdt = fx*(dxdt[j]*cdist + x*dcdist_dt +
1043 k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j]));
1044 double dmydt = fy*(dydt[j]*cdist + y*dcdist_dt +
1045 k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt);
1046 dpdt_p[j] = dmxdt;
1047 dpdt_p[dpdt_step+j] = dmydt;
1048 }
1049 dpdt_p += dpdt_step*2;
1050 }
1051
1052 if( dpdr_p )
1053 {
1054 double dx0dr[] =
1055 {
1056 X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
1057 X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
1058 X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
1059 };
1060 double dy0dr[] =
1061 {
1062 X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
1063 X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
1064 X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
1065 };
1066 double dz0dr[] =
1067 {
1068 X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
1069 X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
1070 X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
1071 };
1072 for( j = 0; j < 3; j++ )
1073 {
1074 double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
1075 double dydr = z*(dy0dr[j] - y*dz0dr[j]);
1076 double dr2dr = 2*x*dxdr + 2*y*dydr;
1077 double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr;
1078 double da1dr = 2*(x*dydr + y*dxdr);
1079 double dmxdr = fx*(dxdr*cdist + x*dcdist_dr +
1080 k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr));
1081 double dmydr = fy*(dydr*cdist + y*dcdist_dr +
1082 k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr);
1083 dpdr_p[j] = dmxdr;
1084 dpdr_p[dpdr_step+j] = dmydr;
1085 }
1086 dpdr_p += dpdr_step*2;
1087 }
1088 }
1089 }
1090
1091 if( _m != imagePoints )
1092 cvConvertPointsHomogeneous( _m, imagePoints );
1093 if( _dpdr != dpdr )
1094 cvConvert( _dpdr, dpdr );
1095 if( _dpdt != dpdt )
1096 cvConvert( _dpdt, dpdt );
1097 if( _dpdf != dpdf )
1098 cvConvert( _dpdf, dpdf );
1099 if( _dpdc != dpdc )
1100 cvConvert( _dpdc, dpdc );
1101 if( _dpdk != dpdk )
1102 cvConvert( _dpdk, dpdk );
1103
1104 __END__;
1105
1106 if( _M != objectPoints )
1107 cvReleaseMat( &_M );
1108 if( _m != imagePoints )
1109 cvReleaseMat( &_m );
1110 if( _dpdr != dpdr )
1111 cvReleaseMat( &_dpdr );
1112 if( _dpdt != dpdt )
1113 cvReleaseMat( &_dpdt );
1114 if( _dpdf != dpdf )
1115 cvReleaseMat( &_dpdf );
1116 if( _dpdc != dpdc )
1117 cvReleaseMat( &_dpdc );
1118 if( _dpdk != dpdk )
1119 cvReleaseMat( &_dpdk );
1120 }
1121
1122
1123 CV_IMPL void
cvFindExtrinsicCameraParams2(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * A,const CvMat * distCoeffs,CvMat * rvec,CvMat * tvec)1124 cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
1125 const CvMat* imagePoints, const CvMat* A,
1126 const CvMat* distCoeffs,
1127 CvMat* rvec, CvMat* tvec )
1128 {
1129 const int max_iter = 20;
1130 CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
1131
1132 CV_FUNCNAME( "cvFindExtrinsicCameraParams2" );
1133
1134 __BEGIN__;
1135
1136 int i, count;
1137 double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];
1138 double MM[9], U[9], V[9], W[3];
1139 CvScalar Mc;
1140 double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
1141 CvMat _A = cvMat( 3, 3, CV_64F, a );
1142 CvMat _Ar = cvMat( 3, 3, CV_64F, ar );
1143 CvMat _R = cvMat( 3, 3, CV_64F, R );
1144 CvMat _r = cvMat( 3, 1, CV_64F, param );
1145 CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
1146 CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val );
1147 CvMat _MM = cvMat( 3, 3, CV_64F, MM );
1148 CvMat _U = cvMat( 3, 3, CV_64F, U );
1149 CvMat _V = cvMat( 3, 3, CV_64F, V );
1150 CvMat _W = cvMat( 3, 1, CV_64F, W );
1151 CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
1152 CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
1153 CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
1154 CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
1155 CvMat _delta = cvMat( 6, 1, CV_64F, delta );
1156 CvMat _param = cvMat( 6, 1, CV_64F, param );
1157 CvMat _dpdr, _dpdt;
1158
1159 CV_ASSERT( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
1160 CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );
1161
1162 count = MAX(objectPoints->cols, objectPoints->rows);
1163 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
1164 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
1165
1166 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
1167 CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m ));
1168 CV_CALL( cvConvert( A, &_A ));
1169
1170 CV_ASSERT( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) &&
1171 (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 );
1172
1173 CV_ASSERT( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) &&
1174 (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 );
1175
1176 CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
1177 CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
1178
1179 // normalize image points
1180 // (unapply the intrinsic matrix transformation and distortion)
1181 cvUndistortPoints( _m, _mn, &_A, distCoeffs, 0, &_Ar );
1182
1183 Mc = cvAvg(_M);
1184 cvReshape( _M, _M, 1, count );
1185 cvMulTransposed( _M, &_MM, 1, &_Mc );
1186 cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
1187
1188 // initialize extrinsic parameters
1189 if( W[2]/W[1] < 1e-3 || count < 4 )
1190 {
1191 // a planar structure case (all M's lie in the same plane)
1192 double tt[3], h[9], h1_norm, h2_norm;
1193 CvMat* R_transform = &_V;
1194 CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
1195 CvMat _H = cvMat( 3, 3, CV_64F, h );
1196 CvMat _h1, _h2, _h3;
1197
1198 if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
1199 cvSetIdentity( R_transform );
1200
1201 if( cvDet(R_transform) < 0 )
1202 cvScale( R_transform, R_transform, -1 );
1203
1204 cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
1205
1206 for( i = 0; i < count; i++ )
1207 {
1208 const double* Rp = R_transform->data.db;
1209 const double* Tp = T_transform.data.db;
1210 const double* src = _M->data.db + i*3;
1211 double* dst = _Mxy->data.db + i*2;
1212
1213 dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
1214 dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
1215 }
1216
1217 cvFindHomography( _Mxy, _mn, &_H );
1218
1219 cvGetCol( &_H, &_h1, 0 );
1220 _h2 = _h1; _h2.data.db++;
1221 _h3 = _h2; _h3.data.db++;
1222 h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
1223 h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
1224
1225 cvScale( &_h1, &_h1, 1./h1_norm );
1226 cvScale( &_h2, &_h2, 1./h2_norm );
1227 cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
1228 cvCrossProduct( &_h1, &_h2, &_h3 );
1229
1230 cvRodrigues2( &_H, &_r );
1231 cvRodrigues2( &_r, &_H );
1232 cvMatMulAdd( &_H, &T_transform, &_t, &_t );
1233 cvMatMul( &_H, R_transform, &_R );
1234 cvRodrigues2( &_R, &_r );
1235 }
1236 else
1237 {
1238 // non-planar structure. Use DLT method
1239 double* L;
1240 double LL[12*12], LW[12], LV[12*12], sc;
1241 CvMat _LL = cvMat( 12, 12, CV_64F, LL );
1242 CvMat _LW = cvMat( 12, 1, CV_64F, LW );
1243 CvMat _LV = cvMat( 12, 12, CV_64F, LV );
1244 CvMat _RRt, _RR, _tt;
1245 CvPoint3D64f* M = (CvPoint3D64f*)_M->data.db;
1246 CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db;
1247
1248 CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
1249 L = _L->data.db;
1250
1251 for( i = 0; i < count; i++, L += 24 )
1252 {
1253 double x = -mn[i].x, y = -mn[i].y;
1254 L[0] = L[16] = M[i].x;
1255 L[1] = L[17] = M[i].y;
1256 L[2] = L[18] = M[i].z;
1257 L[3] = L[19] = 1.;
1258 L[4] = L[5] = L[6] = L[7] = 0.;
1259 L[12] = L[13] = L[14] = L[15] = 0.;
1260 L[8] = x*M[i].x;
1261 L[9] = x*M[i].y;
1262 L[10] = x*M[i].z;
1263 L[11] = x;
1264 L[20] = y*M[i].x;
1265 L[21] = y*M[i].y;
1266 L[22] = y*M[i].z;
1267 L[23] = y;
1268 }
1269
1270 cvMulTransposed( _L, &_LL, 1 );
1271 cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1272 _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
1273 cvGetCols( &_RRt, &_RR, 0, 3 );
1274 cvGetCol( &_RRt, &_tt, 3 );
1275 if( cvDet(&_RR) < 0 )
1276 cvScale( &_RRt, &_RRt, -1 );
1277 sc = cvNorm(&_RR);
1278 cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
1279 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
1280 cvScale( &_tt, &_t, cvNorm(&_R)/sc );
1281 cvRodrigues2( &_R, &_r );
1282 cvReleaseMat( &_L );
1283 }
1284
1285 cvReshape( _M, _M, 3, 1 );
1286 cvReshape( _mn, _mn, 2, 1 );
1287
1288 CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
1289 cvGetCols( _J, &_dpdr, 0, 3 );
1290 cvGetCols( _J, &_dpdt, 3, 6 );
1291
1292 // refine extrinsic parameters using iterative algorithm
1293 for( i = 0; i < max_iter; i++ )
1294 {
1295 double n1, n2;
1296 cvReshape( _mn, _mn, 2, 1 );
1297 cvProjectPoints2( _M, &_r, &_t, &_A, distCoeffs,
1298 _mn, &_dpdr, &_dpdt, 0, 0, 0 );
1299 cvSub( _m, _mn, _mn );
1300 cvReshape( _mn, _mn, 1, 2*count );
1301
1302 cvMulTransposed( _J, &_JtJ, 1 );
1303 cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
1304 cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1305 if( JtJW[5]/JtJW[0] < 1e-12 )
1306 break;
1307 cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
1308 &_delta, CV_SVD_U_T + CV_SVD_V_T );
1309 cvAdd( &_delta, &_param, &_param );
1310 n1 = cvNorm( &_delta );
1311 n2 = cvNorm( &_param );
1312 if( n1/n2 < 1e-10 )
1313 break;
1314 }
1315
1316 _r = cvMat( rvec->rows, rvec->cols,
1317 CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1318 _t = cvMat( tvec->rows, tvec->cols,
1319 CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );
1320
1321 cvConvert( &_r, rvec );
1322 cvConvert( &_t, tvec );
1323
1324 __END__;
1325
1326 cvReleaseMat( &_M );
1327 cvReleaseMat( &_Mxy );
1328 cvReleaseMat( &_m );
1329 cvReleaseMat( &_mn );
1330 cvReleaseMat( &_L );
1331 cvReleaseMat( &_J );
1332 }
1333
1334
1335 CV_IMPL void
cvInitIntrinsicParams2D(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * npoints,CvSize imageSize,CvMat * cameraMatrix,double aspectRatio)1336 cvInitIntrinsicParams2D( const CvMat* objectPoints,
1337 const CvMat* imagePoints,
1338 const CvMat* npoints,
1339 CvSize imageSize,
1340 CvMat* cameraMatrix,
1341 double aspectRatio )
1342 {
1343 CvMat *_A = 0, *_b = 0, *_allH = 0, *_allK = 0;
1344
1345 CV_FUNCNAME( "cvInitIntrinsicParams2D" );
1346
1347 __BEGIN__;
1348
1349 int i, j, pos, nimages, total, ni = 0;
1350 double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
1351 double H[9], f[2];
1352 CvMat _a = cvMat( 3, 3, CV_64F, a );
1353 CvMat _H = cvMat( 3, 3, CV_64F, H );
1354 CvMat _f = cvMat( 2, 1, CV_64F, f );
1355
1356 assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 &&
1357 CV_IS_MAT_CONT(npoints->type) );
1358 nimages = npoints->rows + npoints->cols - 1;
1359
1360 if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 &&
1361 CV_MAT_TYPE(objectPoints->type) != CV_64FC3) ||
1362 (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 &&
1363 CV_MAT_TYPE(imagePoints->type) != CV_64FC2) )
1364 CV_ERROR( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" );
1365
1366 if( objectPoints->rows != 1 || imagePoints->rows != 1 )
1367 CV_ERROR( CV_StsBadSize, "object points and image points must be a single-row matrices" );
1368
1369 _A = cvCreateMat( 2*nimages, 2, CV_64F );
1370 _b = cvCreateMat( 2*nimages, 1, CV_64F );
1371 a[2] = (imageSize.width - 1)*0.5;
1372 a[5] = (imageSize.height - 1)*0.5;
1373 _allH = cvCreateMat( nimages, 9, CV_64F );
1374
1375 total = cvRound(cvSum(npoints).val[0]);
1376
1377 // extract vanishing points in order to obtain initial value for the focal length
1378 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1379 {
1380 double* Ap = _A->data.db + i*4;
1381 double* bp = _b->data.db + i*2;
1382 ni = npoints->data.i[i];
1383 double h[3], v[3], d1[3], d2[3];
1384 double n[4] = {0,0,0,0};
1385 CvMat _m, _M;
1386 cvGetCols( objectPoints, &_M, pos, pos + ni );
1387 cvGetCols( imagePoints, &_m, pos, pos + ni );
1388
1389 cvFindHomography( &_M, &_m, &_H );
1390 memcpy( _allH->data.db + i*9, H, sizeof(H) );
1391
1392 H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
1393 H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
1394
1395 for( j = 0; j < 3; j++ )
1396 {
1397 double t0 = H[j*3], t1 = H[j*3+1];
1398 h[j] = t0; v[j] = t1;
1399 d1[j] = (t0 + t1)*0.5;
1400 d2[j] = (t0 - t1)*0.5;
1401 n[0] += t0*t0; n[1] += t1*t1;
1402 n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
1403 }
1404
1405 for( j = 0; j < 4; j++ )
1406 n[j] = 1./sqrt(n[j]);
1407
1408 for( j = 0; j < 3; j++ )
1409 {
1410 h[j] *= n[0]; v[j] *= n[1];
1411 d1[j] *= n[2]; d2[j] *= n[3];
1412 }
1413
1414 Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
1415 Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
1416 bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
1417 }
1418
1419 cvSolve( _A, _b, &_f, CV_LSQ | CV_SVD );
1420 a[0] = sqrt(fabs(1./f[0]));
1421 a[4] = sqrt(fabs(1./f[1]));
1422 if( aspectRatio != 0 )
1423 {
1424 double tf = (a[0] + a[4])/(aspectRatio + 1.);
1425 a[0] = aspectRatio*tf;
1426 a[4] = tf;
1427 }
1428
1429 cvConvert( &_a, cameraMatrix );
1430
1431 __END__;
1432
1433 cvReleaseMat( &_A );
1434 cvReleaseMat( &_b );
1435 cvReleaseMat( &_allH );
1436 cvReleaseMat( &_allK );
1437 }
1438
1439
1440 /* finds intrinsic and extrinsic camera parameters
1441 from a few views of known calibration pattern */
1442 CV_IMPL void
cvCalibrateCamera2(const CvMat * objectPoints,const CvMat * imagePoints,const CvMat * npoints,CvSize imageSize,CvMat * cameraMatrix,CvMat * distCoeffs,CvMat * rvecs,CvMat * tvecs,int flags)1443 cvCalibrateCamera2( const CvMat* objectPoints,
1444 const CvMat* imagePoints,
1445 const CvMat* npoints,
1446 CvSize imageSize,
1447 CvMat* cameraMatrix, CvMat* distCoeffs,
1448 CvMat* rvecs, CvMat* tvecs,
1449 int flags )
1450 {
1451 const int NINTRINSIC = 9;
1452 CvMat *_M = 0, *_m = 0, *_Ji = 0, *_Je = 0, *_err = 0;
1453 CvLevMarq solver;
1454
1455 CV_FUNCNAME( "cvCalibrateCamera2" );
1456
1457 __BEGIN__;
1458
1459 double A[9], k[5] = {0,0,0,0,0};
1460 CvMat _A = cvMat(3, 3, CV_64F, A), _k;
1461 int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
1462 double aspectRatio = 0.;
1463
1464 // 0. check the parameters & allocate buffers
1465 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
1466 !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
1467 CV_ERROR( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
1468
1469 if( imageSize.width <= 0 || imageSize.height <= 0 )
1470 CV_ERROR( CV_StsOutOfRange, "image width and height must be positive" );
1471
1472 if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
1473 (npoints->rows != 1 && npoints->cols != 1) )
1474 CV_ERROR( CV_StsUnsupportedFormat,
1475 "the array of point counters must be 1-dimensional integer vector" );
1476
1477 nimages = npoints->rows*npoints->cols;
1478 npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
1479
1480 if( rvecs )
1481 {
1482 cn = CV_MAT_CN(rvecs->type);
1483 if( !CV_IS_MAT(rvecs) ||
1484 (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
1485 ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
1486 (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
1487 CV_ERROR( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
1488 "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
1489 }
1490
1491 if( tvecs )
1492 {
1493 cn = CV_MAT_CN(tvecs->type);
1494 if( !CV_IS_MAT(tvecs) ||
1495 (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
1496 ((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
1497 (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
1498 CV_ERROR( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
1499 "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
1500 }
1501
1502 if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
1503 CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
1504 cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
1505 CV_ERROR( CV_StsBadArg,
1506 "Intrinsic parameters must be 3x3 floating-point matrix" );
1507
1508 if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
1509 CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
1510 (distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
1511 (distCoeffs->cols*distCoeffs->rows != 4 &&
1512 distCoeffs->cols*distCoeffs->rows != 5) )
1513 CV_ERROR( CV_StsBadArg,
1514 "Distortion coefficients must be 4x1, 1x4, 5x1 or 1x5 floating-point matrix" );
1515
1516 for( i = 0; i < nimages; i++ )
1517 {
1518 ni = npoints->data.i[i*npstep];
1519 if( ni < 4 )
1520 {
1521 char buf[100];
1522 sprintf( buf, "The number of points in the view #%d is < 4", i );
1523 CV_ERROR( CV_StsOutOfRange, buf );
1524 }
1525 maxPoints = MAX( maxPoints, ni );
1526 total += ni;
1527 }
1528
1529 CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 ));
1530 CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 ));
1531
1532 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
1533 CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m ));
1534
1535 nparams = NINTRINSIC + nimages*6;
1536 CV_CALL( _Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 ));
1537 CV_CALL( _Je = cvCreateMat( maxPoints*2, 6, CV_64FC1 ));
1538 CV_CALL( _err = cvCreateMat( maxPoints*2, 1, CV_64FC1 ));
1539 cvZero( _Ji );
1540
1541 _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
1542 if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 )
1543 flags |= CV_CALIB_FIX_K3;
1544
1545 // 1. initialize intrinsic parameters & LM solver
1546 if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
1547 {
1548 cvConvert( cameraMatrix, &_A );
1549 if( A[0] <= 0 || A[4] <= 0 )
1550 CV_ERROR( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
1551 if( A[2] < 0 || A[2] >= imageSize.width ||
1552 A[5] < 0 || A[5] >= imageSize.height )
1553 CV_ERROR( CV_StsOutOfRange, "Principal point must be within the image" );
1554 if( fabs(A[1]) > 1e-5 )
1555 CV_ERROR( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
1556 if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 ||
1557 fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 )
1558 CV_ERROR( CV_StsOutOfRange,
1559 "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
1560 A[1] = A[3] = A[6] = A[7] = 0.;
1561 A[8] = 1.;
1562
1563 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1564 aspectRatio = A[0]/A[4];
1565 cvConvert( distCoeffs, &_k );
1566 }
1567 else
1568 {
1569 CvScalar mean, sdv;
1570 cvAvgSdv( _M, &mean, &sdv );
1571 if( (fabs(mean.val[2]) > 1e-5 && fabs(mean.val[2] - 1) > 1e-5) || fabs(sdv.val[2]) > 1e-5 )
1572 CV_ERROR( CV_StsBadArg,
1573 "For non-planar calibration rigs the initial intrinsic matrix must be specified" );
1574 for( i = 0; i < total; i++ )
1575 ((CvPoint3D64f*)_M->data.db)[i].z = 0.;
1576
1577 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1578 {
1579 aspectRatio = cvmGet(cameraMatrix,0,0);
1580 aspectRatio /= cvmGet(cameraMatrix,1,1);
1581 if( aspectRatio < 0.01 || aspectRatio > 100 )
1582 CV_ERROR( CV_StsOutOfRange,
1583 "The specified aspect ratio (=A[0][0]/A[1][1]) is incorrect" );
1584 }
1585 cvInitIntrinsicParams2D( _M, _m, npoints, imageSize, &_A, aspectRatio );
1586 }
1587
1588 solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) );
1589
1590 {
1591 double* param = solver.param->data.db;
1592 uchar* mask = solver.mask->data.ptr;
1593
1594 param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5];
1595 param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3];
1596 param[8] = k[4];
1597
1598 if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
1599 mask[0] = mask[1] = 0;
1600 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1601 mask[2] = mask[3] = 0;
1602 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1603 {
1604 param[6] = param[7] = 0;
1605 mask[6] = mask[7] = 0;
1606 }
1607 if( flags & CV_CALIB_FIX_K1 )
1608 mask[4] = 0;
1609 if( flags & CV_CALIB_FIX_K2 )
1610 mask[5] = 0;
1611 if( flags & CV_CALIB_FIX_K3 )
1612 mask[8] = 0;
1613 }
1614
1615 // 2. initialize extrinsic parameters
1616 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1617 {
1618 CvMat _Mi, _mi, _ri, _ti;
1619 ni = npoints->data.i[i*npstep];
1620
1621 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1622 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1623
1624 cvGetCols( _M, &_Mi, pos, pos + ni );
1625 cvGetCols( _m, &_mi, pos, pos + ni );
1626
1627 cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_A, &_k, &_ri, &_ti );
1628 }
1629
1630 // 3. run the optimization
1631 for(;;)
1632 {
1633 const CvMat* _param = 0;
1634 CvMat *_JtJ = 0, *_JtErr = 0;
1635 double* _errNorm = 0;
1636 bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
1637 double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
1638
1639 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1640 {
1641 param[0] = param[1]*aspectRatio;
1642 pparam[0] = pparam[1]*aspectRatio;
1643 }
1644
1645 A[0] = param[0]; A[4] = param[1];
1646 A[2] = param[2]; A[5] = param[3];
1647 k[0] = param[4]; k[1] = param[5]; k[2] = param[6];
1648 k[3] = param[7];
1649 k[4] = param[8];
1650
1651 if( !proceed )
1652 break;
1653
1654 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1655 {
1656 CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part;
1657 ni = npoints->data.i[i*npstep];
1658
1659 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1660 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1661
1662 cvGetCols( _M, &_Mi, pos, pos + ni );
1663 cvGetCols( _m, &_mi, pos, pos + ni );
1664
1665 _Je->rows = _Ji->rows = _err->rows = ni*2;
1666 cvGetCols( _Je, &_dpdr, 0, 3 );
1667 cvGetCols( _Je, &_dpdt, 3, 6 );
1668 cvGetCols( _Ji, &_dpdf, 0, 2 );
1669 cvGetCols( _Ji, &_dpdc, 2, 4 );
1670 cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC );
1671 cvReshape( _err, &_mp, 2, 1 );
1672
1673 if( _JtJ || _JtErr )
1674 {
1675 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp, &_dpdr, &_dpdt,
1676 (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
1677 (flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
1678 (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
1679 }
1680 else
1681 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp );
1682
1683 cvSub( &_mp, &_mi, &_mp );
1684
1685 if( _JtJ || _JtErr )
1686 {
1687 cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) );
1688 cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
1689
1690 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) );
1691 cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1692
1693 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) );
1694 cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1695
1696 cvGetRows( _JtErr, &_part, 0, NINTRINSIC );
1697 cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T );
1698
1699 cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 );
1700 cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
1701 }
1702
1703 if( _errNorm )
1704 {
1705 double errNorm = cvNorm( &_mp, 0, CV_L2 );
1706 *_errNorm += errNorm*errNorm;
1707 }
1708 }
1709 }
1710
1711 // 4. store the results
1712 cvConvert( &_A, cameraMatrix );
1713 cvConvert( &_k, distCoeffs );
1714
1715 for( i = 0; i < nimages; i++ )
1716 {
1717 CvMat src, dst;
1718 if( rvecs )
1719 {
1720 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
1721 if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
1722 {
1723 dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
1724 rvecs->data.ptr + rvecs->step*i );
1725 cvRodrigues2( &src, &_A );
1726 cvConvert( &_A, &dst );
1727 }
1728 else
1729 {
1730 dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
1731 rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
1732 rvecs->data.ptr + rvecs->step*i );
1733 cvConvert( &src, &dst );
1734 }
1735 }
1736 if( tvecs )
1737 {
1738 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
1739 dst = cvMat( 3, 1, CV_MAT_TYPE(tvecs->type), tvecs->rows == 1 ?
1740 tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
1741 tvecs->data.ptr + tvecs->step*i );
1742 cvConvert( &src, &dst );
1743 }
1744 }
1745
1746 __END__;
1747
1748 cvReleaseMat( &_M );
1749 cvReleaseMat( &_m );
1750 cvReleaseMat( &_Ji );
1751 cvReleaseMat( &_Je );
1752 cvReleaseMat( &_err );
1753 }
1754
1755
cvCalibrationMatrixValues(const CvMat * calibMatr,CvSize imgSize,double apertureWidth,double apertureHeight,double * fovx,double * fovy,double * focalLength,CvPoint2D64f * principalPoint,double * pasp)1756 void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize,
1757 double apertureWidth, double apertureHeight, double *fovx, double *fovy,
1758 double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
1759 {
1760 double alphax, alphay, mx, my;
1761 int imgWidth = imgSize.width, imgHeight = imgSize.height;
1762
1763 CV_FUNCNAME("cvCalibrationMatrixValues");
1764 __BEGIN__;
1765
1766 /* Validate parameters. */
1767
1768 if(calibMatr == 0)
1769 CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
1770
1771 if(!CV_IS_MAT(calibMatr))
1772 CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
1773
1774 if(calibMatr->cols != 3 || calibMatr->rows != 3)
1775 CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
1776
1777 alphax = cvmGet(calibMatr, 0, 0);
1778 alphay = cvmGet(calibMatr, 1, 1);
1779 assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0);
1780
1781 /* Calculate pixel aspect ratio. */
1782 if(pasp)
1783 *pasp = alphay / alphax;
1784
1785 /* Calculate number of pixel per realworld unit. */
1786
1787 if(apertureWidth != 0.0 && apertureHeight != 0.0) {
1788 mx = imgWidth / apertureWidth;
1789 my = imgHeight / apertureHeight;
1790 } else {
1791 mx = 1.0;
1792 my = *pasp;
1793 }
1794
1795 /* Calculate fovx and fovy. */
1796
1797 if(fovx)
1798 *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI;
1799
1800 if(fovy)
1801 *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI;
1802
1803 /* Calculate focal length. */
1804
1805 if(focalLength)
1806 *focalLength = alphax / mx;
1807
1808 /* Calculate principle point. */
1809
1810 if(principalPoint)
1811 *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my);
1812
1813 __END__;
1814 }
1815
1816
1817 //////////////////////////////// Stereo Calibration ///////////////////////////////////
1818
dbCmp(const void * _a,const void * _b)1819 static int dbCmp( const void* _a, const void* _b )
1820 {
1821 double a = *(const double*)_a;
1822 double b = *(const double*)_b;
1823
1824 return (a > b) - (a < b);
1825 }
1826
1827
cvStereoCalibrate(const CvMat * _objectPoints,const CvMat * _imagePoints1,const CvMat * _imagePoints2,const CvMat * _npoints,CvMat * _cameraMatrix1,CvMat * _distCoeffs1,CvMat * _cameraMatrix2,CvMat * _distCoeffs2,CvSize imageSize,CvMat * _R,CvMat * _T,CvMat * _E,CvMat * _F,CvTermCriteria termCrit,int flags)1828 void cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
1829 const CvMat* _imagePoints2, const CvMat* _npoints,
1830 CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
1831 CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
1832 CvSize imageSize, CvMat* _R, CvMat* _T,
1833 CvMat* _E, CvMat* _F,
1834 CvTermCriteria termCrit, int flags )
1835 {
1836 const int NINTRINSIC = 9;
1837 CvMat* npoints = 0;
1838 CvMat* err = 0;
1839 CvMat* J_LR = 0;
1840 CvMat* Je = 0;
1841 CvMat* Ji = 0;
1842 CvMat* imagePoints[2] = {0,0};
1843 CvMat* objectPoints = 0;
1844 CvMat* RT0 = 0;
1845 CvLevMarq solver;
1846
1847 CV_FUNCNAME( "cvStereoCalibrate" );
1848
1849 __BEGIN__;
1850
1851 double A[2][9], dk[2][5]={{0,0,0,0,0},{0,0,0,0,0}}, rlr[9];
1852 CvMat K[2], Dist[2], om_LR, T_LR;
1853 CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
1854 int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
1855 int nparams;
1856 bool recomputeIntrinsics = false;
1857 double aspectRatio[2] = {0,0};
1858
1859 CV_ASSERT( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) &&
1860 CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) &&
1861 CV_IS_MAT(_R) && CV_IS_MAT(_T) );
1862
1863 CV_ASSERT( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) &&
1864 CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) );
1865
1866 CV_ASSERT( (_npoints->cols == 1 || _npoints->rows == 1) &&
1867 CV_MAT_TYPE(_npoints->type) == CV_32SC1 );
1868
1869 nimages = _npoints->cols + _npoints->rows - 1;
1870 npoints = cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type );
1871 cvCopy( _npoints, npoints );
1872
1873 for( i = 0, pointsTotal = 0; i < nimages; i++ )
1874 {
1875 maxPoints = MAX(maxPoints, npoints->data.i[i]);
1876 pointsTotal += npoints->data.i[i];
1877 }
1878
1879 objectPoints = cvCreateMat( _objectPoints->rows, _objectPoints->cols,
1880 CV_64FC(CV_MAT_CN(_objectPoints->type)));
1881 cvConvert( _objectPoints, objectPoints );
1882 cvReshape( objectPoints, objectPoints, 3, 1 );
1883
1884 for( k = 0; k < 2; k++ )
1885 {
1886 const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2;
1887 const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
1888 const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
1889
1890 int cn = CV_MAT_CN(_imagePoints1->type);
1891 CV_ASSERT( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F ||
1892 CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) &&
1893 ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) ||
1894 (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) );
1895
1896 K[k] = cvMat(3,3,CV_64F,A[k]);
1897 Dist[k] = cvMat(1,5,CV_64F,dk[k]);
1898
1899 imagePoints[k] = cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type)));
1900 cvConvert( points, imagePoints[k] );
1901 cvReshape( imagePoints[k], imagePoints[k], 2, 1 );
1902
1903 if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS|
1904 CV_CALIB_FIX_ASPECT_RATIO|CV_CALIB_FIX_FOCAL_LENGTH) )
1905 cvConvert( cameraMatrix, &K[k] );
1906
1907 if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS|
1908 CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3) )
1909 {
1910 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
1911 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
1912 cvConvert( distCoeffs, &tdist );
1913 }
1914
1915 if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS)))
1916 {
1917 cvCalibrateCamera2( objectPoints, imagePoints[k],
1918 npoints, imageSize, &K[k], &Dist[k], 0, 0, flags );
1919 }
1920 }
1921
1922 if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
1923 {
1924 static const int avg_idx[] = { 0, 4, 2, 5, -1 };
1925 for( k = 0; avg_idx[k] >= 0; k++ )
1926 A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5;
1927 }
1928
1929 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1930 {
1931 for( k = 0; k < 2; k++ )
1932 aspectRatio[k] = A[k][0]/A[k][4];
1933 }
1934
1935 recomputeIntrinsics = (flags & CV_CALIB_FIX_INTRINSIC) == 0;
1936
1937 err = cvCreateMat( maxPoints*2, 1, CV_64F );
1938 Je = cvCreateMat( maxPoints*2, 6, CV_64F );
1939 J_LR = cvCreateMat( maxPoints*2, 6, CV_64F );
1940 Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F );
1941 cvZero( Ji );
1942
1943 // we optimize for the inter-camera R(3),t(3), then, optionally,
1944 // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
1945 nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0);
1946
1947 // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
1948 RT0 = cvCreateMat( 6, nimages, CV_64F );
1949
1950 solver.init( nparams, 0, termCrit );
1951 if( recomputeIntrinsics )
1952 {
1953 uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2;
1954 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1955 imask[0] = imask[NINTRINSIC] = 0;
1956 if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
1957 imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0;
1958 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1959 imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0;
1960 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1961 imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0;
1962 if( flags & CV_CALIB_FIX_K1 )
1963 imask[4] = imask[NINTRINSIC+4] = 0;
1964 if( flags & CV_CALIB_FIX_K2 )
1965 imask[5] = imask[NINTRINSIC+5] = 0;
1966 if( flags & CV_CALIB_FIX_K3 )
1967 imask[8] = imask[NINTRINSIC+8] = 0;
1968 }
1969
1970 /*
1971 Compute initial estimate of pose
1972
1973 For each image, compute:
1974 R(om) is the rotation matrix of om
1975 om(R) is the rotation vector of R
1976 R_ref = R(om_right) * R(om_left)'
1977 T_ref_list = [T_ref_list; T_right - R_ref * T_left]
1978 om_ref_list = {om_ref_list; om(R_ref)]
1979
1980 om = median(om_ref_list)
1981 T = median(T_ref_list)
1982 */
1983 for( i = ofs = 0; i < nimages; ofs += ni, i++ )
1984 {
1985 ni = npoints->data.i[i];
1986 CvMat objpt_i;
1987 double _om[2][3], r[2][9], t[2][3];
1988 CvMat om[2], R[2], T[2], imgpt_i[2];
1989
1990 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
1991 for( k = 0; k < 2; k++ )
1992 {
1993 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
1994 om[k] = cvMat(3, 1, CV_64F, _om[k]);
1995 R[k] = cvMat(3, 3, CV_64F, r[k]);
1996 T[k] = cvMat(3, 1, CV_64F, t[k]);
1997
1998 // FIXME: here we ignore activePoints[k] because of
1999 // the limited API of cvFindExtrnisicCameraParams2
2000 cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] );
2001 cvRodrigues2( &om[k], &R[k] );
2002 if( k == 0 )
2003 {
2004 // save initial om_left and T_left
2005 solver.param->data.db[(i+1)*6] = _om[0][0];
2006 solver.param->data.db[(i+1)*6 + 1] = _om[0][1];
2007 solver.param->data.db[(i+1)*6 + 2] = _om[0][2];
2008 solver.param->data.db[(i+1)*6 + 3] = t[0][0];
2009 solver.param->data.db[(i+1)*6 + 4] = t[0][1];
2010 solver.param->data.db[(i+1)*6 + 5] = t[0][2];
2011 }
2012 }
2013 cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T );
2014 cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] );
2015 cvRodrigues2( &R[0], &T[0] );
2016 RT0->data.db[i] = t[0][0];
2017 RT0->data.db[i + nimages] = t[0][1];
2018 RT0->data.db[i + nimages*2] = t[0][2];
2019 RT0->data.db[i + nimages*3] = t[1][0];
2020 RT0->data.db[i + nimages*4] = t[1][1];
2021 RT0->data.db[i + nimages*5] = t[1][2];
2022 }
2023
2024 // find the medians and save the first 6 parameters
2025 for( i = 0; i < 6; i++ )
2026 {
2027 qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp );
2028 solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] :
2029 (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5;
2030 }
2031
2032 if( recomputeIntrinsics )
2033 for( k = 0; k < 2; k++ )
2034 {
2035 double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC;
2036 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
2037 dk[k][2] = dk[k][3] = 0;
2038 iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5];
2039 iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2];
2040 iparam[7] = dk[k][3]; iparam[8] = dk[k][4];
2041 }
2042
2043 om_LR = cvMat(3, 1, CV_64F, solver.param->data.db);
2044 T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3);
2045
2046 for(;;)
2047 {
2048 const CvMat* param = 0;
2049 CvMat tmpimagePoints;
2050 CvMat *JtJ = 0, *JtErr = 0;
2051 double* errNorm = 0;
2052 double _omR[3], _tR[3];
2053 double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9];
2054 CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1);
2055 CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2);
2056 //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1);
2057 CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2);
2058 CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1);
2059 CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2);
2060 CvMat om[2], T[2], imgpt_i[2];
2061 CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr;
2062 CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0;
2063
2064 if( !solver.updateAlt( param, JtJ, JtErr, errNorm ))
2065 break;
2066
2067 cvRodrigues2( &om_LR, &R_LR );
2068 om[1] = cvMat(3,1,CV_64F,_omR);
2069 T[1] = cvMat(3,1,CV_64F,_tR);
2070
2071 if( recomputeIntrinsics )
2072 {
2073 double* iparam = solver.param->data.db + (nimages+1)*6;
2074 double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
2075 dpdf = &dpdf_hdr;
2076 dpdc = &dpdc_hdr;
2077 dpdk = &dpdk_hdr;
2078 if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
2079 {
2080 iparam[NINTRINSIC] = iparam[0];
2081 iparam[NINTRINSIC+1] = iparam[1];
2082 ipparam[NINTRINSIC] = ipparam[0];
2083 ipparam[NINTRINSIC+1] = ipparam[1];
2084 }
2085 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
2086 {
2087 iparam[0] = iparam[1]*aspectRatio[0];
2088 iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1];
2089 ipparam[0] = ipparam[1]*aspectRatio[0];
2090 ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1];
2091 }
2092 for( k = 0; k < 2; k++ )
2093 {
2094 A[k][0] = iparam[k*NINTRINSIC+0];
2095 A[k][4] = iparam[k*NINTRINSIC+1];
2096 A[k][2] = iparam[k*NINTRINSIC+2];
2097 A[k][5] = iparam[k*NINTRINSIC+3];
2098 dk[k][0] = iparam[k*NINTRINSIC+4];
2099 dk[k][1] = iparam[k*NINTRINSIC+5];
2100 dk[k][2] = iparam[k*NINTRINSIC+6];
2101 dk[k][3] = iparam[k*NINTRINSIC+7];
2102 dk[k][4] = iparam[k*NINTRINSIC+8];
2103 }
2104 }
2105
2106 for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2107 {
2108 ni = npoints->data.i[i];
2109 CvMat objpt_i, _part;
2110
2111 om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6);
2112 T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3);
2113
2114 if( JtJ || JtErr )
2115 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0,
2116 &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 );
2117 else
2118 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
2119
2120 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2121 err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2;
2122 cvReshape( err, &tmpimagePoints, 2, 1 );
2123
2124 cvGetCols( Ji, &dpdf_hdr, 0, 2 );
2125 cvGetCols( Ji, &dpdc_hdr, 2, 4 );
2126 cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC );
2127 cvGetCols( Je, &dpdrot_hdr, 0, 3 );
2128 cvGetCols( Je, &dpdt_hdr, 3, 6 );
2129
2130 for( k = 0; k < 2; k++ )
2131 {
2132 double maxErr, l2err;
2133 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2134
2135 if( JtJ || JtErr )
2136 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k],
2137 &tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk,
2138 (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0);
2139 else
2140 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
2141 cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
2142
2143 l2err = cvNorm( &tmpimagePoints, 0, CV_L2 );
2144 maxErr = cvNorm( &tmpimagePoints, 0, CV_C );
2145
2146 if( JtJ || JtErr )
2147 {
2148 int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
2149 assert( JtJ && JtErr );
2150
2151 if( k == 1 )
2152 {
2153 // d(err_{x|y}R) ~ de3
2154 // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
2155 for( p = 0; p < ni*2; p++ )
2156 {
2157 CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p );
2158 CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 );
2159 CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p );
2160 CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 );
2161 double _de3dr1[3], _de3dt1[3];
2162 CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 );
2163 CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 );
2164
2165 cvMatMul( &de3dr3, &dr3dr1, &de3dr1 );
2166 cvMatMul( &de3dt3, &dt3dt1, &de3dt1 );
2167
2168 cvMatMul( &de3dr3, &dr3dr2, &de3dr2 );
2169 cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 );
2170
2171 cvMatMul( &de3dt3, &dt3dt2, &de3dt2 );
2172
2173 cvCopy( &de3dr1, &de3dr3 );
2174 cvCopy( &de3dt1, &de3dt3 );
2175 }
2176
2177 cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) );
2178 cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T );
2179
2180 cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) );
2181 cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T );
2182
2183 cvGetRows( JtErr, &_part, 0, 6 );
2184 cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2185 }
2186
2187 cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) );
2188 cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T );
2189
2190 cvGetRows( JtErr, &_part, eofs, eofs + 6 );
2191 cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2192
2193 if( recomputeIntrinsics )
2194 {
2195 cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) );
2196 cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2197 cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) );
2198 cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2199 if( k == 1 )
2200 {
2201 cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) );
2202 cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2203 }
2204 cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC );
2205 cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2206 }
2207 }
2208
2209 if( errNorm )
2210 *errNorm += l2err*l2err;
2211 }
2212 }
2213 }
2214
2215 cvRodrigues2( &om_LR, &R_LR );
2216 if( _R->rows == 1 || _R->cols == 1 )
2217 cvConvert( &om_LR, _R );
2218 else
2219 cvConvert( &R_LR, _R );
2220 cvConvert( &T_LR, _T );
2221
2222 if( recomputeIntrinsics )
2223 {
2224 cvConvert( &K[0], _cameraMatrix1 );
2225 cvConvert( &K[1], _cameraMatrix2 );
2226
2227 for( k = 0; k < 2; k++ )
2228 {
2229 CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
2230 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
2231 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
2232 cvConvert( &tdist, distCoeffs );
2233 }
2234 }
2235
2236 if( _E || _F )
2237 {
2238 double* t = T_LR.data.db;
2239 double tx[] =
2240 {
2241 0, -t[2], t[1],
2242 t[2], 0, -t[0],
2243 -t[1], t[0], 0
2244 };
2245 CvMat Tx = cvMat(3, 3, CV_64F, tx);
2246 double e[9], f[9];
2247 CvMat E = cvMat(3, 3, CV_64F, e);
2248 CvMat F = cvMat(3, 3, CV_64F, f);
2249 cvMatMul( &Tx, &R_LR, &E );
2250 if( _E )
2251 cvConvert( &E, _E );
2252 if( _F )
2253 {
2254 double ik[9];
2255 CvMat iK = cvMat(3, 3, CV_64F, ik);
2256 cvInvert(&K[1], &iK);
2257 cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );
2258 cvInvert(&K[0], &iK);
2259 cvMatMul(&E, &iK, &F);
2260 cvConvertScale( &F, _F, fabs(f[8]) > 0 ? 1./f[8] : 1 );
2261 }
2262 }
2263
2264 __END__;
2265
2266 cvReleaseMat( &npoints );
2267 cvReleaseMat( &err );
2268 cvReleaseMat( &J_LR );
2269 cvReleaseMat( &Je );
2270 cvReleaseMat( &Ji );
2271 cvReleaseMat( &RT0 );
2272 cvReleaseMat( &objectPoints );
2273 cvReleaseMat( &imagePoints[0] );
2274 cvReleaseMat( &imagePoints[1] );
2275 }
2276
2277
cvStereoRectify(const CvMat * _cameraMatrix1,const CvMat * _cameraMatrix2,const CvMat * _distCoeffs1,const CvMat * _distCoeffs2,CvSize imageSize,const CvMat * _R,const CvMat * _T,CvMat * _R1,CvMat * _R2,CvMat * _P1,CvMat * _P2,CvMat * _Q,int flags)2278 void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
2279 const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
2280 CvSize imageSize, const CvMat* _R, const CvMat* _T,
2281 CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
2282 CvMat* _Q, int flags )
2283 {
2284 double _om[3], _t[3], _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
2285 double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3];
2286 CvMat om = cvMat(3, 1, CV_64F, _om);
2287 CvMat t = cvMat(3, 1, CV_64F, _t);
2288 CvMat uu = cvMat(3, 1, CV_64F, _uu);
2289 CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
2290 CvMat pp = cvMat(3, 4, CV_64F, _pp);
2291 CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps
2292 CvMat wR = cvMat(3, 3, CV_64F, _wr);
2293 CvMat Z = cvMat(3, 1, CV_64F, _z);
2294 CvMat Ri = cvMat(3, 3, CV_64F, _ri);
2295 double nx = imageSize.width, ny = imageSize.height;
2296 int i, k;
2297
2298 if( _R->rows == 3 && _R->cols == 3 )
2299 cvRodrigues2(_R, &om); // get vector rotation
2300 else
2301 cvConvert(_R, &om); // it's already a rotation vector
2302 cvConvertScale(&om, &om, -0.5); // get average rotation
2303 cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging
2304 cvMatMul(&r_r, _T, &t);
2305
2306 int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
2307 double c = _t[idx], nt = cvNorm(&t, 0, CV_L2);
2308 _uu[idx] = c > 0 ? 1 : -1;
2309
2310 // calculate global Z rotation
2311 cvCrossProduct(&t,&uu,&ww);
2312 double nw = cvNorm(&ww, 0, CV_L2);
2313 cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);
2314 cvRodrigues2(&ww, &wR);
2315
2316 // apply to both views
2317 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
2318 cvConvert( &Ri, _R1 );
2319 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
2320 cvConvert( &Ri, _R2 );
2321 cvMatMul(&r_r, _T, &t);
2322
2323 // calculate projection/camera matrices
2324 // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
2325 double fc_new = DBL_MAX;
2326 CvPoint2D64f cc_new[2] = {{0,0}, {0,0}};
2327
2328 for( k = 0; k < 2; k++ )
2329 {
2330 const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2331 const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
2332 CvPoint2D32f _pts[4];
2333 CvPoint3D32f _pts_3[4];
2334 CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
2335 CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
2336 double fc, dk1 = Dk ? cvmGet(Dk, 0, 0) : 0;
2337
2338 fc = cvmGet(A,idx^1,idx^1);
2339 if( dk1 < 0 )
2340 fc *= 1 + 0.2*dk1*(nx*nx + ny*ny)/(8*fc*fc);
2341 fc_new = MIN(fc_new, fc);
2342
2343 for( i = 0; i < 4; i++ )
2344 {
2345 _pts[i].x = (float)(((i % 2) + 0.5)*nx*0.5);
2346 _pts[i].y = (float)(((i / 2) + 0.5)*ny*0.5);
2347 }
2348 cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );
2349 cvConvertPointsHomogeneous( &pts, &pts_3 );
2350 cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, A, 0, &pts );
2351 CvScalar avg = cvAvg(&pts);
2352 cc_new[k].x = avg.val[0];
2353 cc_new[k].y = avg.val[1];
2354 }
2355
2356 // vertical focal length must be the same for both images to keep the epipolar constraint
2357 // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)
2358 // use fy for fx also, for simplicity
2359
2360 // For simplicity, set the principal points for both cameras to be the average
2361 // of the two principal points (either one of or both x- and y- coordinates)
2362 if( flags & CV_CALIB_ZERO_DISPARITY )
2363 {
2364 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2365 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2366 }
2367 else if( idx == 0 ) // horizontal stereo
2368 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2369 else // vertical stereo
2370 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2371
2372 cvZero( &pp );
2373 _pp[0][0] = _pp[1][1] = fc_new;
2374 _pp[0][2] = cc_new[0].x;
2375 _pp[1][2] = cc_new[0].y;
2376 _pp[2][2] = 1;
2377 cvConvert(&pp, _P1);
2378
2379 _pp[0][2] = cc_new[1].x;
2380 _pp[1][2] = cc_new[1].y;
2381 _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
2382 cvConvert(&pp, _P2);
2383
2384 if( _Q )
2385 {
2386 double q[] =
2387 {
2388 1, 0, 0, -cc_new[0].x,
2389 0, 1, 0, -cc_new[0].y,
2390 0, 0, 0, fc_new,
2391 0, 0, 1./_t[idx],
2392 (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
2393 };
2394 CvMat Q = cvMat(4, 4, CV_64F, q);
2395 cvConvert( &Q, _Q );
2396 }
2397 }
2398
2399
2400 CV_IMPL int
cvStereoRectifyUncalibrated(const CvMat * _points1,const CvMat * _points2,const CvMat * F0,CvSize imgSize,CvMat * _H1,CvMat * _H2,double threshold)2401 cvStereoRectifyUncalibrated(
2402 const CvMat* _points1, const CvMat* _points2,
2403 const CvMat* F0, CvSize imgSize, CvMat* _H1, CvMat* _H2, double threshold )
2404 {
2405 int result = 0;
2406 CvMat* _m1 = 0;
2407 CvMat* _m2 = 0;
2408 CvMat* _lines1 = 0;
2409 CvMat* _lines2 = 0;
2410
2411 CV_FUNCNAME( "cvStereoCalcHomographiesFromF" );
2412
2413 __BEGIN__;
2414
2415 int i, j, npoints;
2416 double cx, cy;
2417 double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3];
2418 CvMat E2 = cvMat( 3, 1, CV_64F, e2 );
2419 CvMat U = cvMat( 3, 3, CV_64F, u );
2420 CvMat V = cvMat( 3, 3, CV_64F, v );
2421 CvMat W = cvMat( 3, 3, CV_64F, w );
2422 CvMat F = cvMat( 3, 3, CV_64F, f );
2423 CvMat H1 = cvMat( 3, 3, CV_64F, h1 );
2424 CvMat H2 = cvMat( 3, 3, CV_64F, h2 );
2425 CvMat H0 = cvMat( 3, 3, CV_64F, h0 );
2426
2427 CvPoint2D64f* m1;
2428 CvPoint2D64f* m2;
2429 CvPoint3D64f* lines1;
2430 CvPoint3D64f* lines2;
2431
2432 CV_ASSERT( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) &&
2433 (_points1->rows == 1 || _points1->cols == 1) &&
2434 (_points2->rows == 1 || _points2->cols == 1) &&
2435 CV_ARE_SIZES_EQ(_points1, _points2) );
2436
2437 npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2;
2438
2439 _m1 = cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) );
2440 _m2 = cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) );
2441 _lines1 = cvCreateMat( 1, npoints, CV_64FC3 );
2442 _lines2 = cvCreateMat( 1, npoints, CV_64FC3 );
2443
2444 cvConvert( F0, &F );
2445
2446 cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T );
2447 W.data.db[8] = 0.;
2448 cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T );
2449 cvMatMul( &W, &V, &F );
2450
2451 cx = cvRound( (imgSize.width-1)*0.5 );
2452 cy = cvRound( (imgSize.height-1)*0.5 );
2453
2454 cvZero( _H1 );
2455 cvZero( _H2 );
2456
2457 cvConvert( _points1, _m1 );
2458 cvConvert( _points2, _m2 );
2459 cvReshape( _m1, _m1, 2, 1 );
2460 cvReshape( _m1, _m1, 2, 1 );
2461
2462 m1 = (CvPoint2D64f*)_m1->data.ptr;
2463 m2 = (CvPoint2D64f*)_m2->data.ptr;
2464 lines1 = (CvPoint3D64f*)_lines1->data.ptr;
2465 lines2 = (CvPoint3D64f*)_lines2->data.ptr;
2466
2467 if( threshold > 0 )
2468 {
2469 cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 );
2470 cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 );
2471
2472 // measure distance from points to the corresponding epilines, mark outliers
2473 for( i = j = 0; i < npoints; i++ )
2474 {
2475 if( fabs(m1[i].x*lines2[i].x +
2476 m1[i].y*lines2[i].y +
2477 lines2[i].z) <= threshold &&
2478 fabs(m2[i].x*lines1[i].x +
2479 m2[i].y*lines1[i].y +
2480 lines1[i].z) <= threshold )
2481 {
2482 if( j > i )
2483 {
2484 m1[j] = m1[i];
2485 m2[j] = m2[i];
2486 }
2487 j++;
2488 }
2489 }
2490
2491 npoints = j;
2492 if( npoints == 0 )
2493 EXIT;
2494 }
2495
2496 {
2497 _m1->cols = _m2->cols = npoints;
2498 memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2499 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2500
2501 double t[] =
2502 {
2503 1, 0, -cx,
2504 0, 1, -cy,
2505 0, 0, 1
2506 };
2507 CvMat T = cvMat(3, 3, CV_64F, t);
2508 cvMatMul( &T, &E2, &E2 );
2509
2510 int mirror = e2[0] < 0;
2511 double d = MAX(sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON);
2512 double alpha = e2[0]/d;
2513 double beta = e2[1]/d;
2514 double r[] =
2515 {
2516 alpha, beta, 0,
2517 -beta, alpha, 0,
2518 0, 0, 1
2519 };
2520 CvMat R = cvMat(3, 3, CV_64F, r);
2521 cvMatMul( &R, &T, &T );
2522 cvMatMul( &R, &E2, &E2 );
2523 double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0];
2524 double k[] =
2525 {
2526 1, 0, 0,
2527 0, 1, 0,
2528 invf, 0, 1
2529 };
2530 CvMat K = cvMat(3, 3, CV_64F, k);
2531 cvMatMul( &K, &T, &H2 );
2532 cvMatMul( &K, &E2, &E2 );
2533
2534 double it[] =
2535 {
2536 1, 0, cx,
2537 0, 1, cy,
2538 0, 0, 1
2539 };
2540 CvMat iT = cvMat( 3, 3, CV_64F, it );
2541 cvMatMul( &iT, &H2, &H2 );
2542
2543 memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2544 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2545
2546 double e2_x[] =
2547 {
2548 0, -e2[2], e2[1],
2549 e2[2], 0, -e2[0],
2550 -e2[1], e2[0], 0
2551 };
2552 double e2_111[] =
2553 {
2554 e2[0], e2[0], e2[0],
2555 e2[1], e2[1], e2[1],
2556 e2[2], e2[2], e2[2],
2557 };
2558 CvMat E2_x = cvMat(3, 3, CV_64F, e2_x);
2559 CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111);
2560 cvMatMulAdd(&E2_x, &F, &E2_111, &H0 );
2561 cvMatMul(&H2, &H0, &H0);
2562 CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6);
2563 cvMatMul(&H0, &E1, &E1);
2564
2565 cvPerspectiveTransform( _m1, _m1, &H0 );
2566 cvPerspectiveTransform( _m2, _m2, &H2 );
2567 CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B;
2568 double a[9], atb[3], x[3];
2569 CvMat AtA = cvMat( 3, 3, CV_64F, a );
2570 CvMat AtB = cvMat( 3, 1, CV_64F, atb );
2571 CvMat X = cvMat( 3, 1, CV_64F, x );
2572 cvConvertPointsHomogeneous( _m1, &A );
2573 cvReshape( &A, &A, 1, npoints );
2574 cvReshape( _m2, &BxBy, 1, npoints );
2575 cvGetCol( &BxBy, &B, 0 );
2576 cvGEMM( &A, &A, 1, 0, 0, &AtA, CV_GEMM_A_T );
2577 cvGEMM( &A, &B, 1, 0, 0, &AtB, CV_GEMM_A_T );
2578 cvSolve( &AtA, &AtB, &X, CV_SVD_SYM );
2579
2580 double ha[] =
2581 {
2582 x[0], x[1], x[2],
2583 0, 1, 0,
2584 0, 0, 1
2585 };
2586 CvMat Ha = cvMat(3, 3, CV_64F, ha);
2587 cvMatMul( &Ha, &H0, &H1 );
2588 cvPerspectiveTransform( _m1, _m1, &Ha );
2589
2590 if( mirror )
2591 {
2592 double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 };
2593 CvMat MM = cvMat(3, 3, CV_64F, mm);
2594 cvMatMul( &MM, &H1, &H1 );
2595 cvMatMul( &MM, &H2, &H2 );
2596 }
2597
2598 cvConvert( &H1, _H1 );
2599 cvConvert( &H2, _H2 );
2600
2601 result = 1;
2602 }
2603
2604 __END__;
2605
2606 cvReleaseMat( &_m1 );
2607 cvReleaseMat( &_m2 );
2608 cvReleaseMat( &_lines1 );
2609 cvReleaseMat( &_lines2 );
2610
2611 return result;
2612 }
2613
2614
2615 CV_IMPL void
cvReprojectImageTo3D(const CvArr * disparityImage,CvArr * _3dImage,const CvMat * _Q)2616 cvReprojectImageTo3D(
2617 const CvArr* disparityImage,
2618 CvArr* _3dImage, const CvMat* _Q )
2619 {
2620 CV_FUNCNAME( "cvReprojectImageTo3D" );
2621
2622 __BEGIN__;
2623
2624 double q[4][4];
2625 CvMat Q = cvMat(4, 4, CV_64F, q);
2626 CvMat sstub, *src = cvGetMat( disparityImage, &sstub );
2627 CvMat dstub, *dst = cvGetMat( _3dImage, &dstub );
2628 int stype = CV_MAT_TYPE(src->type), dtype = CV_MAT_TYPE(dst->type);
2629 int x, y, rows = src->rows, cols = src->cols;
2630 float* sbuf = (float*)cvStackAlloc( cols*sizeof(sbuf[0]) );
2631 float* dbuf = (float*)cvStackAlloc( cols*3*sizeof(dbuf[0]) );
2632
2633 CV_ASSERT( CV_ARE_SIZES_EQ(src, dst) &&
2634 (CV_MAT_TYPE(stype) == CV_16SC1 || CV_MAT_TYPE(stype) == CV_32FC1) &&
2635 (CV_MAT_TYPE(dtype) == CV_16SC3 || CV_MAT_TYPE(dtype) == CV_32FC3) );
2636
2637 cvConvert( _Q, &Q );
2638
2639 for( y = 0; y < rows; y++ )
2640 {
2641 const float* sptr = (const float*)(src->data.ptr + src->step*y);
2642 float* dptr0 = (float*)(dst->data.ptr + dst->step*y), *dptr = dptr0;
2643 double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3];
2644 double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3];
2645
2646 if( stype == CV_16SC1 )
2647 {
2648 const short* sptr0 = (const short*)sptr;
2649 for( x = 0; x < cols; x++ )
2650 sbuf[x] = (float)sptr0[x];
2651 sptr = sbuf;
2652 }
2653 if( dtype != CV_32FC3 )
2654 dptr = dbuf;
2655
2656 for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] )
2657 {
2658 double d = sptr[x];
2659 double iW = 1./(qw + q[3][2]*d);
2660 double X = (qx + q[0][2]*d)*iW;
2661 double Y = (qy + q[1][2]*d)*iW;
2662 double Z = (qz + q[2][2]*d)*iW;
2663
2664 dptr[x*3] = (float)X;
2665 dptr[x*3+1] = (float)Y;
2666 dptr[x*3+2] = (float)Z;
2667 }
2668
2669 if( dtype == CV_16SC3 )
2670 {
2671 for( x = 0; x < cols*3; x++ )
2672 {
2673 int ival = cvRound(dptr[x]);
2674 ((short*)dptr0)[x] = CV_CAST_16S(ival);
2675 }
2676 }
2677 }
2678
2679 __END__;
2680 }
2681
2682
2683 /* End of file. */
2684