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