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 "_cvaux.h"
43
44 //#include "cvtypes.h"
45 #include <float.h>
46 #include <limits.h>
47 //#include "cv.h"
48 //#include "windows.h"
49
50 #include <stdio.h>
51
52 /* Valery Mosyagin */
53
54 /* Function defenitions */
55
56 /* ----------------- */
57
58 void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints,
59 CvMat** pointsPres, int numImages,
60 CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon );
61
62 int icvComputeProjectMatrices6Points( CvMat* points1,CvMat* points2,CvMat* points3,
63 CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3);
64
65 void icvFindBaseTransform(CvMat* points,CvMat* resultT);
66
67 void GetGeneratorReduceFundSolution(CvMat* points1,CvMat* points2,CvMat* fundReduceCoef1,CvMat* fundReduceCoef2);
68
69 int GetGoodReduceFundamMatrFromTwo(CvMat* fundReduceCoef1,CvMat* fundReduceCoef2,CvMat* resFundReduceCoef);
70
71 void GetProjMatrFromReducedFundamental(CvMat* fundReduceCoefs,CvMat* projMatrCoefs);
72
73 void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr);
74
75 void icvComputeTransform4D(CvMat* points1,CvMat* points2,CvMat* transMatr);
76
77 int icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3,
78 CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
79 double threshold,/* Threshold for good point */
80 double p,/* Probability of good result. */
81 CvMat* status,
82 CvMat* points4D);
83
84 int icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3,
85 CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
86 double threshold,/* Threshold for good point */
87 double p,/* Probability of good result. */
88 CvMat* status,
89 CvMat* points4D);
90
91 void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
92 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
93 CvMat* points4D);
94
95 void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
96 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
97 CvMat* points4D);
98
99 /*==========================================================================================*/
100 /* Functions for calculation the tensor */
101 /*==========================================================================================*/
102 #if 1
fprintMatrix(FILE * file,CvMat * matrix)103 void fprintMatrix(FILE* file,CvMat* matrix)
104 {
105 int i,j;
106 fprintf(file,"\n");
107 for( i=0;i<matrix->rows;i++ )
108 {
109 for(j=0;j<matrix->cols;j++)
110 {
111 fprintf(file,"%10.7lf ",cvmGet(matrix,i,j));
112 }
113 fprintf(file,"\n");
114 }
115 }
116 #endif
117 /*==========================================================================================*/
118
icvNormalizePoints(CvMat * points,CvMat * normPoints,CvMat * cameraMatr)119 void icvNormalizePoints( CvMat* points, CvMat* normPoints,CvMat* cameraMatr )
120 {
121 /* Normalize image points using camera matrix */
122
123 CV_FUNCNAME( "icvNormalizePoints" );
124 __BEGIN__;
125
126 /* Test for null pointers */
127 if( points == 0 || normPoints == 0 || cameraMatr == 0 )
128 {
129 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
130 }
131
132 if( !CV_IS_MAT(points) || !CV_IS_MAT(normPoints) || !CV_IS_MAT(cameraMatr) )
133 {
134 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
135 }
136
137 int numPoints;
138 numPoints = points->cols;
139 if( numPoints <= 0 || numPoints != normPoints->cols )
140 {
141 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same and more than 0" );
142 }
143
144 if( normPoints->rows != 2 || normPoints->rows != points->rows )
145 {
146 CV_ERROR( CV_StsUnmatchedSizes, "Points must have 2 coordinates" );
147 }
148
149 if(cameraMatr->rows != 3 || cameraMatr->cols != 3)
150 {
151 CV_ERROR( CV_StsUnmatchedSizes, "Size of camera matrix must be 3x3" );
152 }
153
154 double fx,fy,cx,cy;
155
156 fx = cvmGet(cameraMatr,0,0);
157 fy = cvmGet(cameraMatr,1,1);
158 cx = cvmGet(cameraMatr,0,2);
159 cy = cvmGet(cameraMatr,1,2);
160
161 int i;
162 for( i = 0; i < numPoints; i++ )
163 {
164 cvmSet(normPoints, 0, i, (cvmGet(points,0,i) - cx) / fx );
165 cvmSet(normPoints, 1, i, (cvmGet(points,1,i) - cy) / fy );
166 }
167
168 __END__;
169
170 return;
171 }
172
173
174 /*=====================================================================================*/
175 /*
176 Computes projection matrices for given 6 points on 3 images
177 May returns 3 results. */
icvComputeProjectMatrices6Points(CvMat * points1,CvMat * points2,CvMat * points3,CvMat * projMatr1,CvMat * projMatr2,CvMat * projMatr3)178 int icvComputeProjectMatrices6Points( CvMat* points1,CvMat* points2,CvMat* points3,
179 CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3/*,
180 CvMat* points4D*/)
181 {
182 /* Test input data correctness */
183
184 int numSol = 0;
185
186 CV_FUNCNAME( "icvComputeProjectMatrices6Points" );
187 __BEGIN__;
188
189 /* Test for null pointers */
190 if( points1 == 0 || points2 == 0 || points3 == 0 ||
191 projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 )
192 {
193 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
194 }
195
196 if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(points3) ||
197 !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) )
198 {
199 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
200 }
201
202 if( (points1->cols != points2->cols) || (points1->cols != points3->cols) || (points1->cols != 6) /* || (points4D->cols !=6) */)
203 {
204 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be same and == 6" );
205 }
206
207 if( points1->rows != 2 || points2->rows != 2 || points3->rows != 2 )
208 {
209 CV_ERROR( CV_StsUnmatchedSizes, "Number of points coordinates must be 2" );
210 }
211
212 if( projMatr1->cols != 4 || projMatr2->cols != 4 || projMatr3->cols != 4 ||
213 !(projMatr1->rows == 3 && projMatr2->rows == 3 && projMatr3->rows == 3) &&
214 !(projMatr1->rows == 9 && projMatr2->rows == 9 && projMatr3->rows == 9) )
215 {
216 CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix must be 3x4 or 9x4 (for 3 matrices)" );
217 }
218
219 #if 0
220 if( points4D->row != 4 )
221 {
222 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" );
223 }
224 #endif
225
226 /* Find transform matrix for each camera */
227 int i;
228 CvMat* points[3];
229 points[0] = points1;
230 points[1] = points2;
231 points[2] = points3;
232
233 CvMat* projMatrs[3];
234 projMatrs[0] = projMatr1;
235 projMatrs[1] = projMatr2;
236 projMatrs[2] = projMatr3;
237
238 CvMat transMatr;
239 double transMatr_dat[9];
240 transMatr = cvMat(3,3,CV_64F,transMatr_dat);
241
242 CvMat corrPoints1;
243 CvMat corrPoints2;
244
245 double corrPoints_dat[3*3*2];/* 3-point(images) by 3-coordinates by 2-correspondence*/
246
247 corrPoints1 = cvMat(3,3,CV_64F,corrPoints_dat); /* 3-coordinates for each of 3-points(3-image) */
248 corrPoints2 = cvMat(3,3,CV_64F,corrPoints_dat+9);/* 3-coordinates for each of 3-points(3-image) */
249
250 for( i = 0; i < 3; i++ )/* for each image */
251 {
252 /* Get last 4 points for computing transformation */
253 CvMat tmpPoints;
254 /* find base points transform for last four points on i-th image */
255 cvGetSubRect(points[i],&tmpPoints,cvRect(2,0,4,2));
256 icvFindBaseTransform(&tmpPoints,&transMatr);
257
258 {/* We have base transform. Compute error scales for three first points */
259 CvMat trPoint;
260 double trPoint_dat[3*3];
261 trPoint = cvMat(3,3,CV_64F,trPoint_dat);
262 /* fill points */
263 for( int kk = 0; kk < 3; kk++ )
264 {
265 cvmSet(&trPoint,0,kk,cvmGet(points[i],0,kk+2));
266 cvmSet(&trPoint,1,kk,cvmGet(points[i],1,kk+2));
267 cvmSet(&trPoint,2,kk,1);
268 }
269
270 /* Transform points */
271 CvMat resPnts;
272 double resPnts_dat[9];
273 resPnts = cvMat(3,3,CV_64F,resPnts_dat);
274 cvmMul(&transMatr,&trPoint,&resPnts);
275 }
276
277 /* Transform two first points */
278 for( int j = 0; j < 2; j++ )
279 {
280 CvMat pnt;
281 double pnt_dat[3];
282 pnt = cvMat(3,1,CV_64F,pnt_dat);
283 pnt_dat[0] = cvmGet(points[i],0,j);
284 pnt_dat[1] = cvmGet(points[i],1,j);
285 pnt_dat[2] = 1.0;
286
287 CvMat trPnt;
288 double trPnt_dat[3];
289 trPnt = cvMat(3,1,CV_64F,trPnt_dat);
290
291 cvmMul(&transMatr,&pnt,&trPnt);
292
293 /* Collect transformed points */
294 corrPoints_dat[j * 9 + 0 * 3 + i] = trPnt_dat[0];/* x */
295 corrPoints_dat[j * 9 + 1 * 3 + i] = trPnt_dat[1];/* y */
296 corrPoints_dat[j * 9 + 2 * 3 + i] = trPnt_dat[2];/* w */
297 }
298 }
299
300 /* We have computed corr points. Now we can compute generators for reduced fundamental matrix */
301
302 /* Compute generators for reduced fundamental matrix from 3 pair of collect points */
303 CvMat fundReduceCoef1;
304 CvMat fundReduceCoef2;
305 double fundReduceCoef1_dat[5];
306 double fundReduceCoef2_dat[5];
307
308 fundReduceCoef1 = cvMat(1,5,CV_64F,fundReduceCoef1_dat);
309 fundReduceCoef2 = cvMat(1,5,CV_64F,fundReduceCoef2_dat);
310
311 GetGeneratorReduceFundSolution(&corrPoints1, &corrPoints2, &fundReduceCoef1, &fundReduceCoef2);
312
313 /* Choose best solutions for two generators. We can get 3 solutions */
314 CvMat resFundReduceCoef;
315 double resFundReduceCoef_dat[3*5];
316
317 resFundReduceCoef = cvMat(3,5,CV_64F,resFundReduceCoef_dat);
318
319 numSol = GetGoodReduceFundamMatrFromTwo(&fundReduceCoef1, &fundReduceCoef2,&resFundReduceCoef);
320
321 int maxSol;
322 maxSol = projMatrs[0]->rows / 3;
323
324 int currSol;
325 for( currSol = 0; (currSol < numSol && currSol < maxSol); currSol++ )
326 {
327 /* For current solution compute projection matrix */
328 CvMat fundCoefs;
329 cvGetSubRect(&resFundReduceCoef, &fundCoefs, cvRect(0,currSol,5,1));
330
331 CvMat projMatrCoefs;
332 double projMatrCoefs_dat[4];
333 projMatrCoefs = cvMat(1,4,CV_64F,projMatrCoefs_dat);
334
335 GetProjMatrFromReducedFundamental(&fundCoefs,&projMatrCoefs);
336 /* we have computed coeffs for reduced project matrix */
337
338 CvMat objPoints;
339 double objPoints_dat[4*6];
340 objPoints = cvMat(4,6,CV_64F,objPoints_dat);
341 cvZero(&objPoints);
342
343 /* fill object points */
344 for( i =0; i < 4; i++ )
345 {
346 objPoints_dat[i*6] = 1;
347 objPoints_dat[i*6+1] = projMatrCoefs_dat[i];
348 objPoints_dat[i*7+2] = 1;
349 }
350
351 int currCamera;
352 for( currCamera = 0; currCamera < 3; currCamera++ )
353 {
354
355 CvMat projPoints;
356 double projPoints_dat[3*6];
357 projPoints = cvMat(3,6,CV_64F,projPoints_dat);
358
359 /* fill projected points for current camera */
360 for( i = 0; i < 6; i++ )/* for each points for current camera */
361 {
362 projPoints_dat[6*0+i] = cvmGet(points[currCamera],0,i);/* x */
363 projPoints_dat[6*1+i] = cvmGet(points[currCamera],1,i);/* y */
364 projPoints_dat[6*2+i] = 1;/* w */
365 }
366
367 /* compute project matrix for current camera */
368 CvMat projMatrix;
369 double projMatrix_dat[3*4];
370 projMatrix = cvMat(3,4,CV_64F,projMatrix_dat);
371
372 icvComputeProjectMatrix(&objPoints,&projPoints,&projMatrix);
373
374 /* Add this matrix to result */
375 CvMat tmpSubRes;
376 cvGetSubRect(projMatrs[currCamera],&tmpSubRes,cvRect(0,currSol*3,4,3));
377 cvConvert(&projMatrix,&tmpSubRes);
378 }
379
380 /* We know project matrices. And we can reconstruct 6 3D-points if need */
381 #if 0
382 if( points4D )
383 {
384 if( currSol < points4D->rows / 4 )
385 {
386 CvMat tmpPoints4D;
387 double tmpPoints4D_dat[4*6];
388 tmpPoints4D = cvMat(4,6,CV_64F,tmpPoints4D_dat);
389
390 icvReconstructPointsFor3View( &wProjMatr[0], &wProjMatr[1], &wProjMatr[2],
391 points1, points2, points3,
392 &tmpPoints4D);
393
394 CvMat tmpSubRes;
395 cvGetSubRect(points4D,tmpSubRes,cvRect(0,currSol*4,6,4));
396 cvConvert(tmpPoints4D,points4D);
397 }
398 }
399 #endif
400
401 }/* for all sollutions */
402
403 __END__;
404 return numSol;
405 }
406
407 /*==========================================================================================*/
icvGetRandNumbers(int range,int count,int * arr)408 int icvGetRandNumbers(int range,int count,int* arr)
409 {
410 /* Generate random numbers [0,range-1] */
411
412 CV_FUNCNAME( "icvGetRandNumbers" );
413 __BEGIN__;
414
415 /* Test input data */
416 if( arr == 0 )
417 {
418 CV_ERROR( CV_StsNullPtr, "Parameter 'arr' is a NULL pointer" );
419 }
420
421
422 /* Test for errors input data */
423 if( range < count || range <= 0 )
424 {
425 CV_ERROR( CV_StsOutOfRange, "Can't generate such numbers. Count must be <= range and range must be > 0" );
426 }
427
428 int i,j;
429 int newRand;
430 for( i = 0; i < count; i++ )
431 {
432
433 int haveRep = 0;/* firstly we have not repeats */
434 do
435 {
436 /* generate new number */
437 newRand = rand()%range;
438 haveRep = 0;
439 /* Test for repeats in previous numbers */
440 for( j = 0; j < i; j++ )
441 {
442 if( arr[j] == newRand )
443 {
444 haveRep = 1;
445 break;
446 }
447 }
448 } while(haveRep);
449
450 /* We have good random number */
451 arr[i] = newRand;
452 }
453 __END__;
454 return 1;
455 }
456 /*==========================================================================================*/
icvSelectColsByNumbers(CvMat * srcMatr,CvMat * dstMatr,int * indexes,int number)457 void icvSelectColsByNumbers(CvMat* srcMatr, CvMat* dstMatr, int* indexes,int number)
458 {
459
460 CV_FUNCNAME( "icvSelectColsByNumbers" );
461 __BEGIN__;
462
463 /* Test input data */
464 if( srcMatr == 0 || dstMatr == 0 || indexes == 0)
465 {
466 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
467 }
468
469 if( !CV_IS_MAT(srcMatr) || !CV_IS_MAT(dstMatr) )
470 {
471 CV_ERROR( CV_StsUnsupportedFormat, "srcMatr and dstMatr must be a matrices" );
472 }
473
474 int srcSize;
475 int numRows;
476 numRows = srcMatr->rows;
477 srcSize = srcMatr->cols;
478
479 if( numRows != dstMatr->rows )
480 {
481 CV_ERROR( CV_StsOutOfRange, "Number of rows of matrices must be the same" );
482 }
483
484 int dst;
485 for( dst = 0; dst < number; dst++ )
486 {
487 int src = indexes[dst];
488 if( src >=0 && src < srcSize )
489 {
490 /* Copy each elements in column */
491 int i;
492 for( i = 0; i < numRows; i++ )
493 {
494 cvmSet(dstMatr,i,dst,cvmGet(srcMatr,i,src));
495 }
496 }
497 }
498
499 __END__;
500 return;
501 }
502
503 /*==========================================================================================*/
icvProject4DPoints(CvMat * points4D,CvMat * projMatr,CvMat * projPoints)504 void icvProject4DPoints(CvMat* points4D,CvMat* projMatr, CvMat* projPoints)
505 {
506
507 CvMat* tmpProjPoints = 0;
508
509 CV_FUNCNAME( "icvProject4DPoints" );
510
511 __BEGIN__;
512
513 if( points4D == 0 || projMatr == 0 || projPoints == 0)
514 {
515 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
516 }
517
518 if( !CV_IS_MAT(points4D) || !CV_IS_MAT(projMatr) || !CV_IS_MAT(projPoints) )
519 {
520 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
521 }
522
523 int numPoints;
524 numPoints = points4D->cols;
525 if( numPoints < 1 )
526 {
527 CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
528 }
529
530 if( numPoints != projPoints->cols )
531 {
532 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same");
533 }
534
535 if( projPoints->rows != 2 )
536 {
537 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of projected points must be 2");
538 }
539
540 if( points4D->rows != 4 )
541 {
542 CV_ERROR(CV_StsUnmatchedSizes, "Number of coordinates of 4D points must be 4");
543 }
544
545 if( projMatr->cols != 4 || projMatr->rows != 3 )
546 {
547 CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4");
548 }
549
550
551 CV_CALL( tmpProjPoints = cvCreateMat(3,numPoints,CV_64F) );
552
553 cvmMul(projMatr,points4D,tmpProjPoints);
554
555 /* Scale points */
556 int i;
557 for( i = 0; i < numPoints; i++ )
558 {
559 double scale,x,y;
560
561 scale = cvmGet(tmpProjPoints,2,i);
562 x = cvmGet(tmpProjPoints,0,i);
563 y = cvmGet(tmpProjPoints,1,i);
564
565 if( fabs(scale) > 1e-7 )
566 {
567 x /= scale;
568 y /= scale;
569 }
570 else
571 {
572 x = 1e8;
573 y = 1e8;
574 }
575
576 cvmSet(projPoints,0,i,x);
577 cvmSet(projPoints,1,i,y);
578 }
579
580 __END__;
581
582 cvReleaseMat(&tmpProjPoints);
583
584 return;
585 }
586 /*==========================================================================================*/
icvCompute3ProjectMatricesNPointsStatus(CvMat ** points,CvMat ** projMatrs,CvMat ** statuses,double threshold,double p,CvMat * resStatus,CvMat * points4D)587 int icvCompute3ProjectMatricesNPointsStatus( CvMat** points,/* 3 arrays of points on image */
588 CvMat** projMatrs,/* array of 3 prejection matrices */
589 CvMat** statuses,/* 3 arrays of status of points */
590 double threshold,/* Threshold for good point */
591 double p,/* Probability of good result. */
592 CvMat* resStatus,
593 CvMat* points4D)
594 {
595 int numProjMatrs = 0;
596 unsigned char *comStat = 0;
597 CvMat *triPoints[3] = {0,0,0};
598 CvMat *status = 0;
599 CvMat *triPoints4D = 0;
600
601 CV_FUNCNAME( "icvCompute3ProjectMatricesNPointsStatus" );
602 __BEGIN__;
603
604 /* Test for errors */
605 if( points == 0 || projMatrs == 0 || statuses == 0 || resStatus == 0 )
606 {
607 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
608 }
609
610 int currImage;
611 for( currImage = 0; currImage < 3; currImage++ )
612 {
613 /* Test for null pointers */
614 if( points[currImage] == 0 )
615 {
616 CV_ERROR( CV_StsNullPtr, "Some of points arrays is a NULL pointer" );
617 }
618
619 if( projMatrs[currImage] == 0 )
620 {
621 CV_ERROR( CV_StsNullPtr, "Some of projMatr is a NULL pointer" );
622 }
623
624 if( statuses[currImage] == 0 )
625 {
626 CV_ERROR( CV_StsNullPtr, "Some of status arrays is a NULL pointer" );
627 }
628
629 /* Test for matrices */
630 if( !CV_IS_MAT(points[currImage]) )
631 {
632 CV_ERROR( CV_StsNullPtr, "Some of points arrays is not a matrix" );
633 }
634
635 if( !CV_IS_MAT(projMatrs[currImage]) )
636 {
637 CV_ERROR( CV_StsNullPtr, "Some of projMatr is not a matrix" );
638 }
639
640 if( !CV_IS_MASK_ARR(statuses[currImage]) )
641 {
642 CV_ERROR( CV_StsNullPtr, "Some of status arrays is not a mask array" );
643 }
644 }
645
646 int numPoints;
647 numPoints = points[0]->cols;
648 if( numPoints < 6 )
649 {
650 CV_ERROR( CV_StsOutOfRange, "Number points must be more than 6" );
651 }
652
653 for( currImage = 0; currImage < 3; currImage++ )
654 {
655 if( points[currImage]->cols != numPoints || statuses[currImage]->cols != numPoints )
656 {
657 CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" );
658 }
659
660 if( points[currImage]->rows != 2 )
661 {
662 CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be == 2" );
663 }
664
665 if( statuses[currImage]->rows != 1 )
666 {
667 CV_ERROR( CV_StsOutOfRange, "Each of status must be matrix 1xN" );
668 }
669
670 if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 )
671 {
672 CV_ERROR( CV_StsOutOfRange, "Each of projection matrix must be 3x4" );
673 }
674 }
675
676
677 /* Create common status for all points */
678
679 int i;
680
681 CV_CALL( comStat = (unsigned char*)cvAlloc(sizeof(unsigned char)*numPoints) );
682
683 unsigned char *stats[3];
684
685 stats[0] = statuses[0]->data.ptr;
686 stats[1] = statuses[1]->data.ptr;
687 stats[2] = statuses[2]->data.ptr;
688
689 int numTripl;
690 numTripl = 0;
691 for( i = 0; i < numPoints; i++ )
692 {
693 comStat[i] = (unsigned char)(stats[0][i] * stats[1][i] * stats[2][i]);
694 numTripl += comStat[i];
695 }
696
697 if( numTripl > 0 )
698 {
699 /* Create new arrays with points */
700 CV_CALL( triPoints[0] = cvCreateMat(2,numTripl,CV_64F) );
701 CV_CALL( triPoints[1] = cvCreateMat(2,numTripl,CV_64F) );
702 CV_CALL( triPoints[2] = cvCreateMat(2,numTripl,CV_64F) );
703 if( points4D )
704 {
705 CV_CALL( triPoints4D = cvCreateMat(4,numTripl,CV_64F) );
706 }
707
708 /* Create status array */
709 CV_CALL( status = cvCreateMat(1,numTripl,CV_64F) );
710
711 /* Copy points to new arrays */
712 int currPnt = 0;
713 for( i = 0; i < numPoints; i++ )
714 {
715 if( comStat[i] )
716 {
717 for( currImage = 0; currImage < 3; currImage++ )
718 {
719 cvmSet(triPoints[currImage],0,currPnt,cvmGet(points[currImage],0,i));
720 cvmSet(triPoints[currImage],1,currPnt,cvmGet(points[currImage],1,i));
721 }
722 currPnt++;
723 }
724 }
725
726 /* Call function */
727 numProjMatrs = icvComputeProjectMatricesNPoints( triPoints[0],triPoints[1],triPoints[2],
728 projMatrs[0],projMatrs[1],projMatrs[2],
729 threshold,/* Threshold for good point */
730 p,/* Probability of good result. */
731 status,
732 triPoints4D);
733
734 /* Get computed status and set to result */
735 cvZero(resStatus);
736 currPnt = 0;
737 for( i = 0; i < numPoints; i++ )
738 {
739 if( comStat[i] )
740 {
741 if( cvmGet(status,0,currPnt) > 0 )
742 {
743 resStatus->data.ptr[i] = 1;
744 }
745 currPnt++;
746 }
747 }
748
749 if( triPoints4D )
750 {
751 /* Copy copmuted 4D points */
752 cvZero(points4D);
753 currPnt = 0;
754 for( i = 0; i < numPoints; i++ )
755 {
756 if( comStat[i] )
757 {
758 if( cvmGet(status,0,currPnt) > 0 )
759 {
760 cvmSet( points4D, 0, i, cvmGet( triPoints4D , 0, currPnt) );
761 cvmSet( points4D, 1, i, cvmGet( triPoints4D , 1, currPnt) );
762 cvmSet( points4D, 2, i, cvmGet( triPoints4D , 2, currPnt) );
763 cvmSet( points4D, 3, i, cvmGet( triPoints4D , 3, currPnt) );
764 }
765 currPnt++;
766 }
767 }
768 }
769 }
770
771 __END__;
772
773 /* Free allocated memory */
774 cvReleaseMat(&status);
775 cvFree( &comStat);
776 cvReleaseMat(&status);
777
778 cvReleaseMat(&triPoints[0]);
779 cvReleaseMat(&triPoints[1]);
780 cvReleaseMat(&triPoints[2]);
781 cvReleaseMat(&triPoints4D);
782
783 return numProjMatrs;
784
785 }
786
787 /*==========================================================================================*/
icvComputeProjectMatricesNPoints(CvMat * points1,CvMat * points2,CvMat * points3,CvMat * projMatr1,CvMat * projMatr2,CvMat * projMatr3,double threshold,double p,CvMat * status,CvMat * points4D)788 int icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3,
789 CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
790 double threshold,/* Threshold for good point */
791 double p,/* Probability of good result. */
792 CvMat* status,
793 CvMat* points4D)
794 {
795 /* Returns status for each point, Good or bad */
796
797 /* Compute projection matrices using N points */
798
799 char* flags = 0;
800 char* bestFlags = 0;
801
802 int numProjMatrs = 0;
803
804 CvMat* tmpProjPoints[3]={0,0,0};
805 CvMat* recPoints4D = 0;
806 CvMat *reconPoints4D = 0;
807
808
809 CV_FUNCNAME( "icvComputeProjectMatricesNPoints" );
810 __BEGIN__;
811
812 CvMat* points[3];
813 points[0] = points1;
814 points[1] = points2;
815 points[2] = points3;
816
817 /* Test for errors */
818 if( points1 == 0 || points2 == 0 || points3 == 0 ||
819 projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 ||
820 status == 0)
821 {
822 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
823 }
824
825 if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(points3) ||
826 !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) ||
827 !CV_IS_MAT(status) )
828 {
829 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
830 }
831
832 int numPoints;
833 numPoints = points1->cols;
834
835 if( numPoints < 6 )
836 {
837 CV_ERROR( CV_StsOutOfRange, "Number points must be more than 6" );
838 }
839
840 if( numPoints != points2->cols || numPoints != points3->cols )
841 {
842 CV_ERROR( CV_StsUnmatchedSizes, "number of points must be the same" );
843 }
844
845 if( p < 0 || p > 1.0 )
846 {
847 CV_ERROR( CV_StsOutOfRange, "Probability must be >=0 and <=1" );
848 }
849
850 if( threshold < 0 )
851 {
852 CV_ERROR( CV_StsOutOfRange, "Threshold for good points must be at least >= 0" );
853 }
854
855 CvMat* projMatrs[3];
856
857 projMatrs[0] = projMatr1;
858 projMatrs[1] = projMatr2;
859 projMatrs[2] = projMatr3;
860
861 int i;
862 for( i = 0; i < 3; i++ )
863 {
864 if( projMatrs[i]->cols != 4 || projMatrs[i]->rows != 3 )
865 {
866 CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
867 }
868 }
869
870 for( i = 0; i < 3; i++ )
871 {
872 if( points[i]->rows != 2)
873 {
874 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points must be 2" );
875 }
876 }
877
878 /* use RANSAC algorithm to compute projection matrices */
879
880 CV_CALL( recPoints4D = cvCreateMat(4,numPoints,CV_64F) );
881 CV_CALL( tmpProjPoints[0] = cvCreateMat(2,numPoints,CV_64F) );
882 CV_CALL( tmpProjPoints[1] = cvCreateMat(2,numPoints,CV_64F) );
883 CV_CALL( tmpProjPoints[2] = cvCreateMat(2,numPoints,CV_64F) );
884
885 CV_CALL( flags = (char*)cvAlloc(sizeof(char)*numPoints) );
886 CV_CALL( bestFlags = (char*)cvAlloc(sizeof(char)*numPoints) );
887
888 {
889 int NumSamples = 500;/* just init number of samples */
890 int wasCount = 0; /* count of choosing samples */
891 int maxGoodPoints = 0;
892 int numGoodPoints = 0;
893
894 double bestProjMatrs_dat[36];
895 CvMat bestProjMatrs[3];
896 bestProjMatrs[0] = cvMat(3,4,CV_64F,bestProjMatrs_dat);
897 bestProjMatrs[1] = cvMat(3,4,CV_64F,bestProjMatrs_dat+12);
898 bestProjMatrs[2] = cvMat(3,4,CV_64F,bestProjMatrs_dat+24);
899
900 double tmpProjMatr_dat[36*3];
901 CvMat tmpProjMatr[3];
902 tmpProjMatr[0] = cvMat(9,4,CV_64F,tmpProjMatr_dat);
903 tmpProjMatr[1] = cvMat(9,4,CV_64F,tmpProjMatr_dat+36);
904 tmpProjMatr[2] = cvMat(9,4,CV_64F,tmpProjMatr_dat+72);
905
906 /* choosen points */
907
908 while( wasCount < NumSamples )
909 {
910 /* select samples */
911 int randNumbs[6];
912 icvGetRandNumbers(numPoints,6,randNumbs);
913
914 /* random numbers of points was generated */
915 /* select points */
916
917 double selPoints_dat[2*6*3];
918 CvMat selPoints[3];
919 selPoints[0] = cvMat(2,6,CV_64F,selPoints_dat);
920 selPoints[1] = cvMat(2,6,CV_64F,selPoints_dat+12);
921 selPoints[2] = cvMat(2,6,CV_64F,selPoints_dat+24);
922
923 /* Copy 6 point for random indexes */
924 icvSelectColsByNumbers( points[0], &selPoints[0], randNumbs,6);
925 icvSelectColsByNumbers( points[1], &selPoints[1], randNumbs,6);
926 icvSelectColsByNumbers( points[2], &selPoints[2], randNumbs,6);
927
928 /* Compute projection matrices for this points */
929 int numProj = icvComputeProjectMatrices6Points( &selPoints[0],&selPoints[1],&selPoints[2],
930 &tmpProjMatr[0],&tmpProjMatr[1],&tmpProjMatr[2]);
931
932 /* Compute number of good points for each matrix */
933 CvMat proj6[3];
934 for( int currProj = 0; currProj < numProj; currProj++ )
935 {
936 cvGetSubArr(&tmpProjMatr[0],&proj6[0],cvRect(0,currProj*3,4,3));
937 cvGetSubArr(&tmpProjMatr[1],&proj6[1],cvRect(0,currProj*3,4,3));
938 cvGetSubArr(&tmpProjMatr[2],&proj6[2],cvRect(0,currProj*3,4,3));
939
940 /* Reconstruct points for projection matrices */
941 icvReconstructPointsFor3View( &proj6[0],&proj6[1],&proj6[2],
942 points[0], points[1], points[2],
943 recPoints4D);
944
945 /* Project points to images using projection matrices */
946 icvProject4DPoints(recPoints4D,&proj6[0],tmpProjPoints[0]);
947 icvProject4DPoints(recPoints4D,&proj6[1],tmpProjPoints[1]);
948 icvProject4DPoints(recPoints4D,&proj6[2],tmpProjPoints[2]);
949
950 /* Compute distances and number of good points (inliers) */
951 int i;
952 int currImage;
953 numGoodPoints = 0;
954 for( i = 0; i < numPoints; i++ )
955 {
956 double dist=-1;
957 dist = 0;
958 /* Choose max distance for each of three points */
959 for( currImage = 0; currImage < 3; currImage++ )
960 {
961 double x1,y1,x2,y2;
962 x1 = cvmGet(tmpProjPoints[currImage],0,i);
963 y1 = cvmGet(tmpProjPoints[currImage],1,i);
964 x2 = cvmGet(points[currImage],0,i);
965 y2 = cvmGet(points[currImage],1,i);
966
967 double dx,dy;
968 dx = x1-x2;
969 dy = y1-y2;
970 #if 1
971 double newDist = dx*dx+dy*dy;
972 if( newDist > dist )
973 {
974 dist = newDist;
975 }
976 #else
977 dist += sqrt(dx*dx+dy*dy)/3.0;
978 #endif
979 }
980 dist = sqrt(dist);
981 flags[i] = (char)(dist > threshold ? 0 : 1);
982 numGoodPoints += flags[i];
983
984 }
985
986
987 if( numGoodPoints > maxGoodPoints )
988 {/* Copy current projection matrices as best */
989
990 cvCopy(&proj6[0],&bestProjMatrs[0]);
991 cvCopy(&proj6[1],&bestProjMatrs[1]);
992 cvCopy(&proj6[2],&bestProjMatrs[2]);
993
994 maxGoodPoints = numGoodPoints;
995 /* copy best flags */
996 memcpy(bestFlags,flags,sizeof(flags[0])*numPoints);
997
998 /* Adaptive number of samples to count*/
999 double ep = 1 - (double)numGoodPoints / (double)numPoints;
1000 if( ep == 1 )
1001 {
1002 ep = 0.5;/* if there is not good points set ration of outliers to 50% */
1003 }
1004
1005 double newNumSamples = (log(1-p) / log(1-pow(1-ep,6)));
1006 if( newNumSamples < double(NumSamples) )
1007 {
1008 NumSamples = cvRound(newNumSamples);
1009 }
1010 }
1011 }
1012
1013 wasCount++;
1014 }
1015 #if 0
1016 char str[300];
1017 sprintf(str,"Initial numPoints = %d\nmaxGoodPoints=%d\nRANSAC made %d steps",
1018 numPoints,
1019 maxGoodPoints,
1020 cvRound(wasCount));
1021 MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);
1022 #endif
1023
1024 /* we may have best 6-point projection matrices. */
1025 /* and best points */
1026 /* use these points to improve matrices */
1027
1028 if( maxGoodPoints < 6 )
1029 {
1030 /* matrix not found */
1031 numProjMatrs = 0;
1032 }
1033 else
1034 {
1035 /* We may Improove matrices using ---- method */
1036 /* We may try to use Levenberg-Marquardt optimization */
1037 //int currIter = 0;
1038 int finalGoodPoints = 0;
1039 char *goodFlags = 0;
1040 goodFlags = (char*)cvAlloc(numPoints*sizeof(char));
1041
1042 int needRepeat;
1043 do
1044 {
1045 #if 0
1046 /* Version without using status for Levenberg-Marquardt minimization */
1047
1048 CvMat *optStatus;
1049 optStatus = cvCreateMat(1,numPoints,CV_64F);
1050 int testNumber = 0;
1051 for( i=0;i<numPoints;i++ )
1052 {
1053 cvmSet(optStatus,0,i,(double)bestFlags[i]);
1054 testNumber += bestFlags[i];
1055 }
1056
1057 char str2[200];
1058 sprintf(str2,"test good num=%d\nmaxGoodPoints=%d",testNumber,maxGoodPoints);
1059 MessageBox(0,str2,"Info",MB_OK|MB_TASKMODAL);
1060
1061 CvMat *gPresPoints;
1062 gPresPoints = cvCreateMat(1,maxGoodPoints,CV_64F);
1063 for( i = 0; i < maxGoodPoints; i++)
1064 {
1065 cvmSet(gPresPoints,0,i,1.0);
1066 }
1067
1068 /* Create array of points pres */
1069 CvMat *pointsPres[3];
1070 pointsPres[0] = gPresPoints;
1071 pointsPres[1] = gPresPoints;
1072 pointsPres[2] = gPresPoints;
1073
1074 /* Create just good points 2D */
1075 CvMat *gPoints[3];
1076 icvCreateGoodPoints(points[0],&gPoints[0],optStatus);
1077 icvCreateGoodPoints(points[1],&gPoints[1],optStatus);
1078 icvCreateGoodPoints(points[2],&gPoints[2],optStatus);
1079
1080 /* Create 4D points array for good points */
1081 CvMat *resPoints4D;
1082 resPoints4D = cvCreateMat(4,maxGoodPoints,CV_64F);
1083
1084 CvMat* projMs[3];
1085
1086 projMs[0] = &bestProjMatrs[0];
1087 projMs[1] = &bestProjMatrs[1];
1088 projMs[2] = &bestProjMatrs[2];
1089
1090
1091 CvMat resProjMatrs[3];
1092 double resProjMatrs_dat[36];
1093 resProjMatrs[0] = cvMat(3,4,CV_64F,resProjMatrs_dat);
1094 resProjMatrs[1] = cvMat(3,4,CV_64F,resProjMatrs_dat+12);
1095 resProjMatrs[2] = cvMat(3,4,CV_64F,resProjMatrs_dat+24);
1096
1097 CvMat* resMatrs[3];
1098 resMatrs[0] = &resProjMatrs[0];
1099 resMatrs[1] = &resProjMatrs[1];
1100 resMatrs[2] = &resProjMatrs[2];
1101
1102 cvOptimizeLevenbergMarquardtBundle( projMs,//projMs,
1103 gPoints,//points,//points2D,
1104 pointsPres,//pointsPres,
1105 3,
1106 resMatrs,//resProjMatrs,
1107 resPoints4D,//resPoints4D,
1108 100, 1e-9 );
1109
1110 /* We found optimized projection matrices */
1111
1112 CvMat *reconPoints4D;
1113 reconPoints4D = cvCreateMat(4,numPoints,CV_64F);
1114
1115 /* Reconstruct all points using found projection matrices */
1116 icvReconstructPointsFor3View( &resProjMatrs[0],&resProjMatrs[1],&resProjMatrs[2],
1117 points[0], points[1], points[2],
1118 reconPoints4D);
1119
1120 /* Project points to images using projection matrices */
1121 icvProject4DPoints(reconPoints4D,&resProjMatrs[0],tmpProjPoints[0]);
1122 icvProject4DPoints(reconPoints4D,&resProjMatrs[1],tmpProjPoints[1]);
1123 icvProject4DPoints(reconPoints4D,&resProjMatrs[2],tmpProjPoints[2]);
1124
1125
1126 /* Compute error for each point and select good */
1127
1128 int currImage;
1129 finalGoodPoints = 0;
1130 for( i = 0; i < numPoints; i++ )
1131 {
1132 double dist=-1;
1133 /* Choose max distance for each of three points */
1134 for( currImage = 0; currImage < 3; currImage++ )
1135 {
1136 double x1,y1,x2,y2;
1137 x1 = cvmGet(tmpProjPoints[currImage],0,i);
1138 y1 = cvmGet(tmpProjPoints[currImage],1,i);
1139 x2 = cvmGet(points[currImage],0,i);
1140 y2 = cvmGet(points[currImage],1,i);
1141
1142 double dx,dy;
1143 dx = x1-x2;
1144 dy = y1-y2;
1145
1146 double newDist = dx*dx+dy*dy;
1147 if( newDist > dist )
1148 {
1149 dist = newDist;
1150 }
1151 }
1152 dist = sqrt(dist);
1153 goodFlags[i] = (char)(dist > threshold ? 0 : 1);
1154 finalGoodPoints += goodFlags[i];
1155 }
1156
1157 char str[200];
1158 sprintf(str,"Was num = %d\nNew num=%d",maxGoodPoints,finalGoodPoints);
1159 MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);
1160 if( finalGoodPoints > maxGoodPoints )
1161 {
1162 /* Copy new version of projection matrices */
1163 cvCopy(&resProjMatrs[0],&bestProjMatrs[0]);
1164 cvCopy(&resProjMatrs[1],&bestProjMatrs[1]);
1165 cvCopy(&resProjMatrs[2],&bestProjMatrs[2]);
1166 memcpy(bestFlags,goodFlags,numPoints*sizeof(char));
1167 maxGoodPoints = finalGoodPoints;
1168 }
1169
1170 cvReleaseMat(&optStatus);
1171 cvReleaseMat(&resPoints4D);
1172 #else
1173 /* Version with using status for Levenberd-Marquardt minimization */
1174
1175 /* Create status */
1176 CvMat *optStatus;
1177 optStatus = cvCreateMat(1,numPoints,CV_64F);
1178 for( i=0;i<numPoints;i++ )
1179 {
1180 cvmSet(optStatus,0,i,(double)bestFlags[i]);
1181 }
1182
1183 CvMat *pointsPres[3];
1184 pointsPres[0] = optStatus;
1185 pointsPres[1] = optStatus;
1186 pointsPres[2] = optStatus;
1187
1188 /* Create 4D points array for good points */
1189 CvMat *resPoints4D;
1190 resPoints4D = cvCreateMat(4,numPoints,CV_64F);
1191
1192 CvMat* projMs[3];
1193
1194 projMs[0] = &bestProjMatrs[0];
1195 projMs[1] = &bestProjMatrs[1];
1196 projMs[2] = &bestProjMatrs[2];
1197
1198 CvMat resProjMatrs[3];
1199 double resProjMatrs_dat[36];
1200 resProjMatrs[0] = cvMat(3,4,CV_64F,resProjMatrs_dat);
1201 resProjMatrs[1] = cvMat(3,4,CV_64F,resProjMatrs_dat+12);
1202 resProjMatrs[2] = cvMat(3,4,CV_64F,resProjMatrs_dat+24);
1203
1204 CvMat* resMatrs[3];
1205 resMatrs[0] = &resProjMatrs[0];
1206 resMatrs[1] = &resProjMatrs[1];
1207 resMatrs[2] = &resProjMatrs[2];
1208
1209 cvOptimizeLevenbergMarquardtBundle( projMs,//projMs,
1210 points,//points2D,
1211 pointsPres,//pointsPres,
1212 3,
1213 resMatrs,//resProjMatrs,
1214 resPoints4D,//resPoints4D,
1215 100, 1e-9 );
1216
1217 /* We found optimized projection matrices */
1218
1219 reconPoints4D = cvCreateMat(4,numPoints,CV_64F);
1220
1221 /* Reconstruct all points using found projection matrices */
1222 icvReconstructPointsFor3View( &resProjMatrs[0],&resProjMatrs[1],&resProjMatrs[2],
1223 points[0], points[1], points[2],
1224 reconPoints4D);
1225
1226 /* Project points to images using projection matrices */
1227 icvProject4DPoints(reconPoints4D,&resProjMatrs[0],tmpProjPoints[0]);
1228 icvProject4DPoints(reconPoints4D,&resProjMatrs[1],tmpProjPoints[1]);
1229 icvProject4DPoints(reconPoints4D,&resProjMatrs[2],tmpProjPoints[2]);
1230
1231
1232 /* Compute error for each point and select good */
1233
1234 int currImage;
1235 finalGoodPoints = 0;
1236 for( i = 0; i < numPoints; i++ )
1237 {
1238 double dist=-1;
1239 /* Choose max distance for each of three points */
1240 for( currImage = 0; currImage < 3; currImage++ )
1241 {
1242 double x1,y1,x2,y2;
1243 x1 = cvmGet(tmpProjPoints[currImage],0,i);
1244 y1 = cvmGet(tmpProjPoints[currImage],1,i);
1245 x2 = cvmGet(points[currImage],0,i);
1246 y2 = cvmGet(points[currImage],1,i);
1247
1248 double dx,dy;
1249 dx = x1-x2;
1250 dy = y1-y2;
1251
1252 double newDist = dx*dx+dy*dy;
1253 if( newDist > dist )
1254 {
1255 dist = newDist;
1256 }
1257 }
1258 dist = sqrt(dist);
1259 goodFlags[i] = (char)(dist > threshold ? 0 : 1);
1260 finalGoodPoints += goodFlags[i];
1261 }
1262
1263 /*char str[200];
1264 sprintf(str,"Was num = %d\nNew num=%d",maxGoodPoints,finalGoodPoints);
1265 MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);*/
1266
1267 needRepeat = 0;
1268 if( finalGoodPoints > maxGoodPoints )
1269 {
1270 /* Copy new version of projection matrices */
1271 cvCopy(&resProjMatrs[0],&bestProjMatrs[0]);
1272 cvCopy(&resProjMatrs[1],&bestProjMatrs[1]);
1273 cvCopy(&resProjMatrs[2],&bestProjMatrs[2]);
1274 memcpy(bestFlags,goodFlags,numPoints*sizeof(char));
1275 maxGoodPoints = finalGoodPoints;
1276 needRepeat = 1;
1277 }
1278
1279 cvReleaseMat(&optStatus);
1280 cvReleaseMat(&resPoints4D);
1281
1282
1283 #endif
1284 } while ( needRepeat );
1285
1286 cvFree( &goodFlags);
1287
1288
1289
1290
1291 numProjMatrs = 1;
1292
1293 /* Copy projection matrices */
1294 cvConvert(&bestProjMatrs[0],projMatr1);
1295 cvConvert(&bestProjMatrs[1],projMatr2);
1296 cvConvert(&bestProjMatrs[2],projMatr3);
1297
1298 if( status )
1299 {
1300 /* copy status for each points if need */
1301 for( int i = 0; i < numPoints; i++)
1302 {
1303 cvmSet(status,0,i,(double)bestFlags[i]);
1304 }
1305 }
1306 }
1307 }
1308
1309 if( points4D )
1310 {/* Fill reconstructed points */
1311
1312 cvZero(points4D);
1313 icvReconstructPointsFor3View( projMatr1,projMatr2,projMatr3,
1314 points[0], points[1], points[2],
1315 points4D);
1316 }
1317
1318
1319
1320 __END__;
1321
1322 cvFree( &flags);
1323 cvFree( &bestFlags);
1324
1325 cvReleaseMat(&recPoints4D);
1326 cvReleaseMat(&tmpProjPoints[0]);
1327 cvReleaseMat(&tmpProjPoints[1]);
1328 cvReleaseMat(&tmpProjPoints[2]);
1329
1330 return numProjMatrs;
1331 }
1332
1333 /*==========================================================================================*/
1334
icvFindBaseTransform(CvMat * points,CvMat * resultT)1335 void icvFindBaseTransform(CvMat* points,CvMat* resultT)
1336 {
1337
1338 CV_FUNCNAME( "icvFindBaseTransform" );
1339 __BEGIN__;
1340
1341 if( points == 0 || resultT == 0 )
1342 {
1343 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
1344 }
1345
1346 if( !CV_IS_MAT(points) || !CV_IS_MAT(resultT) )
1347 {
1348 CV_ERROR( CV_StsUnsupportedFormat, "points and resultT must be a matrices" );
1349 }
1350
1351 if( points->rows != 2 || points->cols != 4 )
1352 {
1353 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be 4. And they must have 2 coordinates" );
1354 }
1355
1356 if( resultT->rows != 3 || resultT->cols != 3 )
1357 {
1358 CV_ERROR( CV_StsUnmatchedSizes, "size of matrix resultT must be 3x3" );
1359 }
1360
1361 /* Function gets four points and compute transformation to e1=(100) e2=(010) e3=(001) e4=(111) */
1362
1363 /* !!! test each three points not collinear. Need to test */
1364
1365 /* Create matrices */
1366 CvMat matrA;
1367 CvMat vectB;
1368 double matrA_dat[3*3];
1369 double vectB_dat[3];
1370 matrA = cvMat(3,3,CV_64F,matrA_dat);
1371 vectB = cvMat(3,1,CV_64F,vectB_dat);
1372
1373 /* fill matrices */
1374 int i;
1375 for( i = 0; i < 3; i++ )
1376 {
1377 cvmSet(&matrA,0,i,cvmGet(points,0,i));
1378 cvmSet(&matrA,1,i,cvmGet(points,1,i));
1379 cvmSet(&matrA,2,i,1);
1380 }
1381
1382 /* Fill vector B */
1383 cvmSet(&vectB,0,0,cvmGet(points,0,3));
1384 cvmSet(&vectB,1,0,cvmGet(points,1,3));
1385 cvmSet(&vectB,2,0,1);
1386
1387 /* result scale */
1388 CvMat scale;
1389 double scale_dat[3];
1390 scale = cvMat(3,1,CV_64F,scale_dat);
1391
1392 cvSolve(&matrA,&vectB,&scale,CV_SVD);
1393
1394 /* multiply by scale */
1395 int j;
1396 for( j = 0; j < 3; j++ )
1397 {
1398 double sc = scale_dat[j];
1399 for( i = 0; i < 3; i++ )
1400 {
1401 matrA_dat[i*3+j] *= sc;
1402 }
1403 }
1404
1405 /* Convert inverse matrix */
1406 CvMat tmpRes;
1407 double tmpRes_dat[9];
1408 tmpRes = cvMat(3,3,CV_64F,tmpRes_dat);
1409 cvInvert(&matrA,&tmpRes);
1410
1411 cvConvert(&tmpRes,resultT);
1412
1413 __END__;
1414
1415 return;
1416 }
1417
1418
1419 /*==========================================================================================*/
GetGeneratorReduceFundSolution(CvMat * points1,CvMat * points2,CvMat * fundReduceCoef1,CvMat * fundReduceCoef2)1420 void GetGeneratorReduceFundSolution(CvMat* points1,CvMat* points2,CvMat* fundReduceCoef1,CvMat* fundReduceCoef2)
1421 {
1422
1423 CV_FUNCNAME( "GetGeneratorReduceFundSolution" );
1424 __BEGIN__;
1425
1426 /* Test input data for errors */
1427
1428 if( points1 == 0 || points2 == 0 || fundReduceCoef1 == 0 || fundReduceCoef2 == 0)
1429 {
1430 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
1431 }
1432
1433 if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(fundReduceCoef1) || !CV_IS_MAT(fundReduceCoef2) )
1434 {
1435 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
1436 }
1437
1438
1439
1440 if( points1->rows != 3 || points1->cols != 3 )
1441 {
1442 CV_ERROR( CV_StsUnmatchedSizes, "Number of points1 must be 3 and and have 3 coordinates" );
1443 }
1444
1445 if( points2->rows != 3 || points2->cols != 3 )
1446 {
1447 CV_ERROR( CV_StsUnmatchedSizes, "Number of points2 must be 3 and and have 3 coordinates" );
1448 }
1449
1450 if( fundReduceCoef1->rows != 1 || fundReduceCoef1->cols != 5 )
1451 {
1452 CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef1 must be 1x5" );
1453 }
1454
1455 if( fundReduceCoef2->rows != 1 || fundReduceCoef2->cols != 5 )
1456 {
1457 CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef2 must be 1x5" );
1458 }
1459
1460 /* Using 3 corr. points compute reduce */
1461
1462 /* Create matrix */
1463 CvMat matrA;
1464 double matrA_dat[3*5];
1465 matrA = cvMat(3,5,CV_64F,matrA_dat);
1466 int i;
1467 for( i = 0; i < 3; i++ )
1468 {
1469 double x1,y1,w1,x2,y2,w2;
1470 x1 = cvmGet(points1,0,i);
1471 y1 = cvmGet(points1,1,i);
1472 w1 = cvmGet(points1,2,i);
1473
1474 x2 = cvmGet(points2,0,i);
1475 y2 = cvmGet(points2,1,i);
1476 w2 = cvmGet(points2,2,i);
1477
1478 cvmSet(&matrA,i,0,y1*x2-y1*w2);
1479 cvmSet(&matrA,i,1,w1*x2-y1*w2);
1480 cvmSet(&matrA,i,2,x1*y2-y1*w2);
1481 cvmSet(&matrA,i,3,w1*y2-y1*w2);
1482 cvmSet(&matrA,i,4,x1*w2-y1*w2);
1483 }
1484
1485 /* solve system using svd */
1486 CvMat matrU;
1487 CvMat matrW;
1488 CvMat matrV;
1489
1490 double matrU_dat[3*3];
1491 double matrW_dat[3*5];
1492 double matrV_dat[5*5];
1493
1494 matrU = cvMat(3,3,CV_64F,matrU_dat);
1495 matrW = cvMat(3,5,CV_64F,matrW_dat);
1496 matrV = cvMat(5,5,CV_64F,matrV_dat);
1497
1498 /* From svd we need just two last vectors of V or two last row V' */
1499 /* We get transposed matrixes U and V */
1500
1501 cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
1502
1503 /* copy results to fundamental matrices */
1504 for(i=0;i<5;i++)
1505 {
1506 cvmSet(fundReduceCoef1,0,i,cvmGet(&matrV,3,i));
1507 cvmSet(fundReduceCoef2,0,i,cvmGet(&matrV,4,i));
1508 }
1509
1510 __END__;
1511 return;
1512
1513 }
1514
1515 /*==========================================================================================*/
1516
GetGoodReduceFundamMatrFromTwo(CvMat * fundReduceCoef1,CvMat * fundReduceCoef2,CvMat * resFundReduceCoef)1517 int GetGoodReduceFundamMatrFromTwo(CvMat* fundReduceCoef1,CvMat* fundReduceCoef2,CvMat* resFundReduceCoef)
1518 {
1519 int numRoots = 0;
1520
1521 CV_FUNCNAME( "GetGoodReduceFundamMatrFromTwo" );
1522 __BEGIN__;
1523
1524 if( fundReduceCoef1 == 0 || fundReduceCoef2 == 0 || resFundReduceCoef == 0 )
1525 {
1526 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
1527 }
1528
1529 if( !CV_IS_MAT(fundReduceCoef1) || !CV_IS_MAT(fundReduceCoef2) || !CV_IS_MAT(resFundReduceCoef) )
1530 {
1531 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
1532 }
1533
1534 /* using two fundamental matrix comute matrixes for det(F)=0 */
1535 /* May compute 1 or 3 matrices. Returns number of solutions */
1536 /* Here we will use case F=a*F1+(1-a)*F2 instead of F=m*F1+l*F2 */
1537
1538 /* Test for errors */
1539 if( fundReduceCoef1->rows != 1 || fundReduceCoef1->cols != 5 )
1540 {
1541 CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef1 must be 1x5" );
1542 }
1543
1544 if( fundReduceCoef2->rows != 1 || fundReduceCoef2->cols != 5 )
1545 {
1546 CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef2 must be 1x5" );
1547 }
1548
1549 if( (resFundReduceCoef->rows != 1 && resFundReduceCoef->rows != 3) || resFundReduceCoef->cols != 5 )
1550 {
1551 CV_ERROR( CV_StsUnmatchedSizes, "Size of resFundReduceCoef must be 1x5" );
1552 }
1553
1554 double p1,q1,r1,s1,t1;
1555 double p2,q2,r2,s2,t2;
1556 p1 = cvmGet(fundReduceCoef1,0,0);
1557 q1 = cvmGet(fundReduceCoef1,0,1);
1558 r1 = cvmGet(fundReduceCoef1,0,2);
1559 s1 = cvmGet(fundReduceCoef1,0,3);
1560 t1 = cvmGet(fundReduceCoef1,0,4);
1561
1562 p2 = cvmGet(fundReduceCoef2,0,0);
1563 q2 = cvmGet(fundReduceCoef2,0,1);
1564 r2 = cvmGet(fundReduceCoef2,0,2);
1565 s2 = cvmGet(fundReduceCoef2,0,3);
1566 t2 = cvmGet(fundReduceCoef2,0,4);
1567
1568 /* solve equation */
1569 CvMat result;
1570 CvMat coeffs;
1571 double result_dat[2*3];
1572 double coeffs_dat[4];
1573 result = cvMat(2,3,CV_64F,result_dat);
1574 coeffs = cvMat(1,4,CV_64F,coeffs_dat);
1575
1576 coeffs_dat[0] = ((r1-r2)*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)*(q1-q2)+(p1-p2)*(s1-s2)*(t1-t2));/* *a^3 */
1577 coeffs_dat[1] = ((r2*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)+(r1-r2)*(-p2-q2-r2-s2-t2))*(q1-q2)+(r1-r2)*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)*q2+(p2*(s1-s2)+(p1-p2)*s2)*(t1-t2)+(p1-p2)*(s1-s2)*t2);/* *a^2 */
1578 coeffs_dat[2] = (r2*(-p2-q2-r2-s2-t2)*(q1-q2)+(r2*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)+(r1-r2)*(-p2-q2-r2-s2-t2))*q2+p2*s2*(t1-t2)+(p2*(s1-s2)+(p1-p2)*s2)*t2);/* *a */
1579 coeffs_dat[3] = r2*(-p2-q2-r2-s2-t2)*q2+p2*s2*t2;/* 1 */
1580
1581 int num;
1582 num = cvSolveCubic(&coeffs,&result);
1583
1584
1585 /* test number of solutions and test for real solutions */
1586 int i;
1587 for( i = 0; i < num; i++ )
1588 {
1589 if( fabs(cvmGet(&result,1,i)) < 1e-8 )
1590 {
1591 double alpha = cvmGet(&result,0,i);
1592 int j;
1593 for( j = 0; j < 5; j++ )
1594 {
1595 cvmSet(resFundReduceCoef,numRoots,j,
1596 alpha * cvmGet(fundReduceCoef1,0,j) + (1-alpha) * cvmGet(fundReduceCoef2,0,j) );
1597 }
1598 numRoots++;
1599 }
1600 }
1601
1602 __END__;
1603 return numRoots;
1604 }
1605
1606 /*==========================================================================================*/
1607
GetProjMatrFromReducedFundamental(CvMat * fundReduceCoefs,CvMat * projMatrCoefs)1608 void GetProjMatrFromReducedFundamental(CvMat* fundReduceCoefs,CvMat* projMatrCoefs)
1609 {
1610 CV_FUNCNAME( "GetProjMatrFromReducedFundamental" );
1611 __BEGIN__;
1612
1613 /* Test for errors */
1614 if( fundReduceCoefs == 0 || projMatrCoefs == 0 )
1615 {
1616 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
1617 }
1618
1619 if( !CV_IS_MAT(fundReduceCoefs) || !CV_IS_MAT(projMatrCoefs) )
1620 {
1621 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
1622 }
1623
1624
1625 if( fundReduceCoefs->rows != 1 || fundReduceCoefs->cols != 5 )
1626 {
1627 CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoefs must be 1x5" );
1628 }
1629
1630 if( projMatrCoefs->rows != 1 || projMatrCoefs->cols != 4 )
1631 {
1632 CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatrCoefs must be 1x4" );
1633 }
1634
1635 /* Computes project matrix from given reduced matrix */
1636 /* we have p,q,r,s,t and need get a,b,c,d */
1637 /* Fill matrix to compute ratio a:b:c as A:B:C */
1638
1639 CvMat matrA;
1640 double matrA_dat[3*3];
1641 matrA = cvMat(3,3,CV_64F,matrA_dat);
1642
1643 double p,q,r,s,t;
1644 p = cvmGet(fundReduceCoefs,0,0);
1645 q = cvmGet(fundReduceCoefs,0,1);
1646 r = cvmGet(fundReduceCoefs,0,2);
1647 s = cvmGet(fundReduceCoefs,0,3);
1648 t = cvmGet(fundReduceCoefs,0,4);
1649
1650 matrA_dat[0] = p;
1651 matrA_dat[1] = r;
1652 matrA_dat[2] = 0;
1653
1654 matrA_dat[3] = q;
1655 matrA_dat[4] = 0;
1656 matrA_dat[5] = t;
1657
1658 matrA_dat[6] = 0;
1659 matrA_dat[7] = s;
1660 matrA_dat[8] = -(p+q+r+s+t);
1661
1662 CvMat matrU;
1663 CvMat matrW;
1664 CvMat matrV;
1665
1666 double matrU_dat[3*3];
1667 double matrW_dat[3*3];
1668 double matrV_dat[3*3];
1669
1670 matrU = cvMat(3,3,CV_64F,matrU_dat);
1671 matrW = cvMat(3,3,CV_64F,matrW_dat);
1672 matrV = cvMat(3,3,CV_64F,matrV_dat);
1673
1674 /* From svd we need just last vector of V or last row V' */
1675 /* We get transposed matrixes U and V */
1676
1677 cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
1678
1679 double A1,B1,C1;
1680 A1 = matrV_dat[6];
1681 B1 = matrV_dat[7];
1682 C1 = matrV_dat[8];
1683
1684 /* Get second coeffs */
1685 matrA_dat[0] = 0;
1686 matrA_dat[1] = r;
1687 matrA_dat[2] = t;
1688
1689 matrA_dat[3] = p;
1690 matrA_dat[4] = 0;
1691 matrA_dat[5] = -(p+q+r+s+t);
1692
1693 matrA_dat[6] = q;
1694 matrA_dat[7] = s;
1695 matrA_dat[8] = 0;
1696
1697 cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
1698
1699 double A2,B2,C2;
1700 A2 = matrV_dat[6];
1701 B2 = matrV_dat[7];
1702 C2 = matrV_dat[8];
1703
1704 double a,b,c,d;
1705 {
1706 CvMat matrK;
1707 double matrK_dat[36];
1708 matrK = cvMat(6,6,CV_64F,matrK_dat);
1709 cvZero(&matrK);
1710
1711 matrK_dat[0] = 1;
1712 matrK_dat[7] = 1;
1713 matrK_dat[14] = 1;
1714
1715 matrK_dat[18] = -1;
1716 matrK_dat[25] = -1;
1717 matrK_dat[32] = -1;
1718
1719 matrK_dat[21] = 1;
1720 matrK_dat[27] = 1;
1721 matrK_dat[33] = 1;
1722
1723 matrK_dat[0*6+4] = -A1;
1724 matrK_dat[1*6+4] = -B1;
1725 matrK_dat[2*6+4] = -C1;
1726
1727 matrK_dat[3*6+5] = -A2;
1728 matrK_dat[4*6+5] = -B2;
1729 matrK_dat[5*6+5] = -C2;
1730
1731 CvMat matrU;
1732 CvMat matrW;
1733 CvMat matrV;
1734
1735 double matrU_dat[36];
1736 double matrW_dat[36];
1737 double matrV_dat[36];
1738
1739 matrU = cvMat(6,6,CV_64F,matrU_dat);
1740 matrW = cvMat(6,6,CV_64F,matrW_dat);
1741 matrV = cvMat(6,6,CV_64F,matrV_dat);
1742
1743 /* From svd we need just last vector of V or last row V' */
1744 /* We get transposed matrixes U and V */
1745
1746 cvSVD(&matrK,&matrW,0,&matrV,CV_SVD_V_T);
1747
1748 a = matrV_dat[6*5+0];
1749 b = matrV_dat[6*5+1];
1750 c = matrV_dat[6*5+2];
1751 d = matrV_dat[6*5+3];
1752 /* we don't need last two coefficients. Because it just a k1,k2 */
1753
1754 cvmSet(projMatrCoefs,0,0,a);
1755 cvmSet(projMatrCoefs,0,1,b);
1756 cvmSet(projMatrCoefs,0,2,c);
1757 cvmSet(projMatrCoefs,0,3,d);
1758
1759 }
1760
1761 __END__;
1762 return;
1763 }
1764
1765 /*==========================================================================================*/
1766
icvComputeProjectMatrix(CvMat * objPoints,CvMat * projPoints,CvMat * projMatr)1767 void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr)
1768 {/* Using SVD method */
1769
1770 /* Reconstruct points using object points and projected points */
1771 /* Number of points must be >=6 */
1772
1773 CvMat matrV;
1774 CvMat* matrA = 0;
1775 CvMat* matrW = 0;
1776 CvMat* workProjPoints = 0;
1777 CvMat* tmpProjPoints = 0;
1778
1779 CV_FUNCNAME( "icvComputeProjectMatrix" );
1780 __BEGIN__;
1781
1782 /* Test for errors */
1783 if( objPoints == 0 || projPoints == 0 || projMatr == 0)
1784 {
1785 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
1786 }
1787
1788 if( !CV_IS_MAT(objPoints) || !CV_IS_MAT(projPoints) || !CV_IS_MAT(projMatr) )
1789 {
1790 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
1791 }
1792
1793 if( projMatr->rows != 3 || projMatr->cols != 4 )
1794 {
1795 CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" );
1796 }
1797
1798 int numPoints;
1799 numPoints = projPoints->cols;
1800 if( numPoints < 6 )
1801 {
1802 CV_ERROR( CV_StsOutOfRange, "Number of points must be at least 6" );
1803 }
1804
1805 if( numPoints != objPoints->cols )
1806 {
1807 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be same" );
1808 }
1809
1810 if( objPoints->rows != 4 )
1811 {
1812 CV_ERROR( CV_StsUnmatchedSizes, "Object points must have 4 coordinates" );
1813 }
1814
1815 if( projPoints->rows != 3 && projPoints->rows != 2 )
1816 {
1817 CV_ERROR( CV_StsUnmatchedSizes, "Projected points must have 2 or 3 coordinates" );
1818 }
1819
1820 /* Create and fill matrix A */
1821 CV_CALL( matrA = cvCreateMat(numPoints*3, 12, CV_64F) );
1822 CV_CALL( matrW = cvCreateMat(numPoints*3, 12, CV_64F) );
1823
1824 if( projPoints->rows == 2 )
1825 {
1826 CV_CALL( tmpProjPoints = cvCreateMat(3,numPoints,CV_64F) );
1827 cvMake3DPoints(projPoints,tmpProjPoints);
1828 workProjPoints = tmpProjPoints;
1829 }
1830 else
1831 {
1832 workProjPoints = projPoints;
1833 }
1834
1835 double matrV_dat[144];
1836 matrV = cvMat(12,12,CV_64F,matrV_dat);
1837 int i;
1838
1839 char* dat;
1840 dat = (char*)(matrA->data.db);
1841
1842 #if 1
1843 FILE *file;
1844 file = fopen("d:\\test\\recProjMatr.txt","w");
1845
1846 #endif
1847 for( i = 0;i < numPoints; i++ )
1848 {
1849 double x,y,w;
1850 double X,Y,Z,W;
1851 double* matrDat = (double*)dat;
1852
1853 x = cvmGet(workProjPoints,0,i);
1854 y = cvmGet(workProjPoints,1,i);
1855 w = cvmGet(workProjPoints,2,i);
1856
1857
1858 X = cvmGet(objPoints,0,i);
1859 Y = cvmGet(objPoints,1,i);
1860 Z = cvmGet(objPoints,2,i);
1861 W = cvmGet(objPoints,3,i);
1862
1863 #if 1
1864 fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf %lf)\n",i,X,Y,Z,W,x,y,w );
1865 #endif
1866
1867 /*---*/
1868 matrDat[ 0] = 0;
1869 matrDat[ 1] = 0;
1870 matrDat[ 2] = 0;
1871 matrDat[ 3] = 0;
1872
1873 matrDat[ 4] = -w*X;
1874 matrDat[ 5] = -w*Y;
1875 matrDat[ 6] = -w*Z;
1876 matrDat[ 7] = -w*W;
1877
1878 matrDat[ 8] = y*X;
1879 matrDat[ 9] = y*Y;
1880 matrDat[10] = y*Z;
1881 matrDat[11] = y*W;
1882 /*---*/
1883 matrDat[12] = w*X;
1884 matrDat[13] = w*Y;
1885 matrDat[14] = w*Z;
1886 matrDat[15] = w*W;
1887
1888 matrDat[16] = 0;
1889 matrDat[17] = 0;
1890 matrDat[18] = 0;
1891 matrDat[19] = 0;
1892
1893 matrDat[20] = -x*X;
1894 matrDat[21] = -x*Y;
1895 matrDat[22] = -x*Z;
1896 matrDat[23] = -x*W;
1897 /*---*/
1898 matrDat[24] = -y*X;
1899 matrDat[25] = -y*Y;
1900 matrDat[26] = -y*Z;
1901 matrDat[27] = -y*W;
1902
1903 matrDat[28] = x*X;
1904 matrDat[29] = x*Y;
1905 matrDat[30] = x*Z;
1906 matrDat[31] = x*W;
1907
1908 matrDat[32] = 0;
1909 matrDat[33] = 0;
1910 matrDat[34] = 0;
1911 matrDat[35] = 0;
1912 /*---*/
1913 dat += (matrA->step)*3;
1914 }
1915 #if 1
1916 fclose(file);
1917
1918 #endif
1919
1920 /* Solve this system */
1921
1922 /* From svd we need just last vector of V or last row V' */
1923 /* We get transposed matrix V */
1924
1925 cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T);
1926
1927 /* projected matrix was computed */
1928 for( i = 0; i < 12; i++ )
1929 {
1930 cvmSet(projMatr,i/4,i%4,cvmGet(&matrV,11,i));
1931 }
1932
1933 cvReleaseMat(&matrA);
1934 cvReleaseMat(&matrW);
1935 cvReleaseMat(&tmpProjPoints);
1936 __END__;
1937 }
1938
1939
1940 /*==========================================================================================*/
1941 /* May be useless function */
icvComputeTransform4D(CvMat * points1,CvMat * points2,CvMat * transMatr)1942 void icvComputeTransform4D(CvMat* points1,CvMat* points2,CvMat* transMatr)
1943 {
1944 CvMat* matrA = 0;
1945 CvMat* matrW = 0;
1946
1947 double matrV_dat[256];
1948 CvMat matrV = cvMat(16,16,CV_64F,matrV_dat);
1949
1950 CV_FUNCNAME( "icvComputeTransform4D" );
1951 __BEGIN__;
1952
1953 if( points1 == 0 || points2 == 0 || transMatr == 0)
1954 {
1955 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
1956 }
1957
1958 if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(transMatr) )
1959 {
1960 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
1961 }
1962
1963 /* Computes transformation matrix (4x4) for points1 -> points2 */
1964 /* p2=H*p1 */
1965
1966 /* Test for errors */
1967 int numPoints;
1968 numPoints = points1->cols;
1969
1970 /* we must have at least 5 points */
1971 if( numPoints < 5 )
1972 {
1973 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be at least 5" );
1974 }
1975
1976 if( numPoints != points2->cols )
1977 {
1978 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
1979 }
1980
1981 if( transMatr->rows != 4 || transMatr->cols != 4 )
1982 {
1983 CV_ERROR( CV_StsUnmatchedSizes, "Size of transMatr must be 4x4" );
1984 }
1985
1986 if( points1->rows != 4 || points2->rows != 4 )
1987 {
1988 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points must be 4" );
1989 }
1990
1991 /* Create matrix */
1992 CV_CALL( matrA = cvCreateMat(6*numPoints,16,CV_64F) );
1993 CV_CALL( matrW = cvCreateMat(6*numPoints,16,CV_64F) );
1994
1995 cvZero(matrA);
1996
1997 /* Fill matrices */
1998 int i;
1999 for( i = 0; i < numPoints; i++ )/* For each point */
2000 {
2001 double X1,Y1,Z1,W1;
2002 double P[4];
2003
2004 P[0] = cvmGet(points1,0,i);
2005 P[1] = cvmGet(points1,1,i);
2006 P[2] = cvmGet(points1,2,i);
2007 P[3] = cvmGet(points1,3,i);
2008
2009 X1 = cvmGet(points2,0,i);
2010 Y1 = cvmGet(points2,1,i);
2011 Z1 = cvmGet(points2,2,i);
2012 W1 = cvmGet(points2,3,i);
2013
2014 /* Fill matrA */
2015 for( int j = 0; j < 4; j++ )/* For each coordinate */
2016 {
2017 double x,y,z,w;
2018
2019 x = X1*P[j];
2020 y = Y1*P[j];
2021 z = Z1*P[j];
2022 w = W1*P[j];
2023
2024 cvmSet(matrA,6*i+0,4*0+j,y);
2025 cvmSet(matrA,6*i+0,4*1+j,-x);
2026
2027 cvmSet(matrA,6*i+1,4*0+j,z);
2028 cvmSet(matrA,6*i+1,4*2+j,-x);
2029
2030 cvmSet(matrA,6*i+2,4*0+j,w);
2031 cvmSet(matrA,6*i+2,4*3+j,-x);
2032
2033 cvmSet(matrA,6*i+3,4*1+j,-z);
2034 cvmSet(matrA,6*i+3,4*2+j,y);
2035
2036 cvmSet(matrA,6*i+4,4*1+j,-w);
2037 cvmSet(matrA,6*i+4,4*3+j,y);
2038
2039 cvmSet(matrA,6*i+5,4*2+j,-w);
2040 cvmSet(matrA,6*i+5,4*3+j,z);
2041 }
2042 }
2043
2044 /* From svd we need just two last vectors of V or two last row V' */
2045 /* We get transposed matrixes U and V */
2046
2047 cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T);
2048
2049 /* Copy result to result matrix */
2050 for( i = 0; i < 16; i++ )
2051 {
2052 cvmSet(transMatr,i/4,i%4,cvmGet(&matrV,15,i));
2053 }
2054
2055 cvReleaseMat(&matrA);
2056 cvReleaseMat(&matrW);
2057
2058 __END__;
2059 return;
2060 }
2061
2062 /*==========================================================================================*/
2063
icvReconstructPointsFor3View(CvMat * projMatr1,CvMat * projMatr2,CvMat * projMatr3,CvMat * projPoints1,CvMat * projPoints2,CvMat * projPoints3,CvMat * points4D)2064 void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
2065 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
2066 CvMat* points4D)
2067 {
2068 CV_FUNCNAME( "icvReconstructPointsFor3View" );
2069 __BEGIN__;
2070
2071 if( projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 ||
2072 projPoints1 == 0 || projPoints2 == 0 || projPoints3 == 0 ||
2073 points4D == 0)
2074 {
2075 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
2076 }
2077
2078 if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) ||
2079 !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) || !CV_IS_MAT(projPoints3) ||
2080 !CV_IS_MAT(points4D) )
2081 {
2082 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
2083 }
2084
2085 int numPoints;
2086 numPoints = projPoints1->cols;
2087
2088 if( numPoints < 1 )
2089 {
2090 CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );
2091 }
2092
2093 if( projPoints2->cols != numPoints || projPoints3->cols != numPoints || points4D->cols != numPoints )
2094 {
2095 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
2096 }
2097
2098 if( projPoints1->rows != 2 || projPoints2->rows != 2 || projPoints3->rows != 2)
2099 {
2100 CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
2101 }
2102
2103 if( points4D->rows != 4 )
2104 {
2105 CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
2106 }
2107
2108 if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
2109 projMatr2->cols != 4 || projMatr2->rows != 3 ||
2110 projMatr3->cols != 4 || projMatr3->rows != 3)
2111 {
2112 CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
2113 }
2114
2115 CvMat matrA;
2116 double matrA_dat[36];
2117 matrA = cvMat(9,4,CV_64F,matrA_dat);
2118
2119 //CvMat matrU;
2120 CvMat matrW;
2121 CvMat matrV;
2122 //double matrU_dat[9*9];
2123 double matrW_dat[9*4];
2124 double matrV_dat[4*4];
2125
2126 //matrU = cvMat(9,9,CV_64F,matrU_dat);
2127 matrW = cvMat(9,4,CV_64F,matrW_dat);
2128 matrV = cvMat(4,4,CV_64F,matrV_dat);
2129
2130 CvMat* projPoints[3];
2131 CvMat* projMatrs[3];
2132
2133 projPoints[0] = projPoints1;
2134 projPoints[1] = projPoints2;
2135 projPoints[2] = projPoints3;
2136
2137 projMatrs[0] = projMatr1;
2138 projMatrs[1] = projMatr2;
2139 projMatrs[2] = projMatr3;
2140
2141 /* Solve system for each point */
2142 int i,j;
2143 for( i = 0; i < numPoints; i++ )/* For each point */
2144 {
2145 /* Fill matrix for current point */
2146 for( j = 0; j < 3; j++ )/* For each view */
2147 {
2148 double x,y;
2149 x = cvmGet(projPoints[j],0,i);
2150 y = cvmGet(projPoints[j],1,i);
2151 for( int k = 0; k < 4; k++ )
2152 {
2153 cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k) );
2154 cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k) );
2155 cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) );
2156 }
2157 }
2158 /* Solve system for current point */
2159 {
2160 cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
2161
2162 /* Copy computed point */
2163 cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */
2164 cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */
2165 cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */
2166 cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */
2167 }
2168 }
2169
2170 /* Points was reconstructed. Try to reproject points */
2171 /* We can compute reprojection error if need */
2172 {
2173 int i;
2174 CvMat point3D;
2175 double point3D_dat[4];
2176 point3D = cvMat(4,1,CV_64F,point3D_dat);
2177
2178 CvMat point2D;
2179 double point2D_dat[3];
2180 point2D = cvMat(3,1,CV_64F,point2D_dat);
2181
2182 for( i = 0; i < numPoints; i++ )
2183 {
2184 double W = cvmGet(points4D,3,i);
2185
2186 point3D_dat[0] = cvmGet(points4D,0,i)/W;
2187 point3D_dat[1] = cvmGet(points4D,1,i)/W;
2188 point3D_dat[2] = cvmGet(points4D,2,i)/W;
2189 point3D_dat[3] = 1;
2190
2191 /* !!! Project this point for each camera */
2192 for( int currCamera = 0; currCamera < 3; currCamera++ )
2193 {
2194 cvmMul(projMatrs[currCamera], &point3D, &point2D);
2195
2196 float x,y;
2197 float xr,yr,wr;
2198 x = (float)cvmGet(projPoints[currCamera],0,i);
2199 y = (float)cvmGet(projPoints[currCamera],1,i);
2200
2201 wr = (float)point2D_dat[2];
2202 xr = (float)(point2D_dat[0]/wr);
2203 yr = (float)(point2D_dat[1]/wr);
2204
2205 float deltaX,deltaY;
2206 deltaX = (float)fabs(x-xr);
2207 deltaY = (float)fabs(y-yr);
2208 }
2209 }
2210 }
2211
2212 __END__;
2213 return;
2214 }
2215
2216
2217
2218
2219 #if 0
2220 void ReconstructPointsFor3View_bySolve( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
2221 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
2222 CvMat* points3D)
2223 {
2224 CV_FUNCNAME( "ReconstructPointsFor3View" );
2225 __BEGIN__;
2226
2227
2228 int numPoints;
2229 numPoints = projPoints1->cols;
2230 if( projPoints2->cols != numPoints || projPoints3->cols != numPoints || points3D->cols != numPoints )
2231 {
2232 CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
2233 }
2234
2235 if( projPoints1->rows != 2 || projPoints2->rows != 2 || projPoints3->rows != 2)
2236 {
2237 CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
2238 }
2239
2240 if( points3D->rows != 4 )
2241 {
2242 CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
2243 }
2244
2245 if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
2246 projMatr2->cols != 4 || projMatr2->rows != 3 ||
2247 projMatr3->cols != 4 || projMatr3->rows != 3)
2248 {
2249 CV_ERROR( CV_StsUnmatchedSizes, "Size of proj matrix must be 3x4" );
2250 }
2251
2252 CvMat matrA;
2253 double matrA_dat[3*3*3];
2254 matrA = cvMat(3*3,3,CV_64F,matrA_dat);
2255
2256 CvMat vectB;
2257 double vectB_dat[9];
2258 vectB = cvMat(9,1,CV_64F,vectB_dat);
2259
2260 CvMat result;
2261 double result_dat[3];
2262 result = cvMat(3,1,CV_64F,result_dat);
2263
2264 CvMat* projPoints[3];
2265 CvMat* projMatrs[3];
2266
2267 projPoints[0] = projPoints1;
2268 projPoints[1] = projPoints2;
2269 projPoints[2] = projPoints3;
2270
2271 projMatrs[0] = projMatr1;
2272 projMatrs[1] = projMatr2;
2273 projMatrs[2] = projMatr3;
2274
2275 /* Solve system for each point */
2276 int i,j;
2277 for( i = 0; i < numPoints; i++ )/* For each point */
2278 {
2279 /* Fill matrix for current point */
2280 for( j = 0; j < 3; j++ )/* For each view */
2281 {
2282 double x,y;
2283 x = cvmGet(projPoints[j],0,i);
2284 y = cvmGet(projPoints[j],1,i);
2285
2286 cvmSet(&vectB,j*3+0,0,x-cvmGet(projMatrs[j],0,3));
2287 cvmSet(&vectB,j*3+1,0,y-cvmGet(projMatrs[j],1,3));
2288 cvmSet(&vectB,j*3+2,0,1-cvmGet(projMatrs[j],2,3));
2289
2290 for( int t = 0; t < 3; t++ )
2291 {
2292 for( int k = 0; k < 3; k++ )
2293 {
2294 cvmSet(&matrA, j*3+t, k, cvmGet(projMatrs[j],t,k) );
2295 }
2296 }
2297 }
2298
2299
2300 /* Solve system for current point */
2301 cvSolve(&matrA,&vectB,&result,CV_SVD);
2302
2303 cvmSet(points3D,0,i,result_dat[0]);/* X */
2304 cvmSet(points3D,1,i,result_dat[1]);/* Y */
2305 cvmSet(points3D,2,i,result_dat[2]);/* Z */
2306 cvmSet(points3D,3,i,1);/* W */
2307
2308 }
2309
2310 /* Points was reconstructed. Try to reproject points */
2311 {
2312 int i;
2313 CvMat point3D;
2314 double point3D_dat[4];
2315 point3D = cvMat(4,1,CV_64F,point3D_dat);
2316
2317 CvMat point2D;
2318 double point2D_dat[3];
2319 point2D = cvMat(3,1,CV_64F,point2D_dat);
2320
2321 for( i = 0; i < numPoints; i++ )
2322 {
2323 double W = cvmGet(points3D,3,i);
2324
2325 point3D_dat[0] = cvmGet(points3D,0,i)/W;
2326 point3D_dat[1] = cvmGet(points3D,1,i)/W;
2327 point3D_dat[2] = cvmGet(points3D,2,i)/W;
2328 point3D_dat[3] = 1;
2329
2330 /* Project this point for each camera */
2331 for( int currCamera = 0; currCamera < 3; currCamera++ )
2332 {
2333 cvmMul(projMatrs[currCamera], &point3D, &point2D);
2334 float x,y;
2335 float xr,yr,wr;
2336 x = (float)cvmGet(projPoints[currCamera],0,i);
2337 y = (float)cvmGet(projPoints[currCamera],1,i);
2338
2339 wr = (float)point2D_dat[2];
2340 xr = (float)(point2D_dat[0]/wr);
2341 yr = (float)(point2D_dat[1]/wr);
2342
2343 }
2344 }
2345 }
2346
2347 __END__;
2348 return;
2349 }
2350 #endif
2351
2352 /*==========================================================================================*/
2353
icvComputeCameraExrinnsicByPosition(CvMat * camPos,CvMat * rotMatr,CvMat * transVect)2354 void icvComputeCameraExrinnsicByPosition(CvMat* camPos, CvMat* rotMatr, CvMat* transVect)
2355 {
2356 /* We know position of camera. we must to compute rotate matrix and translate vector */
2357
2358 CV_FUNCNAME( "icvComputeCameraExrinnsicByPosition" );
2359 __BEGIN__;
2360
2361 /* Test input paramaters */
2362 if( camPos == 0 || rotMatr == 0 || transVect == 0 )
2363 {
2364 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
2365 }
2366
2367 if( !CV_IS_MAT(camPos) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(transVect) )
2368 {
2369 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
2370 }
2371
2372 if( camPos->cols != 1 || camPos->rows != 3 )
2373 {
2374 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of camera position must be 3x1 vector" );
2375 }
2376
2377 if( rotMatr->cols != 3 || rotMatr->rows != 3 )
2378 {
2379 CV_ERROR( CV_StsUnmatchedSizes, "Rotate matrix must be 3x3" );
2380 }
2381
2382 if( transVect->cols != 1 || transVect->rows != 3 )
2383 {
2384 CV_ERROR( CV_StsUnmatchedSizes, "Translate vector must be 3x1" );
2385 }
2386
2387 double x,y,z;
2388 x = cvmGet(camPos,0,0);
2389 y = cvmGet(camPos,1,0);
2390 z = cvmGet(camPos,2,0);
2391
2392 /* Set translate vector. It same as camea position */
2393 cvmSet(transVect,0,0,x);
2394 cvmSet(transVect,1,0,y);
2395 cvmSet(transVect,2,0,z);
2396
2397 /* Compute rotate matrix. Compute each unit transformed vector */
2398
2399 /* normalize flat direction x,y */
2400 double vectorX[3];
2401 double vectorY[3];
2402 double vectorZ[3];
2403
2404 vectorX[0] = -z;
2405 vectorX[1] = 0;
2406 vectorX[2] = x;
2407
2408 vectorY[0] = x*y;
2409 vectorY[1] = x*x+z*z;
2410 vectorY[2] = z*y;
2411
2412 vectorZ[0] = -x;
2413 vectorZ[1] = -y;
2414 vectorZ[2] = -z;
2415
2416 /* normaize vectors */
2417 double norm;
2418 int i;
2419
2420 /* Norm X */
2421 norm = 0;
2422 for( i = 0; i < 3; i++ )
2423 norm += vectorX[i]*vectorX[i];
2424 norm = sqrt(norm);
2425 for( i = 0; i < 3; i++ )
2426 vectorX[i] /= norm;
2427
2428 /* Norm Y */
2429 norm = 0;
2430 for( i = 0; i < 3; i++ )
2431 norm += vectorY[i]*vectorY[i];
2432 norm = sqrt(norm);
2433 for( i = 0; i < 3; i++ )
2434 vectorY[i] /= norm;
2435
2436 /* Norm Z */
2437 norm = 0;
2438 for( i = 0; i < 3; i++ )
2439 norm += vectorZ[i]*vectorZ[i];
2440 norm = sqrt(norm);
2441 for( i = 0; i < 3; i++ )
2442 vectorZ[i] /= norm;
2443
2444 /* Set output results */
2445
2446 for( i = 0; i < 3; i++ )
2447 {
2448 cvmSet(rotMatr,i,0,vectorX[i]);
2449 cvmSet(rotMatr,i,1,vectorY[i]);
2450 cvmSet(rotMatr,i,2,vectorZ[i]);
2451 }
2452
2453 {/* Try to inverse rotate matrix */
2454 CvMat tmpInvRot;
2455 double tmpInvRot_dat[9];
2456 tmpInvRot = cvMat(3,3,CV_64F,tmpInvRot_dat);
2457 cvInvert(rotMatr,&tmpInvRot,CV_SVD);
2458 cvConvert(&tmpInvRot,rotMatr);
2459
2460
2461
2462 }
2463
2464 __END__;
2465
2466 return;
2467 }
2468
2469 /*==========================================================================================*/
2470
FindTransformForProjectMatrices(CvMat * projMatr1,CvMat * projMatr2,CvMat * rotMatr,CvMat * transVect)2471 void FindTransformForProjectMatrices(CvMat* projMatr1,CvMat* projMatr2,CvMat* rotMatr,CvMat* transVect)
2472 {
2473 /* Computes homography for project matrix be "canonical" form */
2474 CV_FUNCNAME( "computeProjMatrHomography" );
2475 __BEGIN__;
2476
2477 /* Test input paramaters */
2478 if( projMatr1 == 0 || projMatr2 == 0 || rotMatr == 0 || transVect == 0 )
2479 {
2480 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
2481 }
2482
2483 if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(transVect) )
2484 {
2485 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
2486 }
2487
2488 if( projMatr1->cols != 4 || projMatr1->rows != 3 )
2489 {
2490 CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix 1 must be 3x4" );
2491 }
2492
2493 if( projMatr2->cols != 4 || projMatr2->rows != 3 )
2494 {
2495 CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix 2 must be 3x4" );
2496 }
2497
2498 if( rotMatr->cols != 3 || rotMatr->rows != 3 )
2499 {
2500 CV_ERROR( CV_StsUnmatchedSizes, "Size of rotation matrix must be 3x3" );
2501 }
2502
2503 if( transVect->cols != 1 || transVect->rows != 3 )
2504 {
2505 CV_ERROR( CV_StsUnmatchedSizes, "Size of translation vector must be 3x1" );
2506 }
2507
2508 CvMat matrA;
2509 double matrA_dat[12*12];
2510 matrA = cvMat(12,12,CV_64F,matrA_dat);
2511 CvMat vectB;
2512 double vectB_dat[12];
2513 vectB = cvMat(12,1,CV_64F,vectB_dat);
2514
2515 cvZero(&matrA);
2516 cvZero(&vectB);
2517 int i,j;
2518 for( i = 0; i < 12; i++ )
2519 {
2520 for( j = 0; j < 12; j++ )
2521 {
2522 cvmSet(&matrA,i,j,cvmGet(projMatr1,i/4,j%4));
2523 }
2524 /* Fill vector B */
2525
2526 double val = cvmGet(projMatr2,i/4,i%4);
2527 if( (i+1)%4 == 0 )
2528 {
2529 val -= cvmGet(projMatr1,i/4,3);
2530
2531 }
2532 cvmSet(&vectB,i,0,val);
2533 }
2534
2535 /* Solve system */
2536 CvMat resVect;
2537 double resVect_dat[12];
2538 resVect = cvMat(12,1,CV_64F,resVect_dat);
2539
2540 int sing;
2541 sing = cvSolve(&matrA,&vectB,&resVect);
2542
2543 /* Fill rotation matrix */
2544 for( i = 0; i < 12; i++ )
2545 {
2546 double val = cvmGet(&resVect,i,0);
2547 if( i < 9 )
2548 cvmSet(rotMatr,i%3,i/3,val);
2549 else
2550 cvmSet(transVect,i-9,0,val);
2551 }
2552
2553 __END__;
2554
2555 return;
2556 }
2557
2558 /*==========================================================================================*/
2559 #if 0
2560 void icvComputeQknowPrincipalPoint(int numImages, CvMat **projMatrs,CvMat *matrQ, double cx,double cy)
2561 {
2562 /* Computes matrix Q */
2563 /* focal x and y eqauls () */
2564 /* we know principal point for camera */
2565 /* focal may differ from image to image */
2566 /* image skew is 0 */
2567
2568 if( numImages < 10 )
2569 {
2570 return;
2571 //Error. Number of images too few
2572 }
2573
2574 /* Create */
2575
2576
2577 return;
2578 }
2579 #endif
2580
2581 /*==========================================================================================*/
2582
2583 /*==========================================================================================*/
2584 /*==========================================================================================*/
2585 /*==========================================================================================*/
2586 /*==========================================================================================*/
2587 /* Part with metric reconstruction */
2588
2589 #if 1
icvComputeQ(int numMatr,CvMat ** projMatr,CvMat ** cameraMatr,CvMat * matrQ)2590 void icvComputeQ(int numMatr, CvMat** projMatr, CvMat** cameraMatr, CvMat* matrQ)
2591 {
2592 /* K*K' = P*Q*P' */
2593 /* try to solve Q by linear method */
2594
2595 CvMat* matrA = 0;
2596 CvMat* vectB = 0;
2597
2598 CV_FUNCNAME( "ComputeQ" );
2599 __BEGIN__;
2600
2601 /* Define number of projection matrices */
2602 if( numMatr < 2 )
2603 {
2604 CV_ERROR( CV_StsUnmatchedSizes, "Number of projection matrices must be at least 2" );
2605 }
2606
2607
2608 /* test matrices sizes */
2609 if( matrQ->cols != 4 || matrQ->rows != 4 )
2610 {
2611 CV_ERROR( CV_StsUnmatchedSizes, "Size of matrix Q must be 3x3" );
2612 }
2613
2614 int currMatr;
2615 for( currMatr = 0; currMatr < numMatr; currMatr++ )
2616 {
2617
2618 if( cameraMatr[currMatr]->cols != 3 || cameraMatr[currMatr]->rows != 3 )
2619 {
2620 CV_ERROR( CV_StsUnmatchedSizes, "Size of each camera matrix must be 3x3" );
2621 }
2622
2623 if( projMatr[currMatr]->cols != 4 || projMatr[currMatr]->rows != 3 )
2624 {
2625 CV_ERROR( CV_StsUnmatchedSizes, "Size of each camera matrix must be 3x3" );
2626 }
2627 }
2628
2629 CvMat matrw;
2630 double matrw_dat[9];
2631 matrw = cvMat(3,3,CV_64F,matrw_dat);
2632
2633 CvMat matrKt;
2634 double matrKt_dat[9];
2635 matrKt = cvMat(3,3,CV_64F,matrKt_dat);
2636
2637
2638 /* Create matrix A and vector B */
2639 CV_CALL( matrA = cvCreateMat(9*numMatr,10,CV_64F) );
2640 CV_CALL( vectB = cvCreateMat(9*numMatr,1,CV_64F) );
2641
2642 double dataQ[16];
2643
2644 for( currMatr = 0; currMatr < numMatr; currMatr++ )
2645 {
2646 int ord10[10] = {0,1,2,3,5,6,7,10,11,15};
2647 /* Fill atrix A by data from matrices */
2648
2649 /* Compute matrix w for current camera matrix */
2650 cvTranspose(cameraMatr[currMatr],&matrKt);
2651 cvmMul(cameraMatr[currMatr],&matrKt,&matrw);
2652
2653 /* Fill matrix A and vector B */
2654
2655 int currWi,currWj;
2656 int currMatr;
2657 for( currMatr = 0; currMatr < numMatr; currMatr++ )
2658 {
2659 for( currWi = 0; currWi < 3; currWi++ )
2660 {
2661 for( currWj = 0; currWj < 3; currWj++ )
2662 {
2663 int i,j;
2664 for( i = 0; i < 4; i++ )
2665 {
2666 for( j = 0; j < 4; j++ )
2667 {
2668 /* get elements from current projection matrix */
2669 dataQ[i*4+j] = cvmGet(projMatr[currMatr],currWi,j) *
2670 cvmGet(projMatr[currMatr],currWj,i);
2671 }
2672 }
2673
2674 /* we know 16 elements in dataQ move them to matrQ 10 */
2675 dataQ[1] += dataQ[4];
2676 dataQ[2] += dataQ[8];
2677 dataQ[3] += dataQ[12];
2678 dataQ[6] += dataQ[9];
2679 dataQ[7] += dataQ[13];
2680 dataQ[11] += dataQ[14];
2681 /* Now first 10 elements has coeffs */
2682
2683 /* copy to matrix A */
2684 for( i = 0; i < 10; i++ )
2685 {
2686 cvmSet(matrA,currMatr*9 + currWi*3+currWj,i,dataQ[ord10[i]]);
2687 }
2688 }
2689 }
2690
2691 /* Fill vector B */
2692 for( int i = 0; i < 9; i++ )
2693 {
2694 cvmSet(vectB,currMatr*9+i,0,matrw_dat[i]);
2695 }
2696 }
2697 }
2698
2699 /* Matrix A and vector B filled and we can solve system */
2700
2701 /* Solve system */
2702 CvMat resQ;
2703 double resQ_dat[10];
2704 resQ = cvMat(10,1,CV_64F,resQ_dat);
2705
2706 cvSolve(matrA,vectB,&resQ,CV_SVD);
2707
2708 /* System was solved. We know matrix Q. But we must have condition det Q=0 */
2709 /* Just copy result matrix Q */
2710 {
2711 int curr = 0;
2712 int ord16[16] = {0,1,2,3,1,4,5,6,2,5,7,8,3,6,8,9};
2713
2714 for( int i = 0; i < 4; i++ )
2715 {
2716 for( int j = 0; j < 4; j++ )
2717 {
2718 cvmSet(matrQ,i,j,resQ_dat[ord16[curr++]]);
2719 }
2720 }
2721 }
2722
2723
2724 __END__;
2725
2726 /* Free allocated memory */
2727 cvReleaseMat(&matrA);
2728 cvReleaseMat(&vectB);
2729
2730 return;
2731 }
2732 #endif
2733 /*-----------------------------------------------------------------------------------------------------*/
2734
icvDecomposeQ(CvMat *,CvMat *)2735 void icvDecomposeQ(CvMat* /*matrQ*/,CvMat* /*matrH*/)
2736 {
2737 #if 0
2738 /* Use SVD to decompose matrix Q=H*I*H' */
2739 /* test input data */
2740
2741 CvMat matrW;
2742 CvMat matrU;
2743 // CvMat matrV;
2744 double matrW_dat[16];
2745 double matrU_dat[16];
2746 // double matrV_dat[16];
2747
2748 matrW = cvMat(4,4,CV_64F,matrW_dat);
2749 matrU = cvMat(4,4,CV_64F,matrU_dat);
2750 // matrV = cvMat(4,4,CV_64F,matrV_dat);
2751
2752 cvSVD(matrQ,&matrW,&matrU,0);
2753
2754 double eig[3];
2755 eig[0] = fsqrt(cvmGet(&matrW,0,0));
2756 eig[1] = fsqrt(cvmGet(&matrW,1,1));
2757 eig[2] = fsqrt(cvmGet(&matrW,2,2));
2758
2759 CvMat matrIS;
2760 double matrIS_dat[16];
2761 matrIS =
2762
2763
2764
2765
2766 /* det for matrix Q with q1-q10 */
2767 /*
2768 + q1*q5*q8*q10
2769 - q1*q5*q9*q9
2770 - q1*q6*q6*q10
2771 + 2*q1*q6*q7*q9
2772 - q1*q7*q7*q8
2773 - q2*q2*q8*q10
2774 + q2*q2*q9*q9
2775 + 2*q2*q6*q3*q10
2776 - 2*q2*q6*q4*q9
2777 - 2*q2*q7*q3*q9
2778 + 2*q2*q7*q4*q8
2779 - q5*q3*q3*q10
2780 + 2*q3*q5*q4*q9
2781 + q3*q3*q7*q7
2782 - 2*q3*q7*q4*q6
2783 - q5*q4*q4*q8
2784 + q4*q4*q6*q6
2785 */
2786
2787 // (1-a)^4 = 1 - 4 * a + 6 * a * a - 4 * a * a * a + a * a * a * a;
2788
2789
2790 #endif
2791 }
2792
2793