opencv/src/cv/cvfundam.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 "_cv.h"
  43
  44 /* Evaluation of Fundamental Matrix from point correspondences.
  45    The original code has been written by Valery Mosyagin */
  46
  47 /* The algorithms (except for RANSAC) and the notation have been taken from
  48    Zhengyou Zhang's research report
  49    "Determining the Epipolar Geometry and its Uncertainty: A Review"
  50    that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
  51
  52 /************************************** 7-point algorithm *******************************/
  53 static int
  54 icvFMatrix_7Point( const CvPoint2D64f* m0, const CvPoint2D64f* m1, double* fmatrix )
  55 {
  56     double a[7*9], w[7], v[9*9], c[4], r[3];
  57     double* f1, *f2;
  58     double t0, t1, t2;
  59     CvMat A = cvMat( 7, 9, CV_64F, a );
  60     CvMat V = cvMat( 9, 9, CV_64F, v );
  61     CvMat W = cvMat( 7, 1, CV_64F, w );
  62     CvMat coeffs = cvMat( 1, 4, CV_64F, c );
  63     CvMat roots = cvMat( 1, 3, CV_64F, r );
  64     int i, k, n;
  65
  66     assert( m0 && m1 && fmatrix );
  67
  68     // form a linear system: i-th row of A(=a) represents
  69     // the equation: (m1[i], 1)'*F*(m0[i], 1) = 0
  70     for( i = 0; i < 7; i++ )
  71     {
  72         double x0 = m0[i].x, y0 = m0[i].y;
  73         double x1 = m1[i].x, y1 = m1[i].y;
  74
  75         a[i*9+0] = x1*x0;
  76         a[i*9+1] = x1*y0;
  77         a[i*9+2] = x1;
  78         a[i*9+3] = y1*x0;
  79         a[i*9+4] = y1*y0;
  80         a[i*9+5] = y1;
  81         a[i*9+6] = x0;
  82         a[i*9+7] = y0;
  83         a[i*9+8] = 1;
  84     }
  85
  86     // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
  87     // the solution is linear subspace of dimensionality 2.
  88     // => use the last two singular vectors as a basis of the space
  89     // (according to SVD properties)
  90     cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
  91     f1 = v + 7*9;
  92     f2 = v + 8*9;
  93
  94     // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
  95     // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
  96     // so f ~ lambda*f1 + (1 - lambda)*f2.
  97     // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
  98     // it will be a cubic equation.
  99     // find c - polynomial coefficients.
 100     for( i = 0; i < 9; i++ )
 101         f1[i] -= f2[i];
 102
 103     t0 = f2[4]*f2[8] - f2[5]*f2[7];
 104     t1 = f2[3]*f2[8] - f2[5]*f2[6];
 105     t2 = f2[3]*f2[7] - f2[4]*f2[6];
 106
 107     c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
 108
 109     c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
 110            f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
 111            f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
 112            f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
 113            f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
 114            f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
 115            f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
 116
 117     t0 = f1[4]*f1[8] - f1[5]*f1[7];
 118     t1 = f1[3]*f1[8] - f1[5]*f1[6];
 119     t2 = f1[3]*f1[7] - f1[4]*f1[6];
 120
 121     c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
 122            f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
 123            f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
 124            f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
 125            f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
 126            f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
 127            f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
 128
 129     c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
 130
 131     // solve the cubic equation; there can be 1 to 3 roots ...
 132     n = cvSolveCubic( &coeffs, &roots );
 133
 134     if( n < 1 || n > 3 )
 135         return n;
 136
 137     for( k = 0; k < n; k++, fmatrix += 9 )
 138     {
 139         // for each root form the fundamental matrix
 140         double lambda = r[k], mu = 1.;
 141         double s = f1[8]*r[k] + f2[8];
 142
 143         // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
 144         if( fabs(s) > DBL_EPSILON )
 145         {
 146             mu = 1./s;
 147             lambda *= mu;
 148             fmatrix[8] = 1.;
 149         }
 150         else
 151             fmatrix[8] = 0.;
 152
 153         for( i = 0; i < 8; i++ )
 154             fmatrix[i] = f1[i]*lambda + f2[i]*mu;
 155     }
 156
 157     return n;
 158 }
 159
 160
 161 /*************************************** 8-point algorithm ******************************/
 162 static int
 163 icvFMatrix_8Point( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
 164                    const uchar* mask, int count, double* fmatrix )
 165 {
 166     int result = 0;
 167     CvMat* A = 0;
 168
 169     double w[9], v[9*9];
 170     CvMat W = cvMat( 1, 9, CV_64F, w);
 171     CvMat V = cvMat( 9, 9, CV_64F, v);
 172     CvMat U, F0, TF;
 173
 174     int i, good_count = 0;
 175     CvPoint2D64f m0c = {0,0}, m1c = {0,0}, m0q = {0,0}, m1q = {0,0};
 176     double t, scale0, scale1;
 177     double* a;
 178     int a_step;
 179
 180     CV_FUNCNAME( "icvFMatrix_8Point" );
 181
 182     __BEGIN__;
 183
 184     assert( m0 && m1 && fmatrix );
 185
 186     // compute centers and average distances for each of the two point sets
 187     for( i = 0; i < count; i++ )
 188         if( !mask || mask[i] )
 189         {
 190             double x = m0[i].x, y = m0[i].y;
 191             m0c.x += x; m0c.y += y;
 192             m0q.x += x*x; m0q.y += y*y;
 193
 194             x = m1[i].x, y = m1[i].y;
 195             m1c.x += x; m1c.y += y;
 196             m1q.x += x*x; m1q.y += y*y;
 197             good_count++;
 198         }
 199
 200     if( good_count < 8 )
 201         EXIT;
 202
 203     // calculate the normalizing transformations for each of the point sets:
 204     // after the transformation each set will have the mass center at the coordinate origin
 205     // and the average distance from the origin will be ~sqrt(2).
 206     t = 1./good_count;
 207     m0c.x *= t; m0c.y *= t;
 208     scale0 = t * sqrt( m0q.x + m0q.y - good_count*(m0c.x*m0c.x + m0c.y*m0c.y) );
 209     m1c.x *= t; m1c.y *= t;
 210     scale1 = t * sqrt( m1q.x + m1q.y - good_count*(m1c.x*m1c.x + m1c.y*m1c.y) );
 211
 212     if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
 213         EXIT;
 214
 215     scale0 = sqrt(2.)/scale0;
 216     scale1 = sqrt(2.)/scale1;
 217
 218     CV_CALL( A = cvCreateMat( good_count, 9, CV_64F ));
 219     a = A->data.db;
 220     a_step = A->step / sizeof(a[0]);
 221
 222     // form a linear system: for each selected pair of points m0 & m1,
 223     // the row of A(=a) represents the equation: (m1, 1)'*F*(m0, 1) = 0
 224     for( i = 0; i < count; i++ )
 225     {
 226         if( !mask || mask[i] )
 227         {
 228             double x0 = (m0[i].x - m0c.x)*scale0;
 229             double y0 = (m0[i].y - m0c.y)*scale0;
 230             double x1 = (m1[i].x - m1c.x)*scale1;
 231             double y1 = (m1[i].y - m1c.y)*scale1;
 232
 233             a[0] = x1*x0;
 234             a[1] = x1*y0;
 235             a[2] = x1;
 236             a[3] = y1*x0;
 237             a[4] = y1*y0;
 238             a[5] = y1;
 239             a[6] = x0;
 240             a[7] = y0;
 241             a[8] = 1;
 242             a += a_step;
 243         }
 244     }
 245
 246     cvSVD( A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
 247
 248     for( i = 0; i < 8; i++ )
 249     {
 250         if( fabs(w[i]) < FLT_EPSILON )
 251             break;
 252     }
 253
 254     if( i < 7 )
 255         EXIT;
 256
 257     F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
 258
 259     // make F0 singular (of rank 2) by decomposing it with SVD,
 260     // zeroing the last diagonal element of W and then composing the matrices back.
 261
 262     // use v as a temporary storage for different 3x3 matrices
 263     W = U = V = TF = F0;
 264     W.data.db = v;
 265     U.data.db = v + 9;
 266     V.data.db = v + 18;
 267     TF.data.db = v + 27;
 268
 269     cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 270     W.data.db[8] = 0.;
 271
 272     // F0 <- U*diag([W(1), W(2), 0])*V'
 273     cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
 274     cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
 275
 276     // apply the transformation that is inverse
 277     // to what we used to normalize the point coordinates
 278     {
 279         double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
 280         double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
 281         CvMat T0, T1;
 282         T0 = T1 = F0;
 283         T0.data.db = tt0;
 284         T1.data.db = tt1;
 285
 286         // F0 <- T1'*F0*T0
 287         cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
 288         F0.data.db = fmatrix;
 289         cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
 290
 291         // make F(3,3) = 1
 292         if( fabs(F0.data.db[8]) > FLT_EPSILON )
 293             cvScale( &F0, &F0, 1./F0.data.db[8] );
 294     }
 295
 296     result = 1;
 297
 298     __END__;
 299
 300     cvReleaseMat( &A );
 301     return result;
 302 }
 303
 304
 305 /************************************ RANSAC algorithm **********************************/
 306 static int
 307 icvFMatrix_RANSAC( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
 308                    uchar* mask, int count, double* fmatrix,
 309                    double threshold, double p,
 310                    unsigned rng_seed, int use_8point )
 311 {
 312     int result = 0;
 313
 314     const int max_random_iters = 1000;
 315     const int sample_size = 7;
 316     uchar* curr_mask = 0;
 317     uchar* temp_mask = 0;
 318
 319     CV_FUNCNAME( "icvFMatrix_RANSAC" );
 320
 321     __BEGIN__;
 322
 323     double ff[9*3];
 324     CvRNG rng = cvRNG(rng_seed);
 325     int i, j, k, sample_count, max_samples = 500;
 326     int best_good_count = 0;
 327
 328     assert( m0 && m1 && fmatrix && 0 < p && p < 1 && threshold > 0 );
 329
 330     threshold *= threshold;
 331
 332     CV_CALL( curr_mask = (uchar*)cvAlloc( count ));
 333     if( !mask && use_8point )
 334     {
 335         CV_CALL( temp_mask = (uchar*)cvAlloc( count ));
 336         mask = temp_mask;
 337     }
 338
 339     // find the best fundamental matrix (giving the least backprojection error)
 340     // by picking at most <max_samples> 7-tuples of corresponding points
 341     // <max_samples> may be updated (decreased) within the loop based on statistics of outliers
 342     for( sample_count = 0; sample_count < max_samples; sample_count++ )
 343     {
 344         int idx[sample_size], n;
 345         CvPoint2D64f ms0[sample_size], ms1[sample_size];
 346
 347         // choose random <sample_size> (=7) points
 348         for( i = 0; i < sample_size; i++ )
 349         {
 350             for( k = 0; k < max_random_iters; k++ )
 351             {
 352                 idx[i] = cvRandInt(&rng) % count;
 353                 for( j = 0; j < i; j++ )
 354                     if( idx[j] == idx[i] )
 355                         break;
 356                 if( j == i )
 357                 {
 358                     ms0[i] = m0[idx[i]];
 359                     ms1[i] = m1[idx[i]];
 360                     break;
 361                 }
 362             }
 363             if( k >= max_random_iters )
 364                 break;
 365         }
 366
 367         if( i < sample_size )
 368             continue;
 369
 370         // find 1 or 3 fundamental matrices out of the 7 point correspondences
 371         n = icvFMatrix_7Point( ms0, ms1, ff );
 372
 373         if( n < 1 || n > 3 )
 374             continue;
 375
 376         // for each matrix calculate the backprojection error
 377         // (distance to the corresponding epipolar lines) for each point and thus find
 378         // the number of in-liers.
 379         for( k = 0; k < n; k++ )
 380         {
 381             const double* f = ff + k*9;
 382             int good_count = 0;
 383
 384             for( i = 0; i < count; i++ )
 385             {
 386                 double d0, d1, s0, s1;
 387
 388                 double a = f[0]*m0[i].x + f[1]*m0[i].y + f[2];
 389                 double b = f[3]*m0[i].x + f[4]*m0[i].y + f[5];
 390                 double c = f[6]*m0[i].x + f[7]*m0[i].y + f[8];
 391
 392                 s1 = a*a + b*b;
 393                 d1 = m1[i].x*a + m1[i].y*b + c;
 394
 395                 a = f[0]*m1[i].x + f[3]*m1[i].y + f[6];
 396                 b = f[1]*m1[i].x + f[4]*m1[i].y + f[7];
 397                 c = f[2]*m1[i].x + f[5]*m1[i].y + f[8];
 398
 399                 s0 = a*a + b*b;
 400                 d0 = m0[i].x*a + m0[i].y*b + c;
 401
 402                 curr_mask[i] = d1*d1 < threshold*s1 && d0*d0 < threshold*s0;
 403                 good_count += curr_mask[i];
 404             }
 405
 406             if( good_count > MAX( best_good_count, 6 ) )
 407             {
 408                 double ep, lp, lep;
 409                 int new_max_samples;
 410
 411                 // update the current best fundamental matrix and "goodness" flags
 412                 if( mask )
 413                     memcpy( mask, curr_mask, count );
 414                 memcpy( fmatrix, f, 9*sizeof(f[0]));
 415                 best_good_count = good_count;
 416
 417                 // try to update (decrease) <max_samples>
 418                 ep = (double)(count - good_count)/count;
 419                 lp = log(1. - p);
 420                 lep = log(1. - pow(ep,7.));
 421                 if( lp < lep || lep >= 0 )
 422                     break;
 423                 else
 424                 {
 425                     new_max_samples = cvRound(lp/lep);
 426                     max_samples = MIN( new_max_samples, max_samples );
 427                 }
 428             }
 429         }
 430     }
 431
 432     if( best_good_count < 7 )
 433         EXIT;
 434
 435     result = 1;
 436
 437     // optionally, use 8-point algorithm to compute fundamental matrix using only the in-liers
 438     if( best_good_count >= 8 && use_8point )
 439         result = icvFMatrix_8Point( m0, m1, mask, count, fmatrix );
 440
 441     __END__;
 442
 443     cvFree( &temp_mask );
 444     cvFree( &curr_mask );
 445
 446     return result;
 447 }
 448
 449
 450 /***************************** Least Median of Squares algorithm ************************/
 451
 452 static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
 453
 454 /* the algorithm is quite similar to RANSAC, but here we choose the matrix that gives
 455    the least median of d(m0[i], F'*m1[i])^2 + d(m1[i], F*m0[i])^2 (0<=i<count),
 456    instead of choosing the matrix that gives the least number of outliers (as it is done in RANSAC) */
 457 static int
 458 icvFMatrix_LMedS( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
 459                   uchar* mask, int count, double* fmatrix,
 460                   double threshold, double p,
 461                   unsigned rng_seed, int use_8point )
 462 {
 463     int result = 0;
 464
 465     const int max_random_iters = 1000;
 466     const int sample_size = 7;
 467
 468     float* dist = 0;
 469     uchar* curr_mask = 0;
 470     uchar* temp_mask = 0;
 471
 472     CV_FUNCNAME( "icvFMatrix_LMedS" );
 473
 474     __BEGIN__;
 475
 476     double ff[9*3];
 477     CvRNG rng = cvRNG(rng_seed);
 478     int i, j, k, sample_count, max_samples = 500;
 479     double least_median = DBL_MAX, median;
 480     int best_good_count = 0;
 481
 482     assert( m0 && m1 && fmatrix && 0 < p && p < 1 && threshold > 0 );
 483
 484     threshold *= threshold;
 485
 486     CV_CALL( curr_mask = (uchar*)cvAlloc( count ));
 487     CV_CALL( dist = (float*)cvAlloc( count*sizeof(dist[0]) ));
 488
 489     if( !mask && use_8point )
 490     {
 491         CV_CALL( temp_mask = (uchar*)cvAlloc( count ));
 492         mask = temp_mask;
 493     }
 494
 495     // find the best fundamental matrix (giving the least backprojection error)
 496     // by picking at most <max_samples> 7-tuples of corresponding points
 497     // <max_samples> may be updated (decreased) within the loop based on statistics of outliers
 498     for( sample_count = 0; sample_count < max_samples; sample_count++ )
 499     {
 500         int idx[sample_size], n;
 501         CvPoint2D64f ms0[sample_size], ms1[sample_size];
 502
 503         // choose random <sample_size> (=7) points
 504         for( i = 0; i < sample_size; i++ )
 505         {
 506             for( k = 0; k < max_random_iters; k++ )
 507             {
 508                 idx[i] = cvRandInt(&rng) % count;
 509                 for( j = 0; j < i; j++ )
 510                     if( idx[j] == idx[i] )
 511                         break;
 512                 if( j == i )
 513                 {
 514                     ms0[i] = m0[idx[i]];
 515                     ms1[i] = m1[idx[i]];
 516                     break;
 517                 }
 518             }
 519             if( k >= max_random_iters )
 520                 break;
 521         }
 522
 523         if( i < sample_size )
 524             continue;
 525
 526         // find 1 or 3 fundamental matrix out of the 7 point correspondences
 527         n = icvFMatrix_7Point( ms0, ms1, ff );
 528
 529         if( n < 1 || n > 3 )
 530             continue;
 531
 532         // for each matrix calculate the backprojection error
 533         // (distance to the corresponding epipolar lines) for each point and thus find
 534         // the number of in-liers.
 535         for( k = 0; k < n; k++ )
 536         {
 537             const double* f = ff + k*9;
 538             int good_count = 0;
 539
 540             for( i = 0; i < count; i++ )
 541             {
 542                 double d0, d1, s;
 543
 544                 double a = f[0]*m0[i].x + f[1]*m0[i].y + f[2];
 545                 double b = f[3]*m0[i].x + f[4]*m0[i].y + f[5];
 546                 double c = f[6]*m0[i].x + f[7]*m0[i].y + f[8];
 547
 548                 s = 1./(a*a + b*b);
 549                 d1 = m1[i].x*a + m1[i].y*b + c;
 550                 d1 = s*d1*d1;
 551
 552                 a = f[0]*m1[i].x + f[3]*m1[i].y + f[6];
 553                 b = f[1]*m1[i].x + f[4]*m1[i].y + f[7];
 554                 c = f[2]*m1[i].x + f[5]*m1[i].y + f[8];
 555
 556                 s = 1./(a*a + b*b);
 557                 d0 = m0[i].x*a + m0[i].y*b + c;
 558                 d0 = s*d0*d0;
 559
 560                 curr_mask[i] = d1 < threshold && d0 < threshold;
 561                 good_count += curr_mask[i];
 562
 563                 dist[i] = (float)(d0 + d1);
 564             }
 565
 566             icvSortDistances( (int*)dist, count, 0 );
 567             median = (double)dist[count/2];
 568
 569             if( median < least_median )
 570             {
 571                 double ep, lp, lep;
 572                 int new_max_samples;
 573
 574                 // update the current best fundamental matrix and "goodness" flags
 575                 if( mask )
 576                     memcpy( mask, curr_mask, count );
 577                 memcpy( fmatrix, f, 9*sizeof(f[0]));
 578                 least_median = median;
 579                 best_good_count = good_count;
 580
 581                 // try to update (decrease) <max_samples>
 582                 ep = (double)(count - good_count)/count;
 583                 lp = log(1. - p);
 584                 lep = log(1. - pow(ep,7.));
 585                 if( lp < lep || lep >= 0 )
 586                     break;
 587                 else
 588                 {
 589                     new_max_samples = cvRound(lp/lep);
 590                     max_samples = MIN( new_max_samples, max_samples );
 591                 }
 592             }
 593         }
 594     }
 595
 596     if( best_good_count < 7 )
 597         EXIT;
 598
 599     result = 1;
 600
 601     // optionally, use 8-point algorithm to compute fundamental matrix using only the in-liers
 602     if( best_good_count >= 8 && use_8point )
 603         result = icvFMatrix_8Point( m0, m1, mask, count, fmatrix );
 604
 605     __END__;
 606
 607     cvFree( &temp_mask );
 608     cvFree( &curr_mask );
 609     cvFree( &dist );
 610
 611     return result;
 612 }
 613
 614
 615 CV_IMPL int
 616 cvFindFundamentalMat( const CvMat* points0, const CvMat* points1,
 617                       CvMat* fmatrix, int method,
 618                       double param1, double param2, CvMat* status )
 619 {
 620     const unsigned rng_seed = 0xffffffff;
 621     int result = 0;
 622     int pt_alloc_flag[2] = { 0, 0 };
 623     int i, k;
 624     CvPoint2D64f* pt[2] = { 0, 0 };
 625     CvMat* _status = 0;
 626
 627     CV_FUNCNAME( "cvFindFundamentalMat" );
 628
 629     __BEGIN__;
 630
 631     int count, dims;
 632     int depth, cn;
 633     uchar* status_data = 0;
 634     double fmatrix_data0[9*3];
 635     double* fmatrix_data = 0;
 636
 637     if( !CV_IS_MAT(points0) )
 638         CV_ERROR( !points0 ? CV_StsNullPtr : CV_StsBadArg, "points0 is not a valid matrix" );
 639
 640     if( !CV_IS_MAT(points1) )
 641         CV_ERROR( !points1 ? CV_StsNullPtr : CV_StsBadArg, "points1 is not a valid matrix" );
 642
 643     if( !CV_ARE_TYPES_EQ(points0, points1) )
 644         CV_ERROR( CV_StsUnmatchedFormats, "The matrices of points should have the same data type" );
 645
 646     if( !CV_ARE_SIZES_EQ(points0, points1) )
 647         CV_ERROR( CV_StsUnmatchedSizes, "The matrices of points should have the same size" );
 648
 649     depth = CV_MAT_DEPTH(points0->type);
 650     cn = CV_MAT_CN(points0->type);
 651     if( depth != CV_32S && depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
 652         CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrices is unsupported" );
 653
 654     if( points0->rows > points0->cols )
 655     {
 656         dims = cn*points0->cols;
 657         count = points0->rows;
 658     }
 659     else
 660     {
 661         if( points0->rows > 1 && cn > 1 || points0->rows == 1 && cn == 1 )
 662             CV_ERROR( CV_StsBadSize, "The point matrices do not have a proper layout (2xn, 3xn, nx2 or nx3)" );
 663         dims = cn * points0->rows;
 664         count = points0->cols;
 665     }
 666
 667     if( dims != 2 && dims != 3 )
 668         CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
 669
 670     if( method == CV_FM_7POINT && count != 7 ||
 671         method != CV_FM_7POINT && count < 7 + (method == CV_FM_8POINT) )
 672         CV_ERROR( CV_StsOutOfRange,
 673         "The number of points must be 7 for 7-point algorithm, "
 674         ">=8 for 8-point algorithm and >=7 for other algorithms" );
 675
 676     if( !CV_IS_MAT(fmatrix) )
 677         CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
 678
 679     if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
 680         CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
 681
 682     if( fmatrix->cols != 3 || (fmatrix->rows != 3 && (method != CV_FM_7POINT || fmatrix->rows != 9)))
 683         CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3 or 3x9 (for 7-point method only)" );
 684
 685     fmatrix_data = fmatrix->data.db;
 686     if( !CV_IS_MAT_CONT(fmatrix->type) || CV_MAT_TYPE(fmatrix->type) != CV_64FC1 ||
 687         method == CV_FM_7POINT && fmatrix->rows != 9 )
 688         fmatrix_data = fmatrix_data0;
 689
 690     if( status )
 691     {
 692         if( !CV_IS_MAT(status) )
 693             CV_ERROR( CV_StsBadArg, "The output status is not a valid matrix" );
 694
 695         if( status->cols != 1 && status->rows != 1 || status->cols + status->rows - 1 != count )
 696             CV_ERROR( CV_StsUnmatchedSizes,
 697             "The status matrix must have the same size as the point matrices" );
 698
 699         if( method == CV_FM_7POINT || method == CV_FM_8POINT )
 700             cvSet( status, cvScalarAll(1.) );
 701         else
 702         {
 703             status_data = status->data.ptr;
 704             if( !CV_IS_MAT_CONT(status->type) || !CV_IS_MASK_ARR(status) )
 705             {
 706                 CV_CALL( _status = cvCreateMat( status->rows, status->cols, CV_8UC1 ));
 707                 status_data = _status->data.ptr;
 708             }
 709         }
 710     }
 711
 712     for( k = 0; k < 2; k++ )
 713     {
 714         const CvMat* spt = k == 0 ? points0 : points1;
 715         CvPoint2D64f* dpt = pt[k] = (CvPoint2D64f*)spt->data.db;
 716         int plane_stride, stride, elem_size;
 717
 718         if( CV_IS_MAT_CONT(spt->type) && CV_MAT_DEPTH(spt->type) == CV_64F &&
 719             dims == 2 && (spt->rows == 1 || spt->rows == count) )
 720             continue;
 721
 722         elem_size = CV_ELEM_SIZE(depth);
 723
 724         if( spt->rows == dims )
 725         {
 726             plane_stride = spt->step / elem_size;
 727             stride = 1;
 728         }
 729         else
 730         {
 731             plane_stride = 1;
 732             stride = spt->rows == 1 ? dims : spt->step / elem_size;
 733         }
 734
 735         CV_CALL( dpt = pt[k] = (CvPoint2D64f*)cvAlloc( count*sizeof(dpt[0]) ));
 736         pt_alloc_flag[k] = 1;
 737
 738         if( depth == CV_32F )
 739         {
 740             const float* xp = spt->data.fl;
 741             const float* yp = xp + plane_stride;
 742             const float* zp = dims == 3 ? yp + plane_stride : 0;
 743
 744             for( i = 0; i < count; i++ )
 745             {
 746                 double x = *xp, y = *yp;
 747                 xp += stride;
 748                 yp += stride;
 749                 if( dims == 3 )
 750                 {
 751                     double z = *zp;
 752                     zp += stride;
 753                     z = z ? 1./z : 1.;
 754                     x *= z;
 755                     y *= z;
 756                 }
 757                 dpt[i].x = x;
 758                 dpt[i].y = y;
 759             }
 760         }
 761         else
 762         {
 763             const double* xp = spt->data.db;
 764             const double* yp = xp + plane_stride;
 765             const double* zp = dims == 3 ? yp + plane_stride : 0;
 766
 767             for( i = 0; i < count; i++ )
 768             {
 769                 double x = *xp, y = *yp;
 770                 xp += stride;
 771                 yp += stride;
 772                 if( dims == 3 )
 773                 {
 774                     double z = *zp;
 775                     zp += stride;
 776                     z = z ? 1./z : 1.;
 777                     x *= z;
 778                     y *= z;
 779                 }
 780                 dpt[i].x = x;
 781                 dpt[i].y = y;
 782             }
 783         }
 784     }
 785
 786     if( method == CV_FM_7POINT )
 787         result = icvFMatrix_7Point( pt[0], pt[1], fmatrix_data );
 788     else if( method == CV_FM_8POINT )
 789         result = icvFMatrix_8Point( pt[0], pt[1], 0, count, fmatrix_data );
 790     else
 791     {
 792         if( param1 < 0 )
 793             CV_ERROR( CV_StsOutOfRange, "param1 (threshold) must be > 0" );
 794
 795         if( param2 < 0 || param2 > 1 )
 796             CV_ERROR( CV_StsOutOfRange, "param2 (confidence level) must be between 0 and 1" );
 797
 798         if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
 799             param2 = 0.99;
 800
 801         if( method < CV_FM_RANSAC_ONLY )
 802             result = icvFMatrix_LMedS( pt[0], pt[1], status_data, count, fmatrix_data,
 803                                        param1, param2, rng_seed, method & CV_FM_8POINT );
 804         else
 805             result = icvFMatrix_RANSAC( pt[0], pt[1], status_data, count, fmatrix_data,
 806                                         param1, param2, rng_seed, method & CV_FM_8POINT );
 807     }
 808
 809     if( result && fmatrix->data.db != fmatrix_data )
 810     {
 811         CvMat hdr;
 812         cvZero( fmatrix );
 813         hdr = cvMat( MIN(fmatrix->rows, result*3), fmatrix->cols, CV_64F, fmatrix_data );
 814         cvConvert( &hdr, fmatrix );
 815     }
 816
 817     if( status && status_data && status->data.ptr != status_data )
 818         cvConvert( _status, status );
 819
 820     __END__;
 821
 822     cvReleaseMat( &_status );
 823     for( k = 0; k < 2; k++ )
 824         if( pt_alloc_flag[k] )
 825             cvFree( &pt[k] );
 826
 827     return result;
 828 }
 829
 830
 831 CV_IMPL void
 832 cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
 833                              const CvMat* fmatrix, CvMat* lines )
 834 {
 835     CV_FUNCNAME( "cvComputeCorrespondEpilines" );
 836
 837     __BEGIN__;
 838
 839     int abc_stride, abc_plane_stride, abc_elem_size;
 840     int plane_stride, stride, elem_size;
 841     int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
 842     uchar *ap, *bp, *cp;
 843     const uchar *xp, *yp, *zp;
 844     double f[9];
 845     CvMat F = cvMat( 3, 3, CV_64F, f );
 846
 847     if( !CV_IS_MAT(points) )
 848         CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
 849
 850     depth = CV_MAT_DEPTH(points->type);
 851     cn = CV_MAT_CN(points->type);
 852     if( depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
 853         CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
 854
 855     if( points->rows > points->cols )
 856     {
 857         dims = cn*points->cols;
 858         count = points->rows;
 859     }
 860     else
 861     {
 862         if( points->rows > 1 && cn > 1 || points->rows == 1 && cn == 1 )
 863             CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
 864         dims = cn * points->rows;
 865         count = points->cols;
 866     }
 867
 868     if( dims != 2 && dims != 3 )
 869         CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
 870
 871     if( !CV_IS_MAT(fmatrix) )
 872         CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
 873
 874     if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
 875         CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
 876
 877     if( fmatrix->cols != 3 || fmatrix->rows != 3 )
 878         CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );
 879
 880     if( !CV_IS_MAT(lines) )
 881         CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
 882
 883     abc_depth = CV_MAT_DEPTH(lines->type);
 884     abc_cn = CV_MAT_CN(lines->type);
 885     if( abc_depth != CV_32F && abc_depth != CV_64F || abc_cn != 1 && abc_cn != 3 )
 886         CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
 887
 888     if( lines->rows > lines->cols )
 889     {
 890         abc_dims = abc_cn*lines->cols;
 891         abc_count = lines->rows;
 892     }
 893     else
 894     {
 895         if( lines->rows > 1 && abc_cn > 1 || lines->rows == 1 && abc_cn == 1 )
 896             CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
 897         abc_dims = abc_cn * lines->rows;
 898         abc_count = lines->cols;
 899     }
 900
 901     if( abc_dims != 3 )
 902         CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
 903
 904     if( abc_count != count )
 905         CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
 906
 907     elem_size = CV_ELEM_SIZE(depth);
 908     abc_elem_size = CV_ELEM_SIZE(abc_depth);
 909
 910     if( points->rows == dims )
 911     {
 912         plane_stride = points->step;
 913         stride = elem_size;
 914     }
 915     else
 916     {
 917         plane_stride = elem_size;
 918         stride = points->rows == 1 ? dims*elem_size : points->step;
 919     }
 920
 921     if( lines->rows == 3 )
 922     {
 923         abc_plane_stride = lines->step;
 924         abc_stride = abc_elem_size;
 925     }
 926     else
 927     {
 928         abc_plane_stride = abc_elem_size;
 929         abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
 930     }
 931
 932     CV_CALL( cvConvert( fmatrix, &F ));
 933     if( pointImageID == 2 )
 934         cvTranspose( &F, &F );
 935
 936     xp = points->data.ptr;
 937     yp = xp + plane_stride;
 938     zp = dims == 3 ? yp + plane_stride : 0;
 939
 940     ap = lines->data.ptr;
 941     bp = ap + abc_plane_stride;
 942     cp = bp + abc_plane_stride;
 943
 944     for( i = 0; i < count; i++ )
 945     {
 946         double x, y, z = 1.;
 947         double a, b, c, nu;
 948
 949         if( depth == CV_32F )
 950         {
 951             x = *(float*)xp; y = *(float*)yp;
 952             if( zp )
 953                 z = *(float*)zp, zp += stride;
 954         }
 955         else
 956         {
 957             x = *(double*)xp; y = *(double*)yp;
 958             if( zp )
 959                 z = *(double*)zp, zp += stride;
 960         }
 961
 962         xp += stride; yp += stride;
 963
 964         a = f[0]*x + f[1]*y + f[2]*z;
 965         b = f[3]*x + f[4]*y + f[5]*z;
 966         c = f[6]*x + f[7]*y + f[8]*z;
 967         nu = a*a + b*b;
 968         nu = nu ? 1./sqrt(nu) : 1.;
 969         a *= nu; b *= nu; c *= nu;
 970
 971         if( abc_depth == CV_32F )
 972         {
 973             *(float*)ap = (float)a;
 974             *(float*)bp = (float)b;
 975             *(float*)cp = (float)c;
 976         }
 977         else
 978         {
 979             *(double*)ap = a;
 980             *(double*)bp = b;
 981             *(double*)cp = c;
 982         }
 983
 984         ap += abc_stride;
 985         bp += abc_stride;
 986         cp += abc_stride;
 987     }
 988
 989     __END__;
 990 }
 991
 992
 993 CV_IMPL void
 994 cvConvertPointsHomogenious( const CvMat* src, CvMat* dst )
 995 {
 996     CvMat* temp = 0;
 997     CvMat* denom = 0;
 998
 999     CV_FUNCNAME( "cvConvertPointsHomogenious" );
1000
1001     __BEGIN__;
1002
1003     int i, s_count, s_dims, d_count, d_dims;
1004     CvMat _src, _dst, _ones;
1005     CvMat* ones = 0;
1006
1007     if( !CV_IS_MAT(src) )
1008         CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
1009         "The input parameter is not a valid matrix" );
1010
1011     if( !CV_IS_MAT(dst) )
1012         CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
1013         "The output parameter is not a valid matrix" );
1014
1015     if( src == dst || src->data.ptr == dst->data.ptr )
1016     {
1017         if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
1018             CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
1019         EXIT;
1020     }
1021
1022     if( src->rows > src->cols )
1023     {
1024         if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
1025             CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
1026
1027         s_dims = CV_MAT_CN(src->type)*src->cols;
1028         s_count = src->rows;
1029     }
1030     else
1031     {
1032         if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
1033             CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
1034
1035         s_dims = CV_MAT_CN(src->type)*src->rows;
1036         s_count = src->cols;
1037     }
1038
1039     if( src->rows == 1 || src->cols == 1 )
1040         src = cvReshape( src, &_src, 1, s_count );
1041
1042     if( dst->rows > dst->cols )
1043     {
1044         if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
1045             CV_ERROR( CV_StsBadSize,
1046             "Either the number of channels or columns or rows in the input matrix must be =1" );
1047
1048         d_dims = CV_MAT_CN(dst->type)*dst->cols;
1049         d_count = dst->rows;
1050     }
1051     else
1052     {
1053         if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
1054             CV_ERROR( CV_StsBadSize,
1055             "Either the number of channels or columns or rows in the output matrix must be =1" );
1056
1057         d_dims = CV_MAT_CN(dst->type)*dst->rows;
1058         d_count = dst->cols;
1059     }
1060
1061     if( dst->rows == 1 || dst->cols == 1 )
1062         dst = cvReshape( dst, &_dst, 1, d_count );
1063
1064     if( s_count != d_count )
1065         CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
1066
1067     if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
1068         CV_ERROR( CV_StsUnsupportedFormat,
1069         "Both matrices must be floating-point (single or double precision)" );
1070
1071     if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
1072         CV_ERROR( CV_StsOutOfRange,
1073         "Both input and output point dimensionality must be 2, 3 or 4" );
1074
1075     if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
1076         CV_ERROR( CV_StsUnmatchedSizes,
1077         "The dimensionalities of input and output point sets differ too much" );
1078
1079     if( s_dims == d_dims - 1 )
1080     {
1081         if( d_count == dst->rows )
1082         {
1083             ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
1084             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
1085         }
1086         else
1087         {
1088             ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
1089             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
1090         }
1091     }
1092
1093     if( s_dims <= d_dims )
1094     {
1095         if( src->rows == dst->rows && src->cols == dst->cols )
1096         {
1097             if( CV_ARE_TYPES_EQ( src, dst ) )
1098                 cvCopy( src, dst );
1099             else
1100                 cvConvert( src, dst );
1101         }
1102         else
1103         {
1104             if( !CV_ARE_TYPES_EQ( src, dst ))
1105             {
1106                 CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
1107                 cvConvert( src, temp );
1108                 src = temp;
1109             }
1110             cvTranspose( src, dst );
1111         }
1112
1113         if( ones )
1114             cvSet( ones, cvRealScalar(1.) );
1115     }
1116     else
1117     {
1118         int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
1119
1120         if( !CV_ARE_TYPES_EQ( src, dst ))
1121         {
1122             CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
1123             cvConvert( src, temp );
1124             src = temp;
1125         }
1126
1127         elem_size = CV_ELEM_SIZE(src->type);
1128
1129         if( s_count == src->cols )
1130             s_plane_stride = src->step / elem_size, s_stride = 1;
1131         else
1132             s_stride = src->step / elem_size, s_plane_stride = 1;
1133
1134         if( d_count == dst->cols )
1135             d_plane_stride = dst->step / elem_size, d_stride = 1;
1136         else
1137             d_stride = dst->step / elem_size, d_plane_stride = 1;
1138
1139         CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
1140
1141         if( CV_MAT_DEPTH(dst->type) == CV_32F )
1142         {
1143             const float* xs = src->data.fl;
1144             const float* ys = xs + s_plane_stride;
1145             const float* zs = 0;
1146             const float* ws = xs + (s_dims - 1)*s_plane_stride;
1147
1148             float* iw = denom->data.fl;
1149
1150             float* xd = dst->data.fl;
1151             float* yd = xd + d_plane_stride;
1152             float* zd = 0;
1153
1154             if( d_dims == 3 )
1155             {
1156                 zs = ys + s_plane_stride;
1157                 zd = yd + d_plane_stride;
1158             }
1159
1160             for( i = 0; i < d_count; i++, ws += s_stride )
1161             {
1162                 float t = *ws;
1163                 iw[i] = t ? t : 1.f;
1164             }
1165
1166             cvDiv( 0, denom, denom );
1167
1168             if( d_dims == 3 )
1169                 for( i = 0; i < d_count; i++ )
1170                 {
1171                     float w = iw[i];
1172                     float x = *xs * w, y = *ys * w, z = *zs * w;
1173                     xs += s_stride; ys += s_stride; zs += s_stride;
1174                     *xd = x; *yd = y; *zd = z;
1175                     xd += d_stride; yd += d_stride; zd += d_stride;
1176                 }
1177             else
1178                 for( i = 0; i < d_count; i++ )
1179                 {
1180                     float w = iw[i];
1181                     float x = *xs * w, y = *ys * w;
1182                     xs += s_stride; ys += s_stride;
1183                     *xd = x; *yd = y;
1184                     xd += d_stride; yd += d_stride;
1185                 }
1186         }
1187         else
1188         {
1189             const double* xs = src->data.db;
1190             const double* ys = xs + s_plane_stride;
1191             const double* zs = 0;
1192             const double* ws = xs + (s_dims - 1)*s_plane_stride;
1193
1194             double* iw = denom->data.db;
1195
1196             double* xd = dst->data.db;
1197             double* yd = xd + d_plane_stride;
1198             double* zd = 0;
1199
1200             if( d_dims == 3 )
1201             {
1202                 zs = ys + s_plane_stride;
1203                 zd = yd + d_plane_stride;
1204             }
1205
1206             for( i = 0; i < d_count; i++, ws += s_stride )
1207             {
1208                 double t = *ws;
1209                 iw[i] = t ? t : 1.;
1210             }
1211
1212             cvDiv( 0, denom, denom );
1213
1214             if( d_dims == 3 )
1215                 for( i = 0; i < d_count; i++ )
1216                 {
1217                     double w = iw[i];
1218                     double x = *xs * w, y = *ys * w, z = *zs * w;
1219                     xs += s_stride; ys += s_stride; zs += s_stride;
1220                     *xd = x; *yd = y; *zd = z;
1221                     xd += d_stride; yd += d_stride; zd += d_stride;
1222                 }
1223             else
1224                 for( i = 0; i < d_count; i++ )
1225                 {
1226                     double w = iw[i];
1227                     double x = *xs * w, y = *ys * w;
1228                     xs += s_stride; ys += s_stride;
1229                     *xd = x; *yd = y;
1230                     xd += d_stride; yd += d_stride;
1231                 }
1232         }
1233     }
1234
1235     __END__;
1236
1237     cvReleaseMat( &denom );
1238     cvReleaseMat( &temp );
1239 }
1240
1241 /* End of file. */