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
44template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
45{
46    int i, j;
47    for( i = j = 0; i < count; i++ )
48        if( mask[i*mstep] )
49        {
50            if( i > j )
51                ptr[j] = ptr[i];
52            j++;
53        }
54    return j;
55}
56
57class CvModelEstimator2
58{
59public:
60    CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions);
61    virtual ~CvModelEstimator2();
62
63    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0;
64    virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
65                           CvMat* mask, double confidence=0.99, int maxIters=1000 );
66    virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
67                            CvMat* mask, double threshold,
68                            double confidence=0.99, int maxIters=1000 );
69    virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; }
70    virtual void setSeed( int64 seed );
71
72protected:
73    virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
74                                     const CvMat* model, CvMat* error ) = 0;
75    virtual int findInliers( const CvMat* m1, const CvMat* m2,
76                             const CvMat* model, CvMat* error,
77                             CvMat* mask, double threshold );
78    virtual bool getSubset( const CvMat* m1, const CvMat* m2,
79                            CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );
80    virtual bool checkSubset( const CvMat* ms1, int count );
81
82    CvRNG rng;
83    int modelPoints;
84    CvSize modelSize;
85    int maxBasicSolutions;
86    bool checkPartialSubsets;
87};
88
89
90CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
91{
92    modelPoints = _modelPoints;
93    modelSize = _modelSize;
94    maxBasicSolutions = _maxBasicSolutions;
95    checkPartialSubsets = true;
96    rng = cvRNG(-1);
97}
98
99CvModelEstimator2::~CvModelEstimator2()
100{
101}
102
103void CvModelEstimator2::setSeed( int64 seed )
104{
105    rng = cvRNG(seed);
106}
107
108
109int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
110                                    const CvMat* model, CvMat* _err,
111                                    CvMat* _mask, double threshold )
112{
113    int i, count = _err->rows*_err->cols, goodCount = 0;
114    const float* err = _err->data.fl;
115    uchar* mask = _mask->data.ptr;
116
117    computeReprojError( m1, m2, model, _err );
118    threshold *= threshold;
119    for( i = 0; i < count; i++ )
120        goodCount += mask[i] = err[i] <= threshold;
121    return goodCount;
122}
123
124
125CV_IMPL int
126cvRANSACUpdateNumIters( double p, double ep,
127                        int model_points, int max_iters )
128{
129    int result = 0;
130
131    CV_FUNCNAME( "cvRANSACUpdateNumIters" );
132
133    __BEGIN__;
134
135    double num, denom;
136
137    if( model_points <= 0 )
138        CV_ERROR( CV_StsOutOfRange, "the number of model points should be positive" );
139
140    p = MAX(p, 0.);
141    p = MIN(p, 1.);
142    ep = MAX(ep, 0.);
143    ep = MIN(ep, 1.);
144
145    // avoid inf's & nan's
146    num = MAX(1. - p, DBL_MIN);
147    denom = 1. - pow(1. - ep,model_points);
148    if( denom < DBL_MIN )
149        EXIT;
150
151    num = log(num);
152    denom = log(denom);
153
154    result = denom >= 0 || -num >= max_iters*(-denom) ?
155        max_iters : cvRound(num/denom);
156
157    __END__;
158
159    return result;
160}
161
162bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
163                                        CvMat* mask, double reprojThreshold,
164                                        double confidence, int maxIters )
165{
166    bool result = false;
167    CvMat* mask0 = mask, *tmask = 0, *t;
168    CvMat* models = 0, *err = 0;
169    CvMat *ms1 = 0, *ms2 = 0;
170
171    CV_FUNCNAME( "CvModelEstimator2::estimateRansac" );
172
173    __BEGIN__;
174
175    int iter, niters = maxIters;
176    int count = m1->rows*m1->cols, maxGoodCount = 0;
177    CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
178
179    if( count < modelPoints )
180        EXIT;
181
182    models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
183    err = cvCreateMat( 1, count, CV_32FC1 );
184    tmask = cvCreateMat( 1, count, CV_8UC1 );
185
186    if( count > modelPoints )
187    {
188        ms1 = cvCreateMat( 1, modelPoints, m1->type );
189        ms2 = cvCreateMat( 1, modelPoints, m2->type );
190    }
191    else
192    {
193        niters = 1;
194        ms1 = (CvMat*)m1;
195        ms2 = (CvMat*)m2;
196    }
197
198    for( iter = 0; iter < niters; iter++ )
199    {
200        int i, goodCount, nmodels;
201        if( count > modelPoints )
202        {
203            bool found = getSubset( m1, m2, ms1, ms2, modelPoints );
204            if( !found )
205            {
206                if( iter == 0 )
207                    EXIT;
208                break;
209            }
210        }
211
212        nmodels = runKernel( ms1, ms2, models );
213        if( nmodels <= 0 )
214            continue;
215        for( i = 0; i < nmodels; i++ )
216        {
217            CvMat model_i;
218            cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
219            goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
220
221            if( goodCount > MAX(maxGoodCount, modelPoints-1) )
222            {
223                CV_SWAP( tmask, mask, t );
224                cvCopy( &model_i, model );
225                maxGoodCount = goodCount;
226                niters = cvRANSACUpdateNumIters( confidence,
227                    (double)(count - goodCount)/count, modelPoints, niters );
228            }
229        }
230    }
231
232    if( maxGoodCount > 0 )
233    {
234        if( mask != mask0 )
235        {
236            CV_SWAP( tmask, mask, t );
237            cvCopy( tmask, mask );
238        }
239        result = true;
240    }
241
242    __END__;
243
244    if( ms1 != m1 )
245        cvReleaseMat( &ms1 );
246    if( ms2 != m2 )
247        cvReleaseMat( &ms2 );
248    cvReleaseMat( &models );
249    cvReleaseMat( &err );
250    cvReleaseMat( &tmask );
251    return result;
252}
253
254
255static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
256
257bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
258                                  CvMat* mask, double confidence, int maxIters )
259{
260    const double outlierRatio = 0.45;
261    bool result = false;
262    CvMat* models = 0;
263    CvMat *ms1 = 0, *ms2 = 0;
264    CvMat* err = 0;
265
266    CV_FUNCNAME( "CvModelEstimator2::estimateLMeDS" );
267
268    __BEGIN__;
269
270    int iter, niters = maxIters;
271    int count = m1->rows*m1->cols;
272    double minMedian = DBL_MAX, sigma;
273
274    CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
275
276    if( count < modelPoints )
277        EXIT;
278
279    models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
280    err = cvCreateMat( 1, count, CV_32FC1 );
281
282    if( count > modelPoints )
283    {
284        ms1 = cvCreateMat( 1, modelPoints, m1->type );
285        ms2 = cvCreateMat( 1, modelPoints, m2->type );
286    }
287    else
288    {
289        niters = 1;
290        ms1 = (CvMat*)m1;
291        ms2 = (CvMat*)m2;
292    }
293
294    niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
295    niters = MIN( MAX(niters, 3), maxIters );
296
297    for( iter = 0; iter < niters; iter++ )
298    {
299        int i, nmodels;
300        if( count > modelPoints )
301        {
302            bool found = getSubset( m1, m2, ms1, ms2, 300 );
303            if( !found )
304            {
305                if( iter == 0 )
306                    EXIT;
307                break;
308            }
309        }
310
311        nmodels = runKernel( ms1, ms2, models );
312        if( nmodels <= 0 )
313            continue;
314        for( i = 0; i < nmodels; i++ )
315        {
316            CvMat model_i;
317            cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
318            computeReprojError( m1, m2, &model_i, err );
319            icvSortDistances( err->data.i, count, 0 );
320
321            double median = count % 2 != 0 ?
322                err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
323
324            if( median < minMedian )
325            {
326                minMedian = median;
327                cvCopy( &model_i, model );
328            }
329        }
330    }
331
332    if( minMedian < DBL_MAX )
333    {
334        sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian);
335        sigma = MAX( sigma, FLT_EPSILON*100 );
336
337        count = findInliers( m1, m2, model, err, mask, sigma );
338        result = count >= modelPoints;
339    }
340
341    __END__;
342
343    if( ms1 != m1 )
344        cvReleaseMat( &ms1 );
345    if( ms2 != m2 )
346        cvReleaseMat( &ms2 );
347    cvReleaseMat( &models );
348    cvReleaseMat( &err );
349    return result;
350}
351
352
353bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
354                                   CvMat* ms1, CvMat* ms2, int maxAttempts )
355{
356    int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) );
357    int i, j, k, idx_i, iters = 0;
358    int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
359    const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
360    int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
361    int count = m1->cols*m1->rows;
362
363    assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
364    elemSize /= sizeof(int);
365
366    for(;;)
367    {
368        for( i = 0; i < modelPoints && iters < maxAttempts; iters++ )
369        {
370            idx[i] = idx_i = cvRandInt(&rng) % count;
371            for( j = 0; j < i; j++ )
372                if( idx_i == idx[j] )
373                    break;
374            if( j < i )
375                continue;
376            for( k = 0; k < elemSize; k++ )
377            {
378                ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
379                ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
380            }
381            if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
382                continue;
383            i++;
384            iters = 0;
385        }
386        if( !checkPartialSubsets && i == modelPoints &&
387            (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
388            continue;
389        break;
390    }
391
392    return i == modelPoints;
393}
394
395
396bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
397{
398    int j, k, i = count-1;
399    CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
400
401    assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
402
403    // check that the i-th selected point does not belong
404    // to a line connecting some previously selected points
405    for( j = 0; j < i; j++ )
406    {
407        double dx1 = ptr[j].x - ptr[i].x;
408        double dy1 = ptr[j].y - ptr[i].y;
409        for( k = 0; k < j; k++ )
410        {
411            double dx2 = ptr[k].x - ptr[i].x;
412            double dy2 = ptr[k].y - ptr[i].y;
413            if( fabs(dx2*dy1 - dy2*dx1) < FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
414                break;
415        }
416        if( k < j )
417            break;
418    }
419
420    return j == i;
421}
422
423
424class CvHomographyEstimator : public CvModelEstimator2
425{
426public:
427    CvHomographyEstimator( int modelPoints );
428
429    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
430    virtual bool refine( const CvMat* m1, const CvMat* m2,
431                         CvMat* model, int maxIters );
432protected:
433    virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
434                                     const CvMat* model, CvMat* error );
435};
436
437
438CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
439    : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
440{
441    assert( _modelPoints == 4 || _modelPoints == 5 );
442}
443
444int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
445{
446    int i, count = m1->rows*m1->cols;
447    const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
448    const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
449
450    double LtL[9][9], W[9][9], V[9][9];
451    CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
452    CvMat _W = cvMat( 9, 9, CV_64F, W );
453    CvMat _V = cvMat( 9, 9, CV_64F, V );
454    CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
455    CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
456    CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
457
458    for( i = 0; i < count; i++ )
459    {
460        cm.x += m[i].x; cm.y += m[i].y;
461        cM.x += M[i].x; cM.y += M[i].y;
462    }
463
464    cm.x /= count; cm.y /= count;
465    cM.x /= count; cM.y /= count;
466
467    for( i = 0; i < count; i++ )
468    {
469        sm.x += fabs(m[i].x - cm.x);
470        sm.y += fabs(m[i].y - cm.y);
471        sM.x += fabs(M[i].x - cM.x);
472        sM.y += fabs(M[i].y - cM.y);
473    }
474
475    sm.x = count/sm.x; sm.y = count/sm.y;
476    sM.x = count/sM.x; sM.y = count/sM.y;
477
478    double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
479    double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
480    CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
481    CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
482
483    cvZero( &_LtL );
484    for( i = 0; i < count; i++ )
485    {
486        double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
487        double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
488        double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
489        double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
490        int j, k;
491        for( j = 0; j < 9; j++ )
492            for( k = j; k < 9; k++ )
493                LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
494    }
495    cvCompleteSymm( &_LtL );
496
497    cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
498    cvMatMul( &_invHnorm, &_H0, &_Htemp );
499    cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
500    cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
501
502    return 1;
503}
504
505
506void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
507                                                const CvMat* model, CvMat* _err )
508{
509    int i, count = m1->rows*m1->cols;
510    const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
511    const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
512    const double* H = model->data.db;
513    float* err = _err->data.fl;
514
515    for( i = 0; i < count; i++ )
516    {
517        double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
518        double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
519        double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
520        err[i] = (float)(dx*dx + dy*dy);
521    }
522}
523
524bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
525{
526    CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
527    int i, j, k, count = m1->rows*m1->cols;
528    const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
529    const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
530    CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
531    cvCopy( &modelPart, solver.param );
532
533    for(;;)
534    {
535        const CvMat* _param = 0;
536        CvMat *_JtJ = 0, *_JtErr = 0;
537        double* _errNorm = 0;
538
539        if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
540            break;
541
542        for( i = 0; i < count; i++ )
543        {
544            const double* h = _param->data.db;
545            double Mx = M[i].x, My = M[i].y;
546            double ww = 1./(h[6]*Mx + h[7]*My + 1.);
547            double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
548            double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
549            double err[] = { _xi - m[i].x, _yi - m[i].y };
550            if( _JtJ || _JtErr )
551            {
552                double J[][8] =
553                {
554                    { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
555                    { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
556                };
557
558                for( j = 0; j < 8; j++ )
559                {
560                    for( k = j; k < 8; k++ )
561                        _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
562                    _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
563                }
564            }
565            if( _errNorm )
566                *_errNorm += err[0]*err[0] + err[1]*err[1];
567        }
568    }
569
570    cvCopy( solver.param, &modelPart );
571    return true;
572}
573
574
575CV_IMPL int
576cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
577                  CvMat* __H, int method, double ransacReprojThreshold,
578                  CvMat* mask )
579{
580    const double confidence = 0.99;
581    bool result = false;
582    CvMat *m = 0, *M = 0, *tempMask = 0;
583
584    CV_FUNCNAME( "cvFindHomography" );
585
586    __BEGIN__;
587
588    double H[9];
589    CvMat _H = cvMat( 3, 3, CV_64FC1, H );
590    int count;
591
592    CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
593
594    count = MAX(imagePoints->cols, imagePoints->rows);
595    CV_ASSERT( count >= 4 );
596
597    m = cvCreateMat( 1, count, CV_64FC2 );
598    cvConvertPointsHomogeneous( imagePoints, m );
599
600    M = cvCreateMat( 1, count, CV_64FC2 );
601    cvConvertPointsHomogeneous( objectPoints, M );
602
603    if( mask )
604    {
605        CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
606            (mask->rows == 1 || mask->cols == 1) &&
607            mask->rows*mask->cols == count );
608        tempMask = mask;
609    }
610    else if( count > 4 )
611        tempMask = cvCreateMat( 1, count, CV_8U );
612    if( tempMask )
613        cvSet( tempMask, cvScalarAll(1.) );
614
615    {
616    CvHomographyEstimator estimator( MIN(count, 5) );
617    if( count == 4 )
618        method = 0;
619    if( method == CV_LMEDS )
620        result = estimator.runLMeDS( M, m, &_H, tempMask, confidence );
621    else if( method == CV_RANSAC )
622        result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence );
623    else
624        result = estimator.runKernel( M, m, &_H ) > 0;
625
626    if( result && count > 4 )
627    {
628        icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
629        count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
630        M->cols = m->cols = count;
631        estimator.refine( M, m, &_H, 10 );
632    }
633    }
634
635    if( result )
636        cvConvert( &_H, __H );
637
638    __END__;
639
640    cvReleaseMat( &m );
641    cvReleaseMat( &M );
642    if( tempMask != mask )
643        cvReleaseMat( &tempMask );
644
645    return (int)result;
646}
647
648
649/* Evaluation of Fundamental Matrix from point correspondences.
650   The original code has been written by Valery Mosyagin */
651
652/* The algorithms (except for RANSAC) and the notation have been taken from
653   Zhengyou Zhang's research report
654   "Determining the Epipolar Geometry and its Uncertainty: A Review"
655   that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
656
657/************************************** 7-point algorithm *******************************/
658class CvFMEstimator : public CvModelEstimator2
659{
660public:
661    CvFMEstimator( int _modelPoints );
662
663    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
664    virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
665    virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
666protected:
667    virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
668                                     const CvMat* model, CvMat* error );
669};
670
671CvFMEstimator::CvFMEstimator( int _modelPoints )
672: CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
673{
674    assert( _modelPoints == 7 || _modelPoints == 8 );
675}
676
677
678int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
679{
680    return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
681}
682
683int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
684{
685    double a[7*9], w[7], v[9*9], c[4], r[3];
686    double* f1, *f2;
687    double t0, t1, t2;
688    CvMat A = cvMat( 7, 9, CV_64F, a );
689    CvMat V = cvMat( 9, 9, CV_64F, v );
690    CvMat W = cvMat( 7, 1, CV_64F, w );
691    CvMat coeffs = cvMat( 1, 4, CV_64F, c );
692    CvMat roots = cvMat( 1, 3, CV_64F, r );
693    const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
694    const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
695    double* fmatrix = _fmatrix->data.db;
696    int i, k, n;
697
698    // form a linear system: i-th row of A(=a) represents
699    // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
700    for( i = 0; i < 7; i++ )
701    {
702        double x0 = m1[i].x, y0 = m1[i].y;
703        double x1 = m2[i].x, y1 = m2[i].y;
704
705        a[i*9+0] = x1*x0;
706        a[i*9+1] = x1*y0;
707        a[i*9+2] = x1;
708        a[i*9+3] = y1*x0;
709        a[i*9+4] = y1*y0;
710        a[i*9+5] = y1;
711        a[i*9+6] = x0;
712        a[i*9+7] = y0;
713        a[i*9+8] = 1;
714    }
715
716    // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
717    // the solution is linear subspace of dimensionality 2.
718    // => use the last two singular vectors as a basis of the space
719    // (according to SVD properties)
720    cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
721    f1 = v + 7*9;
722    f2 = v + 8*9;
723
724    // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
725    // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
726    // so f ~ lambda*f1 + (1 - lambda)*f2.
727    // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
728    // it will be a cubic equation.
729    // find c - polynomial coefficients.
730    for( i = 0; i < 9; i++ )
731        f1[i] -= f2[i];
732
733    t0 = f2[4]*f2[8] - f2[5]*f2[7];
734    t1 = f2[3]*f2[8] - f2[5]*f2[6];
735    t2 = f2[3]*f2[7] - f2[4]*f2[6];
736
737    c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
738
739    c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
740           f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
741           f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
742           f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
743           f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
744           f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
745           f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
746
747    t0 = f1[4]*f1[8] - f1[5]*f1[7];
748    t1 = f1[3]*f1[8] - f1[5]*f1[6];
749    t2 = f1[3]*f1[7] - f1[4]*f1[6];
750
751    c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
752           f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
753           f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
754           f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
755           f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
756           f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
757           f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
758
759    c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
760
761    // solve the cubic equation; there can be 1 to 3 roots ...
762    n = cvSolveCubic( &coeffs, &roots );
763
764    if( n < 1 || n > 3 )
765        return n;
766
767    for( k = 0; k < n; k++, fmatrix += 9 )
768    {
769        // for each root form the fundamental matrix
770        double lambda = r[k], mu = 1.;
771        double s = f1[8]*r[k] + f2[8];
772
773        // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
774        if( fabs(s) > DBL_EPSILON )
775        {
776            mu = 1./s;
777            lambda *= mu;
778            fmatrix[8] = 1.;
779        }
780        else
781            fmatrix[8] = 0.;
782
783        for( i = 0; i < 8; i++ )
784            fmatrix[i] = f1[i]*lambda + f2[i]*mu;
785    }
786
787    return n;
788}
789
790
791int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
792{
793    double a[9*9], w[9], v[9*9];
794    CvMat W = cvMat( 1, 9, CV_64F, w );
795    CvMat V = cvMat( 9, 9, CV_64F, v );
796    CvMat A = cvMat( 9, 9, CV_64F, a );
797    CvMat U, F0, TF;
798
799    CvPoint2D64f m0c = {0,0}, m1c = {0,0};
800    double t, scale0 = 0, scale1 = 0;
801
802    const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
803    const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
804    double* fmatrix = _fmatrix->data.db;
805    int i, j, k, count = _m1->cols*_m1->rows;
806
807    // compute centers and average distances for each of the two point sets
808    for( i = 0; i < count; i++ )
809    {
810        double x = m1[i].x, y = m1[i].y;
811        m0c.x += x; m0c.y += y;
812
813        x = m2[i].x, y = m2[i].y;
814        m1c.x += x; m1c.y += y;
815    }
816
817    // calculate the normalizing transformations for each of the point sets:
818    // after the transformation each set will have the mass center at the coordinate origin
819    // and the average distance from the origin will be ~sqrt(2).
820    t = 1./count;
821    m0c.x *= t; m0c.y *= t;
822    m1c.x *= t; m1c.y *= t;
823
824    for( i = 0; i < count; i++ )
825    {
826        double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
827        scale0 += sqrt(x*x + y*y);
828
829        x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y);
830        scale1 += sqrt(x*x + y*y);
831    }
832
833    scale0 *= t;
834    scale1 *= t;
835
836    if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
837        return 0;
838
839    scale0 = sqrt(2.)/scale0;
840    scale1 = sqrt(2.)/scale1;
841
842    cvZero( &A );
843
844    // form a linear system Ax=0: for each selected pair of points m1 & m2,
845    // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
846    // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
847    for( i = 0; i < count; i++ )
848    {
849        double x0 = (m1[i].x - m0c.x)*scale0;
850        double y0 = (m1[i].y - m0c.y)*scale0;
851        double x1 = (m2[i].x - m1c.x)*scale1;
852        double y1 = (m2[i].y - m1c.y)*scale1;
853        double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
854        for( j = 0; j < 9; j++ )
855            for( k = 0; k < 9; k++ )
856                a[j*9+k] += r[j]*r[k];
857    }
858
859    cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
860
861    for( i = 0; i < 8; i++ )
862    {
863        if( fabs(w[i]) < DBL_EPSILON )
864            break;
865    }
866
867    if( i < 7 )
868        return 0;
869
870    F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
871
872    // make F0 singular (of rank 2) by decomposing it with SVD,
873    // zeroing the last diagonal element of W and then composing the matrices back.
874
875    // use v as a temporary storage for different 3x3 matrices
876    W = U = V = TF = F0;
877    W.data.db = v;
878    U.data.db = v + 9;
879    V.data.db = v + 18;
880    TF.data.db = v + 27;
881
882    cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
883    W.data.db[8] = 0.;
884
885    // F0 <- U*diag([W(1), W(2), 0])*V'
886    cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
887    cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
888
889    // apply the transformation that is inverse
890    // to what we used to normalize the point coordinates
891    {
892        double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
893        double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
894        CvMat T0, T1;
895        T0 = T1 = F0;
896        T0.data.db = tt0;
897        T1.data.db = tt1;
898
899        // F0 <- T1'*F0*T0
900        cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
901        F0.data.db = fmatrix;
902        cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
903
904        // make F(3,3) = 1
905        if( fabs(F0.data.db[8]) > FLT_EPSILON )
906            cvScale( &F0, &F0, 1./F0.data.db[8] );
907    }
908
909    return 1;
910}
911
912
913void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
914                                        const CvMat* model, CvMat* _err )
915{
916    int i, count = _m1->rows*_m1->cols;
917    const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
918    const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
919    const double* F = model->data.db;
920    float* err = _err->data.fl;
921
922    for( i = 0; i < count; i++ )
923    {
924        double a, b, c, d1, d2, s1, s2;
925
926        a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
927        b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
928        c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
929
930        s2 = 1./(a*a + b*b);
931        d2 = m2[i].x*a + m2[i].y*b + c;
932
933        a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
934        b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
935        c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
936
937        s1 = 1./(a*a + b*b);
938        d1 = m1[i].x*a + m1[i].y*b + c;
939
940        err[i] = (float)(d1*d1*s1 + d2*d2*s2);
941    }
942}
943
944
945CV_IMPL int
946cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
947                      CvMat* fmatrix, int method,
948                      double param1, double param2, CvMat* mask )
949{
950    int result = 0;
951    CvMat *m1 = 0, *m2 = 0, *tempMask = 0;
952
953    CV_FUNCNAME( "cvFindFundamentalMat" );
954
955    __BEGIN__;
956
957    double F[3*9];
958    CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
959    int count;
960
961    CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
962    CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
963        (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );
964
965    count = MAX(points1->cols, points1->rows);
966    if( count < 7 )
967        EXIT;
968
969    m1 = cvCreateMat( 1, count, CV_64FC2 );
970    cvConvertPointsHomogeneous( points1, m1 );
971
972    m2 = cvCreateMat( 1, count, CV_64FC2 );
973    cvConvertPointsHomogeneous( points2, m2 );
974
975    if( mask )
976    {
977        CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
978            (mask->rows == 1 || mask->cols == 1) &&
979            mask->rows*mask->cols == count );
980        tempMask = mask;
981    }
982    else if( count > 8 )
983        tempMask = cvCreateMat( 1, count, CV_8U );
984    if( tempMask )
985        cvSet( tempMask, cvScalarAll(1.) );
986
987    {
988    CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) );
989    if( count == 7 )
990        result = estimator.run7Point(m1, m2, &_F9x3);
991    else if( count == 8 || method == CV_FM_8POINT )
992        result = estimator.run8Point(m1, m2, &_F3x3);
993    else if( count > 8 )
994    {
995        if( param1 <= 0 )
996            param1 = 3;
997        if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
998            param2 = 0.99;
999
1000        if( (method & ~3) == CV_RANSAC )
1001            result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
1002        else
1003            result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
1004        if( result <= 0 )
1005            EXIT;
1006        icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
1007        count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
1008        assert( count >= 8 );
1009        m1->cols = m2->cols = count;
1010        estimator.run8Point(m1, m2, &_F3x3);
1011    }
1012    }
1013
1014    if( result )
1015        cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
1016
1017    __END__;
1018
1019    cvReleaseMat( &m1 );
1020    cvReleaseMat( &m2 );
1021    if( tempMask != mask )
1022        cvReleaseMat( &tempMask );
1023
1024    return result;
1025}
1026
1027
1028CV_IMPL void
1029cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
1030                             const CvMat* fmatrix, CvMat* lines )
1031{
1032    CV_FUNCNAME( "cvComputeCorrespondEpilines" );
1033
1034    __BEGIN__;
1035
1036    int abc_stride, abc_plane_stride, abc_elem_size;
1037    int plane_stride, stride, elem_size;
1038    int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
1039    uchar *ap, *bp, *cp;
1040    const uchar *xp, *yp, *zp;
1041    double f[9];
1042    CvMat F = cvMat( 3, 3, CV_64F, f );
1043
1044    if( !CV_IS_MAT(points) )
1045        CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
1046
1047    depth = CV_MAT_DEPTH(points->type);
1048    cn = CV_MAT_CN(points->type);
1049    if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )
1050        CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
1051
1052    if( points->rows > points->cols )
1053    {
1054        dims = cn*points->cols;
1055        count = points->rows;
1056    }
1057    else
1058    {
1059        if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )
1060            CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
1061        dims = cn * points->rows;
1062        count = points->cols;
1063    }
1064
1065    if( dims != 2 && dims != 3 )
1066        CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
1067
1068    if( !CV_IS_MAT(fmatrix) )
1069        CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
1070
1071    if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
1072        CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
1073
1074    if( fmatrix->cols != 3 || fmatrix->rows != 3 )
1075        CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );
1076
1077    if( !CV_IS_MAT(lines) )
1078        CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
1079
1080    abc_depth = CV_MAT_DEPTH(lines->type);
1081    abc_cn = CV_MAT_CN(lines->type);
1082    if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )
1083        CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
1084
1085    if( lines->rows > lines->cols )
1086    {
1087        abc_dims = abc_cn*lines->cols;
1088        abc_count = lines->rows;
1089    }
1090    else
1091    {
1092        if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )
1093            CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
1094        abc_dims = abc_cn * lines->rows;
1095        abc_count = lines->cols;
1096    }
1097
1098    if( abc_dims != 3 )
1099        CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
1100
1101    if( abc_count != count )
1102        CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
1103
1104    elem_size = CV_ELEM_SIZE(depth);
1105    abc_elem_size = CV_ELEM_SIZE(abc_depth);
1106
1107    if( points->rows == dims )
1108    {
1109        plane_stride = points->step;
1110        stride = elem_size;
1111    }
1112    else
1113    {
1114        plane_stride = elem_size;
1115        stride = points->rows == 1 ? dims*elem_size : points->step;
1116    }
1117
1118    if( lines->rows == 3 )
1119    {
1120        abc_plane_stride = lines->step;
1121        abc_stride = abc_elem_size;
1122    }
1123    else
1124    {
1125        abc_plane_stride = abc_elem_size;
1126        abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
1127    }
1128
1129    CV_CALL( cvConvert( fmatrix, &F ));
1130    if( pointImageID == 2 )
1131        cvTranspose( &F, &F );
1132
1133    xp = points->data.ptr;
1134    yp = xp + plane_stride;
1135    zp = dims == 3 ? yp + plane_stride : 0;
1136
1137    ap = lines->data.ptr;
1138    bp = ap + abc_plane_stride;
1139    cp = bp + abc_plane_stride;
1140
1141    for( i = 0; i < count; i++ )
1142    {
1143        double x, y, z = 1.;
1144        double a, b, c, nu;
1145
1146        if( depth == CV_32F )
1147        {
1148            x = *(float*)xp; y = *(float*)yp;
1149            if( zp )
1150                z = *(float*)zp, zp += stride;
1151        }
1152        else
1153        {
1154            x = *(double*)xp; y = *(double*)yp;
1155            if( zp )
1156                z = *(double*)zp, zp += stride;
1157        }
1158
1159        xp += stride; yp += stride;
1160
1161        a = f[0]*x + f[1]*y + f[2]*z;
1162        b = f[3]*x + f[4]*y + f[5]*z;
1163        c = f[6]*x + f[7]*y + f[8]*z;
1164        nu = a*a + b*b;
1165        nu = nu ? 1./sqrt(nu) : 1.;
1166        a *= nu; b *= nu; c *= nu;
1167
1168        if( abc_depth == CV_32F )
1169        {
1170            *(float*)ap = (float)a;
1171            *(float*)bp = (float)b;
1172            *(float*)cp = (float)c;
1173        }
1174        else
1175        {
1176            *(double*)ap = a;
1177            *(double*)bp = b;
1178            *(double*)cp = c;
1179        }
1180
1181        ap += abc_stride;
1182        bp += abc_stride;
1183        cp += abc_stride;
1184    }
1185
1186    __END__;
1187}
1188
1189
1190CV_IMPL void
1191cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
1192{
1193    CvMat* temp = 0;
1194    CvMat* denom = 0;
1195
1196    CV_FUNCNAME( "cvConvertPointsHomogeneous" );
1197
1198    __BEGIN__;
1199
1200    int i, s_count, s_dims, d_count, d_dims;
1201    CvMat _src, _dst, _ones;
1202    CvMat* ones = 0;
1203
1204    if( !CV_IS_MAT(src) )
1205        CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
1206        "The input parameter is not a valid matrix" );
1207
1208    if( !CV_IS_MAT(dst) )
1209        CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
1210        "The output parameter is not a valid matrix" );
1211
1212    if( src == dst || src->data.ptr == dst->data.ptr )
1213    {
1214        if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
1215            CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
1216        EXIT;
1217    }
1218
1219    if( src->rows > src->cols )
1220    {
1221        if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
1222            CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
1223
1224        s_dims = CV_MAT_CN(src->type)*src->cols;
1225        s_count = src->rows;
1226    }
1227    else
1228    {
1229        if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
1230            CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
1231
1232        s_dims = CV_MAT_CN(src->type)*src->rows;
1233        s_count = src->cols;
1234    }
1235
1236    if( src->rows == 1 || src->cols == 1 )
1237        src = cvReshape( src, &_src, 1, s_count );
1238
1239    if( dst->rows > dst->cols )
1240    {
1241        if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
1242            CV_ERROR( CV_StsBadSize,
1243            "Either the number of channels or columns or rows in the input matrix must be =1" );
1244
1245        d_dims = CV_MAT_CN(dst->type)*dst->cols;
1246        d_count = dst->rows;
1247    }
1248    else
1249    {
1250        if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
1251            CV_ERROR( CV_StsBadSize,
1252            "Either the number of channels or columns or rows in the output matrix must be =1" );
1253
1254        d_dims = CV_MAT_CN(dst->type)*dst->rows;
1255        d_count = dst->cols;
1256    }
1257
1258    if( dst->rows == 1 || dst->cols == 1 )
1259        dst = cvReshape( dst, &_dst, 1, d_count );
1260
1261    if( s_count != d_count )
1262        CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
1263
1264    if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
1265        CV_ERROR( CV_StsUnsupportedFormat,
1266        "Both matrices must be floating-point (single or double precision)" );
1267
1268    if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
1269        CV_ERROR( CV_StsOutOfRange,
1270        "Both input and output point dimensionality must be 2, 3 or 4" );
1271
1272    if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
1273        CV_ERROR( CV_StsUnmatchedSizes,
1274        "The dimensionalities of input and output point sets differ too much" );
1275
1276    if( s_dims == d_dims - 1 )
1277    {
1278        if( d_count == dst->rows )
1279        {
1280            ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
1281            dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
1282        }
1283        else
1284        {
1285            ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
1286            dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
1287        }
1288    }
1289
1290    if( s_dims <= d_dims )
1291    {
1292        if( src->rows == dst->rows && src->cols == dst->cols )
1293        {
1294            if( CV_ARE_TYPES_EQ( src, dst ) )
1295                cvCopy( src, dst );
1296            else
1297                cvConvert( src, dst );
1298        }
1299        else
1300        {
1301            if( !CV_ARE_TYPES_EQ( src, dst ))
1302            {
1303                CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
1304                cvConvert( src, temp );
1305                src = temp;
1306            }
1307            cvTranspose( src, dst );
1308        }
1309
1310        if( ones )
1311            cvSet( ones, cvRealScalar(1.) );
1312    }
1313    else
1314    {
1315        int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
1316
1317        if( !CV_ARE_TYPES_EQ( src, dst ))
1318        {
1319            CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
1320            cvConvert( src, temp );
1321            src = temp;
1322        }
1323
1324        elem_size = CV_ELEM_SIZE(src->type);
1325
1326        if( s_count == src->cols )
1327            s_plane_stride = src->step / elem_size, s_stride = 1;
1328        else
1329            s_stride = src->step / elem_size, s_plane_stride = 1;
1330
1331        if( d_count == dst->cols )
1332            d_plane_stride = dst->step / elem_size, d_stride = 1;
1333        else
1334            d_stride = dst->step / elem_size, d_plane_stride = 1;
1335
1336        CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
1337
1338        if( CV_MAT_DEPTH(dst->type) == CV_32F )
1339        {
1340            const float* xs = src->data.fl;
1341            const float* ys = xs + s_plane_stride;
1342            const float* zs = 0;
1343            const float* ws = xs + (s_dims - 1)*s_plane_stride;
1344
1345            float* iw = denom->data.fl;
1346
1347            float* xd = dst->data.fl;
1348            float* yd = xd + d_plane_stride;
1349            float* zd = 0;
1350
1351            if( d_dims == 3 )
1352            {
1353                zs = ys + s_plane_stride;
1354                zd = yd + d_plane_stride;
1355            }
1356
1357            for( i = 0; i < d_count; i++, ws += s_stride )
1358            {
1359                float t = *ws;
1360                iw[i] = t ? t : 1.f;
1361            }
1362
1363            cvDiv( 0, denom, denom );
1364
1365            if( d_dims == 3 )
1366                for( i = 0; i < d_count; i++ )
1367                {
1368                    float w = iw[i];
1369                    float x = *xs * w, y = *ys * w, z = *zs * w;
1370                    xs += s_stride; ys += s_stride; zs += s_stride;
1371                    *xd = x; *yd = y; *zd = z;
1372                    xd += d_stride; yd += d_stride; zd += d_stride;
1373                }
1374            else
1375                for( i = 0; i < d_count; i++ )
1376                {
1377                    float w = iw[i];
1378                    float x = *xs * w, y = *ys * w;
1379                    xs += s_stride; ys += s_stride;
1380                    *xd = x; *yd = y;
1381                    xd += d_stride; yd += d_stride;
1382                }
1383        }
1384        else
1385        {
1386            const double* xs = src->data.db;
1387            const double* ys = xs + s_plane_stride;
1388            const double* zs = 0;
1389            const double* ws = xs + (s_dims - 1)*s_plane_stride;
1390
1391            double* iw = denom->data.db;
1392
1393            double* xd = dst->data.db;
1394            double* yd = xd + d_plane_stride;
1395            double* zd = 0;
1396
1397            if( d_dims == 3 )
1398            {
1399                zs = ys + s_plane_stride;
1400                zd = yd + d_plane_stride;
1401            }
1402
1403            for( i = 0; i < d_count; i++, ws += s_stride )
1404            {
1405                double t = *ws;
1406                iw[i] = t ? t : 1.;
1407            }
1408
1409            cvDiv( 0, denom, denom );
1410
1411            if( d_dims == 3 )
1412                for( i = 0; i < d_count; i++ )
1413                {
1414                    double w = iw[i];
1415                    double x = *xs * w, y = *ys * w, z = *zs * w;
1416                    xs += s_stride; ys += s_stride; zs += s_stride;
1417                    *xd = x; *yd = y; *zd = z;
1418                    xd += d_stride; yd += d_stride; zd += d_stride;
1419                }
1420            else
1421                for( i = 0; i < d_count; i++ )
1422                {
1423                    double w = iw[i];
1424                    double x = *xs * w, y = *ys * w;
1425                    xs += s_stride; ys += s_stride;
1426                    *xd = x; *yd = y;
1427                    xd += d_stride; yd += d_stride;
1428                }
1429        }
1430    }
1431
1432    __END__;
1433
1434    cvReleaseMat( &denom );
1435    cvReleaseMat( &temp );
1436}
1437
1438/* End of file. */
1439