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