Merge pull request #592 from vpisarev:c2cpp_calib3d_ptsetreg

modules/calib3d/src/_modelest.h → modules/calib3d/src/calib3d_init.cpp

@@ -7,10 +7,11 @@
 //  copy or use the software.
 //
 //
-//                        Intel License Agreement
+//                          License Agreement
 //                For Open Source Computer Vision Library
 //
-// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
@@ -23,7 +24,7 @@
 //     this list of conditions and the following disclaimer in the documentation
 //     and/or other materials provided with the distribution.
 //
-//   * The name of Intel Corporation may not be used to endorse or promote products
+//   * The name of the copyright holders may not be used to endorse or promote products
 //     derived from this software without specific prior written permission.
 //
 // This software is provided by the copyright holders and contributors "as is" and
@@ -39,44 +40,27 @@
 //
 //M*/
 
+#include "precomp.hpp"
 
-#ifndef _CV_MODEL_EST_H_
-#define _CV_MODEL_EST_H_
+using namespace cv;
 
-#include "opencv2/calib3d/calib3d.hpp"
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-class CV_EXPORTS CvModelEstimator2
-{
-public:
-    CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions);
-    virtual ~CvModelEstimator2();
 
-    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0;
-    virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
-                           CvMat* mask, double confidence=0.99, int maxIters=2000 );
-    virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
-                            CvMat* mask, double threshold,
-                            double confidence=0.99, int maxIters=2000 );
-    virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; }
-    virtual void setSeed( int64 seed );
+//////////////////////////////////////////////////////////////////////////////////////////////////////////                  
+
 
-protected:
-    virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
-                                     const CvMat* model, CvMat* error ) = 0;
-    virtual int findInliers( const CvMat* m1, const CvMat* m2,
-                             const CvMat* model, CvMat* error,
-                             CvMat* mask, double threshold );
-    virtual bool getSubset( const CvMat* m1, const CvMat* m2,
-                            CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );
-    virtual bool checkSubset( const CvMat* ms1, int count );
-    virtual bool isMinimalSetConsistent( const CvMat* /*m1*/, const CvMat* /*m2*/ ) { return true; };
 
-    CvRNG rng;
-    int modelPoints;
-    CvSize modelSize;
-    int maxBasicSolutions;
-    bool checkPartialSubsets;
-};
+///////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-#endif // _CV_MODEL_EST_H_
+#if 0
+bool cv::initModule_calib3d(void)
+{
+    bool all = true;
+    all &= !RANSACPointSetRegistrator_info_auto.name().empty();
+    all &= !LMeDSPointSetRegistrator_info_auto.name().empty();
+    all &= !LMSolverImpl_info_auto.name().empty();
 
+    return all;
+}
+#endif

modules/calib3d/src/calibration.cpp

@@ -55,247 +55,6 @@
 
 using namespace cv;
 
-CvLevMarq::CvLevMarq()
-{
-    mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
-    lambdaLg10 = 0; state = DONE;
-    criteria = cvTermCriteria(0,0,0);
-    iters = 0;
-    completeSymmFlag = false;
-}
-
-CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
-{
-    mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
-    init(nparams, nerrs, criteria0, _completeSymmFlag);
-}
-
-void CvLevMarq::clear()
-{
-    mask.release();
-    prevParam.release();
-    param.release();
-    J.release();
-    err.release();
-    JtJ.release();
-    JtJN.release();
-    JtErr.release();
-    JtJV.release();
-    JtJW.release();
-}
-
-CvLevMarq::~CvLevMarq()
-{
-    clear();
-}
-
-void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
-{
-    if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
-        clear();
-    mask = cvCreateMat( nparams, 1, CV_8U );
-    cvSet(mask, cvScalarAll(1));
-    prevParam = cvCreateMat( nparams, 1, CV_64F );
-    param = cvCreateMat( nparams, 1, CV_64F );
-    JtJ = cvCreateMat( nparams, nparams, CV_64F );
-    JtJN = cvCreateMat( nparams, nparams, CV_64F );
-    JtJV = cvCreateMat( nparams, nparams, CV_64F );
-    JtJW = cvCreateMat( nparams, 1, CV_64F );
-    JtErr = cvCreateMat( nparams, 1, CV_64F );
-    if( nerrs > 0 )
-    {
-        J = cvCreateMat( nerrs, nparams, CV_64F );
-        err = cvCreateMat( nerrs, 1, CV_64F );
-    }
-    prevErrNorm = DBL_MAX;
-    lambdaLg10 = -3;
-    criteria = criteria0;
-    if( criteria.type & CV_TERMCRIT_ITER )
-        criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
-    else
-        criteria.max_iter = 30;
-    if( criteria.type & CV_TERMCRIT_EPS )
-        criteria.epsilon = MAX(criteria.epsilon, 0);
-    else
-        criteria.epsilon = DBL_EPSILON;
-    state = STARTED;
-    iters = 0;
-    completeSymmFlag = _completeSymmFlag;
-}
-
-bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
-{
-    double change;
-
-    matJ = _err = 0;
-
-    assert( !err.empty() );
-    if( state == DONE )
-    {
-        _param = param;
-        return false;
-    }
-
-    if( state == STARTED )
-    {
-        _param = param;
-        cvZero( J );
-        cvZero( err );
-        matJ = J;
-        _err = err;
-        state = CALC_J;
-        return true;
-    }
-
-    if( state == CALC_J )
-    {
-        cvMulTransposed( J, JtJ, 1 );
-        cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
-        cvCopy( param, prevParam );
-        step();
-        if( iters == 0 )
-            prevErrNorm = cvNorm(err, 0, CV_L2);
-        _param = param;
-        cvZero( err );
-        _err = err;
-        state = CHECK_ERR;
-        return true;
-    }
-
-    assert( state == CHECK_ERR );
-    errNorm = cvNorm( err, 0, CV_L2 );
-    if( errNorm > prevErrNorm )
-    {
-        if( ++lambdaLg10 <= 16 )
-        {
-            step();
-            _param = param;
-            cvZero( err );
-            _err = err;
-            state = CHECK_ERR;
-            return true;
-        }
-    }
-
-    lambdaLg10 = MAX(lambdaLg10-1, -16);
-    if( ++iters >= criteria.max_iter ||
-        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
-    {
-        _param = param;
-        state = DONE;
-        return true;
-    }
-
-    prevErrNorm = errNorm;
-    _param = param;
-    cvZero(J);
-    matJ = J;
-    _err = err;
-    state = CALC_J;
-    return true;
-}
-
-
-bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
-{
-    double change;
-
-    CV_Assert( err.empty() );
-    if( state == DONE )
-    {
-        _param = param;
-        return false;
-    }
-
-    if( state == STARTED )
-    {
-        _param = param;
-        cvZero( JtJ );
-        cvZero( JtErr );
-        errNorm = 0;
-        _JtJ = JtJ;
-        _JtErr = JtErr;
-        _errNorm = &errNorm;
-        state = CALC_J;
-        return true;
-    }
-
-    if( state == CALC_J )
-    {
-        cvCopy( param, prevParam );
-        step();
-        _param = param;
-        prevErrNorm = errNorm;
-        errNorm = 0;
-        _errNorm = &errNorm;
-        state = CHECK_ERR;
-        return true;
-    }
-
-    assert( state == CHECK_ERR );
-    if( errNorm > prevErrNorm )
-    {
-        if( ++lambdaLg10 <= 16 )
-        {
-            step();
-            _param = param;
-            errNorm = 0;
-            _errNorm = &errNorm;
-            state = CHECK_ERR;
-            return true;
-        }
-    }
-
-    lambdaLg10 = MAX(lambdaLg10-1, -16);
-    if( ++iters >= criteria.max_iter ||
-        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
-    {
-        _param = param;
-        state = DONE;
-        return false;
-    }
-
-    prevErrNorm = errNorm;
-    cvZero( JtJ );
-    cvZero( JtErr );
-    _param = param;
-    _JtJ = JtJ;
-    _JtErr = JtErr;
-    state = CALC_J;
-    return true;
-}
-
-void CvLevMarq::step()
-{
-    const double LOG10 = log(10.);
-    double lambda = exp(lambdaLg10*LOG10);
-    int i, j, nparams = param->rows;
-
-    for( i = 0; i < nparams; i++ )
-        if( mask->data.ptr[i] == 0 )
-        {
-            double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
-            for( j = 0; j < nparams; j++ )
-                row[j] = col[j*nparams] = 0;
-            JtErr->data.db[i] = 0;
-        }
-
-    if( !err )
-        cvCompleteSymm( JtJ, completeSymmFlag );
-#if 1
-    cvCopy( JtJ, JtJN );
-    for( i = 0; i < nparams; i++ )
-        JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
-#else
-    cvSetIdentity(JtJN, cvRealScalar(lambda));
-    cvAdd( JtJ, JtJN, JtJN );
-#endif
-    cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
-    cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
-    for( i = 0; i < nparams; i++ )
-        param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
-}
-
 // reimplementation of dAB.m
 CV_IMPL void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
 {

modules/calib3d/src/circlesgrid.cpp

@@ -402,14 +402,16 @@ void CirclesGridClusterFinder::parsePatternPoints(const std::vector<cv::Point2f>
       else
         idealPt = Point2f(j*squareSize, i*squareSize);
 
-      std::vector<float> query = Mat(idealPt);
-      int knn = 1;
-      std::vector<int> indices(knn);
-      std::vector<float> dists(knn);
+      Mat query(1, 2, CV_32F, &idealPt);
+      const int knn = 1;
+      int indicesbuf[knn] = {0};
+      float distsbuf[knn] = {0.f};
+      Mat indices(1, knn, CV_32S, &indicesbuf);
+      Mat dists(1, knn, CV_32F, &distsbuf);
       flannIndex.knnSearch(query, indices, dists, knn, flann::SearchParams());
-      centers.push_back(patternPoints.at(indices[0]));
+      centers.push_back(patternPoints.at(indicesbuf[0]));
 
-      if(dists[0] > maxRectifiedDistance)
+      if(distsbuf[0] > maxRectifiedDistance)
       {
 #ifdef DEBUG_CIRCLES
         cout << "Pattern not detected: too large rectified distance" << endl;

modules/calib3d/src/compat_ptsetreg.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "precomp.hpp"
+
+/************************************************************************************\
+       Some backward compatibility stuff, to be moved to legacy or compat module
+\************************************************************************************/
+
+using cv::Ptr;
+
+////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
+
+CvLevMarq::CvLevMarq()
+{
+    mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
+    lambdaLg10 = 0; state = DONE;
+    criteria = cvTermCriteria(0,0,0);
+    iters = 0;
+    completeSymmFlag = false;
+}
+
+CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
+{
+    mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
+    init(nparams, nerrs, criteria0, _completeSymmFlag);
+}
+
+void CvLevMarq::clear()
+{
+    mask.release();
+    prevParam.release();
+    param.release();
+    J.release();
+    err.release();
+    JtJ.release();
+    JtJN.release();
+    JtErr.release();
+    JtJV.release();
+    JtJW.release();
+}
+
+CvLevMarq::~CvLevMarq()
+{
+    clear();
+}
+
+void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
+{
+    if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
+        clear();
+    mask = cvCreateMat( nparams, 1, CV_8U );
+    cvSet(mask, cvScalarAll(1));
+    prevParam = cvCreateMat( nparams, 1, CV_64F );
+    param = cvCreateMat( nparams, 1, CV_64F );
+    JtJ = cvCreateMat( nparams, nparams, CV_64F );
+    JtJN = cvCreateMat( nparams, nparams, CV_64F );
+    JtJV = cvCreateMat( nparams, nparams, CV_64F );
+    JtJW = cvCreateMat( nparams, 1, CV_64F );
+    JtErr = cvCreateMat( nparams, 1, CV_64F );
+    if( nerrs > 0 )
+    {
+        J = cvCreateMat( nerrs, nparams, CV_64F );
+        err = cvCreateMat( nerrs, 1, CV_64F );
+    }
+    prevErrNorm = DBL_MAX;
+    lambdaLg10 = -3;
+    criteria = criteria0;
+    if( criteria.type & CV_TERMCRIT_ITER )
+        criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
+    else
+        criteria.max_iter = 30;
+    if( criteria.type & CV_TERMCRIT_EPS )
+        criteria.epsilon = MAX(criteria.epsilon, 0);
+    else
+        criteria.epsilon = DBL_EPSILON;
+    state = STARTED;
+    iters = 0;
+    completeSymmFlag = _completeSymmFlag;
+}
+
+bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
+{
+    double change;
+
+    matJ = _err = 0;
+
+    assert( !err.empty() );
+    if( state == DONE )
+    {
+        _param = param;
+        return false;
+    }
+
+    if( state == STARTED )
+    {
+        _param = param;
+        cvZero( J );
+        cvZero( err );
+        matJ = J;
+        _err = err;
+        state = CALC_J;
+        return true;
+    }
+
+    if( state == CALC_J )
+    {
+        cvMulTransposed( J, JtJ, 1 );
+        cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
+        cvCopy( param, prevParam );
+        step();
+        if( iters == 0 )
+            prevErrNorm = cvNorm(err, 0, CV_L2);
+        _param = param;
+        cvZero( err );
+        _err = err;
+        state = CHECK_ERR;
+        return true;
+    }
+
+    assert( state == CHECK_ERR );
+    errNorm = cvNorm( err, 0, CV_L2 );
+    if( errNorm > prevErrNorm )
+    {
+        if( ++lambdaLg10 <= 16 )
+        {
+            step();
+            _param = param;
+            cvZero( err );
+            _err = err;
+            state = CHECK_ERR;
+            return true;
+        }
+    }
+
+    lambdaLg10 = MAX(lambdaLg10-1, -16);
+    if( ++iters >= criteria.max_iter ||
+        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
+    {
+        _param = param;
+        state = DONE;
+        return true;
+    }
+
+    prevErrNorm = errNorm;
+    _param = param;
+    cvZero(J);
+    matJ = J;
+    _err = err;
+    state = CALC_J;
+    return true;
+}
+
+
+bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
+{
+    double change;
+
+    CV_Assert( err.empty() );
+    if( state == DONE )
+    {
+        _param = param;
+        return false;
+    }
+
+    if( state == STARTED )
+    {
+        _param = param;
+        cvZero( JtJ );
+        cvZero( JtErr );
+        errNorm = 0;
+        _JtJ = JtJ;
+        _JtErr = JtErr;
+        _errNorm = &errNorm;
+        state = CALC_J;
+        return true;
+    }
+
+    if( state == CALC_J )
+    {
+        cvCopy( param, prevParam );
+        step();
+        _param = param;
+        prevErrNorm = errNorm;
+        errNorm = 0;
+        _errNorm = &errNorm;
+        state = CHECK_ERR;
+        return true;
+    }
+
+    assert( state == CHECK_ERR );
+    if( errNorm > prevErrNorm )
+    {
+        if( ++lambdaLg10 <= 16 )
+        {
+            step();
+            _param = param;
+            errNorm = 0;
+            _errNorm = &errNorm;
+            state = CHECK_ERR;
+            return true;
+        }
+    }
+
+    lambdaLg10 = MAX(lambdaLg10-1, -16);
+    if( ++iters >= criteria.max_iter ||
+        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
+    {
+        _param = param;
+        state = DONE;
+        return false;
+    }
+
+    prevErrNorm = errNorm;
+    cvZero( JtJ );
+    cvZero( JtErr );
+    _param = param;
+    _JtJ = JtJ;
+    _JtErr = JtErr;
+    state = CALC_J;
+    return true;
+}
+
+void CvLevMarq::step()
+{
+    const double LOG10 = log(10.);
+    double lambda = exp(lambdaLg10*LOG10);
+    int i, j, nparams = param->rows;
+
+    for( i = 0; i < nparams; i++ )
+        if( mask->data.ptr[i] == 0 )
+        {
+            double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
+            for( j = 0; j < nparams; j++ )
+                row[j] = col[j*nparams] = 0;
+            JtErr->data.db[i] = 0;
+        }
+
+    if( !err )
+        cvCompleteSymm( JtJ, completeSymmFlag );
+#if 1
+    cvCopy( JtJ, JtJN );
+    for( i = 0; i < nparams; i++ )
+        JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
+#else
+    cvSetIdentity(JtJN, cvRealScalar(lambda));
+    cvAdd( JtJ, JtJN, JtJN );
+#endif
+    cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
+    cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
+    for( i = 0; i < nparams; i++ )
+        param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
+}
+
+
+CV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
+{
+    return cv::RANSACUpdateNumIters(p, ep, modelPoints, maxIters);
+}
+
+
+CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method,
+                              double ransacReprojThreshold, CvMat* _mask )
+{
+    cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
+
+    if( src.channels() == 1 && (src.rows == 2 || src.rows == 3) && src.cols > 3 )
+        cv::transpose(src, src);
+    if( dst.channels() == 1 && (dst.rows == 2 || dst.rows == 3) && dst.cols > 3 )
+        cv::transpose(dst, dst);
+
+    const cv::Mat H = cv::cvarrToMat(__H), mask = cv::cvarrToMat(_mask);
+    cv::Mat H0 = cv::findHomography(src, dst, method, ransacReprojThreshold,
+                                    _mask ? cv::_OutputArray(mask) : cv::_OutputArray());
+
+    if( H0.empty() )
+    {
+        cv::Mat Hz = cv::cvarrToMat(__H);
+        Hz.setTo(cv::Scalar::all(0));
+        return 0;
+    }
+    H0.convertTo(H, H.type());
+    return 1;
+}
+
+
+CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
+                                  CvMat* fmatrix, int method,
+                                  double param1, double param2, CvMat* _mask )
+{
+    cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2);
+
+    if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 )
+        cv::transpose(m1, m1);
+    if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 )
+        cv::transpose(m2, m2);
+
+    const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask);
+    cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2,
+                                         _mask ? cv::_OutputArray(mask) : cv::_OutputArray());
+
+    if( FM0.empty() )
+    {
+        cv::Mat FM0z = cv::cvarrToMat(fmatrix);
+        FM0z.setTo(cv::Scalar::all(0));
+        return 0;
+    }
+
+    CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 );
+    cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows));
+    FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type());
+    return FM1.rows / 3;
+}
+
+
+CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
+                                          const CvMat* fmatrix, CvMat* _lines )
+{
+    cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix);
+    cv::Mat lines = cv::cvarrToMat(_lines);
+    const cv::Mat lines0 = lines;
+
+    if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 )
+        cv::transpose(pt, pt);
+
+    cv::computeCorrespondEpilines(pt, pointImageID, fm, lines);
+
+    bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3;
+    lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows));
+
+    if( tflag )
+    {
+        CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows );
+        if( lines0.type() == lines.type() )
+            transpose( lines, lines0 );
+        else
+        {
+            transpose( lines, lines );
+            lines.convertTo( lines0, lines0.type() );
+        }
+    }
+    else
+    {
+        CV_Assert( lines.size() == lines0.size() );
+        if( lines.data != lines0.data )
+            lines.convertTo(lines0, lines0.type());
+    }
+}
+
+
+CV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst )
+{
+    cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
+    const cv::Mat dst0 = dst;
+
+    int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);
+
+    if( src.channels() == 1 && src.cols > d0 )
+        cv::transpose(src, src);
+
+    int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);
+
+    if( d0 == d1 )
+        src.copyTo(dst);
+    else if( d0 < d1 )
+        cv::convertPointsToHomogeneous(src, dst);
+    else
+        cv::convertPointsFromHomogeneous(src, dst);
+
+    bool tflag = dst0.channels() == 1 && dst0.cols > d1;
+    dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));
+
+    if( tflag )
+    {
+        CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );
+        if( dst0.type() == dst.type() )
+            transpose( dst, dst0 );
+        else
+        {
+            transpose( dst, dst );
+            dst.convertTo( dst0, dst0.type() );
+        }
+    }
+    else
+    {
+        CV_Assert( dst.size() == dst0.size() );
+        if( dst.data != dst0.data )
+            dst.convertTo(dst0, dst0.type());
+    }
+}
+

modules/calib3d/src/fundam.cpp

@@ -7,10 +7,11 @@
 //  copy or use the software.
 //
 //
-//                        Intel License Agreement
+//                          License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
@@ -23,7 +24,7 @@
 //     this list of conditions and the following disclaimer in the documentation
 //     and/or other materials provided with the distribution.
 //
-//   * The name of Intel Corporation may not be used to endorse or promote products
+//   * The name of the copyright holders may not be used to endorse or promote products
 //     derived from this software without specific prior written permission.
 //
 // This software is provided by the copyright holders and contributors "as is" and
@@ -40,11 +41,35 @@
 //M*/
 
 #include "precomp.hpp"
-#include "_modelest.h"
+#include <iostream>
 
-using namespace cv;
+namespace cv
+{
 
-template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
+static bool haveCollinearPoints( const Mat& m, int count )
+{
+    int j, k, i = count-1;
+    const Point2f* ptr = m.ptr<Point2f>();
+
+    // check that the i-th selected point does not belong
+    // to a line connecting some previously selected points
+    for( j = 0; j < i; j++ )
+    {
+        double dx1 = ptr[j].x - ptr[i].x;
+        double dy1 = ptr[j].y - ptr[i].y;
+        for( k = 0; k < j; k++ )
+        {
+            double dx2 = ptr[k].x - ptr[i].x;
+            double dy2 = ptr[k].y - ptr[i].y;
+            if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
+                return true;
+        }
+    }
+    return false;
+}
+
+
+template<typename T> int compressPoints( T* ptr, const uchar* mask, int mstep, int count )
 {
     int i, j;
     for( i = j = 0; i < count; i++ )
@@ -57,308 +82,280 @@ template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep
     return j;
 }
 
-class CvHomographyEstimator : public CvModelEstimator2
+
+class HomographyEstimatorCallback : public PointSetRegistrator::Callback
 {
 public:
-    CvHomographyEstimator( int modelPoints );
-
-    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
-    virtual bool refine( const CvMat* m1, const CvMat* m2,
-                         CvMat* model, int maxIters );
-protected:
-    virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
-                                     const CvMat* model, CvMat* error );
-    virtual bool isMinimalSetConsistent( const CvMat* m1, const CvMat* m2 );
-    virtual bool weakConstraint ( const CvMat* srcPoints, const CvMat* dstPoints, int t1, int t2, int t3 );
-};
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
+    {
+        Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
+        if( haveCollinearPoints(ms1, count) || haveCollinearPoints(ms2, count) )
+            return false;
+
+        // We check whether the minimal set of points for the homography estimation
+        // are geometrically consistent. We check if every 3 correspondences sets
+        // fulfills the constraint.
+        //
+        // The usefullness of this constraint is explained in the paper:
+        //
+        // "Speeding-up homography estimation in mobile devices"
+        // Journal of Real-Time Image Processing. 2013. DOI: 10.1007/s11554-012-0314-1
+        // Pablo Marquez-Neila, Javier Lopez-Alberca, Jose M. Buenaposada, Luis Baumela
+        if( count == 4 )
+        {
+            static const int tt[][3] = {{0, 1, 2}, {1, 2, 3}, {0, 2, 3}, {0, 1, 3}};
+            const Point2f* src = ms1.ptr<Point2f>();
+            const Point2f* dst = ms2.ptr<Point2f>();
+            int negative = 0;
 
+            for( int i = 0; i < 4; i++ )
+            {
+                const int* t = tt[i];
+                Matx33d A(src[t[0]].x, src[t[0]].y, 1., src[t[1]].x, src[t[1]].y, 1., src[t[2]].x, src[t[2]].y, 1.);
+                Matx33d B(dst[t[0]].x, dst[t[0]].y, 1., dst[t[1]].x, dst[t[1]].y, 1., dst[t[2]].x, dst[t[2]].y, 1.);
 
-CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
-    : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
-{
-    assert( _modelPoints == 4 || _modelPoints == 5 );
-    checkPartialSubsets = false;
-}
+                negative += determinant(A)*determinant(B) < 0;
+            }
+            if( negative != 0 && negative != 4 )
+                return false;
+        }
 
-int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
-{
-    int i, count = m1->rows*m1->cols;
-    const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
-    const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
-
-    double LtL[9][9], W[9][1], V[9][9];
-    CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
-    CvMat matW = cvMat( 9, 1, CV_64F, W );
-    CvMat matV = cvMat( 9, 9, CV_64F, V );
-    CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
-    CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
-    CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
+        return true;
+    }
 
-    for( i = 0; i < count; i++ )
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
     {
-        cm.x += m[i].x; cm.y += m[i].y;
-        cM.x += M[i].x; cM.y += M[i].y;
-    }
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        int i, count = m1.checkVector(2);
+        const Point2f* M = m1.ptr<Point2f>();
+        const Point2f* m = m2.ptr<Point2f>();
 
-    cm.x /= count; cm.y /= count;
-    cM.x /= count; cM.y /= count;
+        double LtL[9][9], W[9][1], V[9][9];
+        Mat _LtL( 9, 9, CV_64F, &LtL[0][0] );
+        Mat matW( 9, 1, CV_64F, W );
+        Mat matV( 9, 9, CV_64F, V );
+        Mat _H0( 3, 3, CV_64F, V[8] );
+        Mat _Htemp( 3, 3, CV_64F, V[7] );
+        Point2d cM(0,0), cm(0,0), sM(0,0), sm(0,0);
 
-    for( i = 0; i < count; i++ )
-    {
-        sm.x += fabs(m[i].x - cm.x);
-        sm.y += fabs(m[i].y - cm.y);
-        sM.x += fabs(M[i].x - cM.x);
-        sM.y += fabs(M[i].y - cM.y);
-    }
+        for( i = 0; i < count; i++ )
+        {
+            cm.x += m[i].x; cm.y += m[i].y;
+            cM.x += M[i].x; cM.y += M[i].y;
+        }
 
-    if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
-        fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )
-        return 0;
-    sm.x = count/sm.x; sm.y = count/sm.y;
-    sM.x = count/sM.x; sM.y = count/sM.y;
+        cm.x /= count;
+        cm.y /= count;
+        cM.x /= count;
+        cM.y /= count;
+
+        for( i = 0; i < count; i++ )
+        {
+            sm.x += fabs(m[i].x - cm.x);
+            sm.y += fabs(m[i].y - cm.y);
+            sM.x += fabs(M[i].x - cM.x);
+            sM.y += fabs(M[i].y - cM.y);
+        }
 
-    double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
-    double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
-    CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
-    CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
+        if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
+            fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )
+            return 0;
+        sm.x = count/sm.x; sm.y = count/sm.y;
+        sM.x = count/sM.x; sM.y = count/sM.y;
 
-    cvZero( &_LtL );
-    for( i = 0; i < count; i++ )
-    {
-        double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
-        double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
-        double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
-        double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
-        int j, k;
-        for( j = 0; j < 9; j++ )
-            for( k = j; k < 9; k++ )
-                LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
+        double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
+        double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
+        Mat _invHnorm( 3, 3, CV_64FC1, invHnorm );
+        Mat _Hnorm2( 3, 3, CV_64FC1, Hnorm2 );
+
+        _LtL.setTo(Scalar::all(0));
+        for( i = 0; i < count; i++ )
+        {
+            double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
+            double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
+            double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
+            double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
+            int j, k;
+            for( j = 0; j < 9; j++ )
+                for( k = j; k < 9; k++ )
+                    LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
+        }
+        completeSymm( _LtL );
+        
+        eigen( _LtL, matW, matV );
+        _Htemp = _invHnorm*_H0;
+        _H0 = _Htemp*_Hnorm2;
+        _H0.convertTo(_model, _H0.type(), 1./_H0.at<double>(2,2) );
+        
+        return 1;
     }
-    cvCompleteSymm( &_LtL );
 
-    //cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
-    cvEigenVV( &_LtL, &matV, &matW );
-    cvMatMul( &_invHnorm, &_H0, &_Htemp );
-    cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
-    cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
+        int i, count = m1.checkVector(2);
+        const Point2f* M = m1.ptr<Point2f>();
+        const Point2f* m = m2.ptr<Point2f>();
+        const double* H = model.ptr<double>();
+        float Hf[] = { (float)H[0], (float)H[1], (float)H[2], (float)H[3], (float)H[4], (float)H[5], (float)H[6], (float)H[7] };
 
-    return 1;
-}
+        _err.create(count, 1, CV_32F);
+        float* err = _err.getMat().ptr<float>();
 
+        for( i = 0; i < count; i++ )
+        {
+            float ww = 1.f/(Hf[6]*M[i].x + Hf[7]*M[i].y + 1.f);
+            float dx = (Hf[0]*M[i].x + Hf[1]*M[i].y + Hf[2])*ww - m[i].x;
+            float dy = (Hf[3]*M[i].x + Hf[4]*M[i].y + Hf[5])*ww - m[i].y;
+            err[i] = (float)(dx*dx + dy*dy);
+        }
+    }
+};
 
-void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
-                                                const CvMat* model, CvMat* _err )
-{
-    int i, count = m1->rows*m1->cols;
-    const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
-    const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
-    const double* H = model->data.db;
-    float* err = _err->data.fl;
 
-    for( i = 0; i < count; i++ )
+class HomographyRefineCallback : public LMSolver::Callback
+{
+public:
+    HomographyRefineCallback(InputArray _src, InputArray _dst)
     {
-        double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
-        double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
-        double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
-        err[i] = (float)(dx*dx + dy*dy);
+        src = _src.getMat();
+        dst = _dst.getMat();
     }
-}
 
-bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
-{
-    CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
-    int i, j, k, count = m1->rows*m1->cols;
-    const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
-    const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
-    CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
-    cvCopy( &modelPart, solver.param );
-
-    for(;;)
+    bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const
     {
-        const CvMat* _param = 0;
-        CvMat *_JtJ = 0, *_JtErr = 0;
-        double* _errNorm = 0;
+        int i, count = src.checkVector(2);
+        Mat param = _param.getMat();
+        _err.create(count*2, 1, CV_64F);
+        Mat err = _err.getMat(), J;
+        if( _Jac.needed())
+        {
+            _Jac.create(count*2, param.rows, CV_64F);
+            J = _Jac.getMat();
+            CV_Assert( J.isContinuous() && J.cols == 8 );
+        }
 
-        if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
-            break;
+        const Point2f* M = src.ptr<Point2f>();
+        const Point2f* m = dst.ptr<Point2f>();
+        const double* h = param.ptr<double>();
+        double* errptr = err.ptr<double>();
+        double* Jptr = J.data ? J.ptr<double>() : 0;
 
         for( i = 0; i < count; i++ )
         {
-            const double* h = _param->data.db;
             double Mx = M[i].x, My = M[i].y;
             double ww = h[6]*Mx + h[7]*My + 1.;
             ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0;
-            double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
-            double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
-            double err[] = { _xi - m[i].x, _yi - m[i].y };
-            if( _JtJ || _JtErr )
+            double xi = (h[0]*Mx + h[1]*My + h[2])*ww;
+            double yi = (h[3]*Mx + h[4]*My + h[5])*ww;
+            errptr[i*2] = xi - m[i].x;
+            errptr[i*2+1] = yi - m[i].y;
+
+            if( Jptr )
             {
-                double J[][8] =
-                {
-                    { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
-                    { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
-                };
-
-                for( j = 0; j < 8; j++ )
-                {
-                    for( k = j; k < 8; k++ )
-                        _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
-                    _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
-                }
+                Jptr[0] = Mx*ww; Jptr[1] = My*ww; Jptr[2] = ww;
+                Jptr[3] = Jptr[4] = Jptr[5] = 0.;
+                Jptr[6] = -Mx*ww*xi; Jptr[7] = -My*ww*xi;
+                Jptr[8] = Jptr[9] = Jptr[10] = 0.;
+                Jptr[11] = Mx*ww; Jptr[12] = My*ww; Jptr[13] = ww;
+                Jptr[14] = -Mx*ww*yi; Jptr[15] = -My*ww*yi;
             }
-            if( _errNorm )
-                *_errNorm += err[0]*err[0] + err[1]*err[1];
         }
+
+        return true;
     }
 
-    cvCopy( solver.param, &modelPart );
-    return true;
+    Mat src, dst;
+};
+
 }
 
 
-CV_IMPL int
-cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
-                  CvMat* __H, int method, double ransacReprojThreshold,
-                  CvMat* mask )
+cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
+                            int method, double ransacReprojThreshold, OutputArray _mask )
 {
     const double confidence = 0.995;
     const int maxIters = 2000;
     const double defaultRANSACReprojThreshold = 3;
     bool result = false;
-    Ptr<CvMat> m, M, tempMask;
 
-    double H[9];
-    CvMat matH = cvMat( 3, 3, CV_64FC1, H );
-    int count;
+    Mat points1 = _points1.getMat(), points2 = _points2.getMat();
+    Mat src, dst, H, tempMask;
+    int npoints = -1;
+
+    for( int i = 1; i <= 2; i++ )
+    {
+        Mat& p = i == 1 ? points1 : points2;
+        Mat& m = i == 1 ? src : dst;
+        npoints = p.checkVector(2, -1, false);
+        if( npoints < 0 )
+        {
+            npoints = p.checkVector(3, -1, false);
+            if( npoints < 0 )
+                CV_Error(CV_StsBadArg, "The input arrays should be 2D or 3D point sets");
+            if( npoints == 0 )
+                return Mat();
+            convertPointsFromHomogeneous(p, p);
+        }
+        p.reshape(2, npoints).convertTo(m, CV_32F);
+    }
 
-    CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
+    CV_Assert( src.checkVector(2) == dst.checkVector(2) );
 
-    count = MAX(imagePoints->cols, imagePoints->rows);
-    CV_Assert( count >= 4 );
     if( ransacReprojThreshold <= 0 )
         ransacReprojThreshold = defaultRANSACReprojThreshold;
 
-    m = cvCreateMat( 1, count, CV_64FC2 );
-    cvConvertPointsHomogeneous( imagePoints, m );
-
-    M = cvCreateMat( 1, count, CV_64FC2 );
-    cvConvertPointsHomogeneous( objectPoints, M );
+    Ptr<PointSetRegistrator::Callback> cb = new HomographyEstimatorCallback;
 
-    if( mask )
+    if( method == 0 || npoints == 4 )
     {
-        CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
-            (mask->rows == 1 || mask->cols == 1) &&
-            mask->rows*mask->cols == count );
+        tempMask = Mat::ones(npoints, 1, CV_8U);
+        result = cb->runKernel(src, dst, H) > 0;
     }
-    if( mask || count > 4 )
-        tempMask = cvCreateMat( 1, count, CV_8U );
-    if( !tempMask.empty() )
-        cvSet( tempMask, cvScalarAll(1.) );
-
-    CvHomographyEstimator estimator(4);
-    if( count == 4 )
-        method = 0;
-    if( method == CV_LMEDS )
-        result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );
-    else if( method == CV_RANSAC )
-        result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
+    else if( method == RANSAC )
+        result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask);
+    else if( method == LMEDS )
+        result = createLMeDSPointSetRegistrator(cb, 4, confidence, maxIters)->run(src, dst, H, tempMask);
     else
-        result = estimator.runKernel( M, m, &matH ) > 0;
+        CV_Error(CV_StsBadArg, "Unknown estimation method");
 
-    if( result && count > 4 )
+    if( result && npoints > 4 )
     {
-        icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
-        count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
-        M->cols = m->cols = count;
-        if( method == CV_RANSAC )
-            estimator.runKernel( M, m, &matH );
-        estimator.refine( M, m, &matH, 10 );
+        compressPoints( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
+        npoints = compressPoints( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
+        if( npoints > 0 )
+        {
+            Mat src1 = src.rowRange(0, npoints);
+            Mat dst1 = dst.rowRange(0, npoints);
+            src = src1;
+            dst = dst1;
+            if( method == RANSAC || method == LMEDS )
+                cb->runKernel( src, dst, H );
+            Mat H8(8, 1, CV_64F, H.ptr<double>());
+            createLMSolver(new HomographyRefineCallback(src, dst), 10)->run(H8);
+        }
     }
 
     if( result )
-        cvConvert( &matH, __H );
-
-    if( mask && tempMask )
     {
-        if( CV_ARE_SIZES_EQ(mask, tempMask) )
-           cvCopy( tempMask, mask );
-        else
-           cvTranspose( tempMask, mask );
+        if( _mask.needed() )
+            tempMask.copyTo(_mask);
     }
+    else
+        H.release();
 
-    return (int)result;
+    return H;
 }
 
-// We check whether three correspondences for the homography estimation
-// are geometrically consistent (the points in the source image should 
-// maintain the same circular order than in the destination image).
-//
-// The usefullness of this constraint is explained in the paper:
-//
-// "Speeding-up homography estimation in mobile devices"
-// Journal of Real-Time Image Processing. 2013. DOI: 10.1007/s11554-012-0314-1
-// Pablo Marquez-Neila, Javier Lopez-Alberca, Jose M. Buenaposada, Luis Baumela
-bool
-CvHomographyEstimator::weakConstraint ( const CvMat* srcPoints, const CvMat* dstPoints, int t1, int t2, int t3 )
-{
-  const CvPoint2D64f* src = (const CvPoint2D64f*)srcPoints->data.ptr;
-  const CvPoint2D64f* dst = (const CvPoint2D64f*)dstPoints->data.ptr;
-
-  CvMat* A = cvCreateMat( 3, 3, CV_64F );
-  CvMat* B = cvCreateMat( 3, 3, CV_64F );
-
-  double detA;
-  double detB;
-
-  cvmSet(A, 0, 0, src[t1].x);
-  cvmSet(A, 0, 1, src[t1].y);
-  cvmSet(A, 0, 2, 1);
-  cvmSet(A, 1, 0, src[t2].x);
-  cvmSet(A, 1, 1, src[t2].y);
-  cvmSet(A, 1, 2, 1);
-  cvmSet(A, 2, 0, src[t3].x);
-  cvmSet(A, 2, 1, src[t3].y);
-  cvmSet(A, 2, 2, 1);
-
-  cvmSet(B, 0, 0, dst[t1].x);
-  cvmSet(B, 0, 1, dst[t1].y);
-  cvmSet(B, 0, 2, 1);
-  cvmSet(B, 1, 0, dst[t2].x);
-  cvmSet(B, 1, 1, dst[t2].y);
-  cvmSet(B, 1, 2, 1);
-  cvmSet(B, 2, 0, dst[t3].x);
-  cvmSet(B, 2, 1, dst[t3].y);
-  cvmSet(B, 2, 2, 1);
-
-  detA = cvDet(A);
-  detB = cvDet(B);
-
-  cvReleaseMat(&A);
-  cvReleaseMat(&B);
-
-  return (detA*detB >= 0);
-};
-
-// We check whether the minimal set of points for the homography estimation
-// are geometrically consistent. We check if every 3 correspondences sets
-// fulfills the constraint.
-//
-// The usefullness of this constraint is explained in the paper:
-//
-// "Speeding-up homography estimation in mobile devices"
-// Journal of Real-Time Image Processing. 2013. DOI: 10.1007/s11554-012-0314-1
-// Pablo Marquez-Neila, Javier Lopez-Alberca, Jose M. Buenaposada, Luis Baumela
-bool
-CvHomographyEstimator::isMinimalSetConsistent ( const CvMat* srcPoints, const CvMat* dstPoints )
+cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
+                           OutputArray _mask, int method, double ransacReprojThreshold )
 {
-  return weakConstraint(srcPoints, dstPoints, 0, 1, 2) &&
-         weakConstraint(srcPoints, dstPoints, 1, 2, 3) &&
-         weakConstraint(srcPoints, dstPoints, 0, 2, 3) &&
-         weakConstraint(srcPoints, dstPoints, 0, 1, 3);
+    return cv::findHomography(_points1, _points2, method, ransacReprojThreshold, _mask);
 }
 
 
-/* Evaluation of Fundamental Matrix from point correspondences.
+
+/* Estimation of Fundamental Matrix from point correspondences.
    The original code has been written by Valery Mosyagin */
 
 /* The algorithms (except for RANSAC) and the notation have been taken from
@@ -367,44 +364,23 @@ CvHomographyEstimator::isMinimalSetConsistent ( const CvMat* srcPoints, const Cv
    that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
 
 /************************************** 7-point algorithm *******************************/
-class CvFMEstimator : public CvModelEstimator2
+namespace cv
 {
-public:
-    CvFMEstimator( int _modelPoints );
-
-    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
-    virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
-    virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
-protected:
-    virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
-                                     const CvMat* model, CvMat* error );
-};
 
-CvFMEstimator::CvFMEstimator( int _modelPoints )
-: CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
+static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
 {
-    assert( _modelPoints == 7 || _modelPoints == 8 );
-}
-
-
-int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
-{
-    return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
-}
-
-int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
-{
-    double a[7*9], w[7], v[9*9], c[4], r[3];
+    double a[7*9], w[7], u[9*9], v[9*9], c[4], r[3];
     double* f1, *f2;
     double t0, t1, t2;
-    CvMat A = cvMat( 7, 9, CV_64F, a );
-    CvMat V = cvMat( 9, 9, CV_64F, v );
-    CvMat W = cvMat( 7, 1, CV_64F, w );
-    CvMat coeffs = cvMat( 1, 4, CV_64F, c );
-    CvMat roots = cvMat( 1, 3, CV_64F, r );
-    const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
-    const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
-    double* fmatrix = _fmatrix->data.db;
+    Mat A( 7, 9, CV_64F, a );
+    Mat U( 7, 9, CV_64F, u );
+    Mat Vt( 9, 9, CV_64F, v );
+    Mat W( 7, 1, CV_64F, w );
+    Mat coeffs( 1, 4, CV_64F, c );
+    Mat roots( 1, 3, CV_64F, r );
+    const Point2f* m1 = _m1.ptr<Point2f>();
+    const Point2f* m2 = _m2.ptr<Point2f>();
+    double* fmatrix = _fmatrix.ptr<double>();
     int i, k, n;
 
     // form a linear system: i-th row of A(=a) represents
@@ -429,7 +405,7 @@ int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatri
     // the solution is linear subspace of dimensionality 2.
     // => use the last two singular vectors as a basis of the space
     // (according to SVD properties)
-    cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
+    SVDecomp( A, W, U, Vt, SVD::MODIFY_A + SVD::FULL_UV );
     f1 = v + 7*9;
     f2 = v + 8*9;
 
@@ -449,29 +425,29 @@ int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatri
     c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
 
     c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
-           f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
-           f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
-           f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
-           f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
-           f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
-           f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
+    f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
+    f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
+    f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
+    f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
+    f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
+    f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
 
     t0 = f1[4]*f1[8] - f1[5]*f1[7];
     t1 = f1[3]*f1[8] - f1[5]*f1[6];
     t2 = f1[3]*f1[7] - f1[4]*f1[6];
 
     c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
-           f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
-           f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
-           f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
-           f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
-           f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
-           f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
+    f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
+    f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
+    f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
+    f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
+    f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
+    f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
 
     c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
 
     // solve the cubic equation; there can be 1 to 3 roots ...
-    n = cvSolveCubic( &coeffs, &roots );
+    n = solveCubic( coeffs, roots );
 
     if( n < 1 || n > 3 )
         return n;
@@ -499,77 +475,77 @@ int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatri
     return n;
 }
 
-
-int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
+    
+static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
 {
     double a[9*9], w[9], v[9*9];
-    CvMat W = cvMat( 1, 9, CV_64F, w );
-    CvMat V = cvMat( 9, 9, CV_64F, v );
-    CvMat A = cvMat( 9, 9, CV_64F, a );
-    CvMat U, F0, TF;
+    Mat W( 9, 1, CV_64F, w );
+    Mat V( 9, 9, CV_64F, v );
+    Mat A( 9, 9, CV_64F, a );
+    Mat U, F0, TF;
 
-    CvPoint2D64f m0c = {0,0}, m1c = {0,0};
-    double t, scale0 = 0, scale1 = 0;
+    Point2d m1c(0,0), m2c(0,0);
+    double t, scale1 = 0, scale2 = 0;
 
-    const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
-    const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
-    double* fmatrix = _fmatrix->data.db;
-    CV_Assert( (_m1->cols == 1 || _m1->rows == 1) && CV_ARE_SIZES_EQ(_m1, _m2));
-    int i, j, k, count = _m1->cols*_m1->rows;
+    const Point2f* m1 = _m1.ptr<Point2f>();
+    const Point2f* m2 = _m2.ptr<Point2f>();
+    double* fmatrix = _fmatrix.ptr<double>();
+    CV_Assert( (_m1.cols == 1 || _m1.rows == 1) && _m1.size() == _m2.size());
+    int i, j, k, count = _m1.checkVector(2);
 
     // compute centers and average distances for each of the two point sets
     for( i = 0; i < count; i++ )
     {
         double x = m1[i].x, y = m1[i].y;
-        m0c.x += x; m0c.y += y;
+        m1c.x += x; m1c.y += y;
 
         x = m2[i].x, y = m2[i].y;
-        m1c.x += x; m1c.y += y;
+        m2c.x += x; m2c.y += y;
     }
 
     // calculate the normalizing transformations for each of the point sets:
     // after the transformation each set will have the mass center at the coordinate origin
     // and the average distance from the origin will be ~sqrt(2).
     t = 1./count;
-    m0c.x *= t; m0c.y *= t;
     m1c.x *= t; m1c.y *= t;
+    m2c.x *= t; m2c.y *= t;
 
     for( i = 0; i < count; i++ )
     {
-        double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
-        scale0 += sqrt(x*x + y*y);
+        double x = m1[i].x - m1c.x, y = m1[i].y - m1c.y;
+        scale1 += std::sqrt(x*x + y*y);
 
-        x = m2[i].x - m1c.x, y = m2[i].y - m1c.y;
-        scale1 += sqrt(x*x + y*y);
+        x = m2[i].x - m2c.x, y = m2[i].y - m2c.y;
+        scale2 += std::sqrt(x*x + y*y);
     }
 
-    scale0 *= t;
     scale1 *= t;
+    scale2 *= t;
 
-    if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
+    if( scale1 < FLT_EPSILON || scale2 < FLT_EPSILON )
         return 0;
 
-    scale0 = sqrt(2.)/scale0;
-    scale1 = sqrt(2.)/scale1;
+    scale1 = std::sqrt(2.)/scale1;
+    scale2 = std::sqrt(2.)/scale2;
 
-    cvZero( &A );
+    A.setTo(Scalar::all(0));
 
     // form a linear system Ax=0: for each selected pair of points m1 & m2,
     // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
     // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
     for( i = 0; i < count; i++ )
     {
-        double x0 = (m1[i].x - m0c.x)*scale0;
-        double y0 = (m1[i].y - m0c.y)*scale0;
-        double x1 = (m2[i].x - m1c.x)*scale1;
-        double y1 = (m2[i].y - m1c.y)*scale1;
-        double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
+        double x1 = (m1[i].x - m1c.x)*scale1;
+        double y1 = (m1[i].y - m1c.y)*scale1;
+        double x2 = (m2[i].x - m2c.x)*scale2;
+        double y2 = (m2[i].y - m2c.y)*scale2;
+        double r[9] = { x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 };
         for( j = 0; j < 9; j++ )
             for( k = 0; k < 9; k++ )
                 a[j*9+k] += r[j]*r[k];
     }
 
-    cvEigenVV(&A, &V, &W);
+    eigen(A, W, V);
 
     for( i = 0; i < 9; i++ )
     {
@@ -580,122 +556,148 @@ int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatri
     if( i < 8 )
         return 0;
 
-    F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
+    F0 = Mat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
 
     // make F0 singular (of rank 2) by decomposing it with SVD,
     // zeroing the last diagonal element of W and then composing the matrices back.
 
     // use v as a temporary storage for different 3x3 matrices
     W = U = V = TF = F0;
-    W.data.db = v;
-    U.data.db = v + 9;
-    V.data.db = v + 18;
-    TF.data.db = v + 27;
+    W = Mat(3, 1, CV_64F, v);
+    U = Mat(3, 3, CV_64F, v + 9);
+    V = Mat(3, 3, CV_64F, v + 18);
+    TF = Mat(3, 3, CV_64F, v + 27);
 
-    cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
-    W.data.db[8] = 0.;
+    SVDecomp( F0, W, U, V, SVD::MODIFY_A );
+    W.at<double>(2) = 0.;
 
     // F0 <- U*diag([W(1), W(2), 0])*V'
-    cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
-    cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
+    gemm( U, Mat::diag(W), 1., 0, 0., TF, GEMM_1_T );
+    gemm( TF, V, 1., 0, 0., F0, 0/*CV_GEMM_B_T*/ );
 
     // apply the transformation that is inverse
     // to what we used to normalize the point coordinates
-    {
-        double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
-        double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
-        CvMat T0, T1;
-        T0 = T1 = F0;
-        T0.data.db = tt0;
-        T1.data.db = tt1;
-
-        // F0 <- T1'*F0*T0
-        cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
-        F0.data.db = fmatrix;
-        cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
-
-        // make F(3,3) = 1
-        if( fabs(F0.data.db[8]) > FLT_EPSILON )
-            cvScale( &F0, &F0, 1./F0.data.db[8] );
-    }
-
+    double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
+    double tt2[] = { scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 };
+    Mat T1(3, 3, CV_64F, tt1), T2(3, 3, CV_64F, tt2);
+
+    // F0 <- T2'*F0*T1
+    gemm( T2, F0, 1., 0, 0., TF, GEMM_1_T );
+    F0 = Mat(3, 3, CV_64F, fmatrix);
+    gemm( TF, T1, 1., 0, 0., F0, 0 );
+    
+    // make F(3,3) = 1
+    if( fabs(F0.at<double>(2,2)) > FLT_EPSILON )
+        F0 *= 1./F0.at<double>(2,2);
+    
     return 1;
 }
 
 
-void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
-                                        const CvMat* model, CvMat* _err )
+class FMEstimatorCallback : public PointSetRegistrator::Callback
 {
-    int i, count = _m1->rows*_m1->cols;
-    const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
-    const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
-    const double* F = model->data.db;
-    float* err = _err->data.fl;
+public:
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
+    {
+        Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
+        return !haveCollinearPoints(ms1, count) && !haveCollinearPoints(ms2, count);
+    }
 
-    for( i = 0; i < count; i++ )
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    {
+        double f[9*3];
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        int count = m1.checkVector(2);
+        Mat F(count == 7 ? 9 : 3, 3, CV_64F, f);
+        int n = count == 7 ? run7Point(m1, m2, F) : run8Point(m1, m2, F);
+
+        if( n == 0 )
+            _model.release();
+        else
+            F.rowRange(0, n*3).copyTo(_model);
+
+        return n;
+    }
+
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
     {
-        double a, b, c, d1, d2, s1, s2;
+        Mat __m1 = _m1.getMat(), __m2 = _m2.getMat(), __model = _model.getMat();
+        int i, count = __m1.checkVector(2);
+        const Point2f* m1 = __m1.ptr<Point2f>();
+        const Point2f* m2 = __m2.ptr<Point2f>();
+        const double* F = __model.ptr<double>();
+        _err.create(count, 1, CV_32F);
+        float* err = _err.getMat().ptr<float>();
 
-        a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
-        b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
-        c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
+        for( i = 0; i < count; i++ )
+        {
+            double a, b, c, d1, d2, s1, s2;
 
-        s2 = 1./(a*a + b*b);
-        d2 = m2[i].x*a + m2[i].y*b + c;
+            a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
+            b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
+            c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
 
-        a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
-        b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
-        c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
+            s2 = 1./(a*a + b*b);
+            d2 = m2[i].x*a + m2[i].y*b + c;
 
-        s1 = 1./(a*a + b*b);
-        d1 = m1[i].x*a + m1[i].y*b + c;
+            a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
+            b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
+            c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
 
-        err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
+            s1 = 1./(a*a + b*b);
+            d1 = m1[i].x*a + m1[i].y*b + c;
+
+            err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
+        }
     }
-}
+};
 
+}
 
-CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
-                                  CvMat* fmatrix, int method,
-                                  double param1, double param2, CvMat* mask )
+cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
+                                int method, double param1, double param2,
+                                OutputArray _mask )
 {
-    int result = 0;
-    Ptr<CvMat> m1, m2, tempMask;
-
-    double F[3*9];
-    CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
-    int count;
+    Mat points1 = _points1.getMat(), points2 = _points2.getMat();
+    Mat m1, m2, F;
+    int npoints = -1;
 
-    CV_Assert( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
-    CV_Assert( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
-        (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );
+    for( int i = 1; i <= 2; i++ )
+    {
+        Mat& p = i == 1 ? points1 : points2;
+        Mat& m = i == 1 ? m1 : m2;
+        npoints = p.checkVector(2, -1, false);
+        if( npoints < 0 )
+        {
+            npoints = p.checkVector(3, -1, false);
+            if( npoints < 0 )
+                CV_Error(CV_StsBadArg, "The input arrays should be 2D or 3D point sets");
+            if( npoints == 0 )
+                return Mat();
+            convertPointsFromHomogeneous(p, p);
+        }
+        p.reshape(2, npoints).convertTo(m, CV_32F);
+    }
 
-    count = MAX(points1->cols, points1->rows);
-    if( count < 7 )
-        return 0;
+    CV_Assert( m1.checkVector(2) == m2.checkVector(2) );
 
-    m1 = cvCreateMat( 1, count, CV_64FC2 );
-    cvConvertPointsHomogeneous( points1, m1 );
+    if( npoints < 7 )
+        return Mat();
 
-    m2 = cvCreateMat( 1, count, CV_64FC2 );
-    cvConvertPointsHomogeneous( points2, m2 );
+    Ptr<PointSetRegistrator::Callback> cb = new FMEstimatorCallback;
+    int result;
 
-    if( mask )
+    if( npoints == 7 || method == FM_8POINT )
     {
-        CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
-            (mask->rows == 1 || mask->cols == 1) &&
-            mask->rows*mask->cols == count );
+        result = cb->runKernel(m1, m2, F);
+        if( _mask.needed() )
+        {
+            _mask.create(npoints, 1, CV_8U, -1, true);
+            Mat mask = _mask.getMat();
+            CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == npoints );
+            mask.setTo(Scalar::all(1));
+        }
     }
-    if( mask || count >= 8 )
-        tempMask = cvCreateMat( 1, count, CV_8U );
-    if( !tempMask.empty() )
-        cvSet( tempMask, cvScalarAll(1.) );
-
-    CvFMEstimator estimator(7);
-    if( count == 7 )
-        result = estimator.run7Point(m1, m2, &_F9x3);
-    else if( method == CV_FM_8POINT )
-        result = estimator.run8Point(m1, m2, &_F3x3);
     else
     {
         if( param1 <= 0 )
@@ -703,546 +705,279 @@ CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
         if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
             param2 = 0.99;
 
-        if( (method & ~3) == CV_RANSAC && count >= 15 )
-            result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
+        if( (method & ~3) == FM_RANSAC && npoints >= 15 )
+            result = createRANSACPointSetRegistrator(cb, 7, param1, param2)->run(m1, m2, F, _mask);
         else
-            result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
-        if( result <= 0 )
-            return 0;
-        /*icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
-        count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
-        assert( count >= 8 );
-        m1->cols = m2->cols = count;
-        estimator.run8Point(m1, m2, &_F3x3);*/
+            result = createLMeDSPointSetRegistrator(cb, 7, param2)->run(m1, m2, F, _mask);
     }
 
-    if( result )
-        cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
+    if( result <= 0 )
+        return Mat();
 
-    if( mask && tempMask )
-    {
-        if( CV_ARE_SIZES_EQ(mask, tempMask) )
-            cvCopy( tempMask, mask );
-        else
-            cvTranspose( tempMask, mask );
-    }
+    return F;
+}
 
-    return result;
+cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
+                               OutputArray _mask, int method, double param1, double param2 )
+{
+    return cv::findFundamentalMat(_points1, _points2, method, param1, param2, _mask);
 }
 
 
-CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
-                                          const CvMat* fmatrix, CvMat* lines )
+void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
+                                    InputArray _Fmat, OutputArray _lines )
 {
-    int abc_stride, abc_plane_stride, abc_elem_size;
-    int plane_stride, stride, elem_size;
-    int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
-    uchar *ap, *bp, *cp;
-    const uchar *xp, *yp, *zp;
     double f[9];
-    CvMat F = cvMat( 3, 3, CV_64F, f );
-
-    if( !CV_IS_MAT(points) )
-        CV_Error( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
-
-    depth = CV_MAT_DEPTH(points->type);
-    cn = CV_MAT_CN(points->type);
-    if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )
-        CV_Error( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
-
-    if( cn > 1 )
-    {
-        dims = cn;
-        CV_Assert( points->rows == 1 || points->cols == 1 );
-        count = points->rows * points->cols;
-    }
-    else if( points->rows > points->cols )
-    {
-        dims = cn*points->cols;
-        count = points->rows;
-    }
-    else
-    {
-        if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )
-            CV_Error( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
-        dims = points->rows;
-        count = points->cols;
-    }
-
-    if( dims != 2 && dims != 3 )
-        CV_Error( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
-
-    if( !CV_IS_MAT(fmatrix) )
-        CV_Error( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
-
-    if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
-        CV_Error( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
-
-    if( fmatrix->cols != 3 || fmatrix->rows != 3 )
-        CV_Error( CV_StsBadSize, "fundamental matrix must be 3x3" );
-
-    if( !CV_IS_MAT(lines) )
-        CV_Error( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
+    Mat tempF(3, 3, CV_64F, f);
+    Mat points = _points.getMat(), F = _Fmat.getMat();
 
-    abc_depth = CV_MAT_DEPTH(lines->type);
-    abc_cn = CV_MAT_CN(lines->type);
-    if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )
-        CV_Error( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
+    if( !points.isContinuous() )
+        points = points.clone();
 
-    if( abc_cn > 1 )
-    {
-        abc_dims = abc_cn;
-        CV_Assert( lines->rows == 1 || lines->cols == 1 );
-        abc_count = lines->rows * lines->cols;
-    }
-    else if( lines->rows > lines->cols )
-    {
-        abc_dims = abc_cn*lines->cols;
-        abc_count = lines->rows;
-    }
-    else
+    int npoints = points.checkVector(2);
+    if( npoints < 0 )
     {
-        if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )
-            CV_Error( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
-        abc_dims = lines->rows;
-        abc_count = lines->cols;
+        npoints = points.checkVector(3);
+        if( npoints < 0 )
+            CV_Error( CV_StsBadArg, "The input should be a 2D or 3D point set");
+        Mat temp;
+        convertPointsFromHomogeneous(points, temp);
+        points = temp;
     }
+    int depth = points.depth();
+    CV_Assert( depth == CV_32F || depth == CV_32S || depth == CV_64F );
 
-    if( abc_dims != 3 )
-        CV_Error( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
-
-    if( abc_count != count )
-        CV_Error( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
-
-    elem_size = CV_ELEM_SIZE(depth);
-    abc_elem_size = CV_ELEM_SIZE(abc_depth);
+    CV_Assert(F.size() == Size(3,3));
+    F.convertTo(tempF, CV_64F);
+    if( whichImage == 2 )
+        transpose(tempF, tempF);
 
-    if( cn == 1 && points->rows == dims )
+    int ltype = CV_MAKETYPE(MAX(depth, CV_32F), 3);
+    _lines.create(npoints, 1, ltype);
+    Mat lines = _lines.getMat();
+    if( !lines.isContinuous() )
     {
-        plane_stride = points->step;
-        stride = elem_size;
-    }
-    else
-    {
-        plane_stride = elem_size;
-        stride = points->rows == 1 ? dims*elem_size : points->step;
+        _lines.release();
+        _lines.create(npoints, 1, ltype);
+        lines = _lines.getMat();
     }
+    CV_Assert( lines.isContinuous());
 
-    if( abc_cn == 1 && lines->rows == 3 )
+    if( depth == CV_32S || depth == CV_32F )
     {
-        abc_plane_stride = lines->step;
-        abc_stride = abc_elem_size;
+        const Point* ptsi = (const Point*)points.data;
+        const Point2f* ptsf = (const Point2f*)points.data;
+        Point3f* dstf = lines.ptr<Point3f>();
+        for( int i = 0; i < npoints; i++ )
+        {
+            Point2f pt = depth == CV_32F ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
+            double a = f[0]*pt.x + f[1]*pt.y + f[2];
+            double b = f[3]*pt.x + f[4]*pt.y + f[5];
+            double c = f[6]*pt.x + f[7]*pt.y + f[8];
+            double nu = a*a + b*b;
+            nu = nu ? 1./std::sqrt(nu) : 1.;
+            a *= nu; b *= nu; c *= nu;
+            dstf[i] = Point3f((float)a, (float)b, (float)c);
+        }
     }
     else
     {
-        abc_plane_stride = abc_elem_size;
-        abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
-    }
-
-    cvConvert( fmatrix, &F );
-    if( pointImageID == 2 )
-        cvTranspose( &F, &F );
-
-    xp = points->data.ptr;
-    yp = xp + plane_stride;
-    zp = dims == 3 ? yp + plane_stride : 0;
-
-    ap = lines->data.ptr;
-    bp = ap + abc_plane_stride;
-    cp = bp + abc_plane_stride;
-
-    for( i = 0; i < count; i++ )
-    {
-        double x, y, z = 1.;
-        double a, b, c, nu;
-
-        if( depth == CV_32F )
+        const Point2d* ptsd = (const Point2d*)points.data;
+        Point3d* dstd = lines.ptr<Point3d>();
+        for( int i = 0; i < npoints; i++ )
         {
-            x = *(float*)xp; y = *(float*)yp;
-            if( zp )
-                z = *(float*)zp, zp += stride;
+            Point2d pt = ptsd[i];
+            double a = f[0]*pt.x + f[1]*pt.y + f[2];
+            double b = f[3]*pt.x + f[4]*pt.y + f[5];
+            double c = f[6]*pt.x + f[7]*pt.y + f[8];
+            double nu = a*a + b*b;
+            nu = nu ? 1./std::sqrt(nu) : 1.;
+            a *= nu; b *= nu; c *= nu;
+            dstd[i] = Point3d(a, b, c);
         }
-        else
-        {
-            x = *(double*)xp; y = *(double*)yp;
-            if( zp )
-                z = *(double*)zp, zp += stride;
-        }
-
-        xp += stride; yp += stride;
-
-        a = f[0]*x + f[1]*y + f[2]*z;
-        b = f[3]*x + f[4]*y + f[5]*z;
-        c = f[6]*x + f[7]*y + f[8]*z;
-        nu = a*a + b*b;
-        nu = nu ? 1./sqrt(nu) : 1.;
-        a *= nu; b *= nu; c *= nu;
-
-        if( abc_depth == CV_32F )
-        {
-            *(float*)ap = (float)a;
-            *(float*)bp = (float)b;
-            *(float*)cp = (float)c;
-        }
-        else
-        {
-            *(double*)ap = a;
-            *(double*)bp = b;
-            *(double*)cp = c;
-        }
-
-        ap += abc_stride;
-        bp += abc_stride;
-        cp += abc_stride;
     }
 }
 
-
-CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
+void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
 {
-    Ptr<CvMat> temp, denom;
-
-    int i, s_count, s_dims, d_count, d_dims;
-    CvMat _src, _dst, _ones;
-    CvMat* ones = 0;
-
-    if( !CV_IS_MAT(src) )
-        CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg,
-        "The input parameter is not a valid matrix" );
-
-    if( !CV_IS_MAT(dst) )
-        CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
-        "The output parameter is not a valid matrix" );
-
-    if( src == dst || src->data.ptr == dst->data.ptr )
-    {
-        if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
-            CV_Error( CV_StsBadArg, "Invalid inplace operation" );
-        return;
-    }
-
-    if( src->rows > src->cols )
-    {
-        if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
-            CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
-
-        s_dims = CV_MAT_CN(src->type)*src->cols;
-        s_count = src->rows;
-    }
-    else
+    Mat src = _src.getMat();
+    if( !src.isContinuous() )
+        src = src.clone();
+    int i, npoints = src.checkVector(3), depth = src.depth(), cn = 3;
+    if( npoints < 0 )
     {
-        if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
-            CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
-
-        s_dims = CV_MAT_CN(src->type)*src->rows;
-        s_count = src->cols;
+        npoints = src.checkVector(4);
+        CV_Assert(npoints >= 0);
+        cn = 4;
     }
+    CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F));
 
-    if( src->rows == 1 || src->cols == 1 )
-        src = cvReshape( src, &_src, 1, s_count );
-
-    if( dst->rows > dst->cols )
-    {
-        if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
-            CV_Error( CV_StsBadSize,
-            "Either the number of channels or columns or rows in the input matrix must be =1" );
-
-        d_dims = CV_MAT_CN(dst->type)*dst->cols;
-        d_count = dst->rows;
-    }
-    else
+    int dtype = CV_MAKETYPE(depth <= CV_32F ? CV_32F : CV_64F, cn-1);
+    _dst.create(npoints, 1, dtype);
+    Mat dst = _dst.getMat();
+    if( !dst.isContinuous() )
     {
-        if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
-            CV_Error( CV_StsBadSize,
-            "Either the number of channels or columns or rows in the output matrix must be =1" );
-
-        d_dims = CV_MAT_CN(dst->type)*dst->rows;
-        d_count = dst->cols;
+        _dst.release();
+        _dst.create(npoints, 1, dtype);
+        dst = _dst.getMat();
     }
+    CV_Assert( dst.isContinuous() );
 
-    if( dst->rows == 1 || dst->cols == 1 )
-        dst = cvReshape( dst, &_dst, 1, d_count );
-
-    if( s_count != d_count )
-        CV_Error( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
-
-    if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
-        CV_Error( CV_StsUnsupportedFormat,
-        "Both matrices must be floating-point (single or double precision)" );
-
-    if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
-        CV_Error( CV_StsOutOfRange,
-        "Both input and output point dimensionality must be 2, 3 or 4" );
-
-    if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
-        CV_Error( CV_StsUnmatchedSizes,
-        "The dimensionalities of input and output point sets differ too much" );
-
-    if( s_dims == d_dims - 1 )
+    if( depth == CV_32S )
     {
-        if( d_count == dst->rows )
+        if( cn == 3 )
         {
-            ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
-            dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
+            const Point3i* sptr = (const Point3i*)src.data;
+            Point2f* dptr = (Point2f*)dst.data;
+            for( i = 0; i < npoints; i++ )
+            {
+                float scale = sptr[i].z != 0 ? 1.f/sptr[i].z : 1.f;
+                dptr[i] = Point2f(sptr[i].x*scale, sptr[i].y*scale);
+            }
         }
         else
         {
-            ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
-            dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
+            const Vec4i* sptr = (const Vec4i*)src.data;
+            Point3f* dptr = (Point3f*)dst.data;
+            for( i = 0; i < npoints; i++ )
+            {
+                float scale = sptr[i][3] != 0 ? 1.f/sptr[i][3] : 1.f;
+                dptr[i] = Point3f(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale);
+            }
         }
     }
-
-    if( s_dims <= d_dims )
+    else if( depth == CV_32F )
     {
-        if( src->rows == dst->rows && src->cols == dst->cols )
+        if( cn == 3 )
         {
-            if( CV_ARE_TYPES_EQ( src, dst ) )
-                cvCopy( src, dst );
-            else
-                cvConvert( src, dst );
+            const Point3f* sptr = (const Point3f*)src.data;
+            Point2f* dptr = (Point2f*)dst.data;
+            for( i = 0; i < npoints; i++ )
+            {
+                float scale = sptr[i].z != 0.f ? 1.f/sptr[i].z : 1.f;
+                dptr[i] = Point2f(sptr[i].x*scale, sptr[i].y*scale);
+            }
         }
         else
         {
-            if( !CV_ARE_TYPES_EQ( src, dst ))
+            const Vec4f* sptr = (const Vec4f*)src.data;
+            Point3f* dptr = (Point3f*)dst.data;
+            for( i = 0; i < npoints; i++ )
             {
-                temp = cvCreateMat( src->rows, src->cols, dst->type );
-                cvConvert( src, temp );
-                src = temp;
+                float scale = sptr[i][3] != 0.f ? 1.f/sptr[i][3] : 1.f;
+                dptr[i] = Point3f(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale);
             }
-            cvTranspose( src, dst );
         }
-
-        if( ones )
-            cvSet( ones, cvRealScalar(1.) );
     }
-    else
+    else if( depth == CV_64F )
     {
-        int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
-
-        if( !CV_ARE_TYPES_EQ( src, dst ))
-        {
-            temp = cvCreateMat( src->rows, src->cols, dst->type );
-            cvConvert( src, temp );
-            src = temp;
-        }
-
-        elem_size = CV_ELEM_SIZE(src->type);
-
-        if( s_count == src->cols )
-            s_plane_stride = src->step / elem_size, s_stride = 1;
-        else
-            s_stride = src->step / elem_size, s_plane_stride = 1;
-
-        if( d_count == dst->cols )
-            d_plane_stride = dst->step / elem_size, d_stride = 1;
-        else
-            d_stride = dst->step / elem_size, d_plane_stride = 1;
-
-        denom = cvCreateMat( 1, d_count, dst->type );
-
-        if( CV_MAT_DEPTH(dst->type) == CV_32F )
+        if( cn == 3 )
         {
-            const float* xs = src->data.fl;
-            const float* ys = xs + s_plane_stride;
-            const float* zs = 0;
-            const float* ws = xs + (s_dims - 1)*s_plane_stride;
-
-            float* iw = denom->data.fl;
-
-            float* xd = dst->data.fl;
-            float* yd = xd + d_plane_stride;
-            float* zd = 0;
-
-            if( d_dims == 3 )
+            const Point3d* sptr = (const Point3d*)src.data;
+            Point2d* dptr = (Point2d*)dst.data;
+            for( i = 0; i < npoints; i++ )
             {
-                zs = ys + s_plane_stride;
-                zd = yd + d_plane_stride;
+                double scale = sptr[i].z != 0. ? 1./sptr[i].z : 1.;
+                dptr[i] = Point2d(sptr[i].x*scale, sptr[i].y*scale);
             }
-
-            for( i = 0; i < d_count; i++, ws += s_stride )
-            {
-                float t = *ws;
-                iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;
-            }
-
-            cvDiv( 0, denom, denom );
-
-            if( d_dims == 3 )
-                for( i = 0; i < d_count; i++ )
-                {
-                    float w = iw[i];
-                    float x = *xs * w, y = *ys * w, z = *zs * w;
-                    xs += s_stride; ys += s_stride; zs += s_stride;
-                    *xd = x; *yd = y; *zd = z;
-                    xd += d_stride; yd += d_stride; zd += d_stride;
-                }
-            else
-                for( i = 0; i < d_count; i++ )
-                {
-                    float w = iw[i];
-                    float x = *xs * w, y = *ys * w;
-                    xs += s_stride; ys += s_stride;
-                    *xd = x; *yd = y;
-                    xd += d_stride; yd += d_stride;
-                }
         }
         else
         {
-            const double* xs = src->data.db;
-            const double* ys = xs + s_plane_stride;
-            const double* zs = 0;
-            const double* ws = xs + (s_dims - 1)*s_plane_stride;
-
-            double* iw = denom->data.db;
-
-            double* xd = dst->data.db;
-            double* yd = xd + d_plane_stride;
-            double* zd = 0;
-
-            if( d_dims == 3 )
+            const Vec4d* sptr = (const Vec4d*)src.data;
+            Point3d* dptr = (Point3d*)dst.data;
+            for( i = 0; i < npoints; i++ )
             {
-                zs = ys + s_plane_stride;
-                zd = yd + d_plane_stride;
+                double scale = sptr[i][3] != 0.f ? 1./sptr[i][3] : 1.;
+                dptr[i] = Point3d(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale);
             }
-
-            for( i = 0; i < d_count; i++, ws += s_stride )
-            {
-                double t = *ws;
-                iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;
-            }
-
-            cvDiv( 0, denom, denom );
-
-            if( d_dims == 3 )
-                for( i = 0; i < d_count; i++ )
-                {
-                    double w = iw[i];
-                    double x = *xs * w, y = *ys * w, z = *zs * w;
-                    xs += s_stride; ys += s_stride; zs += s_stride;
-                    *xd = x; *yd = y; *zd = z;
-                    xd += d_stride; yd += d_stride; zd += d_stride;
-                }
-            else
-                for( i = 0; i < d_count; i++ )
-                {
-                    double w = iw[i];
-                    double x = *xs * w, y = *ys * w;
-                    xs += s_stride; ys += s_stride;
-                    *xd = x; *yd = y;
-                    xd += d_stride; yd += d_stride;
-                }
         }
     }
+    else
+        CV_Error(CV_StsUnsupportedFormat, "");
 }
 
-cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
-                            int method, double ransacReprojThreshold, OutputArray _mask )
+
+void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
 {
-    Mat points1 = _points1.getMat(), points2 = _points2.getMat();
-    int npoints = points1.checkVector(2);
-    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
-               points1.type() == points2.type());
-
-    Mat H(3, 3, CV_64F);
-    CvMat _pt1 = points1, _pt2 = points2;
-    CvMat matH = H, c_mask, *p_mask = 0;
-    if( _mask.needed() )
+    Mat src = _src.getMat();
+    if( !src.isContinuous() )
+        src = src.clone();
+    int i, npoints = src.checkVector(2), depth = src.depth(), cn = 2;
+    if( npoints < 0 )
     {
-        _mask.create(npoints, 1, CV_8U, -1, true);
-        p_mask = &(c_mask = _mask.getMat());
+        npoints = src.checkVector(3);
+        CV_Assert(npoints >= 0);
+        cn = 3;
     }
-    bool ok = cvFindHomography( &_pt1, &_pt2, &matH, method, ransacReprojThreshold, p_mask ) > 0;
-    if( !ok )
-        H = Scalar(0);
-    return H;
-}
-
-cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
-                            OutputArray _mask, int method, double ransacReprojThreshold )
-{
-    return cv::findHomography(_points1, _points2, method, ransacReprojThreshold, _mask);
-}
+    CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F));
 
-cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
-                               int method, double param1, double param2,
-                               OutputArray _mask )
-{
-    Mat points1 = _points1.getMat(), points2 = _points2.getMat();
-    int npoints = points1.checkVector(2);
-    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
-              points1.type() == points2.type());
-
-    Mat F(method == CV_FM_7POINT ? 9 : 3, 3, CV_64F);
-    CvMat _pt1 = points1, _pt2 = points2;
-    CvMat matF = F, c_mask, *p_mask = 0;
-    if( _mask.needed() )
+    int dtype = CV_MAKETYPE(depth <= CV_32F ? CV_32F : CV_64F, cn+1);
+    _dst.create(npoints, 1, dtype);
+    Mat dst = _dst.getMat();
+    if( !dst.isContinuous() )
     {
-        _mask.create(npoints, 1, CV_8U, -1, true);
-        p_mask = &(c_mask = _mask.getMat());
+        _dst.release();
+        _dst.create(npoints, 1, dtype);
+        dst = _dst.getMat();
     }
-    int n = cvFindFundamentalMat( &_pt1, &_pt2, &matF, method, param1, param2, p_mask );
-    if( n <= 0 )
-        F = Scalar(0);
-    if( n == 1 )
-        F = F.rowRange(0, 3);
-    return F;
-}
-
-cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
-                                OutputArray _mask, int method, double param1, double param2 )
-{
-    return cv::findFundamentalMat(_points1, _points2, method, param1, param2, _mask);
-}
-
+    CV_Assert( dst.isContinuous() );
 
-void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
-                                    InputArray _Fmat, OutputArray _lines )
-{
-    Mat points = _points.getMat(), F = _Fmat.getMat();
-    int npoints = points.checkVector(2);
-    if( npoints < 0 )
-        npoints = points.checkVector(3);
-    CV_Assert( npoints >= 0 && (points.depth() == CV_32F || points.depth() == CV_32S));
-
-    _lines.create(npoints, 1, CV_32FC3, -1, true);
-    CvMat c_points = points, c_lines = _lines.getMat(), c_F = F;
-    cvComputeCorrespondEpilines(&c_points, whichImage, &c_F, &c_lines);
-}
-
-void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
-{
-    Mat src = _src.getMat();
-    int npoints = src.checkVector(3), cn = 3;
-    if( npoints < 0 )
+    if( depth == CV_32S )
     {
-        npoints = src.checkVector(4);
-        if( npoints >= 0 )
-            cn = 4;
+        if( cn == 2 )
+        {
+            const Point2i* sptr = (const Point2i*)src.data;
+            Point3i* dptr = (Point3i*)dst.data;
+            for( i = 0; i < npoints; i++ )
+                dptr[i] = Point3i(sptr[i].x, sptr[i].y, 1);
+        }
+        else
+        {
+            const Point3i* sptr = (const Point3i*)src.data;
+            Vec4i* dptr = (Vec4i*)dst.data;
+            for( i = 0; i < npoints; i++ )
+                dptr[i] = Vec4i(sptr[i].x, sptr[i].y, sptr[i].z, 1);
+        }
     }
-    CV_Assert( npoints >= 0 && (src.depth() == CV_32F || src.depth() == CV_32S));
-
-    _dst.create(npoints, 1, CV_MAKETYPE(CV_32F, cn-1));
-    CvMat c_src = src, c_dst = _dst.getMat();
-    cvConvertPointsHomogeneous(&c_src, &c_dst);
-}
-
-void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
-{
-    Mat src = _src.getMat();
-    int npoints = src.checkVector(2), cn = 2;
-    if( npoints < 0 )
+    else if( depth == CV_32F )
     {
-        npoints = src.checkVector(3);
-        if( npoints >= 0 )
-            cn = 3;
+        if( cn == 2 )
+        {
+            const Point2f* sptr = (const Point2f*)src.data;
+            Point3f* dptr = (Point3f*)dst.data;
+            for( i = 0; i < npoints; i++ )
+                dptr[i] = Point3f(sptr[i].x, sptr[i].y, 1.f);
+        }
+        else
+        {
+            const Point3f* sptr = (const Point3f*)src.data;
+            Vec4f* dptr = (Vec4f*)dst.data;
+            for( i = 0; i < npoints; i++ )
+                dptr[i] = Vec4f(sptr[i].x, sptr[i].y, sptr[i].z, 1.f);
+        }
     }
-    CV_Assert( npoints >= 0 && (src.depth() == CV_32F || src.depth() == CV_32S));
-
-    _dst.create(npoints, 1, CV_MAKETYPE(CV_32F, cn+1));
-    CvMat c_src = src, c_dst = _dst.getMat();
-    cvConvertPointsHomogeneous(&c_src, &c_dst);
+    else if( depth == CV_64F )
+    {
+        if( cn == 2 )
+        {
+            const Point2d* sptr = (const Point2d*)src.data;
+            Point3d* dptr = (Point3d*)dst.data;
+            for( i = 0; i < npoints; i++ )
+                dptr[i] = Point3d(sptr[i].x, sptr[i].y, 1.);
+        }
+        else
+        {
+            const Point3d* sptr = (const Point3d*)src.data;
+            Vec4d* dptr = (Vec4d*)dst.data;
+            for( i = 0; i < npoints; i++ )
+                dptr[i] = Vec4d(sptr[i].x, sptr[i].y, sptr[i].z, 1.);
+        }
+    }
+    else
+        CV_Error(CV_StsUnsupportedFormat, "");
 }
 
+
 void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst )
 {
     int stype = _src.type(), dtype = _dst.type();

modules/calib3d/src/levmarq.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "precomp.hpp"
+#include <stdio.h>
+
+/*
+   This is translation to C++ of the Matlab's LMSolve package by Miroslav Balda.
+   Here is the original copyright:
+   ============================================================================
+   
+   Copyright (c) 2007, Miroslav Balda
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without 
+   modification, are permitted provided that the following conditions are 
+   met:
+
+       * Redistributions of source code must retain the above copyright 
+         notice, this list of conditions and the following disclaimer.
+       * Redistributions in binary form must reproduce the above copyright 
+         notice, this list of conditions and the following disclaimer in 
+         the documentation and/or other materials provided with the distribution
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+
+namespace cv
+{
+
+class LMSolverImpl : public LMSolver
+{
+public:
+    LMSolverImpl() : maxIters(100) { init(); };
+    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); }
+
+    void init()
+    {
+        epsx = epsf = FLT_EPSILON;
+        printInterval = 0;
+    }
+
+    int run(InputOutputArray _param0) const
+    {
+        Mat param0 = _param0.getMat(), x, xd, r, rd, J, A, Ap, v, temp_d, d;
+        int ptype = param0.type();
+
+        CV_Assert( (param0.cols == 1 || param0.rows == 1) && (ptype == CV_32F || ptype == CV_64F));
+        CV_Assert( !cb.empty() );
+
+        int lx = param0.rows + param0.cols - 1;
+        param0.convertTo(x, CV_64F);
+
+        if( x.cols != 1 )
+            transpose(x, x);
+
+        if( !cb->compute(x, r, J) )
+            return -1;
+        double S = norm(r, NORM_L2SQR);
+        int nfJ = 2;
+
+        mulTransposed(J, A, true);
+        gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
+
+        Mat D = A.diag().clone();
+        
+        const double Rlo = 0.25, Rhi = 0.75;
+        double lambda = 1, lc = 0.75;
+        int i, iter = 0;
+
+        if( printInterval != 0 )
+        {
+            printf("************************************************************************************\n");
+            printf("\titr\tnfJ\t\tSUM(r^2)\t\tx\t\tdx\t\tl\t\tlc\n");
+            printf("************************************************************************************\n");
+        }
+
+        for( ;; )
+        {
+            CV_Assert( A.type() == CV_64F && A.rows == lx );
+            A.copyTo(Ap);
+            for( i = 0; i < lx; i++ )
+                Ap.at<double>(i, i) += lambda*D.at<double>(i);
+            solve(Ap, v, d, DECOMP_EIG);
+            subtract(x, d, xd);
+            if( !cb->compute(xd, rd, noArray()) )
+                return -1;
+            nfJ++;
+            double Sd = norm(rd, NORM_L2SQR);
+            gemm(A, d, -1, v, 2, temp_d);
+            double dS = d.dot(temp_d);
+            double R = (S - Sd)/(fabs(dS) > DBL_EPSILON ? dS : 1);
+
+            if( R > Rhi )
+            {
+                lambda *= 0.5;
+                if( lambda < lc )
+                    lambda = 0;
+            }
+            else if( R < Rlo )
+            {
+                // find new nu if R too low
+                double t = d.dot(v);
+                double nu = (Sd - S)/(fabs(t) > DBL_EPSILON ? t : 1) + 2;
+                nu = std::min(std::max(nu, 2.), 10.);
+                if( lambda == 0 )
+                {
+                    invert(A, Ap, DECOMP_EIG);
+                    double maxval = DBL_EPSILON;
+                    for( i = 0; i < lx; i++ )
+                        maxval = std::max(maxval, std::abs(Ap.at<double>(i,i)));
+                    lambda = lc = 1./maxval;
+                    nu *= 0.5;
+                }
+                lambda *= nu;
+            }
+
+            if( Sd < S )
+            {
+                nfJ++;
+                S = Sd;
+                std::swap(x, xd);
+                if( !cb->compute(x, r, J) )
+                    return -1;
+                mulTransposed(J, A, true);
+                gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
+            }
+
+            iter++;
+            bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && norm(r, NORM_INF) >= epsf;
+
+            if( printInterval != 0 && (iter % printInterval == 0 || iter == 1 || !proceed) )
+            {
+                printf("%c%10d %10d %15.4e %16.4e %17.4e %16.4e %17.4e\n",
+                       (proceed ? ' ' : '*'), iter, nfJ, S, x.at<double>(0), d.at<double>(0), lambda, lc);
+            }
+
+            if(!proceed)
+                break;
+        }
+
+        if( param0.size != x.size )
+            transpose(x, x);
+
+        x.convertTo(param0, ptype);
+        if( iter == maxIters )
+            iter = -iter;
+
+        return iter;
+    }
+
+    void setCallback(const Ptr<LMSolver::Callback>& _cb) { cb = _cb; }
+
+    AlgorithmInfo* info() const;
+
+    Ptr<LMSolver::Callback> cb;
+
+    double epsx;
+    double epsf;
+    int maxIters;
+    int printInterval;
+};
+
+
+CV_INIT_ALGORITHM(LMSolverImpl, "LMSolver",
+                  obj.info()->addParam(obj, "epsx", obj.epsx);
+                  obj.info()->addParam(obj, "epsf", obj.epsf);
+                  obj.info()->addParam(obj, "maxIters", obj.maxIters);
+                  obj.info()->addParam(obj, "printInterval", obj.printInterval));
+
+Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters)
+{
+    CV_Assert( !LMSolverImpl_info_auto.name().empty() );
+    return new LMSolverImpl(cb, maxIters);
+}
+    
+}

modules/calib3d/src/modelest.cpp

-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                        Intel License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2000, Intel Corporation, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's in binary form must reproduce the above copyright notice,
-//     this list of conditions and the following disclaimer in the documentation
-//     and/or other materials provided with the distribution.
-//
-//   * The name of Intel Corporation may not be used to endorse or promote products
-//     derived from this software without specific prior written permission.
-//
-// This software is provided by the copyright holders and contributors "as is" and
-// any express or implied warranties, including, but not limited to, the implied
-// warranties of merchantability and fitness for a particular purpose are disclaimed.
-// In no event shall the Intel Corporation or contributors be liable for any direct,
-// indirect, incidental, special, exemplary, or consequential damages
-// (including, but not limited to, procurement of substitute goods or services;
-// loss of use, data, or profits; or business interruption) however caused
-// and on any theory of liability, whether in contract, strict liability,
-// or tort (including negligence or otherwise) arising in any way out of
-// the use of this software, even if advised of the possibility of such damage.
-//
-//M*/
-
-#include "precomp.hpp"
-#include "_modelest.h"
-#include <algorithm>
-#include <iterator>
-#include <limits>
-
-CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
-{
-    modelPoints = _modelPoints;
-    modelSize = _modelSize;
-    maxBasicSolutions = _maxBasicSolutions;
-    checkPartialSubsets = true;
-    rng = cvRNG(-1);
-}
-
-CvModelEstimator2::~CvModelEstimator2()
-{
-}
-
-void CvModelEstimator2::setSeed( int64 seed )
-{
-    rng = cvRNG(seed);
-}
-
-
-int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
-                                    const CvMat* model, CvMat* _err,
-                                    CvMat* _mask, double threshold )
-{
-    int i, count = _err->rows*_err->cols, goodCount = 0;
-    const float* err = _err->data.fl;
-    uchar* mask = _mask->data.ptr;
-
-    computeReprojError( m1, m2, model, _err );
-    threshold *= threshold;
-    for( i = 0; i < count; i++ )
-        goodCount += mask[i] = err[i] <= threshold;
-    return goodCount;
-}
-
-
-CV_IMPL int
-cvRANSACUpdateNumIters( double p, double ep,
-                        int model_points, int max_iters )
-{
-    if( model_points <= 0 )
-        CV_Error( CV_StsOutOfRange, "the number of model points should be positive" );
-
-    p = MAX(p, 0.);
-    p = MIN(p, 1.);
-    ep = MAX(ep, 0.);
-    ep = MIN(ep, 1.);
-
-    // avoid inf's & nan's
-    double num = MAX(1. - p, DBL_MIN);
-    double denom = 1. - pow(1. - ep,model_points);
-    if( denom < DBL_MIN )
-        return 0;
-
-    num = log(num);
-    denom = log(denom);
-
-    return denom >= 0 || -num >= max_iters*(-denom) ?
-        max_iters : cvRound(num/denom);
-}
-
-bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
-                                    CvMat* mask0, double reprojThreshold,
-                                    double confidence, int maxIters )
-{
-    bool result = false;
-    cv::Ptr<CvMat> mask = cvCloneMat(mask0);
-    cv::Ptr<CvMat> models, err, tmask;
-    cv::Ptr<CvMat> ms1, ms2;
-
-    int iter, niters = maxIters;
-    int count = m1->rows*m1->cols, maxGoodCount = 0;
-    CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
-
-    if( count < modelPoints )
-        return false;
-
-    models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
-    err = cvCreateMat( 1, count, CV_32FC1 );
-    tmask = cvCreateMat( 1, count, CV_8UC1 );
-
-    if( count > modelPoints )
-    {
-        ms1 = cvCreateMat( 1, modelPoints, m1->type );
-        ms2 = cvCreateMat( 1, modelPoints, m2->type );
-    }
-    else
-    {
-        niters = 1;
-        ms1 = cvCloneMat(m1);
-        ms2 = cvCloneMat(m2);
-    }
-
-    for( iter = 0; iter < niters; iter++ )
-    {
-        int i, goodCount, nmodels;
-        if( count > modelPoints )
-        {
-            bool found = getSubset( m1, m2, ms1, ms2, 300 );
-            if( !found )
-            {
-                if( iter == 0 )
-                    return false;
-                break;
-            }
-
-            // Here we check for model specific geometrical 
-            // constraints that allow to avoid "runKernel" 
-            // and not checking for inliers if not fulfilled. 
-            //
-            // The usefullness of this constraint for homographies is explained in the paper: 
-            //
-            // "Speeding-up homography estimation in mobile devices"
-            // Journal of Real-Time Image Processing. 2013. DOI: 10.1007/s11554-012-0314-1
-            // Pablo Márquez-Neila, Javier López-Alberca, José M. Buenaposada, Luis Baumela
-            if ( !isMinimalSetConsistent( ms1, ms2 ) )
-                continue;
-        }
-
-        nmodels = runKernel( ms1, ms2, models );
-        if( nmodels <= 0 )
-            continue;
-        for( i = 0; i < nmodels; i++ )
-        {
-            CvMat model_i;
-            cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
-            goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
-
-            if( goodCount > MAX(maxGoodCount, modelPoints-1) )
-            {
-                std::swap(tmask, mask);
-                cvCopy( &model_i, model );
-                maxGoodCount = goodCount;
-                niters = cvRANSACUpdateNumIters( confidence,
-                    (double)(count - goodCount)/count, modelPoints, niters );
-            }
-        }
-    }
-
-    if( maxGoodCount > 0 )
-    {
-        if( mask != mask0 )
-            cvCopy( mask, mask0 );
-        result = true;
-    }
-
-    return result;
-}
-
-
-static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
-
-bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
-                                  CvMat* mask, double confidence, int maxIters )
-{
-    const double outlierRatio = 0.45;
-    bool result = false;
-    cv::Ptr<CvMat> models;
-    cv::Ptr<CvMat> ms1, ms2;
-    cv::Ptr<CvMat> err;
-
-    int iter, niters = maxIters;
-    int count = m1->rows*m1->cols;
-    double minMedian = DBL_MAX, sigma;
-
-    CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
-
-    if( count < modelPoints )
-        return false;
-
-    models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
-    err = cvCreateMat( 1, count, CV_32FC1 );
-
-    if( count > modelPoints )
-    {
-        ms1 = cvCreateMat( 1, modelPoints, m1->type );
-        ms2 = cvCreateMat( 1, modelPoints, m2->type );
-    }
-    else
-    {
-        niters = 1;
-        ms1 = cvCloneMat(m1);
-        ms2 = cvCloneMat(m2);
-    }
-
-    niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
-    niters = MIN( MAX(niters, 3), maxIters );
-
-    for( iter = 0; iter < niters; iter++ )
-    {
-        int i, nmodels;
-        if( count > modelPoints )
-        {
-            bool found = getSubset( m1, m2, ms1, ms2, 300 );
-            if( !found )
-            {
-                if( iter == 0 )
-                    return false;
-                break;
-            }
-        }
-
-        nmodels = runKernel( ms1, ms2, models );
-        if( nmodels <= 0 )
-            continue;
-        for( i = 0; i < nmodels; i++ )
-        {
-            CvMat model_i;
-            cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
-            computeReprojError( m1, m2, &model_i, err );
-            icvSortDistances( err->data.i, count, 0 );
-
-            double median = count % 2 != 0 ?
-                err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
-
-            if( median < minMedian )
-            {
-                minMedian = median;
-                cvCopy( &model_i, model );
-            }
-        }
-    }
-
-    if( minMedian < DBL_MAX )
-    {
-        sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
-        sigma = MAX( sigma, 0.001 );
-
-        count = findInliers( m1, m2, model, err, mask, sigma );
-        result = count >= modelPoints;
-    }
-
-    return result;
-}
-
-
-bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
-                                   CvMat* ms1, CvMat* ms2, int maxAttempts )
-{
-    cv::AutoBuffer<int> _idx(modelPoints);
-    int* idx = _idx;
-    int i = 0, j, k, idx_i, iters = 0;
-    int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
-    const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
-    int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
-    int count = m1->cols*m1->rows;
-
-    assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
-    elemSize /= sizeof(int);
-
-    for(; iters < maxAttempts; iters++)
-    {
-        for( i = 0; i < modelPoints && iters < maxAttempts; )
-        {
-            idx[i] = idx_i = cvRandInt(&rng) % count;
-            for( j = 0; j < i; j++ )
-                if( idx_i == idx[j] )
-                    break;
-            if( j < i )
-                continue;
-            for( k = 0; k < elemSize; k++ )
-            {
-                ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
-                ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
-            }
-            if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
-            {
-                iters++;
-                continue;
-            }
-            i++;
-        }
-        if( !checkPartialSubsets && i == modelPoints &&
-            (!checkSubset( ms1, i ) || !checkSubset( ms2, i )))
-            continue;
-        break;
-    }
-
-    return i == modelPoints && iters < maxAttempts;
-}
-
-
-bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
-{
-    if( count <= 2 )
-        return true;
-
-    int j, k, i, i0, i1;
-    CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
-
-    assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
-
-    if( checkPartialSubsets )
-        i0 = i1 = count - 1;
-    else
-        i0 = 0, i1 = count - 1;
-
-    for( i = i0; i <= i1; i++ )
-    {
-        // check that the i-th selected point does not belong
-        // to a line connecting some previously selected points
-        for( j = 0; j < i; j++ )
-        {
-            double dx1 = ptr[j].x - ptr[i].x;
-            double dy1 = ptr[j].y - ptr[i].y;
-            for( k = 0; k < j; k++ )
-            {
-                double dx2 = ptr[k].x - ptr[i].x;
-                double dy2 = ptr[k].y - ptr[i].y;
-                if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
-                    break;
-            }
-            if( k < j )
-                break;
-        }
-        if( j < i )
-            break;
-    }
-
-    return i > i1;
-}
-
-
-namespace cv
-{
-
-class Affine3DEstimator : public CvModelEstimator2
-{
-public:
-    Affine3DEstimator() : CvModelEstimator2(4, cvSize(4, 3), 1) {}
-    virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
-protected:
-    virtual void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error );
-    virtual bool checkSubset( const CvMat* ms1, int count );
-};
-
-}
-
-int cv::Affine3DEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
-{
-    const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
-    const Point3d* to   = reinterpret_cast<const Point3d*>(m2->data.ptr);
-
-    Mat A(12, 12, CV_64F);
-    Mat B(12, 1, CV_64F);
-    A = Scalar(0.0);
-
-    for(int i = 0; i < modelPoints; ++i)
-    {
-        *B.ptr<Point3d>(3*i) = to[i];
-
-        double *aptr = A.ptr<double>(3*i);
-        for(int k = 0; k < 3; ++k)
-        {
-            aptr[3] = 1.0;
-            *reinterpret_cast<Point3d*>(aptr) = from[i];
-            aptr += 16;
-        }
-    }
-
-    CvMat cvA = A;
-    CvMat cvB = B;
-    CvMat cvX;
-    cvReshape(model, &cvX, 1, 12);
-    cvSolve(&cvA, &cvB, &cvX, CV_SVD );
-
-    return 1;
-}
-
-void cv::Affine3DEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error )
-{
-    int count = m1->rows * m1->cols;
-    const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
-    const Point3d* to   = reinterpret_cast<const Point3d*>(m2->data.ptr);
-    const double* F = model->data.db;
-    float* err = error->data.fl;
-
-    for(int i = 0; i < count; i++ )
-    {
-        const Point3d& f = from[i];
-        const Point3d& t = to[i];
-
-        double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
-        double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
-        double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
-
-        err[i] = (float)std::sqrt(a*a + b*b + c*c);
-    }
-}
-
-bool cv::Affine3DEstimator::checkSubset( const CvMat* ms1, int count )
-{
-    CV_Assert( CV_MAT_TYPE(ms1->type) == CV_64FC3 );
-
-    int j, k, i = count - 1;
-    const Point3d* ptr = reinterpret_cast<const Point3d*>(ms1->data.ptr);
-
-    // check that the i-th selected point does not belong
-    // to a line connecting some previously selected points
-
-    for(j = 0; j < i; ++j)
-    {
-        Point3d d1 = ptr[j] - ptr[i];
-        double n1 = norm(d1);
-
-        for(k = 0; k < j; ++k)
-        {
-            Point3d d2 = ptr[k] - ptr[i];
-            double n = norm(d2) * n1;
-
-            if (fabs(d1.dot(d2) / n) > 0.996)
-                break;
-        }
-        if( k < j )
-            break;
-    }
-
-    return j == i;
-}
-
-int cv::estimateAffine3D(InputArray _from, InputArray _to,
-                         OutputArray _out, OutputArray _inliers,
-                         double param1, double param2)
-{
-    Mat from = _from.getMat(), to = _to.getMat();
-    int count = from.checkVector(3);
-
-    CV_Assert( count >= 0 && to.checkVector(3) == count );
-
-    _out.create(3, 4, CV_64F);
-    Mat out = _out.getMat();
-
-    Mat inliers(1, count, CV_8U);
-    inliers = Scalar::all(1);
-
-    Mat dFrom, dTo;
-    from.convertTo(dFrom, CV_64F);
-    to.convertTo(dTo, CV_64F);
-    dFrom = dFrom.reshape(3, 1);
-    dTo = dTo.reshape(3, 1);
-
-    CvMat F3x4 = out;
-    CvMat mask = inliers;
-    CvMat m1 = dFrom;
-    CvMat m2 = dTo;
-
-    const double epsilon = std::numeric_limits<double>::epsilon();
-    param1 = param1 <= 0 ? 3 : param1;
-    param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2;
-
-    int ok = Affine3DEstimator().runRANSAC(&m1, &m2, &F3x4, &mask, param1, param2 );
-    if( _inliers.needed() )
-        transpose(inliers, _inliers);
-
-    return ok;
-}

modules/calib3d/src/precomp.hpp

@@ -59,4 +59,51 @@
 #define GET_OPTIMIZED(func) (func)
 #endif
 
+
+namespace cv
+{
+
+int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters );
+
+class CV_EXPORTS LMSolver : public Algorithm
+{
+public:
+    class CV_EXPORTS Callback
+    {
+    public:
+        virtual ~Callback() {}
+        virtual bool compute(InputArray param, OutputArray err, OutputArray J) const = 0;
+    };
+
+    virtual void setCallback(const Ptr<LMSolver::Callback>& cb) = 0;
+    virtual int run(InputOutputArray _param0) const = 0;
+};
+
+CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters);
+
+class CV_EXPORTS PointSetRegistrator : public Algorithm
+{
+public:
+    class CV_EXPORTS Callback
+    {
+    public:
+        virtual ~Callback() {}
+        virtual int runKernel(InputArray m1, InputArray m2, OutputArray model) const = 0;
+        virtual void computeError(InputArray m1, InputArray m2, InputArray model, OutputArray err) const = 0;
+        virtual bool checkSubset(InputArray, InputArray, int) const { return true; }
+    };
+
+    virtual void setCallback(const Ptr<PointSetRegistrator::Callback>& cb) = 0;
+    virtual bool run(InputArray m1, InputArray m2, OutputArray model, OutputArray mask) const = 0;
+};
+
+CV_EXPORTS Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
+                                                                    int modelPoints, double threshold,
+                                                                    double confidence=0.99, int maxIters=1000 );
+
+CV_EXPORTS Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
+                                                                   int modelPoints, double confidence=0.99, int maxIters=1000 );
+
+}
+
 #endif

modules/calib3d/src/ptsetreg.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "precomp.hpp"
+
+#include <algorithm>
+#include <iterator>
+#include <limits>
+
+namespace cv
+{
+
+int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
+{
+    if( modelPoints <= 0 )
+        CV_Error( CV_StsOutOfRange, "the number of model points should be positive" );
+
+    p = MAX(p, 0.);
+    p = MIN(p, 1.);
+    ep = MAX(ep, 0.);
+    ep = MIN(ep, 1.);
+
+    // avoid inf's & nan's
+    double num = MAX(1. - p, DBL_MIN);
+    double denom = 1. - std::pow(1. - ep, modelPoints);
+    if( denom < DBL_MIN )
+        return 0;
+
+    num = std::log(num);
+    denom = std::log(denom);
+
+    return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
+}
+
+
+class RANSACPointSetRegistrator : public PointSetRegistrator
+{
+public:
+    RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
+                              int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
+    : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
+    {
+        checkPartialSubsets = true;
+    }
+
+    int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
+    {
+        cb->computeError( m1, m2, model, err );
+        mask.create(err.size(), CV_8U);
+
+        CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
+        const float* errptr = err.ptr<float>();
+        uchar* maskptr = mask.ptr<uchar>();
+        float t = (float)(thresh*thresh);
+        int i, n = (int)err.total(), nz = 0;
+        for( i = 0; i < n; i++ )
+        {
+            int f = errptr[i] <= t;
+            maskptr[i] = (uchar)f;
+            nz += f;
+        }
+        return nz;
+    }
+
+    bool getSubset( const Mat& m1, const Mat& m2,
+                    Mat& ms1, Mat& ms2, RNG& rng,
+                    int maxAttempts=1000 ) const
+    {
+        cv::AutoBuffer<int> _idx(modelPoints);
+        int* idx = _idx;
+        int i = 0, j, k, iters = 0;
+        int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize();
+        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
+        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
+        int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
+        const int *m1ptr = (const int*)m1.data, *m2ptr = (const int*)m2.data;
+
+        ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
+        ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));
+
+        int *ms1ptr = (int*)ms1.data, *ms2ptr = (int*)ms2.data;
+
+        CV_Assert( count >= modelPoints && count == count2 );
+        CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 );
+        esz1 /= sizeof(int);
+        esz2 /= sizeof(int);
+
+        for(; iters < maxAttempts; iters++)
+        {
+            for( i = 0; i < modelPoints && iters < maxAttempts; )
+            {
+                int idx_i = 0;
+                for(;;)
+                {
+                    idx_i = idx[i] = rng.uniform(0, count);
+                    for( j = 0; j < i; j++ )
+                        if( idx_i == idx[j] )
+                            break;
+                    if( j == i )
+                        break;
+                }
+                for( k = 0; k < esz1; k++ )
+                    ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
+                for( k = 0; k < esz2; k++ )
+                    ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
+                if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
+                {
+                    iters++;
+                    continue;
+                }
+                i++;
+            }
+            if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i))
+                continue;
+            break;
+        }
+        
+        return i == modelPoints && iters < maxAttempts;
+    }
+
+    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
+    {
+        bool result = false;
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        Mat err, mask, model, bestModel, ms1, ms2;
+
+        int iter, niters = MAX(maxIters, 1);
+        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
+        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
+        int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0;
+
+        RNG rng((uint64)-1);
+
+        CV_Assert( !cb.empty() );
+        CV_Assert( confidence > 0 && confidence < 1 );
+
+        CV_Assert( count >= 0 && count2 == count );
+        if( count < modelPoints )
+            return false;
+
+        Mat bestMask0, bestMask;
+
+        if( _mask.needed() )
+        {
+            _mask.create(count, 1, CV_8U, -1, true);
+            bestMask0 = bestMask = _mask.getMat();
+            CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count );
+        }
+        else
+        {
+            bestMask.create(count, 1, CV_8U);
+            bestMask0 = bestMask;
+        }
+
+        if( count == modelPoints )
+        {
+            if( cb->runKernel(m1, m2, bestModel) <= 0 )
+                return false;
+            bestModel.copyTo(_model);
+            bestMask.setTo(Scalar::all(1));
+            return true;
+        }
+
+        for( iter = 0; iter < niters; iter++ )
+        {
+            int i, goodCount, nmodels;
+            if( count > modelPoints )
+            {
+                bool found = getSubset( m1, m2, ms1, ms2, rng );
+                if( !found )
+                {
+                    if( iter == 0 )
+                        return false;
+                    break;
+                }
+            }
+
+            nmodels = cb->runKernel( ms1, ms2, model );
+            if( nmodels <= 0 )
+                continue;
+            CV_Assert( model.rows % nmodels == 0 );
+            Size modelSize(model.cols, model.rows/nmodels);
+
+            for( i = 0; i < nmodels; i++ )
+            {
+                Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
+                goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
+
+                if( goodCount > MAX(maxGoodCount, modelPoints-1) )
+                {
+                    std::swap(mask, bestMask);
+                    model_i.copyTo(bestModel);
+                    maxGoodCount = goodCount;
+                    niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters );
+                }
+            }
+        }
+        
+        if( maxGoodCount > 0 )
+        {
+            if( bestMask.data != bestMask0.data )
+            {
+                if( bestMask.size() == bestMask0.size() )
+                    bestMask.copyTo(bestMask0);
+                else
+                    transpose(bestMask, bestMask0);
+            }
+            bestModel.copyTo(_model);
+            result = true;
+        }
+        else
+            _model.release();
+        
+        return result;
+    }
+
+    void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; }
+
+    AlgorithmInfo* info() const;
+
+    Ptr<PointSetRegistrator::Callback> cb;
+    int modelPoints;
+    int maxBasicSolutions;
+    bool checkPartialSubsets;
+    double threshold;
+    double confidence;
+    int maxIters;
+};
+
+
+static CV_IMPLEMENT_QSORT( sortDistances, int, CV_LT )
+
+class LMeDSPointSetRegistrator : public RANSACPointSetRegistrator
+{
+public:
+    LMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
+                              int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
+    : RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {}
+
+    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
+    {
+        const double outlierRatio = 0.45;
+        bool result = false;
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        Mat ms1, ms2, err, errf, model, bestModel, mask, mask0;
+
+        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
+        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
+        int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
+        double minMedian = DBL_MAX, sigma;
+
+        RNG rng((uint64)-1);
+
+        CV_Assert( !cb.empty() );
+        CV_Assert( confidence > 0 && confidence < 1 );
+
+        CV_Assert( count >= 0 && count2 == count );
+        if( count < modelPoints )
+            return false;
+
+        if( _mask.needed() )
+        {
+            _mask.create(count, 1, CV_8U, -1, true);
+            mask0 = mask = _mask.getMat();
+            CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == count );
+        }
+
+        if( count == modelPoints )
+        {
+            if( cb->runKernel(m1, m2, bestModel) <= 0 )
+                return false;
+            bestModel.copyTo(_model);
+            mask.setTo(Scalar::all(1));
+            return true;
+        }
+
+        int iter, niters = RANSACUpdateNumIters(confidence, outlierRatio, modelPoints, maxIters);
+        niters = MAX(niters, 3);
+
+        for( iter = 0; iter < niters; iter++ )
+        {
+            int i, nmodels;
+            if( count > modelPoints )
+            {
+                bool found = getSubset( m1, m2, ms1, ms2, rng );
+                if( !found )
+                {
+                    if( iter == 0 )
+                        return false;
+                    break;
+                }
+            }
+
+            nmodels = cb->runKernel( ms1, ms2, model );
+            if( nmodels <= 0 )
+                continue;
+
+            CV_Assert( model.rows % nmodels == 0 );
+            Size modelSize(model.cols, model.rows/nmodels);
+
+            for( i = 0; i < nmodels; i++ )
+            {
+                Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
+                cb->computeError( m1, m2, model_i, err );
+                if( err.depth() != CV_32F )
+                    err.convertTo(errf, CV_32F);
+                else
+                    errf = err;
+                CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count );
+                sortDistances( (int*)errf.data, count, 0 );
+
+                double median = count % 2 != 0 ?
+                errf.at<float>(count/2) : (errf.at<float>(count/2-1) + errf.at<float>(count/2))*0.5;
+
+                if( median < minMedian )
+                {
+                    minMedian = median;
+                    model_i.copyTo(bestModel);
+                }
+            }
+        }
+        
+        if( minMedian < DBL_MAX )
+        {
+            sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
+            sigma = MAX( sigma, 0.001 );
+
+            count = findInliers( m1, m2, bestModel, err, mask, sigma );
+            if( _mask.needed() && mask0.data != mask.data )
+            {
+                if( mask0.size() == mask.size() )
+                    mask.copyTo(mask0);
+                else
+                    transpose(mask, mask0);
+            }
+            bestModel.copyTo(_model);
+            result = count >= modelPoints;
+        }
+        else
+            _model.release();
+        
+        return result;
+    }
+
+    AlgorithmInfo* info() const;
+};
+
+
+CV_INIT_ALGORITHM(RANSACPointSetRegistrator, "PointSetRegistrator.RANSAC",
+                  obj.info()->addParam(obj, "threshold", obj.threshold);
+                  obj.info()->addParam(obj, "confidence", obj.confidence);
+                  obj.info()->addParam(obj, "maxIters", obj.maxIters));
+
+CV_INIT_ALGORITHM(LMeDSPointSetRegistrator, "PointSetRegistrator.LMeDS",
+                  obj.info()->addParam(obj, "confidence", obj.confidence);
+                  obj.info()->addParam(obj, "maxIters", obj.maxIters));
+
+Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
+                                                         int _modelPoints, double _threshold,
+                                                         double _confidence, int _maxIters)
+{
+    CV_Assert( !RANSACPointSetRegistrator_info_auto.name().empty() );
+    return new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters);
+}
+
+
+Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
+                             int _modelPoints, double _confidence, int _maxIters)
+{
+    CV_Assert( !LMeDSPointSetRegistrator_info_auto.name().empty() );
+    return new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters);
+}
+
+class Affine3DEstimatorCallback : public PointSetRegistrator::Callback
+{
+public:
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        const Point3f* from = m1.ptr<Point3f>();
+        const Point3f* to   = m2.ptr<Point3f>();
+
+        const int N = 12;
+        double buf[N*N + N + N];
+        Mat A(N, N, CV_64F, &buf[0]);
+        Mat B(N, 1, CV_64F, &buf[0] + N*N);
+        Mat X(N, 1, CV_64F, &buf[0] + N*N + N);
+        double* Adata = A.ptr<double>();
+        double* Bdata = B.ptr<double>();
+        A = Scalar::all(0);
+
+        for( int i = 0; i < (N/3); i++ )
+        {
+            Bdata[i*3] = to[i].x;
+            Bdata[i*3+1] = to[i].y;
+            Bdata[i*3+2] = to[i].z;
+
+            double *aptr = Adata + i*3*N;
+            for(int k = 0; k < 3; ++k)
+            {
+                aptr[0] = from[i].x;
+                aptr[1] = from[i].y;
+                aptr[2] = from[i].z;
+                aptr[3] = 1.0;
+                aptr += 16;
+            }
+        }
+
+        solve(A, B, X, DECOMP_SVD);
+        X.reshape(1, 3).copyTo(_model);
+
+        return 1;
+    }
+
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
+        const Point3f* from = m1.ptr<Point3f>();
+        const Point3f* to   = m2.ptr<Point3f>();
+        const double* F = model.ptr<double>();
+
+        int count = m1.checkVector(3);
+        CV_Assert( count > 0 );
+
+        _err.create(count, 1, CV_32F);
+        Mat err = _err.getMat();
+        float* errptr = err.ptr<float>();
+
+        for(int i = 0; i < count; i++ )
+        {
+            const Point3f& f = from[i];
+            const Point3f& t = to[i];
+
+            double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
+            double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
+            double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
+
+            errptr[i] = (float)std::sqrt(a*a + b*b + c*c);
+        }
+    }
+
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
+    {
+        const float threshold = 0.996f;
+        Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
+
+        for( int inp = 1; inp <= 2; inp++ )
+        {
+            int j, k, i = count - 1;
+            const Mat* msi = inp == 1 ? &ms1 : &ms2;
+            const Point3f* ptr = msi->ptr<Point3f>();
+
+            CV_Assert( count <= msi->rows );
+
+            // check that the i-th selected point does not belong
+            // to a line connecting some previously selected points
+            for(j = 0; j < i; ++j)
+            {
+                Point3f d1 = ptr[j] - ptr[i];
+                float n1 = d1.x*d1.x + d1.y*d1.y;
+
+                for(k = 0; k < j; ++k)
+                {
+                    Point3f d2 = ptr[k] - ptr[i];
+                    float denom = (d2.x*d2.x + d2.y*d2.y)*n1;
+                    float num = d1.x*d2.x + d1.y*d2.y;
+
+                    if( num*num > threshold*threshold*denom )
+                        return false;
+                }
+            }
+        }
+        return true;
+    }
+};
+
+}
+
+int cv::estimateAffine3D(InputArray _from, InputArray _to,
+                         OutputArray _out, OutputArray _inliers,
+                         double param1, double param2)
+{
+    Mat from = _from.getMat(), to = _to.getMat();
+    int count = from.checkVector(3);
+
+    CV_Assert( count >= 0 && to.checkVector(3) == count );
+
+    Mat dFrom, dTo;
+    from.convertTo(dFrom, CV_32F);
+    to.convertTo(dTo, CV_32F);
+    dFrom = dFrom.reshape(3, count);
+    dTo = dTo.reshape(3, count);
+
+    const double epsilon = DBL_EPSILON;
+    param1 = param1 <= 0 ? 3 : param1;
+    param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2;
+    
+    return createRANSACPointSetRegistrator(new Affine3DEstimatorCallback, 4, param1, param2)->run(dFrom, dTo, _out, _inliers);
+}
+

modules/calib3d/test/test_affine3d_estimator.cpp

@@ -163,6 +163,8 @@ bool CV_Affine3D_EstTest::testNPoints()
     const double thres = 1e-4;
     if (norm(aff_est, aff, NORM_INF) > thres)
     {
+        cout << "aff est: " << aff_est << endl;
+        cout << "aff ref: " << aff << endl;
         ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
         return false;
     }

modules/calib3d/test/test_fundam.cpp

@@ -1020,7 +1020,7 @@ void CV_FundamentalMatTest::prepare_to_validation( int test_case_idx )
     F0 *= 1./f0[8];
 
     uchar* status = test_mat[TEMP][1].data;
-    double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 );
+    double err_level = method <= CV_FM_8POINT ? 1 : get_success_error_level( test_case_idx, OUTPUT, 1 );
     uchar* mtfm1 = test_mat[REF_OUTPUT][1].data;
     uchar* mtfm2 = test_mat[OUTPUT][1].data;
     double* f_prop1 = (double*)test_mat[REF_OUTPUT][0].data;

modules/calib3d/test/test_modelest.cpp

@@ -40,6 +40,8 @@
 //M*/
 
 #include "test_precomp.hpp"
+
+#if 0
 #include "_modelest.h"
 
 using namespace std;
@@ -225,3 +227,6 @@ void CV_ModelEstimator2_Test::run_func()
 }
 
 TEST(Calib3d_ModelEstimator2, accuracy) { CV_ModelEstimator2_Test test; test.safe_run(); }
+
+#endif
+