1793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/*M/////////////////////////////////////////////////////////////////////////////////////// 2793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 3793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 5793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// By downloading, copying, installing or using the software you agree to this license. 6793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// If you do not agree to this license, do not download, install, 7793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// copy or use the software. 8793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 9793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 10793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// License Agreement 11793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// For Open Source Computer Vision Library 12793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 13793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Copyright (C) 2000, Intel Corporation, all rights reserved. 14793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Copyright (C) 2013, OpenCV Foundation, all rights reserved. 15793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Third party copyrights are property of their respective owners. 16793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 17793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Redistribution and use in source and binary forms, with or without modification, 18793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// are permitted provided that the following conditions are met: 19793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 20793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// * Redistribution's of source code must retain the above copyright notice, 21793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// this list of conditions and the following disclaimer. 22793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 23793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// * Redistribution's in binary form must reproduce the above copyright notice, 24793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// this list of conditions and the following disclaimer in the documentation 25793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// and/or other materials provided with the distribution. 26793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 27793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// * The name of the copyright holders may not be used to endorse or promote products 28793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// derived from this software without specific prior written permission. 29793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 30793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// This software is provided by the copyright holders and contributors "as is" and 31793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// any express or implied warranties, including, but not limited to, the implied 32793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// warranties of merchantability and fitness for a particular purpose are disclaimed. 33793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// In no event shall the Intel Corporation or contributors be liable for any direct, 34793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// indirect, incidental, special, exemplary, or consequential damages 35793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// (including, but not limited to, procurement of substitute goods or services; 36793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// loss of use, data, or profits; or business interruption) however caused 37793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// and on any theory of liability, whether in contract, strict liability, 38793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// or tort (including negligence or otherwise) arising in any way out of 39793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// the use of this software, even if advised of the possibility of such damage. 40793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 41793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler//M*/ 42793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 43793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#include "precomp.hpp" 44793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#include "opencv2/calib3d/calib3d_c.h" 45793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 46793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/************************************************************************************\ 47793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Some backward compatibility stuff, to be moved to legacy or compat module 48793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler\************************************************************************************/ 49793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 50793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslerusing cv::Ptr; 51793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 52793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////// Levenberg-Marquardt engine (the old variant) //////////////////////// 53793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 54793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCvLevMarq::CvLevMarq() 55793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 56793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lambdaLg10 = 0; state = DONE; 57793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler criteria = cvTermCriteria(0,0,0); 58793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler iters = 0; 59793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler completeSymmFlag = false; 60793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errNorm = prevErrNorm = DBL_MAX; 61793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 62793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 63793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) 64793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 65793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler init(nparams, nerrs, criteria0, _completeSymmFlag); 66793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 67793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 68793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid CvLevMarq::clear() 69793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 70793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler mask.release(); 71793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler prevParam.release(); 72793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler param.release(); 73793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.release(); 74793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler err.release(); 75793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJ.release(); 76793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJN.release(); 77793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtErr.release(); 78793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJV.release(); 79793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJW.release(); 80793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 81793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 82793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCvLevMarq::~CvLevMarq() 83793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 84793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler clear(); 85793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 86793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 87793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) 88793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 89793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) ) 90793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler clear(); 91793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler mask.reset(cvCreateMat( nparams, 1, CV_8U )); 92793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvSet(mask, cvScalarAll(1)); 93793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler prevParam.reset(cvCreateMat( nparams, 1, CV_64F )); 94793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler param.reset(cvCreateMat( nparams, 1, CV_64F )); 95793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJ.reset(cvCreateMat( nparams, nparams, CV_64F )); 96793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJN.reset(cvCreateMat( nparams, nparams, CV_64F )); 97793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJV.reset(cvCreateMat( nparams, nparams, CV_64F )); 98793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJW.reset(cvCreateMat( nparams, 1, CV_64F )); 99793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtErr.reset(cvCreateMat( nparams, 1, CV_64F )); 100793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( nerrs > 0 ) 101793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 102793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.reset(cvCreateMat( nerrs, nparams, CV_64F )); 103793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler err.reset(cvCreateMat( nerrs, 1, CV_64F )); 104793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 105793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errNorm = prevErrNorm = DBL_MAX; 106793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lambdaLg10 = -3; 107793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler criteria = criteria0; 108793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( criteria.type & CV_TERMCRIT_ITER ) 109793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000); 110793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 111793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler criteria.max_iter = 30; 112793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( criteria.type & CV_TERMCRIT_EPS ) 113793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler criteria.epsilon = MAX(criteria.epsilon, 0); 114793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 115793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler criteria.epsilon = DBL_EPSILON; 116793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = STARTED; 117793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler iters = 0; 118793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler completeSymmFlag = _completeSymmFlag; 119793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 120793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 121793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslerbool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) 122793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 123793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double change; 124793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 125793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler matJ = _err = 0; 126793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 127793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler assert( !err.empty() ); 128793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( state == DONE ) 129793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 130793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 131793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return false; 132793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 133793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 134793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( state == STARTED ) 135793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 136793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 137793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( J ); 138793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( err ); 139793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler matJ = J; 140793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _err = err; 141793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CALC_J; 142793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 143793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 144793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 145793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( state == CALC_J ) 146793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 147793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvMulTransposed( J, JtJ, 1 ); 148793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T ); 149793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvCopy( param, prevParam ); 150793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler step(); 151793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( iters == 0 ) 152793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler prevErrNorm = cvNorm(err, 0, CV_L2); 153793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 154793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( err ); 155793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _err = err; 156793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CHECK_ERR; 157793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 158793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 159793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 160793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler assert( state == CHECK_ERR ); 161793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errNorm = cvNorm( err, 0, CV_L2 ); 162793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( errNorm > prevErrNorm ) 163793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 164793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( ++lambdaLg10 <= 16 ) 165793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 166793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler step(); 167793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 168793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( err ); 169793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _err = err; 170793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CHECK_ERR; 171793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 172793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 173793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 174793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 175793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lambdaLg10 = MAX(lambdaLg10-1, -16); 176793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( ++iters >= criteria.max_iter || 177793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) 178793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 179793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 180793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = DONE; 181793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 182793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 183793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 184793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler prevErrNorm = errNorm; 185793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 186793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero(J); 187793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler matJ = J; 188793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _err = err; 189793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CALC_J; 190793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 191793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 192793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 193793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 194793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslerbool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ) 195793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 196793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double change; 197793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 198793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( !err ); 199793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( state == DONE ) 200793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 201793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 202793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return false; 203793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 204793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 205793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( state == STARTED ) 206793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 207793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 208793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( JtJ ); 209793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( JtErr ); 210793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errNorm = 0; 211793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _JtJ = JtJ; 212793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _JtErr = JtErr; 213793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _errNorm = &errNorm; 214793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CALC_J; 215793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 216793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 217793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 218793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( state == CALC_J ) 219793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 220793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvCopy( param, prevParam ); 221793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler step(); 222793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 223793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler prevErrNorm = errNorm; 224793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errNorm = 0; 225793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _errNorm = &errNorm; 226793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CHECK_ERR; 227793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 228793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 229793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 230793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler assert( state == CHECK_ERR ); 231793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( errNorm > prevErrNorm ) 232793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 233793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( ++lambdaLg10 <= 16 ) 234793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 235793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler step(); 236793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 237793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errNorm = 0; 238793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _errNorm = &errNorm; 239793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CHECK_ERR; 240793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 241793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 242793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 243793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 244793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lambdaLg10 = MAX(lambdaLg10-1, -16); 245793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( ++iters >= criteria.max_iter || 246793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) 247793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 248793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 249793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = DONE; 250793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return false; 251793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 252793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 253793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler prevErrNorm = errNorm; 254793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( JtJ ); 255793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvZero( JtErr ); 256793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _param = param; 257793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _JtJ = JtJ; 258793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _JtErr = JtErr; 259793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler state = CALC_J; 260793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return true; 261793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 262793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 263793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid CvLevMarq::step() 264793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 265793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double LOG10 = log(10.); 266793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double lambda = exp(lambdaLg10*LOG10); 267793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int i, j, nparams = param->rows; 268793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 269793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( i = 0; i < nparams; i++ ) 270793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( mask->data.ptr[i] == 0 ) 271793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 272793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i; 273793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( j = 0; j < nparams; j++ ) 274793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler row[j] = col[j*nparams] = 0; 275793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtErr->data.db[i] = 0; 276793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 277793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 278793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( !err ) 279793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvCompleteSymm( JtJ, completeSymmFlag ); 280793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#if 1 281793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvCopy( JtJ, JtJN ); 282793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( i = 0; i < nparams; i++ ) 283793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JtJN->data.db[(nparams+1)*i] *= 1. + lambda; 284793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#else 285793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvSetIdentity(JtJN, cvRealScalar(lambda)); 286793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvAdd( JtJ, JtJN, JtJN ); 287793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#endif 288793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 289793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T ); 290793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( i = 0; i < nparams; i++ ) 291793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0); 292793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 293793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 294793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 295793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters ) 296793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 297793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return cv::RANSACUpdateNumIters(p, ep, modelPoints, maxIters); 298793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 299793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 300793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 301793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method, 302793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double ransacReprojThreshold, CvMat* _mask, int maxIters, 303793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double confidence) 304793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 305793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst); 306793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 307793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( src.channels() == 1 && (src.rows == 2 || src.rows == 3) && src.cols > 3 ) 308793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::transpose(src, src); 309793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( dst.channels() == 1 && (dst.rows == 2 || dst.rows == 3) && dst.cols > 3 ) 310793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::transpose(dst, dst); 311793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 312793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ( maxIters < 0 ) 313793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler maxIters = 0; 314793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ( maxIters > 2000 ) 315793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler maxIters = 2000; 316793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 317793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ( confidence < 0 ) 318793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler confidence = 0; 319793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ( confidence > 1 ) 320793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler confidence = 1; 321793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 322793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Mat H = cv::cvarrToMat(__H), mask = cv::cvarrToMat(_mask); 323793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat H0 = cv::findHomography(src, dst, method, ransacReprojThreshold, 324793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _mask ? cv::_OutputArray(mask) : cv::_OutputArray(), maxIters, 325793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler confidence); 326793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 327793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( H0.empty() ) 328793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 329793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat Hz = cv::cvarrToMat(__H); 330793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Hz.setTo(cv::Scalar::all(0)); 331793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return 0; 332793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 333793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler H0.convertTo(H, H.type()); 334793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return 1; 335793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 336793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 337793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 338793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2, 339793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CvMat* fmatrix, int method, 340793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double param1, double param2, CvMat* _mask ) 341793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 342793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2); 343793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 344793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 ) 345793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::transpose(m1, m1); 346793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 ) 347793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::transpose(m2, m2); 348793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 349793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask); 350793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2, 351793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _mask ? cv::_OutputArray(mask) : cv::_OutputArray()); 352793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 353793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( FM0.empty() ) 354793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 355793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat FM0z = cv::cvarrToMat(fmatrix); 356793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler FM0z.setTo(cv::Scalar::all(0)); 357793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return 0; 358793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 359793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 360793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 ); 361793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows)); 362793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type()); 363793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return FM1.rows / 3; 364793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 365793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 366793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 367793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID, 368793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const CvMat* fmatrix, CvMat* _lines ) 369793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 370793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix); 371793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat lines = cv::cvarrToMat(_lines); 372793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Mat lines0 = lines; 373793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 374793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 ) 375793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::transpose(pt, pt); 376793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 377793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::computeCorrespondEpilines(pt, pointImageID, fm, lines); 378793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 379793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3; 380793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows)); 381793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 382793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( tflag ) 383793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 384793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows ); 385793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( lines0.type() == lines.type() ) 386793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler transpose( lines, lines0 ); 387793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 388793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 389793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler transpose( lines, lines ); 390793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lines.convertTo( lines0, lines0.type() ); 391793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 392793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 393793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 394793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 395793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( lines.size() == lines0.size() ); 396793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( lines.data != lines0.data ) 397793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler lines.convertTo(lines0, lines0.type()); 398793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 399793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 400793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 401793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 402793ee12c6df9cad3806238d32528c49a3ff9331dNoah PreslerCV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst ) 403793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 404793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst); 405793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Mat dst0 = dst; 406793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 407793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows); 408793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 409793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( src.channels() == 1 && src.cols > d0 ) 410793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::transpose(src, src); 411793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 412793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows); 413793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 414793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( d0 == d1 ) 415793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler src.copyTo(dst); 416793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else if( d0 < d1 ) 417793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::convertPointsToHomogeneous(src, dst); 418793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 419793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::convertPointsFromHomogeneous(src, dst); 420793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 421793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler bool tflag = dst0.channels() == 1 && dst0.cols > d1; 422793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows)); 423793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 424793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( tflag ) 425793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 426793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows ); 427793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( dst0.type() == dst.type() ) 428793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler transpose( dst, dst0 ); 429793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 430793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 431793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler transpose( dst, dst ); 432793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dst.convertTo( dst0, dst0.type() ); 433793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 434793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 435793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 436793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 437793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( dst.size() == dst0.size() ); 438793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( dst.data != dst0.data ) 439793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dst.convertTo(dst0, dst0.type()); 440793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 441793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 442