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
50using cv::Ptr;
51
52////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
53
54CvLevMarq::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}
62
63CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
64{
65    init(nparams, nerrs, criteria0, _completeSymmFlag);
66}
67
68void CvLevMarq::clear()
69{
70    mask.release();
71    prevParam.release();
72    param.release();
73    J.release();
74    err.release();
75    JtJ.release();
76    JtJN.release();
77    JtErr.release();
78    JtJV.release();
79    JtJW.release();
80}
81
82CvLevMarq::~CvLevMarq()
83{
84    clear();
85}
86
87void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
88{
89    if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
90        clear();
91    mask.reset(cvCreateMat( nparams, 1, CV_8U ));
92    cvSet(mask, cvScalarAll(1));
93    prevParam.reset(cvCreateMat( nparams, 1, CV_64F ));
94    param.reset(cvCreateMat( nparams, 1, CV_64F ));
95    JtJ.reset(cvCreateMat( nparams, nparams, CV_64F ));
96    JtJN.reset(cvCreateMat( nparams, nparams, CV_64F ));
97    JtJV.reset(cvCreateMat( nparams, nparams, CV_64F ));
98    JtJW.reset(cvCreateMat( nparams, 1, CV_64F ));
99    JtErr.reset(cvCreateMat( nparams, 1, CV_64F ));
100    if( nerrs > 0 )
101    {
102        J.reset(cvCreateMat( nerrs, nparams, CV_64F ));
103        err.reset(cvCreateMat( nerrs, 1, CV_64F ));
104    }
105    errNorm = prevErrNorm = DBL_MAX;
106    lambdaLg10 = -3;
107    criteria = criteria0;
108    if( criteria.type & CV_TERMCRIT_ITER )
109        criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
110    else
111        criteria.max_iter = 30;
112    if( criteria.type & CV_TERMCRIT_EPS )
113        criteria.epsilon = MAX(criteria.epsilon, 0);
114    else
115        criteria.epsilon = DBL_EPSILON;
116    state = STARTED;
117    iters = 0;
118    completeSymmFlag = _completeSymmFlag;
119}
120
121bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
122{
123    double change;
124
125    matJ = _err = 0;
126
127    assert( !err.empty() );
128    if( state == DONE )
129    {
130        _param = param;
131        return false;
132    }
133
134    if( state == STARTED )
135    {
136        _param = param;
137        cvZero( J );
138        cvZero( err );
139        matJ = J;
140        _err = err;
141        state = CALC_J;
142        return true;
143    }
144
145    if( state == CALC_J )
146    {
147        cvMulTransposed( J, JtJ, 1 );
148        cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
149        cvCopy( param, prevParam );
150        step();
151        if( iters == 0 )
152            prevErrNorm = cvNorm(err, 0, CV_L2);
153        _param = param;
154        cvZero( err );
155        _err = err;
156        state = CHECK_ERR;
157        return true;
158    }
159
160    assert( state == CHECK_ERR );
161    errNorm = cvNorm( err, 0, CV_L2 );
162    if( errNorm > prevErrNorm )
163    {
164        if( ++lambdaLg10 <= 16 )
165        {
166            step();
167            _param = param;
168            cvZero( err );
169            _err = err;
170            state = CHECK_ERR;
171            return true;
172        }
173    }
174
175    lambdaLg10 = MAX(lambdaLg10-1, -16);
176    if( ++iters >= criteria.max_iter ||
177        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
178    {
179        _param = param;
180        state = DONE;
181        return true;
182    }
183
184    prevErrNorm = errNorm;
185    _param = param;
186    cvZero(J);
187    matJ = J;
188    _err = err;
189    state = CALC_J;
190    return true;
191}
192
193
194bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
195{
196    double change;
197
198    CV_Assert( !err );
199    if( state == DONE )
200    {
201        _param = param;
202        return false;
203    }
204
205    if( state == STARTED )
206    {
207        _param = param;
208        cvZero( JtJ );
209        cvZero( JtErr );
210        errNorm = 0;
211        _JtJ = JtJ;
212        _JtErr = JtErr;
213        _errNorm = &errNorm;
214        state = CALC_J;
215        return true;
216    }
217
218    if( state == CALC_J )
219    {
220        cvCopy( param, prevParam );
221        step();
222        _param = param;
223        prevErrNorm = errNorm;
224        errNorm = 0;
225        _errNorm = &errNorm;
226        state = CHECK_ERR;
227        return true;
228    }
229
230    assert( state == CHECK_ERR );
231    if( errNorm > prevErrNorm )
232    {
233        if( ++lambdaLg10 <= 16 )
234        {
235            step();
236            _param = param;
237            errNorm = 0;
238            _errNorm = &errNorm;
239            state = CHECK_ERR;
240            return true;
241        }
242    }
243
244    lambdaLg10 = MAX(lambdaLg10-1, -16);
245    if( ++iters >= criteria.max_iter ||
246        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
247    {
248        _param = param;
249        state = DONE;
250        return false;
251    }
252
253    prevErrNorm = errNorm;
254    cvZero( JtJ );
255    cvZero( JtErr );
256    _param = param;
257    _JtJ = JtJ;
258    _JtErr = JtErr;
259    state = CALC_J;
260    return true;
261}
262
263void CvLevMarq::step()
264{
265    const double LOG10 = log(10.);
266    double lambda = exp(lambdaLg10*LOG10);
267    int i, j, nparams = param->rows;
268
269    for( i = 0; i < nparams; i++ )
270        if( mask->data.ptr[i] == 0 )
271        {
272            double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
273            for( j = 0; j < nparams; j++ )
274                row[j] = col[j*nparams] = 0;
275            JtErr->data.db[i] = 0;
276        }
277
278    if( !err )
279        cvCompleteSymm( JtJ, completeSymmFlag );
280#if 1
281    cvCopy( JtJ, JtJN );
282    for( i = 0; i < nparams; i++ )
283        JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
284#else
285    cvSetIdentity(JtJN, cvRealScalar(lambda));
286    cvAdd( JtJ, JtJN, JtJN );
287#endif
288    cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
289    cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
290    for( i = 0; i < nparams; i++ )
291        param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
292}
293
294
295CV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
296{
297    return cv::RANSACUpdateNumIters(p, ep, modelPoints, maxIters);
298}
299
300
301CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method,
302                              double ransacReprojThreshold, CvMat* _mask, int maxIters,
303                              double confidence)
304{
305    cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
306
307    if( src.channels() == 1 && (src.rows == 2 || src.rows == 3) && src.cols > 3 )
308        cv::transpose(src, src);
309    if( dst.channels() == 1 && (dst.rows == 2 || dst.rows == 3) && dst.cols > 3 )
310        cv::transpose(dst, dst);
311
312    if ( maxIters < 0 )
313        maxIters = 0;
314    if ( maxIters > 2000 )
315        maxIters = 2000;
316
317    if ( confidence < 0 )
318        confidence = 0;
319    if ( confidence > 1 )
320        confidence = 1;
321
322    const cv::Mat H = cv::cvarrToMat(__H), mask = cv::cvarrToMat(_mask);
323    cv::Mat H0 = cv::findHomography(src, dst, method, ransacReprojThreshold,
324                                    _mask ? cv::_OutputArray(mask) : cv::_OutputArray(), maxIters,
325                                    confidence);
326
327    if( H0.empty() )
328    {
329        cv::Mat Hz = cv::cvarrToMat(__H);
330        Hz.setTo(cv::Scalar::all(0));
331        return 0;
332    }
333    H0.convertTo(H, H.type());
334    return 1;
335}
336
337
338CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
339                                  CvMat* fmatrix, int method,
340                                  double param1, double param2, CvMat* _mask )
341{
342    cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2);
343
344    if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 )
345        cv::transpose(m1, m1);
346    if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 )
347        cv::transpose(m2, m2);
348
349    const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask);
350    cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2,
351                                         _mask ? cv::_OutputArray(mask) : cv::_OutputArray());
352
353    if( FM0.empty() )
354    {
355        cv::Mat FM0z = cv::cvarrToMat(fmatrix);
356        FM0z.setTo(cv::Scalar::all(0));
357        return 0;
358    }
359
360    CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 );
361    cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows));
362    FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type());
363    return FM1.rows / 3;
364}
365
366
367CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
368                                          const CvMat* fmatrix, CvMat* _lines )
369{
370    cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix);
371    cv::Mat lines = cv::cvarrToMat(_lines);
372    const cv::Mat lines0 = lines;
373
374    if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 )
375        cv::transpose(pt, pt);
376
377    cv::computeCorrespondEpilines(pt, pointImageID, fm, lines);
378
379    bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3;
380    lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows));
381
382    if( tflag )
383    {
384        CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows );
385        if( lines0.type() == lines.type() )
386            transpose( lines, lines0 );
387        else
388        {
389            transpose( lines, lines );
390            lines.convertTo( lines0, lines0.type() );
391        }
392    }
393    else
394    {
395        CV_Assert( lines.size() == lines0.size() );
396        if( lines.data != lines0.data )
397            lines.convertTo(lines0, lines0.type());
398    }
399}
400
401
402CV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst )
403{
404    cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
405    const cv::Mat dst0 = dst;
406
407    int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);
408
409    if( src.channels() == 1 && src.cols > d0 )
410        cv::transpose(src, src);
411
412    int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);
413
414    if( d0 == d1 )
415        src.copyTo(dst);
416    else if( d0 < d1 )
417        cv::convertPointsToHomogeneous(src, dst);
418    else
419        cv::convertPointsFromHomogeneous(src, dst);
420
421    bool tflag = dst0.channels() == 1 && dst0.cols > d1;
422    dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));
423
424    if( tflag )
425    {
426        CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );
427        if( dst0.type() == dst.type() )
428            transpose( dst, dst0 );
429        else
430        {
431            transpose( dst, dst );
432            dst.convertTo( dst0, dst0.type() );
433        }
434    }
435    else
436    {
437        CV_Assert( dst.size() == dst0.size() );
438        if( dst.data != dst0.data )
439            dst.convertTo(dst0, dst0.type());
440    }
441}
442