git.maemo.org Git - opencv/blob - cvaux/src/cvlevmartrif.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cvaux.h"
  43
  44 #include "cvtypes.h"
  45 #include <float.h>
  46 #include <limits.h>
  47 #include "cv.h"
  48
  49 /* Valery Mosyagin */
  50
  51 typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
  52 typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );
  53
  54 void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
  55                                     pointer_LMFunc function,
  56                                     /*pointer_Err error_function,*/
  57                                     CvMat *X0,CvMat *observRes,CvMat *resultX,
  58                                     int maxIter,double epsilon);
  59
  60 void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
  61                                 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
  62                                 CvMat* points4D);
  63
  64
  65 /* Jacobian computation for trifocal case */
  66 void icvJacobianFunction_ProjTrifocal(const CvMat *vectX,CvMat *Jacobian)
  67 {
  68     CV_FUNCNAME( "icvJacobianFunction_ProjTrifocal" );
  69     __BEGIN__;
  70
  71     /* Test data for errors */
  72     if( vectX == 0 || Jacobian == 0 )
  73     {
  74         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  75     }
  76
  77     if( !CV_IS_MAT(vectX) || !CV_IS_MAT(Jacobian) )
  78     {
  79         CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
  80     }
  81
  82     int numPoints;
  83     numPoints = (vectX->rows - 36)/4;
  84
  85     if( numPoints < 1 )//!!! Need to correct this minimal number of points
  86     {
  87         CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" );
  88     }
  89
  90     if( Jacobian->rows == numPoints*6 || Jacobian->cols != 36+numPoints*4 )
  91     {
  92         CV_ERROR( CV_StsUnmatchedSizes, "Size of Jacobian is not correct it must be 6*numPoints x (36+numPoints*4)" );
  93     }
  94
  95     /* Computed Jacobian in a given point */
  96     /* This is for function with 3 projection matrices */
  97     /* vector X consists of projection matrices and points3D */
  98     /* each 3D points has X,Y,Z,W */
  99     /* each projection matrices has 3x4 coeffs */
 100     /* For N points 4D we have Jacobian 2N x (12*3+4N) */
 101
 102     /* Will store derivates as  */
 103     /* Fill Jacobian matrix */
 104     int currProjPoint;
 105     int currMatr;
 106
 107     cvZero(Jacobian);
 108     for( currMatr = 0; currMatr < 3; currMatr++ )
 109     {
 110         double p[12];
 111         for( int i=0;i<12;i++ )
 112         {
 113             p[i] = cvmGet(vectX,currMatr*12+i,0);
 114         }
 115
 116         int currVal = 36;
 117         for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ )
 118         {
 119             /* Compute */
 120             double X[4];
 121             X[0] = cvmGet(vectX,currVal++,0);
 122             X[1] = cvmGet(vectX,currVal++,0);
 123             X[2] = cvmGet(vectX,currVal++,0);
 124             X[3] = cvmGet(vectX,currVal++,0);
 125
 126             double piX[3];
 127             piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2]  + X[3]*p[3];
 128             piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6]  + X[3]*p[7];
 129             piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11];
 130
 131             int i,j;
 132             /* fill derivate by point */
 133
 134             double tmp3 = 1/(piX[2]*piX[2]);
 135
 136             double tmp1 = -piX[0]*tmp3;
 137             double tmp2 = -piX[1]*tmp3;
 138             for( j = 0; j < 2; j++ )//for x and y
 139             {
 140                 for( i = 0; i < 4; i++ )// for X,Y,Z,W
 141                 {
 142                     cvmSet( Jacobian,
 143                             currMatr*numPoints*2+currProjPoint*2+j, 36+currProjPoint*4+i,
 144                             (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3  );
 145                 }
 146             }
 147                 /* fill derivate by projection matrix */
 148             for( i = 0; i < 4; i++ )
 149             {
 150                 /* derivate for x */
 151                 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+i,X[i]/piX[2]);//x' p1i
 152                 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+8+i,X[i]*tmp1);//x' p3i
 153
 154                 /* derivate for y */
 155                 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+4+i,X[i]/piX[2]);//y' p2i
 156                 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+8+i,X[i]*tmp2);//y' p3i
 157             }
 158
 159         }
 160     }
 161
 162     __END__;
 163     return;
 164 }
 165
 166 void icvFunc_ProjTrifocal(const CvMat *vectX, CvMat *resFunc)
 167 {
 168     /* Computes function in a given point */
 169     /* Computers project points using 3 projection matrices and points 3D */
 170
 171     /* vector X consists of projection matrices and points3D */
 172     /* each projection matrices has 3x4 coeffs */
 173     /* each 3D points has X,Y,Z,W(?) */
 174
 175     /* result of function is projection of N 3D points using 3 projection matrices */
 176     /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */
 177     /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */
 178
 179     /* Compute projection of points */
 180
 181     /* Fill projection matrices */
 182
 183     CV_FUNCNAME( "icvFunc_ProjTrifocal" );
 184     __BEGIN__;
 185
 186     /* Test data for errors */
 187     if( vectX == 0 || resFunc == 0 )
 188     {
 189         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 190     }
 191
 192     if( !CV_IS_MAT(vectX) || !CV_IS_MAT(resFunc) )
 193     {
 194         CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
 195     }
 196
 197     int numPoints;
 198     numPoints = (vectX->rows - 36)/4;
 199
 200     if( numPoints < 1 )//!!! Need to correct this minimal number of points
 201     {
 202         CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" );
 203     }
 204
 205     if( resFunc->rows == 2*numPoints*3 || resFunc->cols != 1 )
 206     {
 207         CV_ERROR( CV_StsUnmatchedSizes, "Size of resFunc is not correct it must be 2*numPoints*3 x 1");
 208     }
 209
 210
 211     CvMat projMatrs[3];
 212     double projMatrs_dat[36];
 213     projMatrs[0] = cvMat(3,4,CV_64F,projMatrs_dat);
 214     projMatrs[1] = cvMat(3,4,CV_64F,projMatrs_dat+12);
 215     projMatrs[2] = cvMat(3,4,CV_64F,projMatrs_dat+24);
 216
 217     CvMat point3D;
 218     double point3D_dat[3];
 219     point3D = cvMat(3,1,CV_64F,point3D_dat);
 220
 221     int currMatr;
 222     int currV;
 223     int i,j;
 224
 225     currV=0;
 226     for( currMatr = 0; currMatr < 3; currMatr++ )
 227     {
 228         for( i = 0; i < 3; i++ )
 229         {
 230             for( j = 0;j < 4; j++ )
 231             {
 232                 double val = cvmGet(vectX,currV,0);
 233                 cvmSet(&projMatrs[currMatr],i,j,val);
 234                 currV++;
 235             }
 236         }
 237     }
 238
 239     /* Project points */
 240     int currPoint;
 241     CvMat point4D;
 242     double point4D_dat[4];
 243     point4D = cvMat(4,1,CV_64F,point4D_dat);
 244     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 245     {
 246         /* get curr point */
 247         point4D_dat[0] = cvmGet(vectX,currV++,0);
 248         point4D_dat[1] = cvmGet(vectX,currV++,0);
 249         point4D_dat[2] = cvmGet(vectX,currV++,0);
 250         point4D_dat[3] = cvmGet(vectX,currV++,0);
 251
 252         for( currMatr = 0; currMatr < 3; currMatr++ )
 253         {
 254             /* Compute projection for current point */
 255             cvmMul(&projMatrs[currMatr],&point4D,&point3D);
 256             double z = point3D_dat[2];
 257             cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2,  0,point3D_dat[0]/z);
 258             cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2+1,0,point3D_dat[1]/z);
 259         }
 260     }
 261
 262     __END__;
 263     return;
 264 }
 265
 266
 267 /*----------------------------------------------------------------------------------------*/
 268
 269 void icvOptimizeProjectionTrifocal(CvMat **projMatrs,CvMat **projPoints,
 270                                 CvMat **resultProjMatrs, CvMat *resultPoints4D)
 271 {
 272
 273     CvMat *optimX    = 0;
 274     CvMat *points4D  = 0;
 275     CvMat *vectorX0  = 0;
 276     CvMat *observRes = 0;
 277     //CvMat *error     = 0;
 278
 279     CV_FUNCNAME( "icvOptimizeProjectionTrifocal" );
 280     __BEGIN__;
 281
 282     /* Test data for errors */
 283     if( projMatrs == 0 || projPoints == 0 || resultProjMatrs == 0 || resultPoints4D == 0)
 284     {
 285         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 286     }
 287
 288     if( !CV_IS_MAT(resultPoints4D) )
 289     {
 290         CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix" );
 291     }
 292
 293     int numPoints;
 294     numPoints = resultPoints4D->cols;
 295     if( numPoints < 1 )
 296     {
 297         CV_ERROR( CV_StsOutOfRange, "Number points of resultPoints4D must be more than 0" );
 298     }
 299
 300     if( resultPoints4D->rows != 4 )
 301     {
 302         CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" );
 303     }
 304
 305     int i;
 306     for( i = 0; i < 3; i++ )
 307     {
 308         if( projMatrs[i] == 0 )
 309         {
 310             CV_ERROR( CV_StsNullPtr, "Some of projMatrs is a NULL pointer" );
 311         }
 312
 313         if( projPoints[i] == 0 )
 314         {
 315             CV_ERROR( CV_StsNullPtr, "Some of projPoints is a NULL pointer" );
 316         }
 317
 318         if( resultProjMatrs[i] == 0 )
 319         {
 320             CV_ERROR( CV_StsNullPtr, "Some of resultProjMatrs is a NULL pointer" );
 321         }
 322
 323         /* ----------- test for matrix ------------- */
 324         if( !CV_IS_MAT(projMatrs[i]) )
 325         {
 326             CV_ERROR( CV_StsUnsupportedFormat, "Each of projMatrs must be a matrix" );
 327         }
 328
 329         if( !CV_IS_MAT(projPoints[i]) )
 330         {
 331             CV_ERROR( CV_StsUnsupportedFormat, "Each of projPoints must be a matrix" );
 332         }
 333
 334         if( !CV_IS_MAT(resultProjMatrs[i]) )
 335         {
 336             CV_ERROR( CV_StsUnsupportedFormat, "Each of resultProjMatrs must be a matrix" );
 337         }
 338
 339         /* ------------- Test sizes --------------- */
 340         if( projMatrs[i]->rows != 3 || projMatrs[i]->cols != 4 )
 341         {
 342             CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" );
 343         }
 344
 345         if( projPoints[i]->rows != 2 || projPoints[i]->cols != numPoints )
 346         {
 347             CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" );
 348         }
 349
 350         if( resultProjMatrs[i]->rows != 3 || resultProjMatrs[i]->cols != 4 )
 351         {
 352             CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" );
 353         }
 354     }
 355
 356
 357     /* Allocate memory for points 4D */
 358     CV_CALL( points4D  = cvCreateMat(4,numPoints,CV_64F) );
 359     CV_CALL( vectorX0  = cvCreateMat(36 + numPoints*4,1,CV_64F) );
 360     CV_CALL( observRes = cvCreateMat(2*numPoints*3,1,CV_64F) );
 361     CV_CALL( optimX    = cvCreateMat(36+numPoints*4,1,CV_64F) );
 362     //CV_CALL( error     = cvCreateMat(numPoints*2*3,1,CV_64F) );
 363
 364
 365     /* Reconstruct points 4D using projected points and projection matrices */
 366     icvReconstructPointsFor3View( projMatrs[0],projMatrs[1],projMatrs[2],
 367                                   projPoints[0],projPoints[1],projPoints[2],
 368                                   points4D);
 369
 370
 371
 372     /* Fill observed points on images */
 373     /* result of function is projection of N 3D points using 3 projection matrices */
 374     /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */
 375     /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */
 376     int currMatr;
 377     for( currMatr = 0; currMatr < 3; currMatr++ )
 378     {
 379         for( i = 0; i < numPoints; i++ )
 380         {
 381             cvmSet(observRes,currMatr*numPoints*2+i*2  ,0,cvmGet(projPoints[currMatr],0,i) );/* x */
 382             cvmSet(observRes,currMatr*numPoints*2+i*2+1,0,cvmGet(projPoints[currMatr],1,i) );/* y */
 383         }
 384     }
 385
 386     /* Fill with projection matrices */
 387     for( currMatr = 0; currMatr < 3; currMatr++ )
 388     {
 389         int i;
 390         for( i = 0; i < 12; i++ )
 391         {
 392             cvmSet(vectorX0,currMatr*12+i,0,cvmGet(projMatrs[currMatr],i/4,i%4));
 393         }
 394     }
 395
 396     /* Fill with 4D points */
 397
 398     int currPoint;
 399     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 400     {
 401         cvmSet(vectorX0,36 + currPoint*4 + 0,0,cvmGet(points4D,0,currPoint));
 402         cvmSet(vectorX0,36 + currPoint*4 + 1,0,cvmGet(points4D,1,currPoint));
 403         cvmSet(vectorX0,36 + currPoint*4 + 2,0,cvmGet(points4D,2,currPoint));
 404         cvmSet(vectorX0,36 + currPoint*4 + 3,0,cvmGet(points4D,3,currPoint));
 405     }
 406
 407
 408     /* Allocate memory for result */
 409     cvLevenbergMarquardtOptimization( icvJacobianFunction_ProjTrifocal, icvFunc_ProjTrifocal,
 410                                       vectorX0,observRes,optimX,100,1e-6);
 411
 412     /* Copy results */
 413     for( currMatr = 0; currMatr < 3; currMatr++ )
 414     {
 415         /* Copy projection matrices */
 416         for(int i=0;i<12;i++)
 417         {
 418             cvmSet(resultProjMatrs[currMatr],i/4,i%4,cvmGet(optimX,currMatr*12+i,0));
 419         }
 420     }
 421
 422     /* Copy 4D points */
 423     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 424     {
 425         cvmSet(resultPoints4D,0,currPoint,cvmGet(optimX,36 + currPoint*4,0));
 426         cvmSet(resultPoints4D,1,currPoint,cvmGet(optimX,36 + currPoint*4+1,0));
 427         cvmSet(resultPoints4D,2,currPoint,cvmGet(optimX,36 + currPoint*4+2,0));
 428         cvmSet(resultPoints4D,3,currPoint,cvmGet(optimX,36 + currPoint*4+3,0));
 429     }
 430
 431     __END__;
 432
 433     /* Free allocated memory */
 434     cvReleaseMat(&optimX);
 435     cvReleaseMat(&points4D);
 436     cvReleaseMat(&vectorX0);
 437     cvReleaseMat(&observRes);
 438
 439     return;
 440
 441
 442 }
 443
 444 /*------------------------------------------------------------------------------*/
 445 /* Create good points using status information */
 446 void icvCreateGoodPoints(CvMat *points,CvMat **goodPoints, CvMat *status)
 447 {
 448     *goodPoints = 0;
 449
 450     CV_FUNCNAME( "icvCreateGoodPoints" );
 451     __BEGIN__;
 452
 453     int numPoints;
 454     numPoints = points->cols;
 455
 456     if( numPoints < 1 )
 457     {
 458         CV_ERROR( CV_StsOutOfRange, "Number of points must be more than 0" );
 459     }
 460
 461     int numCoord;
 462     numCoord = points->rows;
 463     if( numCoord < 1 )
 464     {
 465         CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be more than 0" );
 466     }
 467
 468     /* Define number of good points */
 469     int goodNum;
 470     int i,j;
 471
 472     goodNum = 0;
 473     for( i = 0; i < numPoints; i++)
 474     {
 475         if( cvmGet(status,0,i) > 0 )
 476             goodNum++;
 477     }
 478
 479     /* Allocate memory for good points */
 480     CV_CALL( *goodPoints = cvCreateMat(numCoord,goodNum,CV_64F) );
 481
 482     for( i = 0; i < numCoord; i++ )
 483     {
 484         int currPoint = 0;
 485         for( j = 0; j < numPoints; j++)
 486         {
 487             if( cvmGet(status,0,j) > 0 )
 488             {
 489                 cvmSet(*goodPoints,i,currPoint,cvmGet(points,i,j));
 490                 currPoint++;
 491             }
 492         }
 493     }
 494     __END__;
 495     return;
 496 }
 497