potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-21 19:33:03 +04:00
Code Packages Releases Wiki Activity

Port CvLevMarq to C++

Browse Source
This commit is contained in:
Vincent Rabaud
2026-04-24 09:41:09 +02:00
parent 46ee0f7954
commit 721f70feff
8 changed files with 399 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/calib3d/include/opencv2/calib3d/private.hpp
+108
View File
@@ -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
modules/calib3d/src/calibration.cpp
+248 -43
View File
@@ -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 <iterator>
/*
@@ -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<uchar>& cols,
const std::vector<uchar>& rows)
static void subMatrixWithIndices(const Mat& src, Mat& dst,
const std::vector<uchar>& cols,
const std::vector<uchar>& 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<uchar>(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<double>();
uchar* mask = solver.mask.ptr<uchar>();
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_<double> param_m = cvarrToMat(solver.param);
Mat mask = cvarrToMat(solver.mask);
Mat_<double> 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<double>(), *pparam = solver.prevParam.ptr<double>();
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<double>();
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<uchar> mask_copy(mask.ptr<uchar>(), mask.ptr<uchar>() + 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<double>();
Mat paramM = Mat(solver.param.rows, solver.param.cols, CV_64F, param);
uchar* mask = solver.mask.ptr<uchar>();
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_<double> param_m(1,nparams, solver.param->data.db);
Mat_<double> param_m(1,nparams, solver.param.ptr<double>());
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<double>(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;
}
}
modules/calib3d/src/calibration_base.cpp
+13 -16
View File
@@ -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 <stdio.h>
#include <iterator>
@@ -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<RefineLMCallback>(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());
modules/calib3d/src/compat_ptsetreg.cpp
+2 -30
View File
@@ -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<uchar>& cols,
const std::vector<uchar>& 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_<double> nonzero_param = cvarrToMat(JtJW);
subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
subMatrix(_JtJ, _JtJN, _mask, _mask);
subMatrixWithMasks(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask, /*resize_dst=*/false);
subMatrixWithMasks(_JtJ, _JtJN, _mask, _mask, /*resize_dst=*/false);
if( !err )
completeSymm( _JtJN, completeSymmFlag );
modules/calib3d/src/fisheye.cpp
+11 -11
View File
@@ -53,8 +53,6 @@ namespace cv { namespace
Vec3d dom, dT;
double dalpha;
};
void subMatrix(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& 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<uchar>(J.rows, 1));
cv::subMatrixWithMasks(J, J, selectedParams, std::vector<uchar>(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<uchar>& cols, const std::vector<uchar>& rows)
namespace cv{
void subMatrixWithMasks(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& 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<uchar>& 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<uchar>& 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<uchar> idxs(param.isEstimate);
idxs.insert(idxs.end(), 6 * n, 1);
subMatrix(JJ2, JJ2, idxs, idxs);
subMatrix(ex3, ex3, std::vector<uchar>(1, 1), idxs);
subMatrixWithMasks(JJ2, JJ2, idxs, idxs, /*resize_dst=*/true);
subMatrixWithMasks(ex3, ex3, std::vector<uchar>(1, 1), idxs, /*resize_dst=*/true);
}
void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
modules/calib3d/src/precomp.hpp
+5
View File
@@ -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_<double>& inner, Rect_<double>& outer );
// cols and rows contains masks to use to get the sub-matrix.
void subMatrixWithMasks(const Mat& src, Mat& dst, const std::vector<uchar>& cols,
const std::vector<uchar>& rows, bool resize_dst);
} // namespace cv
int checkChessboardBinary(const cv::Mat & img, const cv::Size & size);
modules/js/generator/CMakeLists.txt
+1
View File
@@ -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")
modules/stitching/src/motion_estimators.cpp
+11 -19
View File
@@ -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<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_));
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);
}
}