Merge pull request #20225 from vpisarev:remove_c_3d

This commit is contained in:
Vadim Pisarevsky 2021-06-07 16:55:15 +00:00
commit 958d3e8c60
20 changed files with 1583 additions and 3608 deletions

View File

@ -522,6 +522,14 @@ public:
*/
static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters);
static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
static int run(InputOutputArray param, InputArray mask,
int nerrs, const TermCriteria& termcrit, int solveMethod,
std::function<bool (Mat& param, Mat* err, Mat* J)> callb);
static int runAlt(InputOutputArray param, InputArray mask,
const TermCriteria& termcrit, int solveMethod, bool LtoR,
std::function<bool (Mat& param, Mat* JtErr,
Mat* JtJ, double* errnorm)> callb);
};
/** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp
@ -745,7 +753,17 @@ CV_EXPORTS_W void projectPoints( InputArray objectPoints,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArray imagePoints,
OutputArray jacobian = noArray(),
double aspectRatio = 0 );
double aspectRatio = 0);
/** @overload */
CV_EXPORTS_AS(projectPointsSepJ) void projectPoints(
InputArray objectPoints,
InputArray rvec, InputArray tvec,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArray imagePoints, OutputArray dpdr,
OutputArray dpdt, OutputArray dpdf=noArray(),
OutputArray dpdc=noArray(), OutputArray dpdk=noArray(),
OutputArray dpdo=noArray(), double aspectRatio=0.);
/** @example samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp
An example program about homography from the camera displacement
@ -1988,13 +2006,13 @@ correctly only when there are more than 50% of inliers.
@sa estimateAffinePartial2D, getAffineTransform
*/
CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers = noArray(),
CV_EXPORTS_W Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers = noArray(),
int method = RANSAC, double ransacReprojThreshold = 3,
size_t maxIters = 2000, double confidence = 0.99,
size_t refineIters = 10);
CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers,
CV_EXPORTS_W Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers,
const UsacParams &params);
/** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between
@ -2356,46 +2374,6 @@ void undistortPoints(InputArray src, OutputArray dst,
InputArray R = noArray(), InputArray P = noArray(),
TermCriteria criteria=TermCriteria(TermCriteria::MAX_ITER, 5, 0.01));
//////////////////////////////////////////////////////////////////////////////////////////
// the old-style Levenberg-Marquardt solver; to be removed soon
class CV_EXPORTS CvLevMarq
{
public:
CvLevMarq();
CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
bool completeSymmFlag=false );
~CvLevMarq();
void init( int nparams, int nerrs, CvTermCriteria criteria=
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
bool completeSymmFlag=false );
bool update( const CvMat*& param, CvMat*& J, CvMat*& err );
bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm );
void clear();
void step();
enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };
cv::Ptr<CvMat> mask;
cv::Ptr<CvMat> prevParam;
cv::Ptr<CvMat> param;
cv::Ptr<CvMat> J;
cv::Ptr<CvMat> err;
cv::Ptr<CvMat> JtJ;
cv::Ptr<CvMat> JtJN;
cv::Ptr<CvMat> JtErr;
cv::Ptr<CvMat> JtJV;
cv::Ptr<CvMat> JtJW;
double prevErrNorm, errNorm;
int lambdaLg10;
CvTermCriteria criteria;
int state;
int iters;
bool completeSymmFlag;
int solveMethod;
};
//! @} _3d
} //end namespace cv

File diff suppressed because it is too large Load Diff

View File

@ -1,322 +0,0 @@
/*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 "opencv2/core/core_c.h"
/************************************************************************************\
Some backward compatibility stuff, to be moved to legacy or compat module
\************************************************************************************/
namespace cv {
////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
CvLevMarq::CvLevMarq()
{
lambdaLg10 = 0; state = DONE;
criteria = cvTermCriteria(0,0,0);
iters = 0;
completeSymmFlag = false;
errNorm = prevErrNorm = DBL_MAX;
solveMethod = cv::DECOMP_SVD;
}
CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{
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.reset(cvCreateMat( nparams, 1, CV_8U ));
cvSet(mask, cvScalarAll(1));
prevParam.reset(cvCreateMat( nparams, 1, CV_64F ));
param.reset(cvCreateMat( nparams, 1, CV_64F ));
JtJ.reset(cvCreateMat( nparams, nparams, CV_64F ));
JtErr.reset(cvCreateMat( nparams, 1, CV_64F ));
if( nerrs > 0 )
{
J.reset(cvCreateMat( nerrs, nparams, CV_64F ));
err.reset(cvCreateMat( nerrs, 1, CV_64F ));
}
errNorm = 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;
solveMethod = cv::DECOMP_SVD;
}
bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
{
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 ||
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 )
{
CV_Assert( !err );
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 ||
cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
{
_param = param;
_JtJ = JtJ;
_JtErr = JtErr;
state = DONE;
return false;
}
prevErrNorm = errNorm;
cvZero( JtJ );
cvZero( JtErr );
_param = param;
_JtJ = JtJ;
_JtErr = JtErr;
state = CALC_J;
return true;
}
static void subMatrix(const Mat& src, Mat& dst,
const std::vector<uchar>& cols,
const std::vector<uchar>& rows)
{
int nonzeros_cols = countNonZero(cols);
Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)cols.size(); i++)
{
if (cols[i])
{
src.col(i).copyTo(tmp.col(j++));
}
}
int nonzeros_rows = cv::countNonZero(rows);
dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)rows.size(); i++)
{
if (rows[i])
{
tmp.row(i).copyTo(dst.row(j++));
}
}
}
void CvLevMarq::step()
{
using namespace cv;
const double LOG10 = log(10.);
double lambda = exp(lambdaLg10*LOG10);
int nparams = param->rows;
Mat _JtJ = cvarrToMat(JtJ);
Mat _mask = cvarrToMat(mask);
int nparams_nz = countNonZero(_mask);
if(!JtJN || JtJN->rows != nparams_nz) {
// prevent re-allocation in every step
JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));
JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
}
Mat _JtJN = cvarrToMat(JtJN);
Mat _JtErr = cvarrToMat(JtJV);
Mat_<double> nonzero_param = cvarrToMat(JtJW);
subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
subMatrix(_JtJ, _JtJN, _mask, _mask);
if( !err )
completeSymm( _JtJN, completeSymmFlag );
_JtJN.diag() *= 1. + lambda;
solve(_JtJN, _JtErr, nonzero_param, solveMethod);
int j = 0;
for( int i = 0; i < nparams; i++ )
param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
}
}

View File

@ -61,21 +61,22 @@ void epnp::choose_control_points(void)
// Take C1, C2, and C3 from PCA on the reference points:
CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);
Mat PW0(number_of_correspondences, 3, CV_64F);
double pw0tpw0[3 * 3] = {}, dc[3] = {}, uct[3 * 3] = {};
CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
CvMat DC = cvMat(3, 1, CV_64F, dc);
CvMat UCt = cvMat(3, 3, CV_64F, uct);
Mat PW0tPW0(3, 3, CV_64F, pw0tpw0);
Mat DC(3, 1, CV_64F, dc);
Mat UCt(3, 3, CV_64F, uct);
for(int i = 0; i < number_of_correspondences; i++)
for(int i = 0; i < number_of_correspondences; i++) {
double* PW0row = PW0.ptr<double>(i);
for(int j = 0; j < 3; j++)
PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];
PW0row[j] = pws[3 * i + j] - cws[0][j];
}
cvMulTransposed(PW0, &PW0tPW0, 1);
cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
cvReleaseMat(&PW0);
mulTransposed(PW0, PW0tPW0, true);
SVDecomp(PW0tPW0, DC, UCt, noArray(), SVD::MODIFY_A);
transpose(UCt, UCt);
for(int i = 1; i < 4; i++) {
double k = sqrt(dc[i - 1] / number_of_correspondences);
@ -86,16 +87,14 @@ void epnp::choose_control_points(void)
void epnp::compute_barycentric_coordinates(void)
{
double cc[3 * 3] = {}, cc_inv[3 * 3] = {};
CvMat CC = cvMat(3, 3, CV_64F, cc);
CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);
Matx33d CC, CC_inv;
for(int i = 0; i < 3; i++)
for(int j = 1; j < 4; j++)
cc[3 * i + j - 1] = cws[j][i] - cws[0][i];
CC(i, j - 1) = cws[j][i] - cws[0][i];
cvInvert(&CC, &CC_inv, CV_SVD);
double * ci = cc_inv;
cv::invert(CC, CC_inv, DECOMP_SVD);
double * ci = CC_inv.val;
for(int i = 0; i < number_of_correspondences; i++) {
double * pi = &pws[0] + 3 * i;
double * a = &alphas[0] + 4 * i;
@ -111,10 +110,10 @@ void epnp::compute_barycentric_coordinates(void)
}
}
void epnp::fill_M(CvMat * M,
void epnp::fill_M(Mat& M,
const int row, const double * as, const double u, const double v)
{
double * M1 = M->data.db + row * 12;
double * M1 = M.ptr<double>(row);
double * M2 = M1 + 12;
for(int i = 0; i < 4; i++) {
@ -157,23 +156,23 @@ void epnp::compute_pose(Mat& R, Mat& t)
choose_control_points();
compute_barycentric_coordinates();
CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);
Mat M(2 * number_of_correspondences, 12, CV_64F);
for(int i = 0; i < number_of_correspondences; i++)
fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]);
double mtm[12 * 12] = {}, d[12] = {}, ut[12 * 12] = {};
CvMat MtM = cvMat(12, 12, CV_64F, mtm);
CvMat D = cvMat(12, 1, CV_64F, d);
CvMat Ut = cvMat(12, 12, CV_64F, ut);
Mat MtM(12, 12, CV_64F, mtm);
Mat D(12, 1, CV_64F, d);
Mat Ut(12, 12, CV_64F, ut);
cvMulTransposed(M, &MtM, 1);
cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
cvReleaseMat(&M);
mulTransposed(M, MtM, true);
SVDecomp(MtM, D, Ut, noArray(), SVD::MODIFY_A);
transpose(Ut, Ut);
double l_6x10[6 * 10] = {}, rho[6] = {};
CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
CvMat Rho = cvMat(6, 1, CV_64F, rho);
Mat L_6x10(6, 10, CV_64F, l_6x10);
Mat Rho(6, 1, CV_64F, rho);
compute_L_6x10(ut, l_6x10);
compute_rho(rho);
@ -181,16 +180,16 @@ void epnp::compute_pose(Mat& R, Mat& t)
double Betas[4][4] = {}, rep_errors[4] = {};
double Rs[4][3][3] = {}, ts[4][3] = {};
find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
gauss_newton(&L_6x10, &Rho, Betas[1]);
find_betas_approx_1(L_6x10, Rho, Betas[1]);
gauss_newton(L_6x10, Rho, Betas[1]);
rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
gauss_newton(&L_6x10, &Rho, Betas[2]);
find_betas_approx_2(L_6x10, Rho, Betas[2]);
gauss_newton(L_6x10, Rho, Betas[2]);
rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
gauss_newton(&L_6x10, &Rho, Betas[3]);
find_betas_approx_3(L_6x10, Rho, Betas[3]);
gauss_newton(L_6x10, Rho, Betas[3]);
rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);
int N = 1;
@ -245,13 +244,13 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3])
pw0[j] /= number_of_correspondences;
}
double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_v[3 * 3] = {};
CvMat ABt = cvMat(3, 3, CV_64F, abt);
CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);
double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_vt[3 * 3] = {};
Mat ABt(3, 3, CV_64F, abt);
Mat ABt_D(3, 1, CV_64F, abt_d);
Mat ABt_U(3, 3, CV_64F, abt_u);
Mat ABt_Vt(3, 3, CV_64F, abt_vt);
cvSetZero(&ABt);
ABt.setTo(Scalar::all(0.));
for(int i = 0; i < number_of_correspondences; i++) {
double * pc = &pcs[3 * i];
double * pw = &pws[3 * i];
@ -263,15 +262,11 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3])
}
}
cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);
SVDecomp(ABt, ABt_D, ABt_U, ABt_Vt, SVD::MODIFY_A);
Mat mR(3, 3, CV_64F, R);
gemm(ABt_U, ABt_Vt, 1, noArray(), 0, mR);
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
const double det =
R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
const double det = determinant(mR);
if (det < 0) {
R[2][0] = -R[2][0];
@ -334,21 +329,21 @@ double epnp::reprojection_error(const double R[3][3], const double t[3])
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_1 = [B11 B12 B13 B14]
void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
void epnp::find_betas_approx_1(const Mat& L_6x10, const Mat& Rho, double* betas)
{
double l_6x4[6 * 4] = {}, b4[4] = {};
CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
CvMat B4 = cvMat(4, 1, CV_64F, b4);
Mat L_6x4(6, 4, CV_64F, l_6x4);
Mat B4(4, 1, CV_64F, b4);
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));
cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
L_6x4.at<double>(i, 0) = L_6x10.at<double>(i, 0);
L_6x4.at<double>(i, 1) = L_6x10.at<double>(i, 1);
L_6x4.at<double>(i, 2) = L_6x10.at<double>(i, 3);
L_6x4.at<double>(i, 3) = L_6x10.at<double>(i, 6);
}
cvSolve(&L_6x4, Rho, &B4, CV_SVD);
solve(L_6x4, Rho, B4, DECOMP_SVD);
CV_Assert(B4.ptr<double>() == b4);
if (b4[0] < 0) {
betas[0] = sqrt(-b4[0]);
@ -366,20 +361,20 @@ void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_2 = [B11 B12 B22 ]
void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
void epnp::find_betas_approx_2(const Mat& L_6x10, const Mat& Rho, double* betas)
{
double l_6x3[6 * 3] = {}, b3[3] = {};
CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
CvMat B3 = cvMat(3, 1, CV_64F, b3);
Mat L_6x3(6, 3, CV_64F, l_6x3);
Mat B3(3, 1, CV_64F, b3);
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));
L_6x3.at<double>(i, 0) = L_6x10.at<double>(i, 0);
L_6x3.at<double>(i, 1) = L_6x10.at<double>(i, 1);
L_6x3.at<double>(i, 2) = L_6x10.at<double>(i, 2);
}
cvSolve(&L_6x3, Rho, &B3, CV_SVD);
solve(L_6x3, Rho, B3, DECOMP_SVD);
CV_Assert(B3.ptr<double>() == b3);
if (b3[0] < 0) {
betas[0] = sqrt(-b3[0]);
@ -390,7 +385,6 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
}
if (b3[1] < 0) betas[0] = -betas[0];
betas[2] = 0.0;
betas[3] = 0.0;
}
@ -398,22 +392,22 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_3 = [B11 B12 B22 B13 B23 ]
void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
void epnp::find_betas_approx_3(const Mat& L_6x10, const Mat& Rho, double * betas)
{
double l_6x5[6 * 5] = {}, b5[5] = {};
CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
CvMat B5 = cvMat(5, 1, CV_64F, b5);
Mat L_6x5(6, 5, CV_64F, l_6x5);
Mat B5(5, 1, CV_64F, b5);
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));
cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));
cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));
L_6x5.at<double>(i, 0) = L_6x10.at<double>(i, 0);
L_6x5.at<double>(i, 1) = L_6x10.at<double>(i, 1);
L_6x5.at<double>(i, 2) = L_6x10.at<double>(i, 2);
L_6x5.at<double>(i, 3) = L_6x10.at<double>(i, 3);
L_6x5.at<double>(i, 4) = L_6x10.at<double>(i, 4);
}
cvSolve(&L_6x5, Rho, &B5, CV_SVD);
solve(L_6x5, Rho, B5, DECOMP_SVD);
CV_Assert(B5.ptr<double>() == b5);
if (b5[0] < 0) {
betas[0] = sqrt(-b5[0]);
@ -479,19 +473,19 @@ void epnp::compute_rho(double * rho)
rho[5] = dist2(cws[2], cws[3]);
}
void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
const double betas[4], CvMat * A, CvMat * b)
void epnp::compute_A_and_b_gauss_newton(const Mat& L_6x10, const Mat& Rho,
const double betas[4], Mat& A, Mat& b)
{
for(int i = 0; i < 6; i++) {
const double * rowL = l_6x10 + i * 10;
double * rowA = A->data.db + i * 4;
const double * rowL = L_6x10.ptr<double>(i);
double * rowA = A.ptr<double>(i);
rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3];
rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3];
rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3];
rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
cvmSet(b, i, 0, rho[i] -
b.at<double>(i) = Rho.at<double>(i) -
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
@ -503,33 +497,32 @@ void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rh
rowL[7] * betas[1] * betas[3] +
rowL[8] * betas[2] * betas[3] +
rowL[9] * betas[3] * betas[3]
));
);
}
}
void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4])
void epnp::gauss_newton(const Mat& L_6x10, const Mat& Rho, double betas[4])
{
const int iterations_number = 5;
double a[6*4] = {}, b[6] = {}, x[4] = {};
CvMat A = cvMat(6, 4, CV_64F, a);
CvMat B = cvMat(6, 1, CV_64F, b);
CvMat X = cvMat(4, 1, CV_64F, x);
Mat A(6, 4, CV_64F, a);
Mat B(6, 1, CV_64F, b);
Mat X(4, 1, CV_64F, x);
for(int k = 0; k < iterations_number; k++)
{
compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
betas, &A, &B);
qr_solve(&A, &B, &X);
compute_A_and_b_gauss_newton(L_6x10, Rho, betas, A, B);
qr_solve(A, B, X);
for(int i = 0; i < 4; i++)
betas[i] += x[i];
}
}
void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
void epnp::qr_solve(Mat& A, Mat& b, Mat& X)
{
const int nr = A->rows;
const int nc = A->cols;
const int nr = A.rows;
const int nc = A.cols;
if (nc <= 0 || nr <= 0)
return;
@ -545,7 +538,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
A2 = new double[nr];
}
double * pA = A->data.db, * ppAkk = pA;
double * pA = A.ptr<double>(), * ppAkk = pA;
for(int k = 0; k < nc; k++)
{
double * ppAik1 = ppAkk, eta = fabs(*ppAik1);
@ -597,7 +590,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
}
// b <- Qt b
double * ppAjj = pA, * pb = b->data.db;
double * ppAjj = pA, * pb = b.ptr<double>();
for(int j = 0; j < nc; j++)
{
double * ppAij = ppAjj, tau = 0;
@ -617,7 +610,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
}
// X = R-1 b
double * pX = X->data.db;
double * pX = X.ptr<double>();
pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
for(int i = nc - 2; i >= 0; i--)
{

View File

@ -6,7 +6,6 @@
#define epnp_h
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
namespace cv {
@ -46,16 +45,16 @@ class epnp {
double reprojection_error(const double R[3][3], const double t[3]);
void choose_control_points(void);
void compute_barycentric_coordinates(void);
void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
void fill_M(Mat& M, const int row, const double * alphas, const double u, const double v);
void compute_ccs(const double * betas, const double * ut);
void compute_pcs(void);
void solve_for_sign(void);
void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void qr_solve(CvMat * A, CvMat * b, CvMat * X);
void find_betas_approx_1(const Mat& L_6x10, const Mat& Rho, double * betas);
void find_betas_approx_2(const Mat& L_6x10, const Mat& Rho, double * betas);
void find_betas_approx_3(const Mat& L_6x10, const Mat& Rho, double * betas);
void qr_solve(Mat& A, Mat& b, Mat& X);
double dot(const double * v1, const double * v2);
double dist2(const double * p1, const double * p2);
@ -63,9 +62,9 @@ class epnp {
void compute_rho(double * rho);
void compute_L_6x10(const double * ut, double * l_6x10);
void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
const double cb[4], CvMat * A, CvMat * b);
void gauss_newton(const Mat& L_6x10, const Mat& Rho, double current_betas[4]);
void compute_A_and_b_gauss_newton(const Mat& L_6x10, const Mat& Rho,
const double cb[4], Mat& A, Mat& b);
double compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3]);

View File

@ -76,133 +76,53 @@
namespace cv {
static void subMatrix(const Mat& src, Mat& dst,
const Mat& mask)
{
CV_Assert(src.type() == CV_64F && dst.type() == CV_64F);
int m = src.rows, n = src.cols;
int i1 = 0, j1 = 0;
for(int i = 0; i < m; i++)
{
if(mask.at<uchar>(i))
{
const double* srcptr = src.ptr<double>(i);
double* dstptr = dst.ptr<double>(i1++);
for(int j = j1 = 0; j < n; j++)
{
if(n < m || mask.at<uchar>(j))
dstptr[j1++] = srcptr[j];
}
}
}
}
class LMSolverImpl CV_FINAL : public LMSolver
{
public:
LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters, double _eps = FLT_EPSILON)
: cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters)
: cb(_cb), eps(_eps), maxIters(_maxIters)
{
printInterval = 0;
}
int run(InputOutputArray _param0) const CV_OVERRIDE
int run(InputOutputArray param0) const CV_OVERRIDE
{
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 );
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 )
return LMSolver::run(param0, noArray(), 0,
TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, maxIters, eps), DECOMP_SVD,
[&](Mat& param, Mat* err, Mat* J)->bool
{
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;
return cb->compute(param, err ? _OutputArray(*err) : _OutputArray(),
J ? _OutputArray(*J) : _OutputArray());
});
}
void setMaxIters(int iters) CV_OVERRIDE { CV_Assert(iters > 0); maxIters = iters; }
int getMaxIters() const CV_OVERRIDE { return maxIters; }
Ptr<LMSolver::Callback> cb;
double epsx;
double epsf;
double eps;
int maxIters;
int printInterval;
};
@ -216,4 +136,167 @@ Ptr<LMSolver> LMSolver::create(const Ptr<LMSolver::Callback>& cb, int maxIters,
return makePtr<LMSolverImpl>(cb, maxIters, eps);
}
static int LMSolver_run(InputOutputArray _param0, InputArray _mask,
int nerrs, const TermCriteria& termcrit,
int solveMethod, bool LtoR,
std::function<bool (Mat&, Mat*, Mat*)>* cb,
std::function<bool (Mat&, Mat*, Mat*, double*)>* cb_alt)
{
int lambdaLg10 = -3;
Mat mask = _mask.getMat();
Mat param0 = _param0.getMat();
Mat x, xd, r, rd, J, A, Ap, v, temp_d, d, Am, vm, dm;
int ptype = param0.type();
int maxIters = termcrit.type & TermCriteria::COUNT ? termcrit.maxCount : 1000;
double epsx = termcrit.type & TermCriteria::EPS ? termcrit.epsilon : 0, epsf = epsx;
CV_Assert( (param0.cols == 1 || param0.rows == 1) && (ptype == CV_32F || ptype == CV_64F));
CV_Assert( cb || cb_alt );
int lx = param0.rows + param0.cols - 1;
param0.convertTo(x, CV_64F);
d.create(lx, 1, CV_64F);
CV_Assert(!mask.data ||
(mask.depth() == CV_8U &&
(mask.cols == 1 || mask.rows == 1) &&
(mask.rows + mask.cols - 1 == lx)));
int lxm = mask.data ? countNonZero(mask) : lx;
if (lxm < lx) {
Am.create(lxm, lxm, CV_64F);
vm.create(lxm, 1, CV_64F);
}
if( x.cols != 1 )
transpose(x, x);
A.create(lx, lx, CV_64F);
v.create(lx, 1, CV_64F);
if (nerrs > 0) {
J.create(nerrs, lx, CV_64F);
r.create(nerrs, 1, CV_64F);
rd.create(nerrs, 1, CV_64F);
}
double S = 0;
int nfJ = 1;
if (cb_alt) {
if( !(*cb_alt)(x, &v, &A, &S) )
return -1;
completeSymm(A, LtoR);
} else {
if( !(*cb)(x, &r, &J) )
return -1;
S = norm(r, NORM_L2SQR);
mulTransposed(J, A, true);
gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
}
int i, iter = 0;
for( ;; )
{
CV_Assert( A.type() == CV_64F && A.rows == lx );
A.copyTo(Ap);
double lambda = exp(lambdaLg10*log(10.));
for( i = 0; i < lx; i++ )
Ap.at<double>(i, i) *= (1 + lambda);
if (lxm < lx) {
// remove masked-out rows & cols from JtJ and JtErr
subMatrix(Ap, Am, mask);
subMatrix(v, vm, mask);
solve(Am, vm, dm, solveMethod);
int j = 0;
// 'unpack' the param delta
for(i = j = 0; i < lx; i++)
d.at<double>(i) = mask.at<uchar>(i) != 0 ? dm.at<double>(j++) : 0.;
} else {
solve(Ap, v, d, solveMethod);
}
subtract(x, d, xd);
double Sd = 0.;
if (cb_alt) {
if( !(*cb_alt)(xd, 0, 0, &Sd) )
return -1;
} else {
if( !(*cb)(xd, &rd, 0) )
return -1;
Sd = norm(rd, NORM_L2SQR);
}
nfJ++;
if( Sd < S )
{
nfJ++;
S = Sd;
lambdaLg10 = MAX(lambdaLg10-1, -16);
iter++;
std::swap(x, xd);
if (cb_alt) {
v.setZero();
A.setZero();
Sd = 0.;
if( !(*cb_alt)(x, &v, &A, &Sd) )
return -1;
completeSymm(A, LtoR);
} else {
r.setZero();
J.setZero();
if( !(*cb)(x, &r, &J) )
return -1;
mulTransposed(J, A, true);
gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
}
} else {
iter += lambdaLg10 == 16;
lambdaLg10 = MIN(lambdaLg10+1, 16);
}
bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && S >= epsf*epsf;
/*if(lxm < lx)
{
printf("lambda=%g. delta:", lambda);
int j;
for(i = j = 0; i < lx; i++) {
double delta = d.at<double>(i);
j += delta != 0;
if(j < 10)
printf(" %.2g", delta);
}
printf("\n");
printf("%c %d %d, err=%g, param[0]=%g, d[0]=%g, lg10(lambda)=%d\n",
(proceed ? ' ' : '*'), iter, nfJ, S, x.at<double>(0), d.at<double>(0), lambdaLg10);
}*/
if(!proceed)
break;
}
if( param0.size() != x.size() )
transpose(x, x);
x.convertTo(param0, ptype);
if( iter == maxIters )
iter = -iter;
return iter;
}
int LMSolver::run(InputOutputArray param, InputArray mask, int nerrs,
const TermCriteria& termcrit, int solveMethod,
std::function<bool (Mat&, Mat*, Mat*)> cb)
{
return LMSolver_run(param, mask, nerrs, termcrit, solveMethod, true, &cb, 0);
}
int LMSolver::runAlt(InputOutputArray param, InputArray mask,
const TermCriteria& termcrit, int solveMethod, bool LtoR,
std::function<bool (Mat&, Mat*, Mat*, double*)> cb_alt)
{
return LMSolver_run(param, mask, 0, termcrit, solveMethod, LtoR, 0, &cb_alt);
}
}

View File

@ -40,85 +40,112 @@
//M*/
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
#include <iostream>
#include <opencv2/core/hal/hal.hpp>
namespace cv {
// cvCorrectMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen.
// cvTriangulatePoints function is derived from icvReconstructPointsFor3View, originally by Valery Mosyagin.
// correctMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen.
// triangulatePoints function is derived from reconstructPointsFor3View, originally by Valery Mosyagin.
// HZ, R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge Univ. Press, 2003.
// This method is the same as icvReconstructPointsFor3View, with only a few numbers adjusted for two-view geometry
static void
icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D)
// This method is the same as reconstructPointsFor3View, with only a few numbers adjusted for two-view geometry
void triangulatePoints( InputArray _P1, InputArray _P2,
InputArray _points1, InputArray _points2,
OutputArray _points4D )
{
if( projMatr1 == 0 || projMatr2 == 0 ||
projPoints1 == 0 || projPoints2 == 0 ||
points4D == 0)
CV_Error( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
CV_INSTRUMENT_REGION();
if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) ||
!CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) ||
!CV_IS_MAT(points4D) )
CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
int depth1 = points1.depth(), depth2 = points2.depth();
const float *p1f = depth1 == CV_32F ? points1.ptr<float>() : 0;
const float *p2f = depth2 == CV_32F ? points2.ptr<float>() : 0;
const double *p1d = depth1 == CV_64F ? points1.ptr<double>() : 0;
const double *p2d = depth2 == CV_64F ? points2.ptr<double>() : 0;
int pstep1, ystep1, pstep2, ystep2, npoints1, npoints2;
int numPoints = projPoints1->cols;
CV_Assert(depth1 == depth2 && (depth1 == CV_32F || depth1 == CV_64F));
if( numPoints < 1 )
CV_Error( CV_StsOutOfRange, "Number of points must be more than zero" );
if( projPoints2->cols != numPoints || points4D->cols != numPoints )
CV_Error( CV_StsUnmatchedSizes, "Number of points must be the same" );
if( projPoints1->rows != 2 || projPoints2->rows != 2)
CV_Error( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
if( points4D->rows != 4 )
CV_Error( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
projMatr2->cols != 4 || projMatr2->rows != 3)
CV_Error( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
// preallocate SVD matrices on stack
cv::Matx<double, 4, 4> matrA;
cv::Matx<double, 4, 4> matrU;
cv::Matx<double, 4, 1> matrW;
cv::Matx<double, 4, 4> matrV;
CvMat* projPoints[2] = {projPoints1, projPoints2};
CvMat* projMatrs[2] = {projMatr1, projMatr2};
/* Solve system for each point */
for( int i = 0; i < numPoints; i++ )/* For each point */
if ((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2)
{
/* Fill matrix for current point */
for( int j = 0; j < 2; j++ )/* For each view */
{
double x,y;
x = cvmGet(projPoints[j],0,i);
y = cvmGet(projPoints[j],1,i);
for( int k = 0; k < 4; k++ )
{
matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k);
matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k);
npoints1 = points1.rows + points1.cols - 1;
ystep1 = 1;
pstep1 = points1.rows == 1 ? 2 : (int)(points1.step/points1.elemSize());
}
else
{
npoints1 = points1.cols;
ystep1 = (int)(points1.step/points1.elemSize());
pstep1 = 1;
}
/* Solve system for current point */
cv::SVD::compute(matrA, matrW, matrU, matrV);
/* Copy computed point */
cvmSet(points4D,0,i,matrV(3,0));/* X */
cvmSet(points4D,1,i,matrV(3,1));/* Y */
cvmSet(points4D,2,i,matrV(3,2));/* Z */
cvmSet(points4D,3,i,matrV(3,3));/* W */
if ((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2)
{
npoints2 = points2.rows + points2.cols - 1;
ystep2 = 1;
pstep2 = points2.rows == 1 ? 2 : (int)(points2.step/points2.elemSize());
}
else
{
npoints2 = points2.cols;
ystep2 = (int)(points2.step/points2.elemSize());
pstep2 = 1;
}
CV_Assert(npoints1 == npoints2);
_points4D.create(4, npoints1, depth1);
Mat points4D = _points4D.getMat();
Matx<double, 4, 4> matrA;
Matx<double, 4, 4> matrU;
Matx<double, 4, 1> matrW;
Matx<double, 4, 4> matrV;
size_t step4 = 4*sizeof(double);
Matx<double, 3, 4> P1;
Matx<double, 3, 4> P2;
_P1.getMat().convertTo(P1, CV_64F);
_P2.getMat().convertTo(P2, CV_64F);
// Solve system for each point
for( int i = 0; i < npoints1; i++ )
{
// Fill matrix for current point
double x1 = p1f ? (double)p1f[pstep1*i] : p1d[pstep1*i];
double y1 = p1f ? (double)p1f[pstep1*i + ystep1] : p1d[pstep1*i + ystep1];
double x2 = p2f ? (double)p2f[pstep2*i] : p2d[pstep2*i];
double y2 = p2f ? (double)p2f[pstep2*i + ystep2] : p2d[pstep2*i + ystep2];
for(int k = 0; k < 4; k++)
{
matrA(k, 0) = x1*P1(2, k) - P1(0, k);
matrA(k, 1) = y1*P1(2, k) - P1(1, k);
matrA(k, 2) = x2*P2(2, k) - P2(0, k);
matrA(k, 3) = y2*P2(2, k) - P2(1, k);
}
// Solve system for current point
hal::SVD64f(matrA.val, step4, matrW.val, matrU.val, step4, matrV.val, step4, 4, 4, 4);
// Copy computed point
if(depth1 == CV_32F)
{
points4D.at<float>(0, i) = (float)matrV(3, 0);
points4D.at<float>(1, i) = (float)matrV(3, 1);
points4D.at<float>(2, i) = (float)matrV(3, 2);
points4D.at<float>(3, i) = (float)matrV(3, 3);
}
else
{
points4D.at<double>(0, i) = matrV(3, 0);
points4D.at<double>(1, i) = matrV(3, 1);
points4D.at<double>(2, i) = matrV(3, 2);
points4D.at<double>(3, i) = matrV(3, 3);
}
}
}
/*
* The Optimal Triangulation Method (see HZ for details)
* For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F,
@ -127,182 +154,118 @@ icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvM
* is the geometric distance between points a and b) subject to the epipolar constraint
* new_points2' * F * new_points1 = 0.
*
* F_ : 3x3 fundamental matrix
* points1_ : 1xN matrix containing the first set of points
* points2_ : 1xN matrix containing the second set of points
* new_points1 : the optimized points1_. if this is NULL, the corrected points are placed back in points1_
* new_points2 : the optimized points2_. if this is NULL, the corrected points are placed back in points2_
* _F : 3x3 fundamental matrix
* _points1 : 1xN matrix containing the first set of points
* _points2 : 1xN matrix containing the second set of points
* _newPoints1 : the optimized _points1.
* _newPoints2 : the optimized -points2.
*/
static void
icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2)
void correctMatches( InputArray _F, InputArray _points1, InputArray _points2,
OutputArray _newPoints1, OutputArray _newPoints2 )
{
cv::Ptr<CvMat> tmp33;
cv::Ptr<CvMat> tmp31, tmp31_2;
cv::Ptr<CvMat> T1i, T2i;
cv::Ptr<CvMat> R1, R2;
cv::Ptr<CvMat> TFT, TFTt, RTFTR;
cv::Ptr<CvMat> U, S, V;
cv::Ptr<CvMat> e1, e2;
cv::Ptr<CvMat> polynomial;
cv::Ptr<CvMat> result;
cv::Ptr<CvMat> points1, points2;
cv::Ptr<CvMat> F;
CV_INSTRUMENT_REGION();
if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) )
CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
if (!( F_->cols == 3 && F_->rows == 3))
CV_Error( CV_StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix");
if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 ))
CV_Error( CV_StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" );
if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols))
CV_Error( CV_StsUnmatchedSizes, "The point-matrices must have one row, and an equal number of columns" );
if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
int depth1 = points1.depth(), depth2 = points2.depth();
CV_Assert((depth1 == CV_32F || depth1 == CV_64F) && depth1 == depth2);
CV_Assert(points1.size() == points2.size());
CV_Assert(points1.rows == 1 || points1.cols == 1);
if (points1.channels() != 2)
CV_Error( CV_StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" );
if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
if (points2.channels() != 2)
CV_Error( CV_StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" );
if (new_points1 != NULL) {
CV_Assert(CV_IS_MAT(new_points1));
if (new_points1->cols != points1_->cols || new_points1->rows != 1)
CV_Error( CV_StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" );
if (CV_MAT_CN(new_points1->type) != 2)
CV_Error( CV_StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" );
}
if (new_points2 != NULL) {
CV_Assert(CV_IS_MAT(new_points2));
if (new_points2->cols != points2_->cols || new_points2->rows != 1)
CV_Error( CV_StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" );
if (CV_MAT_CN(new_points2->type) != 2)
CV_Error( CV_StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" );
}
_newPoints1.create(points1.size(), points1.type());
_newPoints2.create(points2.size(), points2.type());
Mat newPoints1 = _newPoints1.getMat(), newPoints2 = _newPoints2.getMat();
Matx33d F, U, Vt;
Matx31d S;
int npoints = points1.rows + points1.cols - 1;
// Make sure F uses double precision
F.reset(cvCreateMat(3,3,CV_64FC1));
cvConvert(F_, F);
_F.getMat().convertTo(F, CV_64F);
// Make sure points1 uses double precision
points1.reset(cvCreateMat(points1_->rows,points1_->cols,CV_64FC2));
cvConvert(points1_, points1);
// Make sure points2 uses double precision
points2.reset(cvCreateMat(points2_->rows,points2_->cols,CV_64FC2));
cvConvert(points2_, points2);
tmp33.reset(cvCreateMat(3,3,CV_64FC1));
tmp31.reset(cvCreateMat(3,1,CV_64FC1)), tmp31_2.reset(cvCreateMat(3,1,CV_64FC1));
T1i.reset(cvCreateMat(3,3,CV_64FC1)), T2i.reset(cvCreateMat(3,3,CV_64FC1));
R1.reset(cvCreateMat(3,3,CV_64FC1)), R2.reset(cvCreateMat(3,3,CV_64FC1));
TFT.reset(cvCreateMat(3,3,CV_64FC1)), TFTt.reset(cvCreateMat(3,3,CV_64FC1)), RTFTR.reset(cvCreateMat(3,3,CV_64FC1));
U.reset(cvCreateMat(3,3,CV_64FC1));
S.reset(cvCreateMat(3,3,CV_64FC1));
V.reset(cvCreateMat(3,3,CV_64FC1));
e1.reset(cvCreateMat(3,1,CV_64FC1)), e2.reset(cvCreateMat(3,1,CV_64FC1));
double x1, y1, x2, y2;
double scale;
double f1, f2, a, b, c, d;
polynomial.reset(cvCreateMat(1,7,CV_64FC1));
result.reset(cvCreateMat(1,6,CV_64FC2));
double t_min, s_val, t, s;
for (int p = 0; p < points1->cols; ++p) {
for (int p = 0; p < npoints; ++p) {
// Replace F by T2-t * F * T1-t
x1 = points1->data.db[p*2];
y1 = points1->data.db[p*2+1];
x2 = points2->data.db[p*2];
y2 = points2->data.db[p*2+1];
double x1, y1, x2, y2;
if (depth1 == CV_32F) {
Point2f p1 = points1.at<Point2f>(p);
Point2f p2 = points2.at<Point2f>(p);
x1 = p1.x; y1 = p1.y;
x2 = p2.x; y2 = p2.y;
} else {
Point2d p1 = points1.at<Point2d>(p);
Point2d p2 = points2.at<Point2d>(p);
x1 = p1.x; y1 = p1.y;
x2 = p2.x; y2 = p2.y;
}
cvSetZero(T1i);
cvSetReal2D(T1i,0,0,1);
cvSetReal2D(T1i,1,1,1);
cvSetReal2D(T1i,2,2,1);
cvSetReal2D(T1i,0,2,x1);
cvSetReal2D(T1i,1,2,y1);
cvSetZero(T2i);
cvSetReal2D(T2i,0,0,1);
cvSetReal2D(T2i,1,1,1);
cvSetReal2D(T2i,2,2,1);
cvSetReal2D(T2i,0,2,x2);
cvSetReal2D(T2i,1,2,y2);
cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T);
cvSetZero(TFT);
cvGEMM(tmp33,T1i,1,0,0,TFT);
Matx33d T1i(1, 0, x1,
0, 1, y1,
0, 0, 1);
Matx33d T2i(1, 0, x2,
0, 1, y2,
0, 0, 1);
Matx33d TFT = T2i.t()*F*T1i;
// Compute the right epipole e1 from F * e1 = 0
cvSetZero(U);
cvSetZero(S);
cvSetZero(V);
cvSVD(TFT,S,U,V);
scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale);
cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale);
cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale);
if (cvGetReal2D(e1,2,0) < 0) {
cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0));
cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0));
cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0));
}
SVDecomp(TFT, S, U, Vt);
double scale = sqrt(Vt(2, 0)*Vt(2, 0) + Vt(2, 1)*Vt(2, 1));
Vec3d e1(Vt(2, 0)/scale, Vt(2, 1)/scale, Vt(2, 2)/scale);
if (e1(2) < 0)
e1 = -e1;
// Compute the left epipole e2 from e2' * F = 0 => F' * e2 = 0
cvSetZero(TFTt);
cvTranspose(TFT, TFTt);
cvSetZero(U);
cvSetZero(S);
cvSetZero(V);
cvSVD(TFTt,S,U,V);
cvSetZero(e2);
scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale);
cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale);
cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale);
if (cvGetReal2D(e2,2,0) < 0) {
cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0));
cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0));
cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0));
}
scale = sqrt(U(0, 2)*U(0, 2) + U(1, 2)*U(1, 2));
Vec3d e2(U(0, 2)/scale, U(1, 2)/scale, U(2, 2)/scale);
if (e2(2) < 0)
e2 = -e2;
// Replace F by R2 * F * R1'
cvSetZero(R1);
cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0));
cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0));
cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0));
cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0));
cvSetReal2D(R1,2,2,1);
cvSetZero(R2);
cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0));
cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0));
cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0));
cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0));
cvSetReal2D(R2,2,2,1);
cvGEMM(R2,TFT,1,0,0,tmp33);
cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T);
Matx33d R1_t(e1(0), -e1(1), 0,
e1(1), e1(0), 0,
0, 0, 1);
Matx33d R2(e2(0), e2(1), 0,
-e2(1), e2(0), 0,
0, 0, 1);
Matx33d RTFTR = R2*TFT*R1_t;
// Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33
f1 = cvGetReal2D(e1,2,0);
f2 = cvGetReal2D(e2,2,0);
a = cvGetReal2D(RTFTR,1,1);
b = cvGetReal2D(RTFTR,1,2);
c = cvGetReal2D(RTFTR,2,1);
d = cvGetReal2D(RTFTR,2,2);
double f1 = e1(2);
double f2 = e2(2);
double a = RTFTR(1,1);
double b = RTFTR(1,2);
double c = RTFTR(2,1);
double d = RTFTR(2,2);
// Form the polynomial g(t) = k6*t^6 + k5*t^5 + k4*t^4 + k3*t^3 + k2*t^2 + k1*t + k0
// from f1, f2, a, b, c and d
cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c ));
cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a ));
cvSetReal2D(polynomial,0,4,( +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d ));
cvSetReal2D(polynomial,0,3,( +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d ));
cvSetReal2D(polynomial,0,2,( +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d ));
cvSetReal2D(polynomial,0,1,( +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c ));
cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d ));
Vec<double, 7> polynomial(
-a*d*d*b+b*b*c*d,
+f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c,
+4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d,
+6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d,
+4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d,
+f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a,
+b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c);
// Solve g(t) for t to get 6 roots
cvSetZero(result);
cvSolvePoly(polynomial, result, 100, 20);
double rdata[6*2];
Mat result(6, 1, CV_64FC2, rdata);
solvePoly(polynomial, result);
// Evaluate the cost function s(t) at the real part of the 6 roots
t_min = DBL_MAX;
s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c);
double t_min = DBL_MAX;
double s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c);
for (int ti = 0; ti < 6; ++ti) {
t = result->data.db[2*ti];
s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d));
Vec2d root_i = result.at<Vec2d>(ti);
double t = root_i(0);
double s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d));
if (s < s_val) {
s_val = s;
t_min = t;
@ -310,83 +273,27 @@ icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points
}
// find the optimal x1 and y1 as the points on l1 and l2 closest to the origin
tmp31->data.db[0] = t_min*t_min*f1;
tmp31->data.db[1] = t_min;
tmp31->data.db[2] = t_min*t_min*f1*f1+1;
tmp31->data.db[0] /= tmp31->data.db[2];
tmp31->data.db[1] /= tmp31->data.db[2];
tmp31->data.db[2] /= tmp31->data.db[2];
cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T);
cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
x1 = tmp31_2->data.db[0];
y1 = tmp31_2->data.db[1];
scale = t_min*t_min*f1*f1+1;
Vec3d tmp31(t_min*t_min*f1/scale, t_min/scale, 1);
Vec3d tmp31_2 = T1i*(R1_t*tmp31);
x1 = tmp31_2(0);
y1 = tmp31_2(1);
tmp31->data.db[0] = f2*pow(c*t_min+d,2);
tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d);
tmp31->data.db[2] = f2*f2*pow(c*t_min+d,2) + pow(a*t_min+b,2);
tmp31->data.db[0] /= tmp31->data.db[2];
tmp31->data.db[1] /= tmp31->data.db[2];
tmp31->data.db[2] /= tmp31->data.db[2];
cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T);
cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
x2 = tmp31_2->data.db[0];
y2 = tmp31_2->data.db[1];
scale = f2*f2*(c*t_min+d)*(c*t_min+d) + (a*t_min+b)*(a*t_min+b);
tmp31 = Vec3d(f2*(c*t_min+d)*(c*t_min+d)/scale, -(a*t_min+b)*(c*t_min+d)/scale, 1);
tmp31_2 = T2i*(R2.t()*tmp31);
x2 = tmp31_2(0);
y2 = tmp31_2(1);
// Return the points in the matrix format that the user wants
points1->data.db[p*2] = x1;
points1->data.db[p*2+1] = y1;
points2->data.db[p*2] = x2;
points2->data.db[p*2+1] = y2;
if (depth1 == CV_32F) {
newPoints1.at<Point2f>(p) = Point2f((float)x1, (float)y1);
newPoints2.at<Point2f>(p) = Point2f((float)x2, (float)y2);
} else {
newPoints1.at<Point2d>(p) = Point2d(x1, y1);
newPoints2.at<Point2d>(p) = Point2d(x2, y2);
}
}
if( new_points1 )
cvConvert( points1, new_points1 );
if( new_points2 )
cvConvert( points2, new_points2 );
}
void triangulatePoints( InputArray _projMatr1, InputArray _projMatr2,
InputArray _projPoints1, InputArray _projPoints2,
OutputArray _points4D )
{
CV_INSTRUMENT_REGION();
Mat matr1 = _projMatr1.getMat(), matr2 = _projMatr2.getMat();
Mat points1 = _projPoints1.getMat(), points2 = _projPoints2.getMat();
if((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2)
points1 = points1.reshape(1, static_cast<int>(points1.total())).t();
if((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2)
points2 = points2.reshape(1, static_cast<int>(points2.total())).t();
CvMat cvMatr1 = cvMat(matr1), cvMatr2 = cvMat(matr2);
CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2);
_points4D.create(4, points1.cols, points1.type());
Mat cvPoints4D_ = _points4D.getMat();
CvMat cvPoints4D = cvMat(cvPoints4D_);
icvTriangulatePoints(&cvMatr1, &cvMatr2, &cvPoints1, &cvPoints2, &cvPoints4D);
}
void correctMatches( InputArray _F, InputArray _points1, InputArray _points2,
OutputArray _newPoints1, OutputArray _newPoints2 )
{
CV_INSTRUMENT_REGION();
Mat F = _F.getMat();
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2);
CvMat cvF = cvMat(F);
_newPoints1.create(points1.size(), points1.type());
_newPoints2.create(points2.size(), points2.type());
Mat cvNewPoints1_ = _newPoints1.getMat(), cvNewPoints2_ = _newPoints2.getMat();
CvMat cvNewPoints1 = cvMat(cvNewPoints1_), cvNewPoints2 = cvMat(cvNewPoints2_);
icvCorrectMatches(&cvF, &cvPoints1, &cvPoints2, &cvNewPoints1, &cvNewPoints2);
}
}

View File

@ -1960,6 +1960,7 @@ TEST(Calib3d_SolvePnPRansac, minPoints)
TEST(Calib3d_SolvePnPRansac, inputShape)
{
double eps = 2e-6;
//https://github.com/opencv/opencv/issues/14423
Mat matK = Mat::eye(3,3,CV_64FC1);
Mat distCoeff = Mat::zeros(1,5,CV_64FC1);
@ -1987,8 +1988,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape)
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps);
}
{
//1xN 3-channel
@ -2012,8 +2013,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape)
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps);
}
{
//Nx1 3-channel
@ -2037,8 +2038,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape)
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps);
}
{
//vector<Point3f>
@ -2056,8 +2057,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape)
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps);
}
{
//vector<Point3d>
@ -2075,8 +2076,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape)
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps);
}
}

View File

@ -842,7 +842,7 @@ CV_EXPORTS_AS(calibrateCameraExtended) double calibrateCamera( InputArrayOfArray
OutputArray stdDeviationsExtrinsics,
OutputArray perViewErrors,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) );
/** @overload */
CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
@ -850,7 +850,7 @@ CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) );
/** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern.
@ -919,7 +919,7 @@ CV_EXPORTS_AS(calibrateCameraROExtended) double calibrateCameraRO( InputArrayOfA
OutputArray stdDeviationsObjPoints,
OutputArray perViewErrors,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) );
/** @overload */
CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints,
@ -928,7 +928,7 @@ CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
OutputArray newObjPoints,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) );
/** @brief Computes useful camera characteristics from the camera intrinsic matrix.
@ -1084,7 +1084,7 @@ CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArray
InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2,
Size imageSize, InputOutputArray R,InputOutputArray T, OutputArray E, OutputArray F,
OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) );
/// @overload
CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
@ -1093,7 +1093,7 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2,
Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F,
int flags = CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) );
/** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$

File diff suppressed because it is too large Load Diff

View File

@ -1436,6 +1436,7 @@ void CV_StereoCalibrationTest::run( int )
+ CALIB_FIX_K3
+ CALIB_FIX_K4 + CALIB_FIX_K5 //+ CV_CALIB_FIX_K6
);
err /= nframes*npoints;
if( err > maxReprojErr )
{
@ -1945,7 +1946,8 @@ TEST(Calib3d_StereoCalibrate_CPP, extended)
double res1 = stereoCalibrate( objpt, imgpt1, imgpt2,
K1, c1, K2, c2,
imageSize, R, T, E, F, err, flags);
EXPECT_LE(res1, res0);
printf("res0 = %g, res1 = %g\n", res0, res1);
EXPECT_LE(res1, res0*1.1);
EXPECT_TRUE(err.total() == 2);
}

View File

@ -232,8 +232,6 @@ protected:
void run(int /* start_from */ )
{
Mat zeros(1, sizeof(CvMat), CV_8U, Scalar(0));
Mat src_cpp(3, 1, CV_32F);
Mat dst_cpp(3, 3, CV_32F);

View File

@ -370,6 +370,7 @@ public:
void release() const;
void clear() const;
void setTo(const _InputArray& value, const _InputArray & mask = _InputArray()) const;
void setZero() const;
void assign(const UMat& u) const;
void assign(const Mat& m) const;
@ -1259,6 +1260,10 @@ public:
*/
Mat& setTo(InputArray value, InputArray mask=noArray());
/** @brief Sets all the array elements to 0.
*/
Mat& setZero();
/** @brief Changes the shape and/or the number of channels of a 2D matrix without copying the data.
The method makes a new matrix header for \*this elements. The new matrix may have a different size

View File

@ -58,6 +58,8 @@
namespace cv
{
class CV_EXPORTS _OutputArray;
//! @addtogroup core_basic
//! @{
@ -215,6 +217,10 @@ public:
template<int l> Matx(const Matx<_Tp, m, l>& a, const Matx<_Tp, l, n>& b, Matx_MatMulOp);
Matx(const Matx<_Tp, n, m>& a, Matx_TOp);
//! copy & convert
void copyTo(const _OutputArray& dst) const;
void convertTo(const _OutputArray& dst, int type, double scale=1., double shift=0.) const;
_Tp val[m*n]; //< matrix elements
};
@ -970,8 +976,6 @@ Matx<_Tp, m, n> MatxCommaInitializer<_Tp, m, n>::operator *() const
return *dst;
}
/////////////////////////////////// Vec Implementation ///////////////////////////////////
template<typename _Tp, int cn> inline

View File

@ -247,7 +247,17 @@ Matx<_Tp, n, l> Matx<_Tp, m, n>::solve(const Matx<_Tp, m, l>& rhs, int method) c
return ok ? x : Matx<_Tp, n, l>::zeros();
}
template<typename _Tp, int m, int n> inline
void Matx<_Tp, m, n>::copyTo(OutputArray dst) const
{
Mat(*this, false).copyTo(dst);
}
template<typename _Tp, int m, int n> inline
void Matx<_Tp, m, n>::convertTo(OutputArray dst, int type, double scale, double shift) const
{
Mat(*this, false).convertTo(dst, type, scale, shift);
}
////////////////////////// Augmenting algebraic & logical operations //////////////////////////

View File

@ -870,6 +870,7 @@ public:
@param epsilon The desired accuracy or change in parameters at which the iterative algorithm stops.
*/
TermCriteria(int type, int maxCount, double epsilon);
TermCriteria(int maxCount, double epsilon);
inline bool isValid() const
{
@ -2469,6 +2470,26 @@ inline
TermCriteria::TermCriteria(int _type, int _maxCount, double _epsilon)
: type(_type), maxCount(_maxCount), epsilon(_epsilon) {}
inline TermCriteria::TermCriteria(int _maxCount, double _epsilon)
{
type = 0;
if (_maxCount > 0)
{
maxCount = _maxCount;
type = COUNT;
}
else
maxCount = INT_MAX-1;
if (_epsilon > 0)
{
epsilon = _epsilon;
type |= EPS;
}
else
epsilon = DBL_EPSILON;
}
//! @endcond
} // cv

View File

@ -661,6 +661,25 @@ Mat& Mat::setTo(InputArray _value, InputArray _mask)
}
Mat& Mat::setZero()
{
CV_INSTRUMENT_REGION();
if( empty() )
return *this;
size_t esz = elemSize();
const Mat* arrays[] = { this, 0 };
uchar* ptrs[]={0};
NAryMatIterator it(arrays, ptrs);
for( size_t i = 0; i < it.nplanes; i++, ++it )
memset(ptrs[0], 0, esz*it.size);
return *this;
}
#if defined HAVE_OPENCL && !defined __APPLE__
static bool ocl_repeat(InputArray _src, int ny, int nx, OutputArray _dst)

View File

@ -1863,6 +1863,22 @@ void _OutputArray::setTo(const _InputArray& arr, const _InputArray & mask) const
CV_Error(Error::StsNotImplemented, "");
}
void _OutputArray::setZero() const
{
_InputArray::KindFlag k = kind();
if( k == NONE )
;
else if (k == MAT || k == MATX || k == STD_VECTOR)
{
Mat m = getMat();
m.setZero();
}
else
{
setTo(Scalar::all(0), noArray());
}
}
void _OutputArray::assign(const UMat& u) const
{

View File

@ -40,7 +40,6 @@
//M*/
#include "test_precomp.hpp"
#include "opencv2/core/core_c.h"
namespace opencv_test { namespace {

View File

@ -41,7 +41,6 @@
//M*/
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
#include "opencv2/core/cvdef.h"
using namespace cv;
@ -253,44 +252,17 @@ bool BundleAdjusterBase::estimate(const std::vector<ImageFeatures> &features,
total_num_matches_ += static_cast<int>(pairwise_matches[edges_[i].first * num_images_ +
edges_[i].second].num_inliers);
CvLevMarq solver(num_images_ * num_params_per_cam_,
total_num_matches_ * num_errs_per_measurement_,
cvTermCriteria(term_criteria_));
Mat err, jac;
CvMat matParams = cvMat(cam_params_);
cvCopy(&matParams, solver.param);
int iter = 0;
for(;;)
int nerrs = total_num_matches_ * num_errs_per_measurement_;
LMSolver::run(cam_params_, Mat(), nerrs, term_criteria_, DECOMP_SVD,
[&](Mat& param, Mat* err, Mat* jac)
{
const CvMat* _param = 0;
CvMat* _jac = 0;
CvMat* _err = 0;
bool proceed = solver.update(_param, _jac, _err);
cvCopy(_param, &matParams);
if (!proceed || !_err)
break;
if (_jac)
{
calcJacobian(jac);
CvMat tmp = cvMat(jac);
cvCopy(&tmp, _jac);
}
if (_err)
{
calcError(err);
LOG_CHAT(".");
iter++;
CvMat tmp = cvMat(err);
cvCopy(&tmp, _err);
}
}
param.copyTo(cam_params_);
if (jac)
calcJacobian(*jac);
if (err)
calcError(*err);
return true;
});
LOGLN_CHAT("");
LOGLN_CHAT("Bundle adjustment, final RMS error: " << std::sqrt(err.dot(err) / total_num_matches_));