16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntemplate<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count ) 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = j = 0; i < count; i++ ) 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mask[i*mstep] ) 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i > j ) 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ptr[j] = ptr[i]; 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j++; 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return j; 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennclass CvModelEstimator2 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions); 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual ~CvModelEstimator2(); 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0; 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model, 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask, double confidence=0.99, int maxIters=1000 ); 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model, 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask, double threshold, 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double confidence=0.99, int maxIters=1000 ); 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; } 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual void setSeed( int64 seed ); 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennprotected: 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual void computeReprojError( const CvMat* m1, const CvMat* m2, 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* error ) = 0; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual int findInliers( const CvMat* m1, const CvMat* m2, 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* error, 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask, double threshold ); 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual bool getSubset( const CvMat* m1, const CvMat* m2, 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* ms1, CvMat* ms2, int maxAttempts=1000 ); 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual bool checkSubset( const CvMat* ms1, int count ); 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG rng; 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int modelPoints; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize modelSize; 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxBasicSolutions; 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool checkPartialSubsets; 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}; 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions) 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn modelPoints = _modelPoints; 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn modelSize = _modelSize; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxBasicSolutions = _maxBasicSolutions; 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn checkPartialSubsets = true; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rng = cvRNG(-1); 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvModelEstimator2::~CvModelEstimator2() 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid CvModelEstimator2::setSeed( int64 seed ) 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rng = cvRNG(seed); 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2, 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* _err, 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _mask, double threshold ) 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, count = _err->rows*_err->cols, goodCount = 0; 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* err = _err->data.fl; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* mask = _mask->data.ptr; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn computeReprojError( m1, m2, model, _err ); 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn threshold *= threshold; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodCount += mask[i] = err[i] <= threshold; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return goodCount; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvRANSACUpdateNumIters( double p, double ep, 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int model_points, int max_iters ) 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = 0; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvRANSACUpdateNumIters" ); 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double num, denom; 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( model_points <= 0 ) 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "the number of model points should be positive" ); 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = MAX(p, 0.); 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = MIN(p, 1.); 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ep = MAX(ep, 0.); 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ep = MIN(ep, 1.); 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // avoid inf's & nan's 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn num = MAX(1. - p, DBL_MIN); 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn denom = 1. - pow(1. - ep,model_points); 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( denom < DBL_MIN ) 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn num = log(num); 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn denom = log(denom); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = denom >= 0 || -num >= max_iters*(-denom) ? 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_iters : cvRound(num/denom); 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model, 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask, double reprojThreshold, 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double confidence, int maxIters ) 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool result = false; 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask0 = mask, *tmask = 0, *t; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* models = 0, *err = 0; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *ms1 = 0, *ms2 = 0; 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "CvModelEstimator2::estimateRansac" ); 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int iter, niters = maxIters; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count = m1->rows*m1->cols, maxGoodCount = 0; 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) ); 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count < modelPoints ) 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 ); 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = cvCreateMat( 1, count, CV_32FC1 ); 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmask = cvCreateMat( 1, count, CV_8UC1 ); 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count > modelPoints ) 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms1 = cvCreateMat( 1, modelPoints, m1->type ); 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms2 = cvCreateMat( 1, modelPoints, m2->type ); 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn niters = 1; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms1 = (CvMat*)m1; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms2 = (CvMat*)m2; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( iter = 0; iter < niters; iter++ ) 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, goodCount, nmodels; 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count > modelPoints ) 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool found = getSubset( m1, m2, ms1, ms2, modelPoints ); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !found ) 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( iter == 0 ) 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nmodels = runKernel( ms1, ms2, models ); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( nmodels <= 0 ) 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nmodels; i++ ) 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat model_i; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height ); 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold ); 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( goodCount > MAX(maxGoodCount, modelPoints-1) ) 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( tmask, mask, t ); 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( &model_i, model ); 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxGoodCount = goodCount; 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn niters = cvRANSACUpdateNumIters( confidence, 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (double)(count - goodCount)/count, modelPoints, niters ); 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxGoodCount > 0 ) 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mask != mask0 ) 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( tmask, mask, t ); 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( tmask, mask ); 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = true; 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ms1 != m1 ) 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &ms1 ); 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ms2 != m2 ) 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &ms2 ); 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &models ); 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &err ); 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmask ); 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT ) 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model, 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask, double confidence, int maxIters ) 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double outlierRatio = 0.45; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool result = false; 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* models = 0; 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *ms1 = 0, *ms2 = 0; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* err = 0; 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "CvModelEstimator2::estimateLMeDS" ); 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int iter, niters = maxIters; 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count = m1->rows*m1->cols; 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double minMedian = DBL_MAX, sigma; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) ); 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count < modelPoints ) 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 ); 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = cvCreateMat( 1, count, CV_32FC1 ); 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count > modelPoints ) 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms1 = cvCreateMat( 1, modelPoints, m1->type ); 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms2 = cvCreateMat( 1, modelPoints, m2->type ); 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn niters = 1; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms1 = (CvMat*)m1; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms2 = (CvMat*)m2; 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints))); 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn niters = MIN( MAX(niters, 3), maxIters ); 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( iter = 0; iter < niters; iter++ ) 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, nmodels; 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count > modelPoints ) 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool found = getSubset( m1, m2, ms1, ms2, 300 ); 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !found ) 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( iter == 0 ) 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nmodels = runKernel( ms1, ms2, models ); 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( nmodels <= 0 ) 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nmodels; i++ ) 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat model_i; 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height ); 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn computeReprojError( m1, m2, &model_i, err ); 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSortDistances( err->data.i, count, 0 ); 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double median = count % 2 != 0 ? 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5; 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( median < minMedian ) 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minMedian = median; 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( &model_i, model ); 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( minMedian < DBL_MAX ) 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sigma = MAX( sigma, FLT_EPSILON*100 ); 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = findInliers( m1, m2, model, err, mask, sigma ); 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = count >= modelPoints; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ms1 != m1 ) 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &ms1 ); 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ms2 != m2 ) 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &ms2 ); 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &models ); 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &err ); 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2, 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* ms1, CvMat* ms2, int maxAttempts ) 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) ); 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, idx_i, iters = 0; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type); 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int *m1ptr = m1->data.i, *m2ptr = m2->data.i; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count = m1->cols*m1->rows; 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) ); 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elemSize /= sizeof(int); 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(;;) 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < modelPoints && iters < maxAttempts; iters++ ) 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idx[i] = idx_i = cvRandInt(&rng) % count; 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < i; j++ ) 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( idx_i == idx[j] ) 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( j < i ) 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < elemSize; k++ ) 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k]; 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k]; 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 ))) 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i++; 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iters = 0; 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !checkPartialSubsets && i == modelPoints && 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 ))) 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return i == modelPoints; 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvModelEstimator2::checkSubset( const CvMat* m, int count ) 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j, k, i = count-1; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr; 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( CV_MAT_TYPE(m->type) == CV_64FC2 ); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // check that the i-th selected point does not belong 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // to a line connecting some previously selected points 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < i; j++ ) 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx1 = ptr[j].x - ptr[i].x; 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy1 = ptr[j].y - ptr[i].y; 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < j; k++ ) 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx2 = ptr[k].x - ptr[i].x; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy2 = ptr[k].y - ptr[i].y; 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(dx2*dy1 - dy2*dx1) < FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2))) 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k < j ) 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return j == i; 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennclass CvHomographyEstimator : public CvModelEstimator2 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvHomographyEstimator( int modelPoints ); 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ); 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual bool refine( const CvMat* m1, const CvMat* m2, 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* model, int maxIters ); 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennprotected: 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual void computeReprojError( const CvMat* m1, const CvMat* m2, 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* error ); 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}; 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvHomographyEstimator::CvHomographyEstimator(int _modelPoints) 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : CvModelEstimator2(_modelPoints, cvSize(3,3), 1) 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( _modelPoints == 4 || _modelPoints == 5 ); 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H ) 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, count = m1->rows*m1->cols; 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr; 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double LtL[9][9], W[9][9], V[9][9]; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _LtL = cvMat( 9, 9, CV_64F, LtL ); 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _W = cvMat( 9, 9, CV_64F, W ); 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _V = cvMat( 9, 9, CV_64F, V ); 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] ); 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] ); 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0}; 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cm.x += m[i].x; cm.y += m[i].y; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cM.x += M[i].x; cM.y += M[i].y; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cm.x /= count; cm.y /= count; 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cM.x /= count; cM.y /= count; 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sm.x += fabs(m[i].x - cm.x); 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sm.y += fabs(m[i].y - cm.y); 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sM.x += fabs(M[i].x - cM.x); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sM.y += fabs(M[i].y - cM.y); 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sm.x = count/sm.x; sm.y = count/sm.y; 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sM.x = count/sM.x; sM.y = count/sM.y; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 }; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 }; 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm ); 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 ); 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( &_LtL ); 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y; 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y; 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x }; 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y }; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j, k; 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 9; j++ ) 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = j; k < 9; k++ ) 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k]; 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCompleteSymm( &_LtL ); 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_invHnorm, &_H0, &_Htemp ); 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_Htemp, &_Hnorm2, &_H0 ); 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale( &_H0, H, 1./_H0.data.db[8] ); 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 1; 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* _err ) 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, count = m1->rows*m1->cols; 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr; 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr; 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* H = model->data.db; 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* err = _err->data.fl; 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.); 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y; 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err[i] = (float)(dx*dx + dy*dy); 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters ) 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON)); 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, count = m1->rows*m1->cols; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr; 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr; 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr ); 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( &modelPart, solver.param ); 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(;;) 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _param = 0; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_JtJ = 0, *_JtErr = 0; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* _errNorm = 0; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm )) 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* h = _param->data.db; 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Mx = M[i].x, My = M[i].y; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ww = 1./(h[6]*Mx + h[7]*My + 1.); 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _xi = (h[0]*Mx + h[1]*My + h[2])*ww; 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _yi = (h[3]*Mx + h[4]*My + h[5])*ww; 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err[] = { _xi - m[i].x, _yi - m[i].y }; 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _JtJ || _JtErr ) 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double J[][8] = 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi }, 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi } 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 8; j++ ) 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = j; k < 8; k++ ) 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k]; 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1]; 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _errNorm ) 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *_errNorm += err[0]*err[0] + err[1]*err[1]; 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( solver.param, &modelPart ); 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints, 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* __H, int method, double ransacReprojThreshold, 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mask ) 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double confidence = 0.99; 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool result = false; 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *m = 0, *M = 0, *tempMask = 0; 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFindHomography" ); 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double H[9]; 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _H = cvMat( 3, 3, CV_64FC1, H ); 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count; 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) ); 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = MAX(imagePoints->cols, imagePoints->rows); 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( count >= 4 ); 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m = cvCreateMat( 1, count, CV_64FC2 ); 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( imagePoints, m ); 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn M = cvCreateMat( 1, count, CV_64FC2 ); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( objectPoints, M ); 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mask ) 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) && 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (mask->rows == 1 || mask->cols == 1) && 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask->rows*mask->cols == count ); 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tempMask = mask; 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( count > 4 ) 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tempMask = cvCreateMat( 1, count, CV_8U ); 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tempMask ) 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet( tempMask, cvScalarAll(1.) ); 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvHomographyEstimator estimator( MIN(count, 5) ); 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count == 4 ) 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn method = 0; 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method == CV_LMEDS ) 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.runLMeDS( M, m, &_H, tempMask, confidence ); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( method == CV_RANSAC ) 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence ); 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.runKernel( M, m, &_H ) > 0; 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( result && count > 4 ) 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count ); 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count ); 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn M->cols = m->cols = count; 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn estimator.refine( M, m, &_H, 10 ); 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( result ) 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_H, __H ); 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &m ); 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &M ); 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tempMask != mask ) 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tempMask ); 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return (int)result; 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Evaluation of Fundamental Matrix from point correspondences. 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn The original code has been written by Valery Mosyagin */ 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* The algorithms (except for RANSAC) and the notation have been taken from 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Zhengyou Zhang's research report 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Determining the Epipolar Geometry and its Uncertainty: A Review" 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */ 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/************************************** 7-point algorithm *******************************/ 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennclass CvFMEstimator : public CvModelEstimator2 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvFMEstimator( int _modelPoints ); 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ); 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model ); 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model ); 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennprotected: 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn virtual void computeReprojError( const CvMat* m1, const CvMat* m2, 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* error ); 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}; 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvFMEstimator::CvFMEstimator( int _modelPoints ) 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn: CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 ) 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( _modelPoints == 7 || _modelPoints == 8 ); 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ) 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model ); 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix ) 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a[7*9], w[7], v[9*9], c[4], r[3]; 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* f1, *f2; 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0, t1, t2; 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat A = cvMat( 7, 9, CV_64F, a ); 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat V = cvMat( 9, 9, CV_64F, v ); 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat W = cvMat( 7, 1, CV_64F, w ); 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat coeffs = cvMat( 1, 4, CV_64F, c ); 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat roots = cvMat( 1, 3, CV_64F, r ); 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr; 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr; 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* fmatrix = _fmatrix->data.db; 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k, n; 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // form a linear system: i-th row of A(=a) represents 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 7; i++ ) 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x0 = m1[i].x, y0 = m1[i].y; 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1 = m2[i].x, y1 = m2[i].y; 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+0] = x1*x0; 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+1] = x1*y0; 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+2] = x1; 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+3] = y1*x0; 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+4] = y1*y0; 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+5] = y1; 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+6] = x0; 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+7] = y0; 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[i*9+8] = 1; 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // the solution is linear subspace of dimensionality 2. 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // => use the last two singular vectors as a basis of the space 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (according to SVD properties) 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1 = v + 7*9; 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2 = v + 8*9; 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix. 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1), 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // so f ~ lambda*f1 + (1 - lambda)*f2. 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda. 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // it will be a cubic equation. 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // find c - polynomial coefficients. 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 9; i++ ) 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[i] -= f2[i]; 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = f2[4]*f2[8] - f2[5]*f2[7]; 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = f2[3]*f2[8] - f2[5]*f2[6]; 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t2 = f2[3]*f2[7] - f2[4]*f2[6]; 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2; 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 - 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) + 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) - 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) + 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) - 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) + 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]); 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = f1[4]*f1[8] - f1[5]*f1[7]; 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = f1[3]*f1[8] - f1[5]*f1[6]; 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t2 = f1[3]*f1[7] - f1[4]*f1[6]; 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 - 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) + 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) - 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) + 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) - 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) + 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]); 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2; 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // solve the cubic equation; there can be 1 to 3 roots ... 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = cvSolveCubic( &coeffs, &roots ); 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( n < 1 || n > 3 ) 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return n; 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < n; k++, fmatrix += 9 ) 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // for each root form the fundamental matrix 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double lambda = r[k], mu = 1.; 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double s = f1[8]*r[k] + f2[8]; 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(s) > DBL_EPSILON ) 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mu = 1./s; 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambda *= mu; 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fmatrix[8] = 1.; 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fmatrix[8] = 0.; 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 8; i++ ) 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fmatrix[i] = f1[i]*lambda + f2[i]*mu; 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return n; 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix ) 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a[9*9], w[9], v[9*9]; 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat W = cvMat( 1, 9, CV_64F, w ); 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat V = cvMat( 9, 9, CV_64F, v ); 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat A = cvMat( 9, 9, CV_64F, a ); 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat U, F0, TF; 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f m0c = {0,0}, m1c = {0,0}; 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t, scale0 = 0, scale1 = 0; 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr; 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr; 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* fmatrix = _fmatrix->data.db; 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, count = _m1->cols*_m1->rows; 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // compute centers and average distances for each of the two point sets 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = m1[i].x, y = m1[i].y; 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m0c.x += x; m0c.y += y; 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = m2[i].x, y = m2[i].y; 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1c.x += x; m1c.y += y; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // calculate the normalizing transformations for each of the point sets: 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // after the transformation each set will have the mass center at the coordinate origin 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // and the average distance from the origin will be ~sqrt(2). 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = 1./count; 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m0c.x *= t; m0c.y *= t; 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1c.x *= t; m1c.y *= t; 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y; 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale0 += sqrt(x*x + y*y); 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y); 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale1 += sqrt(x*x + y*y); 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale0 *= t; 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale1 *= t; 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON ) 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 0; 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale0 = sqrt(2.)/scale0; 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale1 = sqrt(2.)/scale1; 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( &A ); 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // form a linear system Ax=0: for each selected pair of points m1 & m2, 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0. 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x0 = (m1[i].x - m0c.x)*scale0; 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y0 = (m1[i].y - m0c.y)*scale0; 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1 = (m2[i].x - m1c.x)*scale1; 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y1 = (m2[i].y - m1c.y)*scale1; 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 }; 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 9; j++ ) 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 9; k++ ) 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[j*9+k] += r[j]*r[k]; 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 8; i++ ) 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(w[i]) < DBL_EPSILON ) 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i < 7 ) 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 0; 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // make F0 singular (of rank 2) by decomposing it with SVD, 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // zeroing the last diagonal element of W and then composing the matrices back. 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // use v as a temporary storage for different 3x3 matrices 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W = U = V = TF = F0; 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W.data.db = v; 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn U.data.db = v + 9; 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn V.data.db = v + 18; 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn TF.data.db = v + 27; 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W.data.db[8] = 0.; 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // F0 <- U*diag([W(1), W(2), 0])*V' 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T ); 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ ); 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // apply the transformation that is inverse 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // to what we used to normalize the point coordinates 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 }; 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 }; 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat T0, T1; 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T0 = T1 = F0; 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T0.data.db = tt0; 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T1.data.db = tt1; 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // F0 <- T1'*F0*T0 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T ); 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn F0.data.db = fmatrix; 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 ); 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // make F(3,3) = 1 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(F0.data.db[8]) > FLT_EPSILON ) 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &F0, &F0, 1./F0.data.db[8] ); 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 1; 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2, 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* model, CvMat* _err ) 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, count = _m1->rows*_m1->cols; 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr; 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr; 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* F = model->data.db; 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* err = _err->data.fl; 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a, b, c, d1, d2, s1, s2; 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a = F[0]*m1[i].x + F[1]*m1[i].y + F[2]; 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b = F[3]*m1[i].x + F[4]*m1[i].y + F[5]; 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = F[6]*m1[i].x + F[7]*m1[i].y + F[8]; 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s2 = 1./(a*a + b*b); 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d2 = m2[i].x*a + m2[i].y*b + c; 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a = F[0]*m2[i].x + F[3]*m2[i].y + F[6]; 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b = F[1]*m2[i].x + F[4]*m2[i].y + F[7]; 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = F[2]*m2[i].x + F[5]*m2[i].y + F[8]; 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s1 = 1./(a*a + b*b); 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = m1[i].x*a + m1[i].y*b + c; 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err[i] = (float)(d1*d1*s1 + d2*d2*s2); 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvFindFundamentalMat( const CvMat* points1, const CvMat* points2, 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* fmatrix, int method, 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double param1, double param2, CvMat* mask ) 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = 0; 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *m1 = 0, *m2 = 0, *tempMask = 0; 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFindFundamentalMat" ); 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double F[3*9]; 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F ); 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count; 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) ); 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 && 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) ); 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = MAX(points1->cols, points1->rows); 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count < 7 ) 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1 = cvCreateMat( 1, count, CV_64FC2 ); 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( points1, m1 ); 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m2 = cvCreateMat( 1, count, CV_64FC2 ); 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( points2, m2 ); 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mask ) 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) && 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (mask->rows == 1 || mask->cols == 1) && 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask->rows*mask->cols == count ); 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tempMask = mask; 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( count > 8 ) 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tempMask = cvCreateMat( 1, count, CV_8U ); 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tempMask ) 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet( tempMask, cvScalarAll(1.) ); 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) ); 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count == 7 ) 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.run7Point(m1, m2, &_F9x3); 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( count == 8 || method == CV_FM_8POINT ) 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.run8Point(m1, m2, &_F3x3); 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( count > 8 ) 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param1 <= 0 ) 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param1 = 3; 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON ) 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param2 = 0.99; 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (method & ~3) == CV_RANSAC ) 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 ); 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 ); 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( result <= 0 ) 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count ); 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count ); 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( count >= 8 ); 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1->cols = m2->cols = count; 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn estimator.run8Point(m1, m2, &_F3x3); 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( result ) 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix ); 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &m1 ); 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &m2 ); 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tempMask != mask ) 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tempMask ); 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvComputeCorrespondEpilines( const CvMat* points, int pointImageID, 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* fmatrix, CvMat* lines ) 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvComputeCorrespondEpilines" ); 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int abc_stride, abc_plane_stride, abc_elem_size; 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int plane_stride, stride, elem_size; 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn; 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *ap, *bp, *cp; 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar *xp, *yp, *zp; 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double f[9]; 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat F = cvMat( 3, 3, CV_64F, f ); 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points) ) 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" ); 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn depth = CV_MAT_DEPTH(points->type); 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cn = CV_MAT_CN(points->type); 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) ) 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" ); 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points->rows > points->cols ) 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dims = cn*points->cols; 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = points->rows; 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) ) 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" ); 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dims = cn * points->rows; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = points->cols; 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dims != 2 && dims != 3 ) 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" ); 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(fmatrix) ) 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" ); 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 ) 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" ); 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fmatrix->cols != 3 || fmatrix->rows != 3 ) 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" ); 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(lines) ) 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" ); 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_depth = CV_MAT_DEPTH(lines->type); 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_cn = CV_MAT_CN(lines->type); 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) ) 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" ); 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( lines->rows > lines->cols ) 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_dims = abc_cn*lines->cols; 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_count = lines->rows; 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) ) 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" ); 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_dims = abc_cn * lines->rows; 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_count = lines->cols; 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( abc_dims != 3 ) 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" ); 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( abc_count != count ) 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" ); 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(depth); 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_elem_size = CV_ELEM_SIZE(abc_depth); 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points->rows == dims ) 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn plane_stride = points->step; 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stride = elem_size; 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn plane_stride = elem_size; 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stride = points->rows == 1 ? dims*elem_size : points->step; 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( lines->rows == 3 ) 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_plane_stride = lines->step; 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_stride = abc_elem_size; 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_plane_stride = abc_elem_size; 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step; 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( fmatrix, &F )); 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pointImageID == 2 ) 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( &F, &F ); 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xp = points->data.ptr; 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yp = xp + plane_stride; 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zp = dims == 3 ? yp + plane_stride : 0; 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ap = lines->data.ptr; 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bp = ap + abc_plane_stride; 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cp = bp + abc_plane_stride; 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x, y, z = 1.; 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a, b, c, nu; 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( depth == CV_32F ) 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = *(float*)xp; y = *(float*)yp; 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( zp ) 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = *(float*)zp, zp += stride; 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = *(double*)xp; y = *(double*)yp; 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( zp ) 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = *(double*)zp, zp += stride; 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xp += stride; yp += stride; 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a = f[0]*x + f[1]*y + f[2]*z; 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b = f[3]*x + f[4]*y + f[5]*z; 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = f[6]*x + f[7]*y + f[8]*z; 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nu = a*a + b*b; 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nu = nu ? 1./sqrt(nu) : 1.; 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a *= nu; b *= nu; c *= nu; 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( abc_depth == CV_32F ) 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *(float*)ap = (float)a; 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *(float*)bp = (float)b; 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *(float*)cp = (float)c; 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *(double*)ap = a; 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *(double*)bp = b; 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *(double*)cp = c; 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ap += abc_stride; 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bp += abc_stride; 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cp += abc_stride; 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvConvertPointsHomogeneous( const CvMat* src, CvMat* dst ) 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* temp = 0; 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* denom = 0; 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvConvertPointsHomogeneous" ); 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, s_count, s_dims, d_count, d_dims; 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _src, _dst, _ones; 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* ones = 0; 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(src) ) 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The input parameter is not a valid matrix" ); 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dst) ) 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg, 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The output parameter is not a valid matrix" ); 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src == dst || src->data.ptr == dst->data.ptr ) 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) ) 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Invalid inplace operation" ); 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->rows > src->cols ) 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) ) 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" ); 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s_dims = CV_MAT_CN(src->type)*src->cols; 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s_count = src->rows; 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) ) 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" ); 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s_dims = CV_MAT_CN(src->type)*src->rows; 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s_count = src->cols; 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->rows == 1 || src->cols == 1 ) 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src = cvReshape( src, &_src, 1, s_count ); 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst->rows > dst->cols ) 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) ) 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Either the number of channels or columns or rows in the input matrix must be =1" ); 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d_dims = CV_MAT_CN(dst->type)*dst->cols; 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d_count = dst->rows; 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) ) 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Either the number of channels or columns or rows in the output matrix must be =1" ); 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d_dims = CV_MAT_CN(dst->type)*dst->rows; 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d_count = dst->cols; 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst->rows == 1 || dst->cols == 1 ) 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvReshape( dst, &_dst, 1, d_count ); 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s_count != d_count ) 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" ); 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F ) 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Both matrices must be floating-point (single or double precision)" ); 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 ) 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Both input and output point dimensionality must be 2, 3 or 4" ); 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s_dims < d_dims - 1 || s_dims > d_dims + 1 ) 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The dimensionalities of input and output point sets differ too much" ); 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s_dims == d_dims - 1 ) 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d_count == dst->rows ) 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count )); 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count )); 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 )); 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims )); 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s_dims <= d_dims ) 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->rows == dst->rows && src->cols == dst->cols ) 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_ARE_TYPES_EQ( src, dst ) ) 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( src, dst ); 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( src, dst ); 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( src, dst )) 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type )); 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( src, temp ); 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src = temp; 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( src, dst ); 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ones ) 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet( ones, cvRealScalar(1.) ); 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size; 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( src, dst )) 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type )); 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( src, temp ); 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src = temp; 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(src->type); 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s_count == src->cols ) 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s_plane_stride = src->step / elem_size, s_stride = 1; 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s_stride = src->step / elem_size, s_plane_stride = 1; 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d_count == dst->cols ) 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d_plane_stride = dst->step / elem_size, d_stride = 1; 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d_stride = dst->step / elem_size, d_plane_stride = 1; 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( denom = cvCreateMat( 1, d_count, dst->type )); 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(dst->type) == CV_32F ) 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* xs = src->data.fl; 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* ys = xs + s_plane_stride; 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* zs = 0; 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* ws = xs + (s_dims - 1)*s_plane_stride; 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* iw = denom->data.fl; 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* xd = dst->data.fl; 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* yd = xd + d_plane_stride; 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* zd = 0; 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d_dims == 3 ) 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zs = ys + s_plane_stride; 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zd = yd + d_plane_stride; 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < d_count; i++, ws += s_stride ) 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t = *ws; 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iw[i] = t ? t : 1.f; 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvDiv( 0, denom, denom ); 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d_dims == 3 ) 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < d_count; i++ ) 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float w = iw[i]; 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = *xs * w, y = *ys * w, z = *zs * w; 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xs += s_stride; ys += s_stride; zs += s_stride; 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *xd = x; *yd = y; *zd = z; 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xd += d_stride; yd += d_stride; zd += d_stride; 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < d_count; i++ ) 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float w = iw[i]; 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = *xs * w, y = *ys * w; 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xs += s_stride; ys += s_stride; 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *xd = x; *yd = y; 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xd += d_stride; yd += d_stride; 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* xs = src->data.db; 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* ys = xs + s_plane_stride; 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* zs = 0; 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* ws = xs + (s_dims - 1)*s_plane_stride; 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* iw = denom->data.db; 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* xd = dst->data.db; 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* yd = xd + d_plane_stride; 13956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* zd = 0; 13966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d_dims == 3 ) 13986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zs = ys + s_plane_stride; 14006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zd = yd + d_plane_stride; 14016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < d_count; i++, ws += s_stride ) 14046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = *ws; 14066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iw[i] = t ? t : 1.; 14076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvDiv( 0, denom, denom ); 14106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d_dims == 3 ) 14126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < d_count; i++ ) 14136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = iw[i]; 14156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = *xs * w, y = *ys * w, z = *zs * w; 14166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xs += s_stride; ys += s_stride; zs += s_stride; 14176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *xd = x; *yd = y; *zd = z; 14186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xd += d_stride; yd += d_stride; zd += d_stride; 14196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 14216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < d_count; i++ ) 14226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = iw[i]; 14246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = *xs * w, y = *ys * w; 14256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xs += s_stride; ys += s_stride; 14266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *xd = x; *yd = y; 14276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xd += d_stride; yd += d_stride; 14286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 14336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &denom ); 14356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &temp ); 14366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 14376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 1439