vcs.maemo.org Git - opencv/blob - src/cvaux/cvtrifocal.cpp

   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