mirror of
https://github.com/opencv/opencv.git
synced 2024-12-04 16:59:12 +08:00
* refactored the remaining old-style functions in 3d and calib modules to use the new C++ API.
* extended C++ version of Levenberg-Marquardt (LM) solver to accommodate all features of the C counterpart. * removed C version of LM solver * made a few other little changes to make the code compile and run smoothly
This commit is contained in:
Vadim Pisarevsky
parent
c105402dfc
commit
eff6d32337
@ -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
|
||||
@ -1961,13 +1979,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 ¶ms);
|
||||
|
||||
/** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between
|
||||
@ -2329,46 +2347,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
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
@ -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--)
|
||||
{
|
||||
|
||||
@ -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]);
|
||||
|
||||
@ -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 )
|
||||
{
|
||||
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 )
|
||||
return LMSolver::run(param0, noArray(), 0,
|
||||
TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, maxIters, eps), DECOMP_SVD,
|
||||
[&](Mat& param, Mat* err, Mat* J)->bool
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
/* Solve system for current point */
|
||||
cv::SVD::compute(matrA, matrW, matrU, matrV);
|
||||
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;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -843,7 +843,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,
|
||||
@ -851,7 +851,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.
|
||||
|
||||
@ -920,7 +920,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,
|
||||
@ -929,7 +929,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.
|
||||
|
||||
@ -1085,7 +1085,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,
|
||||
@ -1094,7 +1094,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
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 //////////////////////////
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -40,7 +40,6 @@
|
||||
//M*/
|
||||
|
||||
#include "test_precomp.hpp"
|
||||
#include "opencv2/core/core_c.h"
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
|
||||
@ -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(;;)
|
||||
{
|
||||
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)
|
||||
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)
|
||||
{
|
||||
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_));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user