2010-05-12 01:44:00 +08:00
|
|
|
/*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.
|
|
|
|
//
|
|
|
|
//
|
2013-02-28 22:25:05 +08:00
|
|
|
// License Agreement
|
2010-05-12 01:44:00 +08:00
|
|
|
// For Open Source Computer Vision Library
|
|
|
|
//
|
|
|
|
// Copyright (C) 2000, Intel Corporation, all rights reserved.
|
2013-02-28 22:25:05 +08:00
|
|
|
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
|
2010-05-12 01:44:00 +08:00
|
|
|
// 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.
|
|
|
|
//
|
2013-02-28 22:25:05 +08:00
|
|
|
// * The name of the copyright holders may not be used to endorse or promote products
|
2010-05-12 01:44:00 +08:00
|
|
|
// 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"
|
2015-03-04 14:43:13 +08:00
|
|
|
#include "rho.h"
|
2013-02-28 22:25:05 +08:00
|
|
|
#include <iostream>
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
namespace cv
|
|
|
|
{
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
class HomographyEstimatorCallback : public PointSetRegistrator::Callback
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
|
|
|
public:
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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.);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
negative += determinant(A)*determinant(B) < 0;
|
|
|
|
}
|
|
|
|
if( negative != 0 && negative != 4 )
|
|
|
|
return false;
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
return true;
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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>();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-03-05 04:00:21 +08:00
|
|
|
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] );
|
2013-02-28 22:25:05 +08:00
|
|
|
Point2d cM(0,0), cm(0,0), sM(0,0), sm(0,0);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-03-05 04:00:21 +08:00
|
|
|
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 );
|
2013-02-28 22:25:05 +08:00
|
|
|
|
2013-03-05 04:00:21 +08:00
|
|
|
_LtL.setTo(Scalar::all(0));
|
2013-02-28 22:25:05 +08:00
|
|
|
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++ )
|
2013-03-05 04:00:21 +08:00
|
|
|
LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
|
2013-02-28 22:25:05 +08:00
|
|
|
}
|
2013-03-05 04:00:21 +08:00
|
|
|
completeSymm( _LtL );
|
2013-04-11 23:27:54 +08:00
|
|
|
|
2013-03-05 04:00:21 +08:00
|
|
|
eigen( _LtL, matW, matV );
|
|
|
|
_Htemp = _invHnorm*_H0;
|
|
|
|
_H0 = _Htemp*_Hnorm2;
|
|
|
|
_H0.convertTo(_model, _H0.type(), 1./_H0.at<double>(2,2) );
|
2013-04-11 23:27:54 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
return 1;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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>();
|
2013-03-05 04:00:21 +08:00
|
|
|
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] };
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
_err.create(count, 1, CV_32F);
|
|
|
|
float* err = _err.getMat().ptr<float>();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
class HomographyRefineCallback : public LMSolver::Callback
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
HomographyRefineCallback(InputArray _src, InputArray _dst)
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
src = _src.getMat();
|
|
|
|
dst = _dst.getMat();
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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 );
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
for( i = 0; i < count; i++ )
|
|
|
|
{
|
|
|
|
double Mx = M[i].x, My = M[i].y;
|
2010-06-07 01:21:40 +08:00
|
|
|
double ww = h[6]*Mx + h[7]*My + 1.;
|
|
|
|
ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0;
|
2013-02-28 22:25:05 +08:00
|
|
|
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 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
2013-08-28 17:20:14 +08:00
|
|
|
|
|
|
|
Jptr += 16;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
|
|
|
|
return true;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat src, dst;
|
|
|
|
};
|
|
|
|
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2014-11-18 04:02:18 +08:00
|
|
|
|
|
|
|
namespace cv{
|
2015-02-04 12:33:52 +08:00
|
|
|
static bool createAndRunRHORegistrator(double confidence,
|
|
|
|
int maxIters,
|
|
|
|
double ransacReprojThreshold,
|
|
|
|
int npoints,
|
|
|
|
InputArray _src,
|
|
|
|
InputArray _dst,
|
|
|
|
OutputArray _H,
|
|
|
|
OutputArray _tempMask){
|
|
|
|
Mat src = _src.getMat();
|
|
|
|
Mat dst = _dst.getMat();
|
2015-02-05 15:18:41 +08:00
|
|
|
Mat tempMask;
|
2015-02-04 12:33:52 +08:00
|
|
|
bool result;
|
|
|
|
double beta = 0.35;/* 0.35 is a value that often works. */
|
2014-11-18 04:02:18 +08:00
|
|
|
|
2015-02-04 12:33:52 +08:00
|
|
|
/* Create temporary output matrix (RHO outputs a single-precision H only). */
|
2014-11-18 04:02:18 +08:00
|
|
|
Mat tmpH = Mat(3, 3, CV_32FC1);
|
2015-02-04 12:33:52 +08:00
|
|
|
|
|
|
|
/* Create output mask. */
|
2015-02-05 15:18:41 +08:00
|
|
|
tempMask = Mat(npoints, 1, CV_8U);
|
2015-02-04 12:33:52 +08:00
|
|
|
|
|
|
|
/**
|
|
|
|
* Make use of the RHO estimator API.
|
|
|
|
*
|
|
|
|
* This is where the math happens. A homography estimation context is
|
|
|
|
* initialized, used, then finalized.
|
|
|
|
*/
|
|
|
|
|
2015-03-04 14:43:13 +08:00
|
|
|
Ptr<RHO_HEST> p = rhoInit();
|
2015-02-04 12:33:52 +08:00
|
|
|
|
|
|
|
/**
|
|
|
|
* Optional. Ideally, the context would survive across calls to
|
|
|
|
* findHomography(), but no clean way appears to exit to do so. The price
|
|
|
|
* to pay is marginally more computational work than strictly needed.
|
|
|
|
*/
|
|
|
|
|
2015-03-04 14:43:13 +08:00
|
|
|
rhoEnsureCapacity(p, npoints, beta);
|
2015-02-04 12:33:52 +08:00
|
|
|
|
|
|
|
/**
|
|
|
|
* The critical call. All parameters are heavily documented in rhorefc.h.
|
|
|
|
*
|
|
|
|
* Currently, NR (Non-Randomness criterion) and Final Refinement (with
|
|
|
|
* internal, optimized Levenberg-Marquardt method) are enabled. However,
|
|
|
|
* while refinement seems to correctly smooth jitter most of the time, when
|
|
|
|
* refinement fails it tends to make the estimate visually very much worse.
|
|
|
|
* It may be necessary to remove the refinement flags in a future commit if
|
|
|
|
* this behaviour is too problematic.
|
|
|
|
*/
|
|
|
|
|
2015-03-04 14:43:13 +08:00
|
|
|
result = !!rhoHest(p,
|
2015-02-04 12:33:52 +08:00
|
|
|
(const float*)src.data,
|
|
|
|
(const float*)dst.data,
|
|
|
|
(char*) tempMask.data,
|
2015-02-04 15:26:01 +08:00
|
|
|
(unsigned) npoints,
|
|
|
|
(float) ransacReprojThreshold,
|
|
|
|
(unsigned) maxIters,
|
|
|
|
(unsigned) maxIters,
|
2015-02-04 12:33:52 +08:00
|
|
|
confidence,
|
2015-02-04 15:26:01 +08:00
|
|
|
4U,
|
2015-02-04 12:33:52 +08:00
|
|
|
beta,
|
|
|
|
RHO_FLAG_ENABLE_NR | RHO_FLAG_ENABLE_FINAL_REFINEMENT,
|
|
|
|
NULL,
|
|
|
|
(float*)tmpH.data);
|
|
|
|
|
|
|
|
/* Convert float homography to double precision. */
|
2014-11-18 04:02:18 +08:00
|
|
|
tmpH.convertTo(_H, CV_64FC1);
|
|
|
|
|
2015-02-04 12:33:52 +08:00
|
|
|
/* Maps non-zero mask elems to 1, for the sake of the testcase. */
|
2014-11-18 04:02:18 +08:00
|
|
|
for(int k=0;k<npoints;k++){
|
|
|
|
tempMask.data[k] = !!tempMask.data[k];
|
|
|
|
}
|
2015-01-15 17:21:16 +08:00
|
|
|
tempMask.copyTo(_tempMask);
|
2014-11-18 04:02:18 +08:00
|
|
|
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
|
2014-08-17 07:13:39 +08:00
|
|
|
int method, double ransacReprojThreshold, OutputArray _mask,
|
|
|
|
const int maxIters, const double confidence)
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
|
|
|
const double defaultRANSACReprojThreshold = 3;
|
|
|
|
bool result = false;
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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 )
|
2013-04-11 23:27:54 +08:00
|
|
|
CV_Error(Error::StsBadArg, "The input arrays should be 2D or 3D point sets");
|
2013-02-28 22:25:05 +08:00
|
|
|
if( npoints == 0 )
|
|
|
|
return Mat();
|
|
|
|
convertPointsFromHomogeneous(p, p);
|
|
|
|
}
|
|
|
|
p.reshape(2, npoints).convertTo(m, CV_32F);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( src.checkVector(2) == dst.checkVector(2) );
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
if( ransacReprojThreshold <= 0 )
|
|
|
|
ransacReprojThreshold = defaultRANSACReprojThreshold;
|
|
|
|
|
2013-08-13 21:03:56 +08:00
|
|
|
Ptr<PointSetRegistrator::Callback> cb = makePtr<HomographyEstimatorCallback>();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( method == 0 || npoints == 4 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
tempMask = Mat::ones(npoints, 1, CV_8U);
|
|
|
|
result = cb->runKernel(src, dst, H) > 0;
|
2015-02-13 21:01:57 +08:00
|
|
|
}
|
|
|
|
else if( method == RANSAC )
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
2015-02-13 10:22:52 +08:00
|
|
|
else if( method == RHO )
|
2014-11-18 04:02:18 +08:00
|
|
|
result = createAndRunRHORegistrator(confidence, maxIters, ransacReprojThreshold, npoints, src, dst, H, tempMask);
|
2015-02-13 10:22:52 +08:00
|
|
|
else
|
2013-04-11 23:27:54 +08:00
|
|
|
CV_Error(Error::StsBadArg, "Unknown estimation method");
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2014-11-18 04:02:18 +08:00
|
|
|
if( result && npoints > 4 && method != RHO)
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2015-03-16 21:56:26 +08:00
|
|
|
compressElems( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
|
|
|
|
npoints = compressElems( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
|
2013-02-28 22:25:05 +08:00
|
|
|
if( npoints > 0 )
|
|
|
|
{
|
|
|
|
Mat src1 = src.rowRange(0, npoints);
|
|
|
|
Mat dst1 = dst.rowRange(0, npoints);
|
|
|
|
src = src1;
|
|
|
|
dst = dst1;
|
2015-02-13 21:01:57 +08:00
|
|
|
if( method == RANSAC || method == LMEDS )
|
2013-02-28 22:25:05 +08:00
|
|
|
cb->runKernel( src, dst, H );
|
|
|
|
Mat H8(8, 1, CV_64F, H.ptr<double>());
|
2013-08-13 21:03:56 +08:00
|
|
|
createLMSolver(makePtr<HomographyRefineCallback>(src, dst), 10)->run(H8);
|
2013-02-28 22:25:05 +08:00
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
if( result )
|
2011-05-27 04:33:55 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
if( _mask.needed() )
|
|
|
|
tempMask.copyTo(_mask);
|
2011-05-27 04:33:55 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
else
|
|
|
|
H.release();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
return H;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
|
|
|
|
OutputArray _mask, int method, double ransacReprojThreshold )
|
2013-01-11 08:55:56 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
return cv::findHomography(_points1, _points2, method, ransacReprojThreshold, _mask);
|
2013-01-11 08:55:56 +08:00
|
|
|
}
|
|
|
|
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
|
|
|
|
/* Estimation of Fundamental Matrix from point correspondences.
|
2010-05-12 01:44:00 +08:00
|
|
|
The original code has been written by Valery Mosyagin */
|
|
|
|
|
|
|
|
/* The algorithms (except for RANSAC) and the notation have been taken from
|
|
|
|
Zhengyou Zhang's research report
|
|
|
|
"Determining the Epipolar Geometry and its Uncertainty: A Review"
|
|
|
|
that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
|
|
|
|
|
|
|
|
/************************************** 7-point algorithm *******************************/
|
2013-02-28 22:25:05 +08:00
|
|
|
namespace cv
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
double a[7*9], w[7], u[9*9], v[9*9], c[4], r[3];
|
2010-05-12 01:44:00 +08:00
|
|
|
double* f1, *f2;
|
|
|
|
double t0, t1, t2;
|
2013-02-28 22:25:05 +08:00
|
|
|
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>();
|
2010-05-12 01:44:00 +08:00
|
|
|
int i, k, n;
|
|
|
|
|
|
|
|
// form a linear system: i-th row of A(=a) represents
|
|
|
|
// the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
|
|
|
|
for( i = 0; i < 7; i++ )
|
|
|
|
{
|
|
|
|
double x0 = m1[i].x, y0 = m1[i].y;
|
|
|
|
double x1 = m2[i].x, y1 = m2[i].y;
|
|
|
|
|
|
|
|
a[i*9+0] = x1*x0;
|
|
|
|
a[i*9+1] = x1*y0;
|
|
|
|
a[i*9+2] = x1;
|
|
|
|
a[i*9+3] = y1*x0;
|
|
|
|
a[i*9+4] = y1*y0;
|
|
|
|
a[i*9+5] = y1;
|
|
|
|
a[i*9+6] = x0;
|
|
|
|
a[i*9+7] = y0;
|
|
|
|
a[i*9+8] = 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
// A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
|
|
|
|
// the solution is linear subspace of dimensionality 2.
|
|
|
|
// => use the last two singular vectors as a basis of the space
|
|
|
|
// (according to SVD properties)
|
2013-02-28 22:25:05 +08:00
|
|
|
SVDecomp( A, W, U, Vt, SVD::MODIFY_A + SVD::FULL_UV );
|
2010-05-12 01:44:00 +08:00
|
|
|
f1 = v + 7*9;
|
|
|
|
f2 = v + 8*9;
|
|
|
|
|
|
|
|
// f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
|
|
|
|
// as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
|
|
|
|
// so f ~ lambda*f1 + (1 - lambda)*f2.
|
|
|
|
// use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
|
|
|
|
// it will be a cubic equation.
|
|
|
|
// find c - polynomial coefficients.
|
|
|
|
for( i = 0; i < 9; i++ )
|
|
|
|
f1[i] -= f2[i];
|
|
|
|
|
|
|
|
t0 = f2[4]*f2[8] - f2[5]*f2[7];
|
|
|
|
t1 = f2[3]*f2[8] - f2[5]*f2[6];
|
|
|
|
t2 = f2[3]*f2[7] - f2[4]*f2[6];
|
|
|
|
|
|
|
|
c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
|
|
|
|
|
|
|
|
c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
|
2013-02-28 22:25:05 +08:00
|
|
|
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]);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
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 -
|
2013-02-28 22:25:05 +08:00
|
|
|
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]);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
|
|
|
|
|
|
|
|
// solve the cubic equation; there can be 1 to 3 roots ...
|
2013-02-28 22:25:05 +08:00
|
|
|
n = solveCubic( coeffs, roots );
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
if( n < 1 || n > 3 )
|
|
|
|
return n;
|
|
|
|
|
|
|
|
for( k = 0; k < n; k++, fmatrix += 9 )
|
|
|
|
{
|
|
|
|
// for each root form the fundamental matrix
|
|
|
|
double lambda = r[k], mu = 1.;
|
|
|
|
double s = f1[8]*r[k] + f2[8];
|
|
|
|
|
|
|
|
// normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
|
|
|
|
if( fabs(s) > DBL_EPSILON )
|
|
|
|
{
|
|
|
|
mu = 1./s;
|
|
|
|
lambda *= mu;
|
|
|
|
fmatrix[8] = 1.;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
fmatrix[8] = 0.;
|
|
|
|
|
|
|
|
for( i = 0; i < 8; i++ )
|
|
|
|
fmatrix[i] = f1[i]*lambda + f2[i]*mu;
|
|
|
|
}
|
|
|
|
|
|
|
|
return n;
|
|
|
|
}
|
|
|
|
|
2013-04-11 23:27:54 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
Point2d m1c(0,0), m2c(0,0);
|
|
|
|
double t, scale1 = 0, scale2 = 0;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
const Point2f* m1 = _m1.ptr<Point2f>();
|
|
|
|
const Point2f* m2 = _m2.ptr<Point2f>();
|
|
|
|
CV_Assert( (_m1.cols == 1 || _m1.rows == 1) && _m1.size() == _m2.size());
|
2015-07-09 21:02:25 +08:00
|
|
|
int i, count = _m1.checkVector(2);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
// compute centers and average distances for each of the two point sets
|
|
|
|
for( i = 0; i < count; i++ )
|
|
|
|
{
|
2015-07-09 21:02:25 +08:00
|
|
|
m1c += Point2d(m1[i]);
|
|
|
|
m2c += Point2d(m2[i]);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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;
|
2015-07-09 21:02:25 +08:00
|
|
|
m1c *= t;
|
|
|
|
m2c *= t;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
for( i = 0; i < count; i++ )
|
|
|
|
{
|
2015-07-09 21:02:25 +08:00
|
|
|
scale1 += norm(Point2d(m1[i].x - m1c.x, m1[i].y - m1c.y));
|
|
|
|
scale2 += norm(Point2d(m2[i].x - m2c.x, m2[i].y - m2c.y));
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
scale1 *= t;
|
2013-02-28 22:25:05 +08:00
|
|
|
scale2 *= t;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( scale1 < FLT_EPSILON || scale2 < FLT_EPSILON )
|
2010-05-12 01:44:00 +08:00
|
|
|
return 0;
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
scale1 = std::sqrt(2.)/scale1;
|
|
|
|
scale2 = std::sqrt(2.)/scale2;
|
2012-10-17 15:12:04 +08:00
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
Matx<double, 9, 9> A;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
// 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
|
2012-10-17 15:12:04 +08:00
|
|
|
// to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
|
2010-05-12 01:44:00 +08:00
|
|
|
for( i = 0; i < count; i++ )
|
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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;
|
2015-07-09 21:02:25 +08:00
|
|
|
Vec<double, 9> r( x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 );
|
|
|
|
A += r*r.t();
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
Vec<double, 9> W;
|
|
|
|
Matx<double, 9, 9> V;
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
eigen(A, W, V);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2011-08-07 03:38:09 +08:00
|
|
|
for( i = 0; i < 9; i++ )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2015-07-09 21:02:25 +08:00
|
|
|
if( fabs(W[i]) < DBL_EPSILON )
|
2010-05-12 01:44:00 +08:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2011-08-07 03:38:09 +08:00
|
|
|
if( i < 8 )
|
2010-05-12 01:44:00 +08:00
|
|
|
return 0;
|
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
Matx33d F0( V.val + 9*8 ); // take the last column of v as a solution of Af = 0
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
// make F0 singular (of rank 2) by decomposing it with SVD,
|
|
|
|
// zeroing the last diagonal element of W and then composing the matrices back.
|
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
Vec3d w;
|
|
|
|
Matx33d U;
|
|
|
|
Matx33d Vt;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
SVD::compute( F0, w, U, Vt);
|
|
|
|
w[2] = 0.;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
F0 = U*Matx33d::diag(w)*Vt;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
|
|
|
// apply the transformation that is inverse
|
|
|
|
// to what we used to normalize the point coordinates
|
2015-07-09 21:02:25 +08:00
|
|
|
Matx33d T1( scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 );
|
|
|
|
Matx33d T2( scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 );
|
2013-02-28 22:25:05 +08:00
|
|
|
|
2015-07-09 21:02:25 +08:00
|
|
|
F0 = T2.t()*F0*T1;
|
2013-04-11 23:27:54 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
// make F(3,3) = 1
|
2015-07-09 21:02:25 +08:00
|
|
|
if( fabs(F0(2,2)) > FLT_EPSILON )
|
|
|
|
F0 *= 1./F0(2,2);
|
|
|
|
|
|
|
|
Mat(F0).copyTo(_fmatrix);
|
2013-04-11 23:27:54 +08:00
|
|
|
|
2010-05-12 01:44:00 +08:00
|
|
|
return 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
class FMEstimatorCallback : public PointSetRegistrator::Callback
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
public:
|
|
|
|
bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
|
|
|
|
{
|
|
|
|
Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
|
|
|
|
return !haveCollinearPoints(ms1, count) && !haveCollinearPoints(ms2, count);
|
|
|
|
}
|
2012-10-17 15:12:04 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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>();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < count; i++ )
|
|
|
|
{
|
|
|
|
double a, b, c, d1, d2, s1, s2;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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];
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
s2 = 1./(a*a + b*b);
|
|
|
|
d2 = m2[i].x*a + m2[i].y*b + c;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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];
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
};
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
|
|
|
|
int method, double param1, double param2,
|
|
|
|
OutputArray _mask )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
|
|
|
|
Mat m1, m2, F;
|
|
|
|
int npoints = -1;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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 )
|
2013-04-11 23:27:54 +08:00
|
|
|
CV_Error(Error::StsBadArg, "The input arrays should be 2D or 3D point sets");
|
2013-02-28 22:25:05 +08:00
|
|
|
if( npoints == 0 )
|
|
|
|
return Mat();
|
|
|
|
convertPointsFromHomogeneous(p, p);
|
|
|
|
}
|
|
|
|
p.reshape(2, npoints).convertTo(m, CV_32F);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( m1.checkVector(2) == m2.checkVector(2) );
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( npoints < 7 )
|
|
|
|
return Mat();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-08-13 21:03:56 +08:00
|
|
|
Ptr<PointSetRegistrator::Callback> cb = makePtr<FMEstimatorCallback>();
|
2013-02-28 22:25:05 +08:00
|
|
|
int result;
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( npoints == 7 || method == FM_8POINT )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
result = cb->runKernel(m1, m2, F);
|
|
|
|
if( _mask.needed() )
|
|
|
|
{
|
2013-03-05 04:03:37 +08:00
|
|
|
_mask.create(npoints, 1, CV_8U, -1, true);
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat mask = _mask.getMat();
|
|
|
|
CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == npoints );
|
|
|
|
mask.setTo(Scalar::all(1));
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2011-08-03 22:04:14 +08:00
|
|
|
else
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
|
|
|
if( param1 <= 0 )
|
|
|
|
param1 = 3;
|
|
|
|
if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
|
|
|
|
param2 = 0.99;
|
2012-10-17 15:12:04 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( (method & ~3) == FM_RANSAC && npoints >= 15 )
|
|
|
|
result = createRANSACPointSetRegistrator(cb, 7, param1, param2)->run(m1, m2, F, _mask);
|
2010-05-12 01:44:00 +08:00
|
|
|
else
|
2013-02-28 22:25:05 +08:00
|
|
|
result = createLMeDSPointSetRegistrator(cb, 7, param2)->run(m1, m2, F, _mask);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( result <= 0 )
|
|
|
|
return Mat();
|
2012-10-17 15:12:04 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
return F;
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
|
|
|
|
InputArray _Fmat, OutputArray _lines )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
|
|
|
double f[9];
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat tempF(3, 3, CV_64F, f);
|
|
|
|
Mat points = _points.getMat(), F = _Fmat.getMat();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( !points.isContinuous() )
|
|
|
|
points = points.clone();
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
int npoints = points.checkVector(2);
|
|
|
|
if( npoints < 0 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
npoints = points.checkVector(3);
|
|
|
|
if( npoints < 0 )
|
2013-04-11 23:27:54 +08:00
|
|
|
CV_Error( Error::StsBadArg, "The input should be a 2D or 3D point set");
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat temp;
|
|
|
|
convertPointsFromHomogeneous(points, temp);
|
|
|
|
points = temp;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
int depth = points.depth();
|
|
|
|
CV_Assert( depth == CV_32F || depth == CV_32S || depth == CV_64F );
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert(F.size() == Size(3,3));
|
|
|
|
F.convertTo(tempF, CV_64F);
|
|
|
|
if( whichImage == 2 )
|
|
|
|
transpose(tempF, tempF);
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
int ltype = CV_MAKETYPE(MAX(depth, CV_32F), 3);
|
|
|
|
_lines.create(npoints, 1, ltype);
|
|
|
|
Mat lines = _lines.getMat();
|
|
|
|
if( !lines.isContinuous() )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
_lines.release();
|
|
|
|
_lines.create(npoints, 1, ltype);
|
|
|
|
lines = _lines.getMat();
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( lines.isContinuous());
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( depth == CV_32S || depth == CV_32F )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point* ptsi = points.ptr<Point>();
|
|
|
|
const Point2f* ptsf = points.ptr<Point2f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point2d* ptsd = points.ptr<Point2d>();
|
2013-02-28 22:25:05 +08:00
|
|
|
Point3d* dstd = lines.ptr<Point3d>();
|
|
|
|
for( int i = 0; i < npoints; i++ )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat src = _src.getMat();
|
|
|
|
if( !src.isContinuous() )
|
|
|
|
src = src.clone();
|
|
|
|
int i, npoints = src.checkVector(3), depth = src.depth(), cn = 3;
|
|
|
|
if( npoints < 0 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
npoints = src.checkVector(4);
|
|
|
|
CV_Assert(npoints >= 0);
|
|
|
|
cn = 4;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F));
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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() )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
_dst.release();
|
|
|
|
_dst.create(npoints, 1, dtype);
|
|
|
|
dst = _dst.getMat();
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( dst.isContinuous() );
|
2010-05-12 01:44:00 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( depth == CV_32S )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
if( cn == 3 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point3i* sptr = src.ptr<Point3i>();
|
|
|
|
Point2f* dptr = dst.ptr<Point2f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Vec4i* sptr = src.ptr<Vec4i>();
|
|
|
|
Point3f* dptr = dst.ptr<Point3f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
else if( depth == CV_32F )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
if( cn == 3 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point3f* sptr = src.ptr<Point3f>();
|
|
|
|
Point2f* dptr = dst.ptr<Point2f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
|
|
|
}
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Vec4f* sptr = src.ptr<Vec4f>();
|
|
|
|
Point3f* dptr = dst.ptr<Point3f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
else if( depth == CV_64F )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
if( cn == 3 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point3d* sptr = src.ptr<Point3d>();
|
|
|
|
Point2d* dptr = dst.ptr<Point2d>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
double scale = sptr[i].z != 0. ? 1./sptr[i].z : 1.;
|
|
|
|
dptr[i] = Point2d(sptr[i].x*scale, sptr[i].y*scale);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Vec4d* sptr = src.ptr<Vec4d>();
|
|
|
|
Point3d* dptr = dst.ptr<Point3d>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
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);
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
else
|
2013-04-11 23:27:54 +08:00
|
|
|
CV_Error(Error::StsUnsupportedFormat, "");
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
|
|
|
|
void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
Mat src = _src.getMat();
|
|
|
|
if( !src.isContinuous() )
|
|
|
|
src = src.clone();
|
|
|
|
int i, npoints = src.checkVector(2), depth = src.depth(), cn = 2;
|
|
|
|
if( npoints < 0 )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
npoints = src.checkVector(3);
|
|
|
|
CV_Assert(npoints >= 0);
|
|
|
|
cn = 3;
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F));
|
2011-04-23 20:49:14 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
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() )
|
2010-05-12 01:44:00 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
_dst.release();
|
|
|
|
_dst.create(npoints, 1, dtype);
|
|
|
|
dst = _dst.getMat();
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
CV_Assert( dst.isContinuous() );
|
2011-04-23 20:49:14 +08:00
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
if( depth == CV_32S )
|
2011-04-17 21:14:45 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
if( cn == 2 )
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point2i* sptr = src.ptr<Point2i>();
|
|
|
|
Point3i* dptr = dst.ptr<Point3i>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
|
|
|
dptr[i] = Point3i(sptr[i].x, sptr[i].y, 1);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point3i* sptr = src.ptr<Point3i>();
|
|
|
|
Vec4i* dptr = dst.ptr<Vec4i>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
|
|
|
dptr[i] = Vec4i(sptr[i].x, sptr[i].y, sptr[i].z, 1);
|
|
|
|
}
|
2011-04-17 21:14:45 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
else if( depth == CV_32F )
|
2011-04-17 21:14:45 +08:00
|
|
|
{
|
2013-02-28 22:25:05 +08:00
|
|
|
if( cn == 2 )
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point2f* sptr = src.ptr<Point2f>();
|
|
|
|
Point3f* dptr = dst.ptr<Point3f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
|
|
|
dptr[i] = Point3f(sptr[i].x, sptr[i].y, 1.f);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point3f* sptr = src.ptr<Point3f>();
|
|
|
|
Vec4f* dptr = dst.ptr<Vec4f>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
|
|
|
dptr[i] = Vec4f(sptr[i].x, sptr[i].y, sptr[i].z, 1.f);
|
|
|
|
}
|
2011-04-17 21:14:45 +08:00
|
|
|
}
|
2013-02-28 22:25:05 +08:00
|
|
|
else if( depth == CV_64F )
|
|
|
|
{
|
|
|
|
if( cn == 2 )
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point2d* sptr = src.ptr<Point2d>();
|
|
|
|
Point3d* dptr = dst.ptr<Point3d>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
|
|
|
dptr[i] = Point3d(sptr[i].x, sptr[i].y, 1.);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2014-08-13 19:08:27 +08:00
|
|
|
const Point3d* sptr = src.ptr<Point3d>();
|
|
|
|
Vec4d* dptr = dst.ptr<Vec4d>();
|
2013-02-28 22:25:05 +08:00
|
|
|
for( i = 0; i < npoints; i++ )
|
|
|
|
dptr[i] = Vec4d(sptr[i].x, sptr[i].y, sptr[i].z, 1.);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
2013-04-11 23:27:54 +08:00
|
|
|
CV_Error(Error::StsUnsupportedFormat, "");
|
2010-05-12 01:44:00 +08:00
|
|
|
}
|
|
|
|
|
2013-02-28 22:25:05 +08:00
|
|
|
|
2011-06-06 22:51:27 +08:00
|
|
|
void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst )
|
2011-05-03 06:20:22 +08:00
|
|
|
{
|
|
|
|
int stype = _src.type(), dtype = _dst.type();
|
|
|
|
CV_Assert( _dst.fixedType() );
|
2012-10-17 15:12:04 +08:00
|
|
|
|
2011-05-03 06:20:22 +08:00
|
|
|
if( CV_MAT_CN(stype) > CV_MAT_CN(dtype) )
|
|
|
|
convertPointsFromHomogeneous(_src, _dst);
|
|
|
|
else
|
|
|
|
convertPointsToHomogeneous(_src, _dst);
|
|
|
|
}
|
|
|
|
|
2015-09-04 22:16:39 +08:00
|
|
|
double cv::sampsonDistance(InputArray _pt1, InputArray _pt2, InputArray _F) {
|
|
|
|
CV_Assert(_pt1.type() == CV_64F && _pt1.type() == CV_64F && _F.type() == CV_64F);
|
|
|
|
CV_DbgAssert(_pt1.rows() == 3 && _F.size() == Size(3, 3) && _pt1.rows() == _pt2.rows());
|
|
|
|
|
|
|
|
Mat pt1(_pt1.getMat());
|
|
|
|
Mat pt2(_pt2.getMat());
|
|
|
|
Mat F(_F.getMat());
|
|
|
|
|
|
|
|
Vec3d F_pt1 = *F.ptr<Matx33d>() * *pt1.ptr<Vec3d>();
|
|
|
|
Vec3d Ft_pt2 = F.ptr<Matx33d>()->t() * *pt2.ptr<Vec3d>();
|
|
|
|
|
|
|
|
double v = pt2.ptr<Vec3d>()->dot(F_pt1);
|
|
|
|
|
|
|
|
// square
|
|
|
|
Ft_pt2 = Ft_pt2.mul(Ft_pt2);
|
|
|
|
F_pt1 = F_pt1.mul(F_pt1);
|
|
|
|
|
|
|
|
return v*v / (F_pt1[0] + F_pt1[1] + Ft_pt2[0] + Ft_pt2[1]);
|
|
|
|
}
|
|
|
|
|
2010-05-12 01:44:00 +08:00
|
|
|
/* End of file. */
|