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 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 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. */ 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 /*==========================================================================================*/ 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 /*==========================================================================================*/ 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 /*==========================================================================================*/ 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 /*==========================================================================================*/ 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 /*==========================================================================================*/ 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 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 /*==========================================================================================*/ 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 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 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 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 */ 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 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 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 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 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 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
