cvfundam.cpp.svn-base
来自「非结构化路识别」· SVN-BASE 代码 · 共 2,143 行 · 第 1/5 页
SVN-BASE
2,143 行
/*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 "_cv.h"
#include "_cvvm.h"
/* Valery Mosyagin */
/*=====================================================================================*/
/* new version of fundamental matrix functions */
/*=====================================================================================*/
int icvComputeFundamental7Point(CvMat* points1,CvMat* points2,
CvMat* fundMatr);
int icvComputeFundamental8Point(CvMat* points1,CvMat* points2,
CvMat* fundMatr);
int icvComputeFundamentalRANSAC( CvMat* points1,CvMat* points2,
CvMat* fundMatr,
double threshold,/* threshold for good point. Distance from epipolar line */
double p,/* probability of good result. Usually = 0.99 */
CvMat* status);
int icvComputeFundamentalLMedS( CvMat* points1,CvMat* points2,
CvMat* fundMatr,
double threshold,/* threshold for good point. Distance from epipolar line */
double p,/* probability of good result. Usually = 0.99 */
CvMat* status);
void icvMakeFundamentalSingular(CvMat* fundMatr);
void icvNormalizeFundPoints( CvMat* points,
CvMat* transfMatr);
int icvSolveCubic(CvMat* coeffs,CvMat* result);
void icvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint);
void icvMake3DPoints(CvMat* srcPoint,CvMat* dstPoint);
void icvComputeCorrespondEpilines(CvMat* points,int pointImageID,CvMat* fundMatr,CvMat* corrLines);
int icvCubicV( double a2, double a1, double a0, double *squares );
/*=====================================================================================*/
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvFindFundamentalMat
// Purpose: find fundamental matrix for given points using
// Context:
// Parameters:
// points1 - points on first image. Size of matrix 2xN or 3xN
// points2 - points on second image Size of matrix 2xN or 3xN
// fundMatr - found fundamental matrix (or matrixes for 7-point). Size 3x3 or 9x3
// method - method for computing fundamental matrix
// CV_FM_7POINT - for 7-point algorithm. Number of points == 7
// CV_FM_8POINT - for 8-point algorithm. Number of points >= 8
// CV_FM_RANSAC - for RANSAC algorithm. Number of points >= 8
// CV_FM_LMEDS - for LMedS algorithm. Number of points >= 8
// param1 and param2 uses for RANSAC method
// param1 - threshold distance from point to epipolar line.
// If distance less than threshold point is good.
// param2 - probability. Usually = 0.99
// status - array, every element of which will be set to 1 if the point was good
// and used for computation. 0 else. (for RANSAC and LMedS only)
// (it is optional parameter, can be NULL)
//
// Returns:
// number of found matrixes
//F*/
int cvFindFundamentalMat( CvMat* points1,
CvMat* points2,
CvMat* fundMatr,
int method,
double param1,
double param2,
CvMat* status)
{
int result = -1;
CvMat* wpoints[2]={0,0};
CvMat* tmpPoints[2]={0,0};
CV_FUNCNAME( "icvComputeFundamentalMat" );
__BEGIN__;
tmpPoints[0] = points1;
tmpPoints[1] = points2;
int numRealPoints[2];
int numPoints = 0;
/* work with points */
{
int i;
for( i = 0; i < 2; i++ )
{
int realW,realH;
realW = tmpPoints[i]->cols;
realH = tmpPoints[i]->rows;
int goodW,goodH;
goodW = realW > realH ? realW : realH;
goodH = realW < realH ? realW : realH;
if( goodH != 2 && goodH != 3 )
{
CV_ERROR(CV_StsBadPoint,"Number of coordinates of points must be 2 or 3");
}
wpoints[i] = cvCreateMat(2,goodW,CV_64F);
numRealPoints[i] = goodW;
/* Test for transpose point matrix */
if( realW != goodW )
{/* need to transpose point matrix */
CvMat* tmpPointMatr = 0;
tmpPointMatr = cvCreateMat(goodH,goodW,CV_64F);
cvTranspose(tmpPoints[i],tmpPointMatr);
cvMake2DPoints(tmpPointMatr,wpoints[i]);
cvReleaseMat(&tmpPointMatr);
}
else
{
cvMake2DPoints(tmpPoints[i],wpoints[i]);
}
}
if( numRealPoints[0] != numRealPoints[1] )
{
CV_ERROR(CV_StsBadPoint,"Number of points must be the same");
}
numPoints = numRealPoints[0];
}
/* work with status if use functions which don't compute it */
if( status && (method == CV_FM_7POINT || method == CV_FM_8POINT ))
{
if( !CV_IS_MAT(status) )
{
CV_ERROR(CV_StsBadPoint,"status is not a matrix");
}
if( !CV_IS_MAT(points1))
{
CV_ERROR(CV_StsBadPoint,"Points1 not a matrix");
}
if( status->cols != numPoints || status->rows != 1 )
{
CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
}
int i;
for( i = 0; i < status->cols; i++)
{
cvmSet(status,0,i,1.0);
}
}
switch( method )
{
case CV_FM_7POINT: result = icvComputeFundamental7Point(wpoints[1], wpoints[0], fundMatr);break;
case CV_FM_8POINT: result = icvComputeFundamental8Point(wpoints[1],wpoints[0], fundMatr);break;
case CV_FM_LMEDS : result = icvComputeFundamentalLMedS( wpoints[1],wpoints[0], fundMatr,
param1,param2,status);break;
case CV_FM_RANSAC: result = icvComputeFundamentalRANSAC( wpoints[1],wpoints[0], fundMatr,
param1,param2,status);break;
//default:return -1/*ERROR*/;
}
__END__;
cvReleaseMat(&wpoints[0]);
cvReleaseMat(&wpoints[1]);
return result;
}
/*=====================================================================================*/
/* Computes 1 or 3 fundamental matrixes using 7-point algorithm */
int icvComputeFundamental7Point(CvMat* points1, CvMat* points2, CvMat* fundMatr)
{
CvMat* squares = 0;
int numberRoots = 0;
CV_FUNCNAME( "icvComputeFundamental7Point" );
__BEGIN__;
/* Test correct of input data */
if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
{
CV_ERROR(CV_StsBadPoint,"Not a matrixes");
}
if( !CV_ARE_TYPES_EQ( points1, points2 ))
{
CV_ERROR( CV_StsUnmatchedSizes, "Data types of points unmatched" );
}
int numPoint;
numPoint = points1->cols;
int type;
type = points1->type;
if( numPoint != points2->cols )
{
CV_ERROR( CV_StsBadSize, "Number of points not equal" );
}
if( numPoint != 7 )
{
CV_ERROR( CV_StsBadSize, "Number of points must be 7" );
}
if( points1->rows != 2 && points1->rows != 3 )
{
CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
}
if( points2->rows != 2 && points2->rows != 3 )
{
CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
}
if( ( fundMatr->rows != 3 && fundMatr->rows != 9 )|| fundMatr->cols != 3 )
{
CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3 or 9x3" );
}
squares = cvCreateMat(2,3,CV_64F);
/* Make it Normalize points if need */
double wPoints1_dat[2*7];
double wPoints2_dat[2*7];
CvMat wPoints1;
CvMat wPoints2;
wPoints1 = cvMat(2,7,CV_64F,wPoints1_dat);
wPoints2 = cvMat(2,7,CV_64F,wPoints2_dat);
icvMake2DPoints(points1,&wPoints1);
icvMake2DPoints(points2,&wPoints2);
/* fill matrix U */
int currPoint;
CvMat matrU;
double matrU_dat[7*9];
matrU = cvMat(7,9,CV_64F,matrU_dat);
double* currDataLine;
currDataLine = matrU_dat;
for( currPoint = 0; currPoint < 7; currPoint++ )
{
double x1,y1,x2,y2;
x1 = cvmGet(&wPoints1,0,currPoint);
y1 = cvmGet(&wPoints1,1,currPoint);
x2 = cvmGet(&wPoints2,0,currPoint);
y2 = cvmGet(&wPoints2,1,currPoint);
currDataLine[0] = x1*x2;
currDataLine[1] = x1*y2;
currDataLine[2] = x1;
currDataLine[3] = y1*x2;
currDataLine[4] = y1*y2;
currDataLine[5] = y1;
currDataLine[6] = x2;
currDataLine[7] = y2;
currDataLine[8] = 1;
currDataLine += 9;
}
CvMat matrUU;
CvMat matrSS;
CvMat matrVV;
double matrUU_dat[7*7];
double matrSS_dat[7*9];
double matrVV_dat[9*9];
matrUU = cvMat(7,7,CV_64F,matrUU_dat);
matrSS = cvMat(7,9,CV_64F,matrSS_dat);
matrVV = cvMat(9,9,CV_64F,matrVV_dat);
cvSVD( &matrU, &matrSS, &matrUU, &matrVV, 0/*CV_SVD_V_T*/ );/* get transposed matrix V */
double F111,F112,F113;
double F121,F122,F123;
double F131,F132,F133;
double F211,F212,F213;
double F221,F222,F223;
double F231,F232,F233;
F111=cvmGet(&matrVV,0,7);
F112=cvmGet(&matrVV,1,7);
F113=cvmGet(&matrVV,2,7);
F121=cvmGet(&matrVV,3,7);
F122=cvmGet(&matrVV,4,7);
F123=cvmGet(&matrVV,5,7);
F131=cvmGet(&matrVV,6,7);
F132=cvmGet(&matrVV,7,7);
F133=cvmGet(&matrVV,8,7);
F211=cvmGet(&matrVV,0,8);
F212=cvmGet(&matrVV,1,8);
F213=cvmGet(&matrVV,2,8);
F221=cvmGet(&matrVV,3,8);
F222=cvmGet(&matrVV,4,8);
F223=cvmGet(&matrVV,5,8);
F231=cvmGet(&matrVV,6,8);
F232=cvmGet(&matrVV,7,8);
F233=cvmGet(&matrVV,8,8);
double a,b,c,d;
a = F231*F112*F223 + F231*F212*F123 - F231*F212*F223 + F231*F113*F122 -
F231*F113*F222 - F231*F213*F122 + F231*F213*F222 - F131*F112*F223 -
F131*F212*F123 + F131*F212*F223 - F131*F113*F122 + F131*F113*F222 +
F131*F213*F122 - F131*F213*F222 + F121*F212*F133 - F121*F212*F233 +
F121*F113*F132 - F121*F113*F232 - F121*F213*F132 + F121*F213*F232 +
F221*F112*F133 - F221*F112*F233 - F221*F212*F133 + F221*F212*F233 -
F221*F113*F132 + F221*F113*F232 + F221*F213*F132 - F221*F213*F232 +
F121*F112*F233 - F111*F222*F133 + F111*F222*F233 - F111*F123*F132 +
F111*F123*F232 + F111*F223*F132 - F111*F223*F232 - F211*F122*F133 +
F211*F122*F233 + F211*F222*F133 - F211*F222*F233 + F211*F123*F132 -
F211*F123*F232 - F211*F223*F132 + F211*F223*F232 + F111*F122*F133 -
F111*F122*F233 - F121*F112*F133 + F131*F112*F123 - F231*F112*F123;
b = 2*F231*F213*F122 - 3*F231*F213*F222 + F231*F112*F123 - 2*F231*F112*F223 -
2*F231*F212*F123 + 3*F231*F212*F223 - F231*F113*F122 + 2*F231*F113*F222 +
F131*F212*F123 - 2*F131*F212*F223 - F131*F113*F222 - F131*F213*F122 +
2*F131*F213*F222 + F121*F113*F232 + F121*F213*F132 - 2*F121*F213*F232 -
F221*F112*F133 + 2*F221*F112*F233 + 2*F221*F212*F133 - 3*F221*F212*F233 +
F221*F113*F132 - 2*F221*F113*F232 - 2*F221*F213*F132 + 3*F221*F213*F232 +
F131*F112*F223 - 2*F211*F122*F233 - 2*F211*F222*F133 + 3*F211*F222*F233 -
F211*F123*F132 + 2*F211*F123*F232 + 2*F211*F223*F132 - 3*F211*F223*F232 -
F121*F112*F233 - F121*F212*F133 + 2*F121*F212*F233 - 2*F111*F222*F233 -
F111*F123*F232 - F111*F223*F132 + 2*F111*F223*F232 + F111*F122*F233 +
F111*F222*F133 + F211*F122*F133;
c = F231*F112*F223 + F231*F212*F123 - 3*F231*F212*F223 - F231*F113*F222 -
F231*F213*F122 + 3*F231*F213*F222 + F131*F212*F223 - F131*F213*F222 +
F121*F213*F232 - F221*F112*F233 - F221*F212*F133 + 3*F221*F212*F233 +
F221*F113*F232 + F221*F213*F132 - 3*F221*F213*F232 + F211*F122*F233 +
F211*F222*F133 - 3*F211*F222*F233 - F211*F123*F232 - F211*F223*F132 +
3*F211*F223*F232 - F121*F212*F233 + F111*F222*F233 - F111*F223*F232;
d = F221*F213*F232 - F211*F223*F232 + F211*F222*F233 - F221*F212*F233 +
F231*F212*F223 - F231*F213*F222;
/* find root */
double coeffs_dat[4];
CvMat coeffs;
coeffs = cvMat(1,4,CV_64F,coeffs_dat);
cvmSet(&coeffs,0,0,a);
cvmSet(&coeffs,0,1,b);
cvmSet(&coeffs,0,2,c);
cvmSet(&coeffs,0,3,d);
int numCubRoots;
numCubRoots = icvSolveCubic(&coeffs,squares);
/* take real solution */
/* Need test all roots */
int i;
for( i = 0; i < numCubRoots; i++ )
{
if( fabs(cvmGet(squares,1,i)) < 1e-8 )
{//It is real square. (Im==0)
double sol;
sol = cvmGet(squares,0,i);
//F=sol*F1+(1-sol)*F2;
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?