1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 #include "precomp.hpp"
44 #include "opencv2/calib3d/calib3d_c.h"
45
46 /************************************************************************************\
47 Some backward compatibility stuff, to be moved to legacy or compat module
48 \************************************************************************************/
49
50 using cv::Ptr;
51
52 ////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
53
CvLevMarq()54 CvLevMarq::CvLevMarq()
55 {
56 lambdaLg10 = 0; state = DONE;
57 criteria = cvTermCriteria(0,0,0);
58 iters = 0;
59 completeSymmFlag = false;
60 errNorm = prevErrNorm = DBL_MAX;
61 solveMethod = cv::DECOMP_SVD;
62 }
63
CvLevMarq(int nparams,int nerrs,CvTermCriteria criteria0,bool _completeSymmFlag)64 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
65 {
66 init(nparams, nerrs, criteria0, _completeSymmFlag);
67 }
68
clear()69 void CvLevMarq::clear()
70 {
71 mask.release();
72 prevParam.release();
73 param.release();
74 J.release();
75 err.release();
76 JtJ.release();
77 JtJN.release();
78 JtErr.release();
79 JtJV.release();
80 JtJW.release();
81 }
82
~CvLevMarq()83 CvLevMarq::~CvLevMarq()
84 {
85 clear();
86 }
87
init(int nparams,int nerrs,CvTermCriteria criteria0,bool _completeSymmFlag)88 void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
89 {
90 if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
91 clear();
92 mask.reset(cvCreateMat( nparams, 1, CV_8U ));
93 cvSet(mask, cvScalarAll(1));
94 prevParam.reset(cvCreateMat( nparams, 1, CV_64F ));
95 param.reset(cvCreateMat( nparams, 1, CV_64F ));
96 JtJ.reset(cvCreateMat( nparams, nparams, CV_64F ));
97 JtErr.reset(cvCreateMat( nparams, 1, CV_64F ));
98 if( nerrs > 0 )
99 {
100 J.reset(cvCreateMat( nerrs, nparams, CV_64F ));
101 err.reset(cvCreateMat( nerrs, 1, CV_64F ));
102 }
103 errNorm = prevErrNorm = DBL_MAX;
104 lambdaLg10 = -3;
105 criteria = criteria0;
106 if( criteria.type & CV_TERMCRIT_ITER )
107 criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
108 else
109 criteria.max_iter = 30;
110 if( criteria.type & CV_TERMCRIT_EPS )
111 criteria.epsilon = MAX(criteria.epsilon, 0);
112 else
113 criteria.epsilon = DBL_EPSILON;
114 state = STARTED;
115 iters = 0;
116 completeSymmFlag = _completeSymmFlag;
117 solveMethod = cv::DECOMP_SVD;
118 }
119
update(const CvMat * & _param,CvMat * & matJ,CvMat * & _err)120 bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
121 {
122 matJ = _err = 0;
123
124 assert( !err.empty() );
125 if( state == DONE )
126 {
127 _param = param;
128 return false;
129 }
130
131 if( state == STARTED )
132 {
133 _param = param;
134 cvZero( J );
135 cvZero( err );
136 matJ = J;
137 _err = err;
138 state = CALC_J;
139 return true;
140 }
141
142 if( state == CALC_J )
143 {
144 cvMulTransposed( J, JtJ, 1 );
145 cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
146 cvCopy( param, prevParam );
147 step();
148 if( iters == 0 )
149 prevErrNorm = cvNorm(err, 0, CV_L2);
150 _param = param;
151 cvZero( err );
152 _err = err;
153 state = CHECK_ERR;
154 return true;
155 }
156
157 assert( state == CHECK_ERR );
158 errNorm = cvNorm( err, 0, CV_L2 );
159 if( errNorm > prevErrNorm )
160 {
161 if( ++lambdaLg10 <= 16 )
162 {
163 step();
164 _param = param;
165 cvZero( err );
166 _err = err;
167 state = CHECK_ERR;
168 return true;
169 }
170 }
171
172 lambdaLg10 = MAX(lambdaLg10-1, -16);
173 if( ++iters >= criteria.max_iter ||
174 cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
175 {
176 _param = param;
177 state = DONE;
178 return true;
179 }
180
181 prevErrNorm = errNorm;
182 _param = param;
183 cvZero(J);
184 matJ = J;
185 _err = err;
186 state = CALC_J;
187 return true;
188 }
189
190
updateAlt(const CvMat * & _param,CvMat * & _JtJ,CvMat * & _JtErr,double * & _errNorm)191 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
192 {
193 CV_Assert( !err );
194 if( state == DONE )
195 {
196 _param = param;
197 return false;
198 }
199
200 if( state == STARTED )
201 {
202 _param = param;
203 cvZero( JtJ );
204 cvZero( JtErr );
205 errNorm = 0;
206 _JtJ = JtJ;
207 _JtErr = JtErr;
208 _errNorm = &errNorm;
209 state = CALC_J;
210 return true;
211 }
212
213 if( state == CALC_J )
214 {
215 cvCopy( param, prevParam );
216 step();
217 _param = param;
218 prevErrNorm = errNorm;
219 errNorm = 0;
220 _errNorm = &errNorm;
221 state = CHECK_ERR;
222 return true;
223 }
224
225 assert( state == CHECK_ERR );
226 if( errNorm > prevErrNorm )
227 {
228 if( ++lambdaLg10 <= 16 )
229 {
230 step();
231 _param = param;
232 errNorm = 0;
233 _errNorm = &errNorm;
234 state = CHECK_ERR;
235 return true;
236 }
237 }
238
239 lambdaLg10 = MAX(lambdaLg10-1, -16);
240 if( ++iters >= criteria.max_iter ||
241 cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
242 {
243 _param = param;
244 _JtJ = JtJ;
245 _JtErr = JtErr;
246 state = DONE;
247 return false;
248 }
249
250 prevErrNorm = errNorm;
251 cvZero( JtJ );
252 cvZero( JtErr );
253 _param = param;
254 _JtJ = JtJ;
255 _JtErr = JtErr;
256 state = CALC_J;
257 return true;
258 }
259
260 namespace {
subMatrix(const cv::Mat & src,cv::Mat & dst,const std::vector<uchar> & cols,const std::vector<uchar> & rows)261 static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
262 const std::vector<uchar>& rows) {
263 int nonzeros_cols = cv::countNonZero(cols);
264 cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
265
266 for (int i = 0, j = 0; i < (int)cols.size(); i++)
267 {
268 if (cols[i])
269 {
270 src.col(i).copyTo(tmp.col(j++));
271 }
272 }
273
274 int nonzeros_rows = cv::countNonZero(rows);
275 dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
276 for (int i = 0, j = 0; i < (int)rows.size(); i++)
277 {
278 if (rows[i])
279 {
280 tmp.row(i).copyTo(dst.row(j++));
281 }
282 }
283 }
284
285 }
286
287
step()288 void CvLevMarq::step()
289 {
290 using namespace cv;
291 const double LOG10 = log(10.);
292 double lambda = exp(lambdaLg10*LOG10);
293 int nparams = param->rows;
294
295 Mat _JtJ = cvarrToMat(JtJ);
296 Mat _mask = cvarrToMat(mask);
297
298 int nparams_nz = countNonZero(_mask);
299 if(!JtJN || JtJN->rows != nparams_nz) {
300 // prevent re-allocation in every step
301 JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));
302 JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
303 JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
304 }
305
306 Mat _JtJN = cvarrToMat(JtJN);
307 Mat _JtErr = cvarrToMat(JtJV);
308 Mat_<double> nonzero_param = cvarrToMat(JtJW);
309
310 subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
311 subMatrix(_JtJ, _JtJN, _mask, _mask);
312
313 if( !err )
314 completeSymm( _JtJN, completeSymmFlag );
315
316 _JtJN.diag() *= 1. + lambda;
317 solve(_JtJN, _JtErr, nonzero_param, solveMethod);
318
319 int j = 0;
320 for( int i = 0; i < nparams; i++ )
321 param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
322 }
323
324
cvRANSACUpdateNumIters(double p,double ep,int modelPoints,int maxIters)325 CV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
326 {
327 return cv::RANSACUpdateNumIters(p, ep, modelPoints, maxIters);
328 }
329
330
cvFindHomography(const CvMat * _src,const CvMat * _dst,CvMat * __H,int method,double ransacReprojThreshold,CvMat * _mask,int maxIters,double confidence)331 CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method,
332 double ransacReprojThreshold, CvMat* _mask, int maxIters,
333 double confidence)
334 {
335 cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
336
337 if( src.channels() == 1 && (src.rows == 2 || src.rows == 3) && src.cols > 3 )
338 cv::transpose(src, src);
339 if( dst.channels() == 1 && (dst.rows == 2 || dst.rows == 3) && dst.cols > 3 )
340 cv::transpose(dst, dst);
341
342 if ( maxIters < 0 )
343 maxIters = 0;
344 if ( maxIters > 2000 )
345 maxIters = 2000;
346
347 if ( confidence < 0 )
348 confidence = 0;
349 if ( confidence > 1 )
350 confidence = 1;
351
352 const cv::Mat H = cv::cvarrToMat(__H), mask = cv::cvarrToMat(_mask);
353 cv::Mat H0 = cv::findHomography(src, dst, method, ransacReprojThreshold,
354 _mask ? cv::_OutputArray(mask) : cv::_OutputArray(), maxIters,
355 confidence);
356
357 if( H0.empty() )
358 {
359 cv::Mat Hz = cv::cvarrToMat(__H);
360 Hz.setTo(cv::Scalar::all(0));
361 return 0;
362 }
363 H0.convertTo(H, H.type());
364 return 1;
365 }
366
367
cvFindFundamentalMat(const CvMat * points1,const CvMat * points2,CvMat * fmatrix,int method,double param1,double param2,CvMat * _mask)368 CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
369 CvMat* fmatrix, int method,
370 double param1, double param2, CvMat* _mask )
371 {
372 cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2);
373
374 if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 )
375 cv::transpose(m1, m1);
376 if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 )
377 cv::transpose(m2, m2);
378
379 const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask);
380 cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2,
381 _mask ? cv::_OutputArray(mask) : cv::_OutputArray());
382
383 if( FM0.empty() )
384 {
385 cv::Mat FM0z = cv::cvarrToMat(fmatrix);
386 FM0z.setTo(cv::Scalar::all(0));
387 return 0;
388 }
389
390 CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 );
391 cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows));
392 FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type());
393 return FM1.rows / 3;
394 }
395
396
cvComputeCorrespondEpilines(const CvMat * points,int pointImageID,const CvMat * fmatrix,CvMat * _lines)397 CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
398 const CvMat* fmatrix, CvMat* _lines )
399 {
400 cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix);
401 cv::Mat lines = cv::cvarrToMat(_lines);
402 const cv::Mat lines0 = lines;
403
404 if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 )
405 cv::transpose(pt, pt);
406
407 cv::computeCorrespondEpilines(pt, pointImageID, fm, lines);
408
409 bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3;
410 lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows));
411
412 if( tflag )
413 {
414 CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows );
415 if( lines0.type() == lines.type() )
416 transpose( lines, lines0 );
417 else
418 {
419 transpose( lines, lines );
420 lines.convertTo( lines0, lines0.type() );
421 }
422 }
423 else
424 {
425 CV_Assert( lines.size() == lines0.size() );
426 if( lines.data != lines0.data )
427 lines.convertTo(lines0, lines0.type());
428 }
429 }
430
431
cvConvertPointsHomogeneous(const CvMat * _src,CvMat * _dst)432 CV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst )
433 {
434 cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
435 const cv::Mat dst0 = dst;
436
437 int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);
438
439 if( src.channels() == 1 && src.cols > d0 )
440 cv::transpose(src, src);
441
442 int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);
443
444 if( d0 == d1 )
445 src.copyTo(dst);
446 else if( d0 < d1 )
447 cv::convertPointsToHomogeneous(src, dst);
448 else
449 cv::convertPointsFromHomogeneous(src, dst);
450
451 bool tflag = dst0.channels() == 1 && dst0.cols > d1;
452 dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));
453
454 if( tflag )
455 {
456 CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );
457 if( dst0.type() == dst.type() )
458 transpose( dst, dst0 );
459 else
460 {
461 transpose( dst, dst );
462 dst.convertTo( dst0, dst0.type() );
463 }
464 }
465 else
466 {
467 CV_Assert( dst.size() == dst0.size() );
468 if( dst.data != dst0.data )
469 dst.convertTo(dst0, dst0.type());
470 }
471 }
472