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-2008, Intel Corporation, all rights reserved.
14// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
16// Copyright (C) 2015, Itseez Inc., all rights reserved.
17// Third party copyrights are property of their respective owners.
18//
19// Redistribution and use in source and binary forms, with or without modification,
20// are permitted provided that the following conditions are met:
21//
22//   * Redistribution's of source code must retain the above copyright notice,
23//     this list of conditions and the following disclaimer.
24//
25//   * Redistribution's in binary form must reproduce the above copyright notice,
26//     this list of conditions and the following disclaimer in the documentation
27//     and/or other materials provided with the distribution.
28//
29//   * The name of the copyright holders may not be used to endorse or promote products
30//     derived from this software without specific prior written permission.
31//
32// This software is provided by the copyright holders and contributors "as is" and
33// any express or implied warranties, including, but not limited to, the implied
34// warranties of merchantability and fitness for a particular purpose are disclaimed.
35// In no event shall the Intel Corporation or contributors be liable for any direct,
36// indirect, incidental, special, exemplary, or consequential damages
37// (including, but not limited to, procurement of substitute goods or services;
38// loss of use, data, or profits; or business interruption) however caused
39// and on any theory of liability, whether in contract, strict liability,
40// or tort (including negligence or otherwise) arising in any way out of
41// the use of this software, even if advised of the possibility of such damage.
42//
43//M*/
44
45#ifndef __OPENCV_CORE_OPERATIONS_HPP__
46#define __OPENCV_CORE_OPERATIONS_HPP__
47
48#ifndef __cplusplus
49#  error operations.hpp header must be compiled as C++
50#endif
51
52#include <cstdio>
53
54//! @cond IGNORED
55
56namespace cv
57{
58
59////////////////////////////// Matx methods depending on core API /////////////////////////////
60
61namespace internal
62{
63
64template<typename _Tp, int m> struct Matx_FastInvOp
65{
66    bool operator()(const Matx<_Tp, m, m>& a, Matx<_Tp, m, m>& b, int method) const
67    {
68        Matx<_Tp, m, m> temp = a;
69
70        // assume that b is all 0's on input => make it a unity matrix
71        for( int i = 0; i < m; i++ )
72            b(i, i) = (_Tp)1;
73
74        if( method == DECOMP_CHOLESKY )
75            return Cholesky(temp.val, m*sizeof(_Tp), m, b.val, m*sizeof(_Tp), m);
76
77        return LU(temp.val, m*sizeof(_Tp), m, b.val, m*sizeof(_Tp), m) != 0;
78    }
79};
80
81template<typename _Tp> struct Matx_FastInvOp<_Tp, 2>
82{
83    bool operator()(const Matx<_Tp, 2, 2>& a, Matx<_Tp, 2, 2>& b, int) const
84    {
85        _Tp d = determinant(a);
86        if( d == 0 )
87            return false;
88        d = 1/d;
89        b(1,1) = a(0,0)*d;
90        b(0,0) = a(1,1)*d;
91        b(0,1) = -a(0,1)*d;
92        b(1,0) = -a(1,0)*d;
93        return true;
94    }
95};
96
97template<typename _Tp> struct Matx_FastInvOp<_Tp, 3>
98{
99    bool operator()(const Matx<_Tp, 3, 3>& a, Matx<_Tp, 3, 3>& b, int) const
100    {
101        _Tp d = (_Tp)determinant(a);
102        if( d == 0 )
103            return false;
104        d = 1/d;
105        b(0,0) = (a(1,1) * a(2,2) - a(1,2) * a(2,1)) * d;
106        b(0,1) = (a(0,2) * a(2,1) - a(0,1) * a(2,2)) * d;
107        b(0,2) = (a(0,1) * a(1,2) - a(0,2) * a(1,1)) * d;
108
109        b(1,0) = (a(1,2) * a(2,0) - a(1,0) * a(2,2)) * d;
110        b(1,1) = (a(0,0) * a(2,2) - a(0,2) * a(2,0)) * d;
111        b(1,2) = (a(0,2) * a(1,0) - a(0,0) * a(1,2)) * d;
112
113        b(2,0) = (a(1,0) * a(2,1) - a(1,1) * a(2,0)) * d;
114        b(2,1) = (a(0,1) * a(2,0) - a(0,0) * a(2,1)) * d;
115        b(2,2) = (a(0,0) * a(1,1) - a(0,1) * a(1,0)) * d;
116        return true;
117    }
118};
119
120
121template<typename _Tp, int m, int n> struct Matx_FastSolveOp
122{
123    bool operator()(const Matx<_Tp, m, m>& a, const Matx<_Tp, m, n>& b,
124                    Matx<_Tp, m, n>& x, int method) const
125    {
126        Matx<_Tp, m, m> temp = a;
127        x = b;
128        if( method == DECOMP_CHOLESKY )
129            return Cholesky(temp.val, m*sizeof(_Tp), m, x.val, n*sizeof(_Tp), n);
130
131        return LU(temp.val, m*sizeof(_Tp), m, x.val, n*sizeof(_Tp), n) != 0;
132    }
133};
134
135template<typename _Tp> struct Matx_FastSolveOp<_Tp, 2, 1>
136{
137    bool operator()(const Matx<_Tp, 2, 2>& a, const Matx<_Tp, 2, 1>& b,
138                    Matx<_Tp, 2, 1>& x, int) const
139    {
140        _Tp d = determinant(a);
141        if( d == 0 )
142            return false;
143        d = 1/d;
144        x(0) = (b(0)*a(1,1) - b(1)*a(0,1))*d;
145        x(1) = (b(1)*a(0,0) - b(0)*a(1,0))*d;
146        return true;
147    }
148};
149
150template<typename _Tp> struct Matx_FastSolveOp<_Tp, 3, 1>
151{
152    bool operator()(const Matx<_Tp, 3, 3>& a, const Matx<_Tp, 3, 1>& b,
153                    Matx<_Tp, 3, 1>& x, int) const
154    {
155        _Tp d = (_Tp)determinant(a);
156        if( d == 0 )
157            return false;
158        d = 1/d;
159        x(0) = d*(b(0)*(a(1,1)*a(2,2) - a(1,2)*a(2,1)) -
160                a(0,1)*(b(1)*a(2,2) - a(1,2)*b(2)) +
161                a(0,2)*(b(1)*a(2,1) - a(1,1)*b(2)));
162
163        x(1) = d*(a(0,0)*(b(1)*a(2,2) - a(1,2)*b(2)) -
164                b(0)*(a(1,0)*a(2,2) - a(1,2)*a(2,0)) +
165                a(0,2)*(a(1,0)*b(2) - b(1)*a(2,0)));
166
167        x(2) = d*(a(0,0)*(a(1,1)*b(2) - b(1)*a(2,1)) -
168                a(0,1)*(a(1,0)*b(2) - b(1)*a(2,0)) +
169                b(0)*(a(1,0)*a(2,1) - a(1,1)*a(2,0)));
170        return true;
171    }
172};
173
174} // internal
175
176template<typename _Tp, int m, int n> inline
177Matx<_Tp,m,n> Matx<_Tp,m,n>::randu(_Tp a, _Tp b)
178{
179    Matx<_Tp,m,n> M;
180    cv::randu(M, Scalar(a), Scalar(b));
181    return M;
182}
183
184template<typename _Tp, int m, int n> inline
185Matx<_Tp,m,n> Matx<_Tp,m,n>::randn(_Tp a, _Tp b)
186{
187    Matx<_Tp,m,n> M;
188    cv::randn(M, Scalar(a), Scalar(b));
189    return M;
190}
191
192template<typename _Tp, int m, int n> inline
193Matx<_Tp, n, m> Matx<_Tp, m, n>::inv(int method, bool *p_is_ok /*= NULL*/) const
194{
195    Matx<_Tp, n, m> b;
196    bool ok;
197    if( method == DECOMP_LU || method == DECOMP_CHOLESKY )
198        ok = cv::internal::Matx_FastInvOp<_Tp, m>()(*this, b, method);
199    else
200    {
201        Mat A(*this, false), B(b, false);
202        ok = (invert(A, B, method) != 0);
203    }
204    if( NULL != p_is_ok ) { *p_is_ok = ok; }
205    return ok ? b : Matx<_Tp, n, m>::zeros();
206}
207
208template<typename _Tp, int m, int n> template<int l> inline
209Matx<_Tp, n, l> Matx<_Tp, m, n>::solve(const Matx<_Tp, m, l>& rhs, int method) const
210{
211    Matx<_Tp, n, l> x;
212    bool ok;
213    if( method == DECOMP_LU || method == DECOMP_CHOLESKY )
214        ok = cv::internal::Matx_FastSolveOp<_Tp, m, l>()(*this, rhs, x, method);
215    else
216    {
217        Mat A(*this, false), B(rhs, false), X(x, false);
218        ok = cv::solve(A, B, X, method);
219    }
220
221    return ok ? x : Matx<_Tp, n, l>::zeros();
222}
223
224
225
226////////////////////////// Augmenting algebraic & logical operations //////////////////////////
227
228#define CV_MAT_AUG_OPERATOR1(op, cvop, A, B) \
229    static inline A& operator op (A& a, const B& b) { cvop; return a; }
230
231#define CV_MAT_AUG_OPERATOR(op, cvop, A, B)   \
232    CV_MAT_AUG_OPERATOR1(op, cvop, A, B)      \
233    CV_MAT_AUG_OPERATOR1(op, cvop, const A, B)
234
235#define CV_MAT_AUG_OPERATOR_T(op, cvop, A, B)                   \
236    template<typename _Tp> CV_MAT_AUG_OPERATOR1(op, cvop, A, B) \
237    template<typename _Tp> CV_MAT_AUG_OPERATOR1(op, cvop, const A, B)
238
239CV_MAT_AUG_OPERATOR  (+=, cv::add(a,b,a), Mat, Mat)
240CV_MAT_AUG_OPERATOR  (+=, cv::add(a,b,a), Mat, Scalar)
241CV_MAT_AUG_OPERATOR_T(+=, cv::add(a,b,a), Mat_<_Tp>, Mat)
242CV_MAT_AUG_OPERATOR_T(+=, cv::add(a,b,a), Mat_<_Tp>, Scalar)
243CV_MAT_AUG_OPERATOR_T(+=, cv::add(a,b,a), Mat_<_Tp>, Mat_<_Tp>)
244
245CV_MAT_AUG_OPERATOR  (-=, cv::subtract(a,b,a), Mat, Mat)
246CV_MAT_AUG_OPERATOR  (-=, cv::subtract(a,b,a), Mat, Scalar)
247CV_MAT_AUG_OPERATOR_T(-=, cv::subtract(a,b,a), Mat_<_Tp>, Mat)
248CV_MAT_AUG_OPERATOR_T(-=, cv::subtract(a,b,a), Mat_<_Tp>, Scalar)
249CV_MAT_AUG_OPERATOR_T(-=, cv::subtract(a,b,a), Mat_<_Tp>, Mat_<_Tp>)
250
251CV_MAT_AUG_OPERATOR  (*=, cv::gemm(a, b, 1, Mat(), 0, a, 0), Mat, Mat)
252CV_MAT_AUG_OPERATOR_T(*=, cv::gemm(a, b, 1, Mat(), 0, a, 0), Mat_<_Tp>, Mat)
253CV_MAT_AUG_OPERATOR_T(*=, cv::gemm(a, b, 1, Mat(), 0, a, 0), Mat_<_Tp>, Mat_<_Tp>)
254CV_MAT_AUG_OPERATOR  (*=, a.convertTo(a, -1, b), Mat, double)
255CV_MAT_AUG_OPERATOR_T(*=, a.convertTo(a, -1, b), Mat_<_Tp>, double)
256
257CV_MAT_AUG_OPERATOR  (/=, cv::divide(a,b,a), Mat, Mat)
258CV_MAT_AUG_OPERATOR_T(/=, cv::divide(a,b,a), Mat_<_Tp>, Mat)
259CV_MAT_AUG_OPERATOR_T(/=, cv::divide(a,b,a), Mat_<_Tp>, Mat_<_Tp>)
260CV_MAT_AUG_OPERATOR  (/=, a.convertTo((Mat&)a, -1, 1./b), Mat, double)
261CV_MAT_AUG_OPERATOR_T(/=, a.convertTo((Mat&)a, -1, 1./b), Mat_<_Tp>, double)
262
263CV_MAT_AUG_OPERATOR  (&=, cv::bitwise_and(a,b,a), Mat, Mat)
264CV_MAT_AUG_OPERATOR  (&=, cv::bitwise_and(a,b,a), Mat, Scalar)
265CV_MAT_AUG_OPERATOR_T(&=, cv::bitwise_and(a,b,a), Mat_<_Tp>, Mat)
266CV_MAT_AUG_OPERATOR_T(&=, cv::bitwise_and(a,b,a), Mat_<_Tp>, Scalar)
267CV_MAT_AUG_OPERATOR_T(&=, cv::bitwise_and(a,b,a), Mat_<_Tp>, Mat_<_Tp>)
268
269CV_MAT_AUG_OPERATOR  (|=, cv::bitwise_or(a,b,a), Mat, Mat)
270CV_MAT_AUG_OPERATOR  (|=, cv::bitwise_or(a,b,a), Mat, Scalar)
271CV_MAT_AUG_OPERATOR_T(|=, cv::bitwise_or(a,b,a), Mat_<_Tp>, Mat)
272CV_MAT_AUG_OPERATOR_T(|=, cv::bitwise_or(a,b,a), Mat_<_Tp>, Scalar)
273CV_MAT_AUG_OPERATOR_T(|=, cv::bitwise_or(a,b,a), Mat_<_Tp>, Mat_<_Tp>)
274
275CV_MAT_AUG_OPERATOR  (^=, cv::bitwise_xor(a,b,a), Mat, Mat)
276CV_MAT_AUG_OPERATOR  (^=, cv::bitwise_xor(a,b,a), Mat, Scalar)
277CV_MAT_AUG_OPERATOR_T(^=, cv::bitwise_xor(a,b,a), Mat_<_Tp>, Mat)
278CV_MAT_AUG_OPERATOR_T(^=, cv::bitwise_xor(a,b,a), Mat_<_Tp>, Scalar)
279CV_MAT_AUG_OPERATOR_T(^=, cv::bitwise_xor(a,b,a), Mat_<_Tp>, Mat_<_Tp>)
280
281#undef CV_MAT_AUG_OPERATOR_T
282#undef CV_MAT_AUG_OPERATOR
283#undef CV_MAT_AUG_OPERATOR1
284
285
286
287///////////////////////////////////////////// SVD /////////////////////////////////////////////
288
289inline SVD::SVD() {}
290inline SVD::SVD( InputArray m, int flags ) { operator ()(m, flags); }
291inline void SVD::solveZ( InputArray m, OutputArray _dst )
292{
293    Mat mtx = m.getMat();
294    SVD svd(mtx, (mtx.rows >= mtx.cols ? 0 : SVD::FULL_UV));
295    _dst.create(svd.vt.cols, 1, svd.vt.type());
296    Mat dst = _dst.getMat();
297    svd.vt.row(svd.vt.rows-1).reshape(1,svd.vt.cols).copyTo(dst);
298}
299
300template<typename _Tp, int m, int n, int nm> inline void
301    SVD::compute( const Matx<_Tp, m, n>& a, Matx<_Tp, nm, 1>& w, Matx<_Tp, m, nm>& u, Matx<_Tp, n, nm>& vt )
302{
303    CV_StaticAssert( nm == MIN(m, n), "Invalid size of output vector.");
304    Mat _a(a, false), _u(u, false), _w(w, false), _vt(vt, false);
305    SVD::compute(_a, _w, _u, _vt);
306    CV_Assert(_w.data == (uchar*)&w.val[0] && _u.data == (uchar*)&u.val[0] && _vt.data == (uchar*)&vt.val[0]);
307}
308
309template<typename _Tp, int m, int n, int nm> inline void
310SVD::compute( const Matx<_Tp, m, n>& a, Matx<_Tp, nm, 1>& w )
311{
312    CV_StaticAssert( nm == MIN(m, n), "Invalid size of output vector.");
313    Mat _a(a, false), _w(w, false);
314    SVD::compute(_a, _w);
315    CV_Assert(_w.data == (uchar*)&w.val[0]);
316}
317
318template<typename _Tp, int m, int n, int nm, int nb> inline void
319SVD::backSubst( const Matx<_Tp, nm, 1>& w, const Matx<_Tp, m, nm>& u,
320                const Matx<_Tp, n, nm>& vt, const Matx<_Tp, m, nb>& rhs,
321                Matx<_Tp, n, nb>& dst )
322{
323    CV_StaticAssert( nm == MIN(m, n), "Invalid size of output vector.");
324    Mat _u(u, false), _w(w, false), _vt(vt, false), _rhs(rhs, false), _dst(dst, false);
325    SVD::backSubst(_w, _u, _vt, _rhs, _dst);
326    CV_Assert(_dst.data == (uchar*)&dst.val[0]);
327}
328
329
330
331/////////////////////////////////// Multiply-with-Carry RNG ///////////////////////////////////
332
333inline RNG::RNG()              { state = 0xffffffff; }
334inline RNG::RNG(uint64 _state) { state = _state ? _state : 0xffffffff; }
335
336inline RNG::operator uchar()    { return (uchar)next(); }
337inline RNG::operator schar()    { return (schar)next(); }
338inline RNG::operator ushort()   { return (ushort)next(); }
339inline RNG::operator short()    { return (short)next(); }
340inline RNG::operator int()      { return (int)next(); }
341inline RNG::operator unsigned() { return next(); }
342inline RNG::operator float()    { return next()*2.3283064365386962890625e-10f; }
343inline RNG::operator double()   { unsigned t = next(); return (((uint64)t << 32) | next()) * 5.4210108624275221700372640043497e-20; }
344
345inline unsigned RNG::operator ()(unsigned N) { return (unsigned)uniform(0,N); }
346inline unsigned RNG::operator ()()           { return next(); }
347
348inline int    RNG::uniform(int a, int b)       { return a == b ? a : (int)(next() % (b - a) + a); }
349inline float  RNG::uniform(float a, float b)   { return ((float)*this)*(b - a) + a; }
350inline double RNG::uniform(double a, double b) { return ((double)*this)*(b - a) + a; }
351
352inline unsigned RNG::next()
353{
354    state = (uint64)(unsigned)state* /*CV_RNG_COEFF*/ 4164903690U + (unsigned)(state >> 32);
355    return (unsigned)state;
356}
357
358//! returns the next unifomly-distributed random number of the specified type
359template<typename _Tp> static inline _Tp randu()
360{
361  return (_Tp)theRNG();
362}
363
364///////////////////////////////// Formatted string generation /////////////////////////////////
365
366CV_EXPORTS String format( const char* fmt, ... );
367
368///////////////////////////////// Formatted output of cv::Mat /////////////////////////////////
369
370static inline
371Ptr<Formatted> format(InputArray mtx, int fmt)
372{
373    return Formatter::get(fmt)->format(mtx.getMat());
374}
375
376static inline
377int print(Ptr<Formatted> fmtd, FILE* stream = stdout)
378{
379    int written = 0;
380    fmtd->reset();
381    for(const char* str = fmtd->next(); str; str = fmtd->next())
382        written += fputs(str, stream);
383
384    return written;
385}
386
387static inline
388int print(const Mat& mtx, FILE* stream = stdout)
389{
390    return print(Formatter::get()->format(mtx), stream);
391}
392
393static inline
394int print(const UMat& mtx, FILE* stream = stdout)
395{
396    return print(Formatter::get()->format(mtx.getMat(ACCESS_READ)), stream);
397}
398
399template<typename _Tp> static inline
400int print(const std::vector<Point_<_Tp> >& vec, FILE* stream = stdout)
401{
402    return print(Formatter::get()->format(Mat(vec)), stream);
403}
404
405template<typename _Tp> static inline
406int print(const std::vector<Point3_<_Tp> >& vec, FILE* stream = stdout)
407{
408    return print(Formatter::get()->format(Mat(vec)), stream);
409}
410
411template<typename _Tp, int m, int n> static inline
412int print(const Matx<_Tp, m, n>& matx, FILE* stream = stdout)
413{
414    return print(Formatter::get()->format(cv::Mat(matx)), stream);
415}
416
417//! @endcond
418
419/****************************************************************************************\
420*                                  Auxiliary algorithms                                  *
421\****************************************************************************************/
422
423/** @brief Splits an element set into equivalency classes.
424
425The generic function partition implements an \f$O(N^2)\f$ algorithm for splitting a set of \f$N\f$ elements
426into one or more equivalency classes, as described in
427<http://en.wikipedia.org/wiki/Disjoint-set_data_structure> . The function returns the number of
428equivalency classes.
429@param _vec Set of elements stored as a vector.
430@param labels Output vector of labels. It contains as many elements as vec. Each label labels[i] is
431a 0-based cluster index of `vec[i]`.
432@param predicate Equivalence predicate (pointer to a boolean function of two arguments or an
433instance of the class that has the method bool operator()(const _Tp& a, const _Tp& b) ). The
434predicate returns true when the elements are certainly in the same class, and returns false if they
435may or may not be in the same class.
436@ingroup core_cluster
437*/
438template<typename _Tp, class _EqPredicate> int
439partition( const std::vector<_Tp>& _vec, std::vector<int>& labels,
440          _EqPredicate predicate=_EqPredicate())
441{
442    int i, j, N = (int)_vec.size();
443    const _Tp* vec = &_vec[0];
444
445    const int PARENT=0;
446    const int RANK=1;
447
448    std::vector<int> _nodes(N*2);
449    int (*nodes)[2] = (int(*)[2])&_nodes[0];
450
451    // The first O(N) pass: create N single-vertex trees
452    for(i = 0; i < N; i++)
453    {
454        nodes[i][PARENT]=-1;
455        nodes[i][RANK] = 0;
456    }
457
458    // The main O(N^2) pass: merge connected components
459    for( i = 0; i < N; i++ )
460    {
461        int root = i;
462
463        // find root
464        while( nodes[root][PARENT] >= 0 )
465            root = nodes[root][PARENT];
466
467        for( j = 0; j < N; j++ )
468        {
469            if( i == j || !predicate(vec[i], vec[j]))
470                continue;
471            int root2 = j;
472
473            while( nodes[root2][PARENT] >= 0 )
474                root2 = nodes[root2][PARENT];
475
476            if( root2 != root )
477            {
478                // unite both trees
479                int rank = nodes[root][RANK], rank2 = nodes[root2][RANK];
480                if( rank > rank2 )
481                    nodes[root2][PARENT] = root;
482                else
483                {
484                    nodes[root][PARENT] = root2;
485                    nodes[root2][RANK] += rank == rank2;
486                    root = root2;
487                }
488                CV_Assert( nodes[root][PARENT] < 0 );
489
490                int k = j, parent;
491
492                // compress the path from node2 to root
493                while( (parent = nodes[k][PARENT]) >= 0 )
494                {
495                    nodes[k][PARENT] = root;
496                    k = parent;
497                }
498
499                // compress the path from node to root
500                k = i;
501                while( (parent = nodes[k][PARENT]) >= 0 )
502                {
503                    nodes[k][PARENT] = root;
504                    k = parent;
505                }
506            }
507        }
508    }
509
510    // Final O(N) pass: enumerate classes
511    labels.resize(N);
512    int nclasses = 0;
513
514    for( i = 0; i < N; i++ )
515    {
516        int root = i;
517        while( nodes[root][PARENT] >= 0 )
518            root = nodes[root][PARENT];
519        // re-use the rank as the class label
520        if( nodes[root][RANK] >= 0 )
521            nodes[root][RANK] = ~nclasses++;
522        labels[i] = ~nodes[root][RANK];
523    }
524
525    return nclasses;
526}
527
528} // cv
529
530#endif
531