diff --git a/modules/calib3d/include/opencv2/calib3d/private.hpp b/modules/calib3d/include/opencv2/calib3d/private.hpp new file mode 100644 index 0000000000..bc3d8a300f --- /dev/null +++ b/modules/calib3d/include/opencv2/calib3d/private.hpp @@ -0,0 +1,108 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#ifndef OPENCV_CALIB3D_PRIVATE_HPP +#define OPENCV_CALIB3D_PRIVATE_HPP + +#ifndef __OPENCV_BUILD +# error this is a private header which should not be used from outside of the OpenCV library +#endif + +#include "opencv2/core.hpp" + +//! @cond IGNORED + +namespace cv +{ + +// C++ version of the CvLevMarq solver. +// Main differences between LMSolver and CvLevMarq: +// 1. Damping Factor ($\lambda$) Adjustment +// LevMarq: Uses a static, simple scaling approach. If an iteration reduces the error, $\lambda$ is divided by 10. If the error increases, $\lambda$ is multiplied by 10 (repeatedly within the same step, up to 16 times) until a descending step is found. +// LMSolver: Uses an advanced adaptive gain ratio $R = \frac{\Delta S_{\text{actual}}}{\Delta S_{\text{predicted}}}$. If the step is very successful ($R > 0.75$), $\lambda$ is aggressively halved or set to 0. If the step is poor ($R < 0.25$), $\lambda$ is dynamically multiplied by a computed factor $\nu$ derived from the exact residual mismatch. +// 2. Diagonal Augmentation Strategy +// LevMarq: Directly scales the updated main diagonal of $J^T J$ by $1 + \lambda$ on every step. +// LMSolver: Caches a reference diagonal $D = \operatorname{diag}(J^T J)$ at the start of every accepted step and augments the diagonal by adding $\lambda \cdot D$. +// 3. Stopping Criteria +// LevMarq: Stops when a maximum iteration count is reached, or when the relative $L_2$ norm of the parameter update step falls below $\epsilon$ ($\frac{|\Delta x|_2}{|x|_2} < \epsilon$). +// LMSolver: Checks both the infinity norm of the parameter updates ($|\Delta x|\infty < \epsilon$) and the infinity norm of the residual errors ($|r|\infty < \epsilon$) independently. +class CV_EXPORTS LevMarq +{ +public: + LevMarq(); + LevMarq( int nparams, int nerrs, TermCriteria criteria= + TermCriteria(TermCriteria::COUNT + TermCriteria::EPS,30,DBL_EPSILON), + bool completeSymmFlag=false ); + void init( int nparams, int nerrs, TermCriteria criteria= + TermCriteria(TermCriteria::COUNT + TermCriteria::EPS,30,DBL_EPSILON), + bool completeSymmFlag=false ); + bool update( Mat1d& param, Mat1d& J, Mat1d& err ); + bool updateAlt( Mat1d& param, Mat1d& JtJ, Mat1d& JtErr, double*& errNorm ); + + void step(); + enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 }; + + Mat1b mask; + Mat1d prevParam; + Mat1d param; + Mat1d J; + Mat1d err; + Mat1d JtJ; + Mat1d JtJN; + Mat1d JtErr; + Mat1d JtJV; + Mat1d JtJW; + double prevErrNorm, errNorm; + int lambdaLg10; + TermCriteria criteria; + int state; + int iters; + bool completeSymmFlag; + int solveMethod; +}; + +} // namespace cv + +//! @endcond + +#endif // OPENCV_CALIB3D_PRIVATE_HPP diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 0c60a9b12c..b586a55707 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -43,8 +43,6 @@ #include "precomp.hpp" #include "hal_replacement.hpp" #include "distortion_model.hpp" -#include "opencv2/core/core_c.h" -#include "opencv2/calib3d/calib3d_c.h" #include /* @@ -542,9 +540,9 @@ static void initIntrinsicParams2D( const Mat& objectPoints, 0, 0, 1)).copyTo(cameraMatrix); } -static void subMatrix(const Mat& src, Mat& dst, - const std::vector& cols, - const std::vector& rows) +static void subMatrixWithIndices(const Mat& src, Mat& dst, + const std::vector& cols, + const std::vector& rows) { CV_Assert(src.type() == CV_64F && dst.type() == CV_64F); int m = (int)rows.size(), n = (int)cols.size(); @@ -569,6 +567,217 @@ static void subMatrix(const Mat& src, Mat& dst, } } +LevMarq::LevMarq() +{ + lambdaLg10 = 0; state = DONE; + iters = 0; + completeSymmFlag = false; + errNorm = prevErrNorm = DBL_MAX; + solveMethod = DECOMP_SVD; +} + +LevMarq::LevMarq( int nparams, int nerrs, TermCriteria criteria0, bool _completeSymmFlag ) +{ + init(nparams, nerrs, criteria0, _completeSymmFlag); +} + +void LevMarq::init( int nparams, int nerrs, TermCriteria criteria0, bool _completeSymmFlag ) +{ + mask.create( nparams, 1); + mask.setTo(1); + prevParam.create( nparams, 1); + param.create(nparams, 1); + JtJ.create(nparams, nparams); + JtErr.create(nparams, 1); + if( nerrs > 0 ) + { + J.create(nerrs, nparams); + err.create( nerrs, 1); + } + errNorm = prevErrNorm = DBL_MAX; + lambdaLg10 = -3; + criteria = criteria0; + if( criteria.type & TermCriteria::COUNT ) + criteria.maxCount = MIN(MAX(criteria.maxCount,1),1000); + else + criteria.maxCount = 30; + if( criteria.type & TermCriteria::EPS ) + criteria.epsilon = MAX(criteria.epsilon, 0); + else + criteria.epsilon = DBL_EPSILON; + state = STARTED; + iters = 0; + completeSymmFlag = _completeSymmFlag; + solveMethod = DECOMP_SVD; +} + +bool LevMarq::update( Mat1d& _param, Mat1d& matJ, Mat1d& _err ) +{ + CV_Assert( !err.empty() ); + if( state == DONE ) + { + _param = param; + return false; + } + + if( state == STARTED ) + { + _param = param; + J.setTo(0); + err.setTo(0); + matJ = J; + _err = err; + state = CALC_J; + return true; + } + + if( state == CALC_J ) + { + Mat(J.t() * J).copyTo(JtJ); + JtErr = J.t() * err; + param.copyTo(prevParam); + step(); + if( iters == 0 ) + prevErrNorm = norm(err, NORM_L2); + _param = param; + err.setTo(0); + _err = err; + state = CHECK_ERR; + return true; + } + + CV_Assert( state == CHECK_ERR ); + errNorm = norm( err, NORM_L2 ); + if( errNorm > prevErrNorm ) + { + if( ++lambdaLg10 <= 16 ) + { + step(); + _param = param; + err.setTo(0); + _err = err; + state = CHECK_ERR; + return true; + } + } + + lambdaLg10 = MAX(lambdaLg10-1, -16); + if( ++iters >= criteria.maxCount || + norm(param, prevParam, NORM_L2 | NORM_RELATIVE) < criteria.epsilon ) + { + _param = param; + state = DONE; + return true; + } + + prevErrNorm = errNorm; + _param = param; + J.setTo(0); + matJ = J; + _err = err; + state = CALC_J; + return true; +} + + +bool LevMarq::updateAlt( Mat1d& _param, Mat1d& _JtJ, Mat1d& _JtErr, double*& _errNorm ) +{ + CV_Assert( err.empty() ); + if( state == DONE ) + { + _param = param; + return false; + } + + if( state == STARTED ) + { + _param = param; + JtJ.setTo(0); + JtErr.setTo(0); + errNorm = 0; + _JtJ = JtJ; + _JtErr = JtErr; + _errNorm = &errNorm; + state = CALC_J; + return true; + } + + if( state == CALC_J ) + { + param.copyTo(prevParam); + step(); + _param = param; + prevErrNorm = errNorm; + errNorm = 0; + _errNorm = &errNorm; + state = CHECK_ERR; + return true; + } + + CV_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.maxCount || + norm(param, prevParam, NORM_L2 | NORM_RELATIVE) < criteria.epsilon ) + { + _param = param; + _JtJ = JtJ; + _JtErr = JtErr; + state = DONE; + return false; + } + + prevErrNorm = errNorm; + JtJ.setTo(0); + JtErr.setTo(0); + _param = param; + _JtJ = JtJ; + _JtErr = JtErr; + state = CALC_J; + return true; +} + +void LevMarq::step() +{ + using namespace cv; + const double LOG10 = log(10.); + double lambda = exp(lambdaLg10*LOG10); + int nparams = param.rows; + + int nparams_nz = countNonZero(mask); + if(JtJN.rows != nparams_nz) { + // prevent re-allocation in every step + JtJN.create(nparams_nz, nparams_nz); + JtJV.create(nparams_nz, 1); + JtJW.create(nparams_nz, 1); + } + + subMatrixWithIndices(JtErr, JtJV, std::vector(1, 1), mask); + subMatrixWithIndices(JtJ, JtJN, mask, mask); + + if( err.empty() ) + completeSymm( JtJN, completeSymmFlag ); + + JtJN.diag() *= 1. + lambda; + solve(JtJN, JtJV, JtJW, solveMethod); + + int j = 0; + for( int i = 0; i < nparams; i++ ) + param(i, 0) = prevParam(i, 0) - (mask(i, 0) ? JtJW(j++, 0) : 0); +} + /* This is straight-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet that is (in a large extent) based on the paper: @@ -747,7 +956,7 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, initIntrinsicParams2D( matM, _m, npoints, imageSize, A, aspectRatio ); } - CvLevMarq solver( nparams, 0, cvTermCriteria(termCrit) ); + LevMarq solver( nparams, 0, termCrit ); if(flags & CALIB_USE_LU) { solver.solveMethod = DECOMP_LU; @@ -757,8 +966,8 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, } { - double* param = solver.param->data.db; - uchar* mask = solver.mask->data.ptr; + double* param = solver.param.ptr(); + uchar* mask = solver.mask.ptr(); param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2); std::copy(k.begin(), k.end(), param + 4); @@ -818,8 +1027,8 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, } } - Mat_ param_m = cvarrToMat(solver.param); - Mat mask = cvarrToMat(solver.mask); + Mat_ param_m = solver.param; + Mat mask = solver.mask; int nparams_nz = countNonZero(mask); if (nparams_nz >= 2 * total) @@ -856,12 +1065,12 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, { bool optimizeObjPoints = releaseObject; - const CvMat* _param = 0; - CvMat *_JtJ = 0, *_JtErr = 0; + Mat1d _param; + Mat1d JtJ, JtErr; double* _errNorm = 0; - bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ); - double *param = solver.param->data.db, *pparam = solver.prevParam->data.db; - bool calcJ = solver.state == CvLevMarq::CALC_J || (!proceed && !stdDevs.empty()); + bool proceed = solver.updateAlt( _param, JtJ, JtErr, _errNorm ); + double *param = _param.ptr(), *pparam = solver.prevParam.ptr(); + bool calcJ = solver.state == LevMarq::CALC_J || (!proceed && !stdDevs.empty()); if( flags & CALIB_FIX_ASPECT_RATIO ) { @@ -878,7 +1087,7 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, if ( !proceed && stdDevs.empty() && perViewErr.empty() ) break; else if ( !proceed && !stdDevs.empty() ) - cvZero(_JtJ); + JtJ.setTo(0); reprojErr = 0; @@ -933,8 +1142,6 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, if( calcJ ) { - Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr)); - // see HZ: (A6.14) for details on the structure of the Jacobian JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += Ji.t() * Ji; JtJ(Rect(si, si, 6, 6)) = Je.t() * Je; @@ -970,7 +1177,7 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, { Mat JtJinv, JtJN; JtJN.create(nparams_nz, nparams_nz, CV_64F); - subMatrix(cvarrToMat(_JtJ), JtJN, mask, mask); + subMatrixWithIndices(JtJ, JtJN, mask, mask); completeSymm(JtJN, false); cv::invert(JtJN, JtJinv, DECOMP_SVD); // an explanation of that denominator correction can be found here: @@ -991,7 +1198,7 @@ static double calibrateCameraInternalBouguet( const Mat& objectPoints, } // 4. store the results - double * param = solver.param->data.db; + double * param = solver.param.ptr(); A = Matx33d(param[0], 0, param[2], 0, param[1], param[3], 0, 0, 1); Mat(A).convertTo(cameraMatrix, cameraMatrix.type()); _k = Mat(distCoeffs.size(), CV_64F, param + 4); @@ -1642,7 +1849,7 @@ static double calibrateCameraInternalSchur( const Mat& objectPoints, Mat JtJinv, JtJN; JtJN.create(nparams_nz, nparams_nz, CV_64F); std::vector mask_copy(mask.ptr(), mask.ptr() + nparams); - subMatrix(JtJ, JtJN, mask_copy, mask_copy); + subMatrixWithIndices(JtJ, JtJN, mask_copy, mask_copy); completeSymm(JtJN, false); cv::invert(JtJN, JtJinv, DECOMP_SVD); @@ -1836,10 +2043,10 @@ static double stereoCalibrateImpl( // - next NINTRINSICS: the same for for 2nd camera nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0); - CvLevMarq solver( nparams, 0, cvTermCriteria(termCrit) ); - double * param = solver.param->data.db; - Mat paramM = Mat(solver.param->rows, solver.param->cols, CV_64F, param); - uchar* mask = solver.mask->data.ptr; + LevMarq solver( nparams, 0, termCrit ); + double * param = solver.param.ptr(); + Mat paramM = Mat(solver.param.rows, solver.param.cols, CV_64F, param); + uchar* mask = solver.mask.ptr(); if(flags & CALIB_USE_LU) { solver.solveMethod = DECOMP_LU; @@ -2001,10 +2208,10 @@ static double stereoCalibrateImpl( for(;;) { - const CvMat* tmp_param = 0; - CvMat *JtJ = 0, *JtErr = 0; + Mat1d tmp_param; + Mat1d JtJ, JtErr; double *_errNorm = 0; - Mat_ param_m(1,nparams, solver.param->data.db); + Mat_ param_m(1,nparams, solver.param.ptr()); Vec3d om_LR(param_m(0), param_m(1), param_m(2)); Vec3d T_LR(param_m(3), param_m(4), param_m(5)); Vec3d om[2], T[2]; @@ -2060,7 +2267,7 @@ static double stereoCalibrateImpl( om[0] = Vec3d(param_m(idx + 0), param_m(idx + 1), param_m(idx + 2)); T[0] = Vec3d(param_m(idx + 3), param_m(idx + 4), param_m(idx + 5)); - if( JtJ || JtErr ) + if( !JtJ.empty() || !JtErr.empty() ) composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(), dr3dr2, noArray(), noArray(), dt3dt1, dt3dr2, dt3dt2 ); else @@ -2083,7 +2290,7 @@ static double stereoCalibrateImpl( { Mat imgpt_ik = imagePoints[k](Range::all(), Range(ptPos, ptPos + ni)); - if( JtJ || JtErr ) + if( !JtJ.empty() || !JtErr.empty() ) projectPoints(objpt_i, om[k], T[k], intrin[k], distCoeffs[k], tmpImagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, noArray(), (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0.); @@ -2091,12 +2298,10 @@ static double stereoCalibrateImpl( projectPoints(objpt_i, om[k], T[k], intrin[k], distCoeffs[k], tmpImagePoints); subtract( tmpImagePoints, imgpt_ik, tmpImagePoints ); - if( solver.state == CvLevMarq::CALC_J ) + if( solver.state == LevMarq::CALC_J ) { int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6; - CV_Assert( JtJ && JtErr ); - - Mat _JtJ(cvarrToMat(JtJ)), _JtErr(cvarrToMat(JtErr)); + CV_Assert( !JtJ.empty() && !JtErr.empty() ); if( k == 1 ) { @@ -2135,23 +2340,23 @@ static double stereoCalibrateImpl( J_LR.at(p, 3+j) = de3dt2(j); } - _JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR; - _JtJ(Rect(eofs, 0, 6, 6)) = J_LR.t()*Je; - _JtErr.rowRange(0, 6) += J_LR.t()*err; + JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR; + JtJ(Rect(eofs, 0, 6, 6)) = J_LR.t()*Je; + JtErr.rowRange(0, 6) += J_LR.t()*err; } - _JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je; - _JtErr.rowRange(eofs, eofs + 6) += Je.t()*err; + JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je; + JtErr.rowRange(eofs, eofs + 6) += Je.t()*err; if( recomputeIntrinsics ) { - _JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji; - _JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji; + JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji; + JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji; if( k == 1 ) { - _JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji; + JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji; } - _JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err; + JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err; } } diff --git a/modules/calib3d/src/calibration_base.cpp b/modules/calib3d/src/calibration_base.cpp index f90a85a064..1583d3de36 100644 --- a/modules/calib3d/src/calibration_base.cpp +++ b/modules/calib3d/src/calibration_base.cpp @@ -43,8 +43,6 @@ #include "precomp.hpp" #include "hal_replacement.hpp" #include "distortion_model.hpp" -#include "opencv2/calib3d/calib3d_c.h" -#include "opencv2/core/core_c.h" #include #include @@ -1237,7 +1235,7 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, Mat matU( 3, 3, CV_64F, U ); Mat matV( 3, 3, CV_64F, V ); Mat matW( 3, 1, CV_64F, W ); - Mat _param( 6, 1, CV_64F, param ); + Mat1d _param( 6, 1, param ); Mat _dpdr, _dpdt; count = MAX(objectPoints.cols, objectPoints.rows); @@ -1394,7 +1392,7 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, // refine extrinsic parameters using iterative algorithm #if 0 - // The C++ LMSolver is not as good as CvLevMarq to pass the tests, maybe due to _completeSymmFlag in CvLevMarq. + // The C++ LMSolver is not as good as LevMarq to pass the tests, maybe due to _completeSymmFlag in LevMarq. class RefineLMCallback CV_FINAL : public LMSolver::Callback { public: @@ -1437,23 +1435,23 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, LMSolver::create(makePtr(matM, _m, matA, distCoeffs), max_iter, FLT_EPSILON)->run(_param); #else - CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true); - _param.copyTo(cvarrToMat(solver.param)); + LevMarq solver( 6, count*2, TermCriteria(TermCriteria::EPS + TermCriteria::COUNT,max_iter,FLT_EPSILON), true); + _param.copyTo(solver.param); for(;;) { - CvMat *matJ = 0, *_err = 0; - const CvMat *__param = 0; - bool proceed = solver.update( __param, matJ, _err ); - cvarrToMat(__param).copyTo(_param ); - if( !proceed || !_err ) + Mat1d _err; + Mat1d Jac; + Mat1d __param; + bool proceed = solver.update( __param, Jac, _err ); + __param.copyTo(_param ); + if( !proceed || _err.empty() ) break; int errCount = matM.rows + matM.cols - 1; - Mat err = cvarrToMat(_err); + Mat err = _err; err = err.reshape(2, errCount); - if( matJ ) + if( !Jac.empty() ) { - Mat Jac = cvarrToMat(matJ); Mat dpdr = Jac.colRange(0, 3); Mat dpdt = Jac.colRange(3, 6); projectPoints(matM, _r, _t, matA, distCoeffs, @@ -1464,9 +1462,8 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, projectPoints(matM, _r, _t, matA, distCoeffs, err); } subtract(err, _m.rows == 1 ? _m.t() : _m, err); - cvReshape( _err, _err, 1, 2*count ); } - cvarrToMat(solver.param).copyTo(_param ); + solver.param.copyTo(_param ); #endif _param.rowRange(0, 3).convertTo(rvec, rvec.depth()); diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index eea17b19d9..a8a65fa7fc 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -258,34 +258,6 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d return true; } -namespace { -static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector& cols, - const std::vector& rows) { - int nonzeros_cols = cv::countNonZero(cols); - cv::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; @@ -308,8 +280,8 @@ void CvLevMarq::step() Mat _JtErr = cvarrToMat(JtJV); Mat_ nonzero_param = cvarrToMat(JtJW); - subMatrix(cvarrToMat(JtErr), _JtErr, std::vector(1, 1), _mask); - subMatrix(_JtJ, _JtJN, _mask, _mask); + subMatrixWithMasks(cvarrToMat(JtErr), _JtErr, std::vector(1, 1), _mask, /*resize_dst=*/false); + subMatrixWithMasks(_JtJ, _JtJN, _mask, _mask, /*resize_dst=*/false); if( !err ) completeSymm( _JtJN, completeSymmFlag ); diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 9a2e04a5eb..bac1b2cca1 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -53,8 +53,6 @@ namespace cv { namespace Vec3d dom, dT; double dalpha; }; - - void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows); }} ////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -1058,7 +1056,7 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]); //update all parameters - cv::subMatrix(J, J, selectedParams, std::vector(J.rows, 1)); + cv::subMatrixWithMasks(J, J, selectedParams, std::vector(J.rows, 1), /*resize_dst=*/true); int a = cv::countNonZero(intrinsicLeft.isEstimate); int b = cv::countNonZero(intrinsicRight.isEstimate); cv::Mat deltas; @@ -1161,12 +1159,12 @@ bool cv::fisheye::solvePnPRansac( InputArray opoints, InputArray ipoints, useExtrinsicGuess, iterationsCount, reprojectionError, confidence, inliers, flags); } -namespace cv{ namespace { -void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows) +namespace cv{ +void subMatrixWithMasks(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows, bool resize_dst) { CV_Assert(src.channels() == 1); - int nonzeros_cols = cv::countNonZero(cols); + int nonzeros_cols = resize_dst ? cv::countNonZero(cols) : dst.cols; Mat tmp(src.rows, nonzeros_cols, CV_64F); for (int i = 0, j = 0; i < (int)cols.size(); i++) @@ -1177,8 +1175,10 @@ void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const s } } - int nonzeros_rows = cv::countNonZero(rows); - dst.create(nonzeros_rows, nonzeros_cols, CV_64F); + if (resize_dst) { + int nonzeros_rows = cv::countNonZero(rows); + dst.create(nonzeros_rows, nonzeros_cols, CV_64F); + } for (int i = 0, j = 0; i < (int)rows.size(); i++) { if (rows[i]) @@ -1188,7 +1188,7 @@ void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const s } } -}} +} cv::internal::IntrinsicParams::IntrinsicParams(): f(Vec2d::all(0)), c(Vec2d::all(0)), k(Vec4d::all(0)), alpha(0), isEstimate(9,0) @@ -1564,8 +1564,8 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO std::vector idxs(param.isEstimate); idxs.insert(idxs.end(), 6 * n, 1); - subMatrix(JJ2, JJ2, idxs, idxs); - subMatrix(ex3, ex3, std::vector(1, 1), idxs); + subMatrixWithMasks(JJ2, JJ2, idxs, idxs, /*resize_dst=*/true); + subMatrixWithMasks(ex3, ex3, std::vector(1, 1), idxs, /*resize_dst=*/true); } void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index e6b11705bb..a32d3b17e3 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -48,6 +48,7 @@ #include "opencv2/core/private.hpp" #include "opencv2/calib3d.hpp" +#include "opencv2/calib3d/private.hpp" #include "opencv2/imgproc.hpp" #include "opencv2/features2d.hpp" @@ -155,6 +156,10 @@ void getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray R, InputArray newCameraMatrix, Size imgSize, Rect_& inner, Rect_& outer ); +// cols and rows contains masks to use to get the sub-matrix. +void subMatrixWithMasks(const Mat& src, Mat& dst, const std::vector& cols, + const std::vector& rows, bool resize_dst); + } // namespace cv int checkChessboardBinary(const cv::Mat & img, const cv::Size & size); diff --git a/modules/js/generator/CMakeLists.txt b/modules/js/generator/CMakeLists.txt index 9fec48f950..8ed7dc8c1a 100644 --- a/modules/js/generator/CMakeLists.txt +++ b/modules/js/generator/CMakeLists.txt @@ -18,6 +18,7 @@ endforeach(m) # header blacklist ocv_list_filterout(opencv_hdrs "modules/.*.h$") +ocv_list_filterout(opencv_hdrs "modules/calib3d/include/opencv2/calib3d/private.hpp") ocv_list_filterout(opencv_hdrs "modules/core/include/opencv2/core/fast_math.hpp") ocv_list_filterout(opencv_hdrs "modules/core/.*/cuda") ocv_list_filterout(opencv_hdrs "modules/core/.*/opencl") diff --git a/modules/stitching/src/motion_estimators.cpp b/modules/stitching/src/motion_estimators.cpp index 8d227409ca..22b186b60f 100644 --- a/modules/stitching/src/motion_estimators.cpp +++ b/modules/stitching/src/motion_estimators.cpp @@ -41,9 +41,7 @@ //M*/ #include "precomp.hpp" -#include "opencv2/core/core_c.h" -#include "opencv2/calib3d/calib3d_c.h" -#include "opencv2/core/cvdef.h" +#include "opencv2/calib3d/private.hpp" using namespace cv; using namespace cv::detail; @@ -254,46 +252,40 @@ bool BundleAdjusterBase::estimate(const std::vector &features, total_num_matches_ += static_cast(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_)); + LevMarq solver(num_images_ * num_params_per_cam_, + total_num_matches_ * num_errs_per_measurement_, term_criteria_); Mat err, jac; - CvMat matParams = cvMat(cam_params_); - cvCopy(&matParams, solver.param); + cam_params_.copyTo(solver.param); #if ENABLE_LOG int iter = 0; #endif for(;;) { - const CvMat* _param = 0; - CvMat* _jac = 0; - CvMat* _err = 0; + Mat1d _param, _jac, _err; bool proceed = solver.update(_param, _jac, _err); - cvCopy(_param, &matParams); + _param.copyTo(cam_params_); - if (!proceed || !_err) + if (!proceed || _err.empty()) break; - if (_jac) + if (!_jac.empty()) { calcJacobian(jac); - CvMat tmp = cvMat(jac); - cvCopy(&tmp, _jac); + jac.copyTo(_jac); } - if (_err) + if (!_err.empty()) { calcError(err); LOG_CHAT("."); #if ENABLE_LOG iter++; #endif - CvMat tmp = cvMat(err); - cvCopy(&tmp, _err); + err.copyTo(_err); } }