--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_cxcore.h"
+
+/****************************************************************************************\
+* [scaled] Identity matrix initialization *
+\****************************************************************************************/
+
+namespace cv {
+
+Mat::Mat(const IplImage* img, bool copyData)
+ : flags(0), rows(0), cols(0), step(0), data(0),
+ refcount(0), datastart(0), dataend(0)
+{
+ CvMat m, dst;
+ int coi=0;
+ cvGetMat( img, &m, &coi );
+
+ if( copyData )
+ {
+ if( coi == 0 )
+ {
+ create( m.rows, m.cols, CV_MAT_TYPE(m.type) );
+ dst = *this;
+ cvCopy( &m, &dst );
+ }
+ else
+ {
+ create( m.rows, m.cols, CV_MAT_DEPTH(m.type) );
+ dst = *this;
+ CvMat* pdst = &dst;
+ const int pairs[] = { coi-1, 0 };
+ cvMixChannels( (const CvArr**)&img, 1, (CvArr**)&pdst, 1, pairs, 1 );
+ }
+ }
+ else
+ {
+ /*if( coi != 0 )
+ CV_Error(CV_BadCOI, "When copyData=false, COI must not be set");*/
+
+ *this = Mat(m.rows, m.cols, CV_MAT_TYPE(m.type), m.data.ptr, m.step);
+ /*if( img->roi )
+ {
+ datastart = (uchar*)img->imageData;
+ dataend = datastart + img->imageSize;
+ }*/
+ }
+}
+
+Mat cvarrToMat(const CvArr* arr, bool copyData, bool allowND, int coiMode)
+{
+ Mat m;
+ if( CV_IS_MAT(arr) )
+ m = Mat((const CvMat*)arr, copyData );
+ else if( CV_IS_IMAGE(arr) )
+ {
+ const IplImage* iplimg = (const IplImage*)arr;
+ m = Mat(iplimg, copyData );
+ if( coiMode == 0 && cvGetImageCOI(iplimg) > 0 )
+ CV_Error(CV_BadCOI, "COI is not supported by the function");
+ }
+ else
+ {
+ CvMat hdr, *cvmat = cvGetMat( arr, &hdr, 0, allowND ? 1 : 0 );
+ if( cvmat )
+ m = Mat(cvmat, copyData);
+ }
+ return m;
+}
+
+void extractImageCOI(const CvArr* arr, Mat& ch, int coi)
+{
+ Mat mat = cvarrToMat(arr, false, true, 1);
+ ch.create(mat.size(), mat.depth());
+ if(coi < 0)
+ CV_Assert( CV_IS_IMAGE(arr) && (coi = cvGetImageCOI((const IplImage*)arr)-1) >= 0 );
+ CV_Assert(0 <= coi && coi < mat.channels());
+ int _pairs[] = { coi, 0 };
+ mixChannels( &mat, 1, &ch, 1, _pairs, 1 );
+}
+
+void insertImageCOI(const Mat& ch, CvArr* arr, int coi)
+{
+ Mat mat = cvarrToMat(arr, false, true, 1);
+ if(coi < 0)
+ CV_Assert( CV_IS_IMAGE(arr) && (coi = cvGetImageCOI((const IplImage*)arr)-1) >= 0 );
+ CV_Assert(ch.size() == mat.size() && ch.depth() == mat.depth() && 0 <= coi && coi < mat.channels());
+ int _pairs[] = { 0, coi };
+ mixChannels( &ch, 1, &mat, 1, _pairs, 1 );
+}
+
+
+Mat Mat::reshape(int new_cn, int new_rows) const
+{
+ Mat hdr = *this;
+
+ int cn = channels();
+ if( new_cn == 0 )
+ new_cn = cn;
+
+ int total_width = cols * cn;
+
+ if( (new_cn > total_width || total_width % new_cn != 0) && new_rows == 0 )
+ new_rows = rows * total_width / new_cn;
+
+ if( new_rows != 0 && new_rows != rows )
+ {
+ int total_size = total_width * rows;
+ if( !isContinuous() )
+ CV_Error( CV_BadStep,
+ "The matrix is not continuous, thus its number of rows can not be changed" );
+
+ if( (unsigned)new_rows > (unsigned)total_size )
+ CV_Error( CV_StsOutOfRange, "Bad new number of rows" );
+
+ total_width = total_size / new_rows;
+
+ if( total_width * new_rows != total_size )
+ CV_Error( CV_StsBadArg, "The total number of matrix elements "
+ "is not divisible by the new number of rows" );
+
+ hdr.rows = new_rows;
+ hdr.step = total_width * elemSize1();
+ }
+
+ int new_width = total_width / new_cn;
+
+ if( new_width * new_cn != total_width )
+ CV_Error( CV_BadNumChannels,
+ "The total width is not divisible by the new number of channels" );
+
+ hdr.cols = new_width;
+ hdr.flags = (hdr.flags & ~CV_MAT_CN_MASK) | ((new_cn-1) << CV_CN_SHIFT);
+ return hdr;
+}
+
+
+void
+setIdentity( Mat& m, const Scalar& s )
+{
+ int i, j, rows = m.rows, cols = m.cols, type = m.type();
+
+ if( type == CV_32FC1 )
+ {
+ float* data = (float*)m.data;
+ float val = (float)s[0];
+ size_t step = m.step/sizeof(data[0]);
+
+ for( i = 0; i < rows; i++, data += step )
+ {
+ for( j = 0; j < cols; j++ )
+ data[j] = 0;
+ if( i < cols )
+ data[i] = val;
+ }
+ }
+ else if( type == CV_64FC1 )
+ {
+ double* data = (double*)m.data;
+ double val = s[0];
+ size_t step = m.step/sizeof(data[0]);
+
+ for( i = 0; i < rows; i++, data += step )
+ {
+ for( j = 0; j < cols; j++ )
+ data[j] = 0;
+ if( i < cols )
+ data[i] = val;
+ }
+ }
+ else
+ {
+ m = Scalar(0);
+ m.diag() = s;
+ }
+}
+
+Scalar trace( const Mat& m )
+{
+ int i, type = m.type();
+ int nm = std::min(m.rows, m.cols);
+
+ if( type == CV_32FC1 )
+ {
+ const float* ptr = (const float*)m.data;
+ size_t step = m.step/sizeof(ptr[0]) + 1;
+ double _s = 0;
+ for( i = 0; i < nm; i++ )
+ _s += ptr[i*step];
+ return _s;
+ }
+
+ if( type == CV_64FC1 )
+ {
+ const double* ptr = (const double*)m.data;
+ size_t step = m.step/sizeof(ptr[0]) + 1;
+ double _s = 0;
+ for( i = 0; i < nm; i++ )
+ _s += ptr[i*step];
+ return _s;
+ }
+
+ return cv::sum(m.diag());
+}
+
+
+/****************************************************************************************\
+* transpose *
+\****************************************************************************************/
+
+template<typename T> static void
+transposeI_( Mat& mat )
+{
+ int rows = mat.rows, cols = mat.cols;
+ uchar* data = mat.data;
+ size_t step = mat.step;
+
+ for( int i = 0; i < rows; i++ )
+ {
+ T* row = (T*)(data + step*i);
+ uchar* data1 = data + i*sizeof(T);
+ for( int j = i+1; j < cols; j++ )
+ std::swap( row[j], *(T*)(data1 + step*j) );
+ }
+}
+
+template<typename T> static void
+transpose_( const Mat& src, Mat& dst )
+{
+ int rows = dst.rows, cols = dst.cols;
+ uchar* data = src.data;
+ size_t step = src.step;
+
+ for( int i = 0; i < rows; i++ )
+ {
+ T* row = (T*)(dst.data + dst.step*i);
+ uchar* data1 = data + i*sizeof(T);
+ for( int j = 0; j < cols; j++ )
+ row[j] = *(T*)(data1 + step*j);
+ }
+}
+
+typedef void (*TransposeInplaceFunc)( Mat& mat );
+typedef void (*TransposeFunc)( const Mat& src, Mat& dst );
+
+void transpose( const Mat& src, Mat& dst )
+{
+ TransposeInplaceFunc itab[] =
+ {
+ 0,
+ transposeI_<uchar>, // 1
+ transposeI_<ushort>, // 2
+ transposeI_<Vec<uchar,3> >, // 3
+ transposeI_<int>, // 4
+ 0,
+ transposeI_<Vec<ushort,3> >, // 6
+ 0,
+ transposeI_<int64>, // 8
+ 0, 0, 0,
+ transposeI_<Vec<int,3> >, // 12
+ 0, 0, 0,
+ transposeI_<Vec<int64,2> >, // 16
+ 0, 0, 0, 0, 0, 0, 0,
+ transposeI_<Vec<int64,3> >, // 24
+ 0, 0, 0, 0, 0, 0, 0,
+ transposeI_<Vec<int64,4> > // 32
+ };
+
+ TransposeFunc tab[] =
+ {
+ 0,
+ transpose_<uchar>, // 1
+ transpose_<ushort>, // 2
+ transpose_<Vec<uchar,3> >, // 3
+ transpose_<int>, // 4
+ 0,
+ transpose_<Vec<ushort,3> >, // 6
+ 0,
+ transpose_<int64>, // 8
+ 0, 0, 0,
+ transpose_<Vec<int,3> >, // 12
+ 0, 0, 0,
+ transpose_<Vec<int64,2> >, // 16
+ 0, 0, 0, 0, 0, 0, 0,
+ transpose_<Vec<int64,3> >, // 24
+ 0, 0, 0, 0, 0, 0, 0,
+ transpose_<Vec<int64,4> > // 32
+ };
+
+ size_t esz = src.elemSize();
+ CV_Assert( esz <= (size_t)32 );
+
+ if( dst.data == src.data && dst.cols == dst.rows )
+ {
+ TransposeInplaceFunc func = itab[esz];
+ CV_Assert( func != 0 );
+ func( dst );
+ }
+ else
+ {
+ dst.create( src.cols, src.rows, src.type() );
+ TransposeFunc func = tab[esz];
+ CV_Assert( func != 0 );
+ func( src, dst );
+ }
+}
+
+
+void completeSymm( Mat& matrix, bool LtoR )
+{
+ int i, j, nrows = matrix.rows, type = matrix.type();
+ int j0 = 0, j1 = nrows;
+ CV_Assert( matrix.rows == matrix.cols );
+
+ if( type == CV_32FC1 || type == CV_32SC1 )
+ {
+ int* data = (int*)matrix.data;
+ size_t step = matrix.step/sizeof(data[0]);
+ for( i = 0; i < nrows; i++ )
+ {
+ if( !LtoR ) j1 = i; else j0 = i+1;
+ for( j = j0; j < j1; j++ )
+ data[i*step + j] = data[j*step + i];
+ }
+ }
+ else if( type == CV_64FC1 )
+ {
+ double* data = (double*)matrix.data;
+ size_t step = matrix.step/sizeof(data[0]);
+ for( i = 0; i < nrows; i++ )
+ {
+ if( !LtoR ) j1 = i; else j0 = i+1;
+ for( j = j0; j < j1; j++ )
+ data[i*step + j] = data[j*step + i];
+ }
+ }
+ else
+ CV_Error( CV_StsUnsupportedFormat, "" );
+}
+
+Mat Mat::cross(const Mat& m) const
+{
+ int t = type(), d = CV_MAT_DEPTH(t);
+ CV_Assert( size() == m.size() && t == m.type() &&
+ ((rows == 3 && cols == 1) || (cols*channels() == 3 && rows == 1)));
+ Mat result(rows, cols, t);
+
+ if( d == CV_32F )
+ {
+ const float *a = (const float*)data, *b = (const float*)m.data;
+ float* c = (float*)result.data;
+ size_t lda = rows > 1 ? step/sizeof(a[0]) : 1;
+ size_t ldb = rows > 1 ? m.step/sizeof(b[0]) : 1;
+
+ c[0] = a[lda] * b[ldb*2] - a[lda*2] * b[ldb];
+ c[1] = a[lda*2] * b[0] - a[0] * b[ldb*2];
+ c[2] = a[0] * b[ldb] - a[lda] * b[0];
+ }
+ else if( d == CV_64F )
+ {
+ const double *a = (const double*)data, *b = (const double*)m.data;
+ double* c = (double*)result.data;
+ size_t lda = rows > 1 ? step/sizeof(a[0]) : 1;
+ size_t ldb = rows > 1 ? m.step/sizeof(b[0]) : 1;
+
+ c[0] = a[lda] * b[ldb*2] - a[lda*2] * b[ldb];
+ c[1] = a[lda*2] * b[0] - a[0] * b[ldb*2];
+ c[2] = a[0] * b[ldb] - a[lda] * b[0];
+ }
+
+ return result;
+}
+
+
+/****************************************************************************************\
+* Reduce Mat to vector *
+\****************************************************************************************/
+
+template<typename T, typename ST, class Op> static void
+reduceR_( const Mat& srcmat, Mat& dstmat )
+{
+ typedef typename Op::rtype WT;
+ Size size = srcmat.size();
+ size.width *= srcmat.channels();
+ AutoBuffer<WT> buffer(size.width);
+ WT* buf = buffer;
+ ST* dst = (ST*)dstmat.data;
+ const T* src = (const T*)srcmat.data;
+ size_t srcstep = srcmat.step/sizeof(src[0]);
+ int i;
+ Op op;
+
+ for( i = 0; i < size.width; i++ )
+ buf[i] = src[i];
+
+ for( ; --size.height; )
+ {
+ src += srcstep;
+ for( i = 0; i <= size.width - 4; i += 4 )
+ {
+ WT s0, s1;
+ s0 = op(buf[i], (WT)src[i]);
+ s1 = op(buf[i+1], (WT)src[i+1]);
+ buf[i] = s0; buf[i+1] = s1;
+
+ s0 = op(buf[i+2], (WT)src[i+2]);
+ s1 = op(buf[i+3], (WT)src[i+3]);
+ buf[i+2] = s0; buf[i+3] = s1;
+ }
+
+ for( ; i < size.width; i++ )
+ buf[i] = op(buf[i], (WT)src[i]);
+ }
+
+ for( i = 0; i < size.width; i++ )
+ dst[i] = (ST)buf[i];
+}
+
+
+template<typename T, typename ST, class Op> static void
+reduceC_( const Mat& srcmat, Mat& dstmat )
+{
+ typedef typename Op::rtype WT;
+ Size size = srcmat.size();
+ int i, k, cn = srcmat.channels();
+ size.width *= cn;
+ Op op;
+
+ for( int y = 0; y < size.height; y++ )
+ {
+ const T* src = (const T*)(srcmat.data + srcmat.step*y);
+ ST* dst = (ST*)(dstmat.data + dstmat.step*y);
+ if( size.width == cn )
+ for( k = 0; k < cn; k++ )
+ dst[k] = src[k];
+ else
+ {
+ for( k = 0; k < cn; k++ )
+ {
+ WT a0 = src[k], a1 = src[k+cn];
+ for( i = 2*cn; i <= size.width - 4*cn; i += 4*cn )
+ {
+ a0 = op(a0, (WT)src[i+k]);
+ a1 = op(a1, (WT)src[i+k+cn]);
+ a0 = op(a0, (WT)src[i+k+cn*2]);
+ a1 = op(a1, (WT)src[i+k+cn*3]);
+ }
+
+ for( ; i < size.width; i += cn )
+ {
+ a0 = op(a0, (WT)src[i]);
+ }
+ a0 = op(a0, a1);
+ dst[k] = (ST)a0;
+ }
+ }
+ }
+}
+
+typedef void (*ReduceFunc)( const Mat& src, Mat& dst );
+
+void reduce(const Mat& src, Mat& dst, int dim, int op, int dtype)
+{
+ int op0 = op;
+ int stype = src.type(), sdepth = src.depth();
+ if( dtype < 0 )
+ dtype = stype;
+ int ddepth = CV_MAT_DEPTH(dtype);
+
+ dst.create(dim == 0 ? 1 : src.rows, dim == 0 ? src.cols : 1, dtype >= 0 ? dtype : stype);
+ Mat temp = dst;
+
+ CV_Assert( op == CV_REDUCE_SUM || op == CV_REDUCE_MAX ||
+ op == CV_REDUCE_MIN || op == CV_REDUCE_AVG );
+ CV_Assert( src.channels() == dst.channels() );
+
+ if( op == CV_REDUCE_AVG )
+ {
+ op = CV_REDUCE_SUM;
+ if( sdepth < CV_32S && ddepth < CV_32S )
+ temp.create(dst.rows, dst.cols, CV_32SC(src.channels()));
+ }
+
+ ReduceFunc func = 0;
+ if( dim == 0 )
+ {
+ if( op == CV_REDUCE_SUM )
+ {
+ if(sdepth == CV_8U && ddepth == CV_32S)
+ func = reduceR_<uchar,int,OpAdd<int> >;
+ if(sdepth == CV_8U && ddepth == CV_32F)
+ func = reduceR_<uchar,float,OpAdd<int> >;
+ if(sdepth == CV_8U && ddepth == CV_64F)
+ func = reduceR_<uchar,double,OpAdd<int> >;
+ if(sdepth == CV_16U && ddepth == CV_32F)
+ func = reduceR_<ushort,float,OpAdd<float> >;
+ if(sdepth == CV_16U && ddepth == CV_64F)
+ func = reduceR_<ushort,double,OpAdd<double> >;
+ if(sdepth == CV_16S && ddepth == CV_32F)
+ func = reduceR_<short,float,OpAdd<float> >;
+ if(sdepth == CV_16S && ddepth == CV_64F)
+ func = reduceR_<short,double,OpAdd<double> >;
+ if(sdepth == CV_32F && ddepth == CV_32F)
+ func = reduceR_<float,float,OpAdd<float> >;
+ if(sdepth == CV_32F && ddepth == CV_64F)
+ func = reduceR_<float,double,OpAdd<double> >;
+ if(sdepth == CV_64F && ddepth == CV_64F)
+ func = reduceR_<double,double,OpAdd<double> >;
+ }
+ else if(op == CV_REDUCE_MAX)
+ {
+ if(sdepth == CV_8U && ddepth == CV_8U)
+ func = reduceR_<uchar, uchar, OpMax<uchar> >;
+ if(sdepth == CV_32F && ddepth == CV_32F)
+ func = reduceR_<float, float, OpMax<float> >;
+ if(sdepth == CV_64F && ddepth == CV_64F)
+ func = reduceR_<double, double, OpMax<double> >;
+ }
+ else if(op == CV_REDUCE_MIN)
+ {
+ if(sdepth == CV_8U && ddepth == CV_8U)
+ func = reduceR_<uchar, uchar, OpMin<uchar> >;
+ if(sdepth == CV_32F && ddepth == CV_32F)
+ func = reduceR_<float, float, OpMin<float> >;
+ if(sdepth == CV_64F && ddepth == CV_64F)
+ func = reduceR_<double, double, OpMin<double> >;
+ }
+ }
+ else
+ {
+ if(op == CV_REDUCE_SUM)
+ {
+ if(sdepth == CV_8U && ddepth == CV_32S)
+ func = reduceC_<uchar,int,OpAdd<int> >;
+ if(sdepth == CV_8U && ddepth == CV_32F)
+ func = reduceC_<uchar,float,OpAdd<int> >;
+ if(sdepth == CV_8U && ddepth == CV_64F)
+ func = reduceC_<uchar,double,OpAdd<int> >;
+ if(sdepth == CV_16U && ddepth == CV_32F)
+ func = reduceC_<ushort,float,OpAdd<float> >;
+ if(sdepth == CV_16U && ddepth == CV_64F)
+ func = reduceC_<ushort,double,OpAdd<double> >;
+ if(sdepth == CV_16S && ddepth == CV_32F)
+ func = reduceC_<short,float,OpAdd<float> >;
+ if(sdepth == CV_16S && ddepth == CV_64F)
+ func = reduceC_<short,double,OpAdd<double> >;
+ if(sdepth == CV_32F && ddepth == CV_32F)
+ func = reduceC_<float,float,OpAdd<float> >;
+ if(sdepth == CV_32F && ddepth == CV_64F)
+ func = reduceC_<float,double,OpAdd<double> >;
+ if(sdepth == CV_64F && ddepth == CV_64F)
+ func = reduceC_<double,double,OpAdd<double> >;
+ }
+ else if(op == CV_REDUCE_MAX)
+ {
+ if(sdepth == CV_8U && ddepth == CV_8U)
+ func = reduceC_<uchar, uchar, OpMax<uchar> >;
+ if(sdepth == CV_32F && ddepth == CV_32F)
+ func = reduceC_<float, float, OpMax<float> >;
+ if(sdepth == CV_64F && ddepth == CV_64F)
+ func = reduceC_<double, double, OpMax<double> >;
+ }
+ else if(op == CV_REDUCE_MIN)
+ {
+ if(sdepth == CV_8U && ddepth == CV_8U)
+ func = reduceC_<uchar, uchar, OpMin<uchar> >;
+ if(sdepth == CV_32F && ddepth == CV_32F)
+ func = reduceC_<float, float, OpMin<float> >;
+ if(sdepth == CV_64F && ddepth == CV_64F)
+ func = reduceC_<double, double, OpMin<double> >;
+ }
+ }
+
+ if( !func )
+ CV_Error( CV_StsUnsupportedFormat,
+ "Unsupported combination of input and output array formats" );
+
+ func( src, temp );
+
+ if( op0 == CV_REDUCE_AVG )
+ temp.convertTo(dst, dst.type(), 1./(dim == 0 ? src.rows : src.cols));
+}
+
+
+template<typename T> static void sort_( const Mat& src, Mat& dst, int flags )
+{
+ AutoBuffer<T> buf;
+ T* bptr;
+ int i, j, n, len;
+ bool sortRows = (flags & 1) == CV_SORT_EVERY_ROW;
+ bool inplace = src.data == dst.data;
+ bool sortDescending = (flags & CV_SORT_DESCENDING) != 0;
+
+ if( sortRows )
+ n = src.rows, len = src.cols;
+ else
+ {
+ n = src.cols, len = src.rows;
+ buf.allocate(len);
+ }
+ bptr = (T*)buf;
+
+ for( i = 0; i < n; i++ )
+ {
+ T* ptr = bptr;
+ if( sortRows )
+ {
+ T* dptr = (T*)(dst.data + dst.step*i);
+ if( !inplace )
+ {
+ const T* sptr = (const T*)(src.data + src.step*i);
+ for( j = 0; j < len; j++ )
+ dptr[j] = sptr[j];
+ }
+ ptr = dptr;
+ }
+ else
+ {
+ for( j = 0; j < len; j++ )
+ ptr[j] = ((const T*)(src.data + src.step*j))[i];
+ }
+ std::sort( ptr, ptr + len, LessThan<T>() );
+ if( sortDescending )
+ for( j = 0; j < len/2; j++ )
+ std::swap(ptr[j], ptr[len-1-j]);
+ if( !sortRows )
+ for( j = 0; j < len; j++ )
+ ((T*)(dst.data + dst.step*j))[i] = ptr[j];
+ }
+}
+
+
+template<typename T> static void sortIdx_( const Mat& src, Mat& dst, int flags )
+{
+ AutoBuffer<T> buf;
+ AutoBuffer<int> ibuf;
+ T* bptr;
+ int* _iptr;
+ int i, j, n, len;
+ bool sortRows = (flags & 1) == CV_SORT_EVERY_ROW;
+ bool sortDescending = (flags & CV_SORT_DESCENDING) != 0;
+
+ CV_Assert( src.data != dst.data );
+
+ if( sortRows )
+ n = src.rows, len = src.cols;
+ else
+ {
+ n = src.cols, len = src.rows;
+ buf.allocate(len);
+ ibuf.allocate(len);
+ }
+ bptr = (T*)buf;
+ _iptr = (int*)ibuf;
+
+ for( i = 0; i < n; i++ )
+ {
+ T* ptr = bptr;
+ int* iptr = _iptr;
+
+ if( sortRows )
+ {
+ ptr = (T*)(src.data + src.step*i);
+ iptr = (int*)(dst.data + dst.step*i);
+ }
+ else
+ {
+ for( j = 0; j < len; j++ )
+ ptr[j] = ((const T*)(src.data + src.step*j))[i];
+ }
+ for( j = 0; j < len; j++ )
+ iptr[j] = j;
+ std::sort( iptr, iptr + len, LessThanIdx<T>(ptr) );
+ if( sortDescending )
+ for( j = 0; j < len/2; j++ )
+ std::swap(iptr[j], iptr[len-1-j]);
+ if( !sortRows )
+ for( j = 0; j < len; j++ )
+ ((int*)(dst.data + dst.step*j))[i] = iptr[j];
+ }
+}
+
+typedef void (*SortFunc)(const Mat& src, Mat& dst, int flags);
+
+void sort( const Mat& src, Mat& dst, int flags )
+{
+ static SortFunc tab[] =
+ {
+ sort_<uchar>, sort_<schar>, sort_<ushort>, sort_<short>,
+ sort_<int>, sort_<float>, sort_<double>, 0
+ };
+ SortFunc func = tab[src.depth()];
+ CV_Assert( src.channels() == 1 && func != 0 );
+ dst.create( src.size(), src.type() );
+ func( src, dst, flags );
+}
+
+void sortIdx( const Mat& src, Mat& dst, int flags )
+{
+ static SortFunc tab[] =
+ {
+ sortIdx_<uchar>, sortIdx_<schar>, sortIdx_<ushort>, sortIdx_<short>,
+ sortIdx_<int>, sortIdx_<float>, sortIdx_<double>, 0
+ };
+ SortFunc func = tab[src.depth()];
+ CV_Assert( src.channels() == 1 && func != 0 );
+ if( dst.data == src.data )
+ dst.release();
+ dst.create( src.size(), CV_32S );
+ func( src, dst, flags );
+}
+
+static void generateRandomCenter(const vector<Vec2f>& box, float* center, RNG& rng)
+{
+ size_t j, dims = box.size();
+ float margin = 1.f/dims;
+ for( j = 0; j < dims; j++ )
+ center[j] = ((float)rng*(1.f+margin*2.f)-margin)*(box[j][1] - box[j][0]) + box[j][0];
+}
+
+
+static inline float distance(const float* a, const float* b, int n)
+{
+ int j = 0; float d = 0.f;
+#if CV_SSE2
+ float CV_DECL_ALIGNED(16) buf[4];
+ __m128 d0 = _mm_setzero_ps(), d1 = _mm_setzero_ps();
+
+ for( ; j <= n - 8; j += 8 )
+ {
+ __m128 t0 = _mm_sub_ps(_mm_loadu_ps(a + j), _mm_loadu_ps(b + j));
+ __m128 t1 = _mm_sub_ps(_mm_loadu_ps(a + j + 4), _mm_loadu_ps(b + j + 4));
+ d0 = _mm_add_ps(d0, _mm_mul_ps(t0, t0));
+ d1 = _mm_add_ps(d1, _mm_mul_ps(t1, t1));
+ }
+ _mm_store_ps(buf, _mm_add_ps(d0, d1));
+ d = buf[0] + buf[1] + buf[2] + buf[3];
+#else
+ for( ; j <= n - 4; j += 4 )
+ {
+ float t0 = a[j] - b[j], t1 = a[j+1] - b[j+1], t2 = a[j+2] - b[j+2], t3 = a[j+3] - b[j+3];
+ d += t0*t0 + t1*t1 + t2*t2 + t3*t3;
+ }
+#endif
+ for( ; j < n; j++ )
+ {
+ float t = a[j] - b[j];
+ d += t*t;
+ }
+ return d;
+}
+
+/*
+k-means center initialization using the following algorithm:
+Arthur & Vassilvitskii (2007) k-means++: The Advantages of Careful Seeding
+*/
+static void generateCentersPP(const Mat& _data, Mat& _out_centers,
+ int K, RNG& rng, int trials)
+{
+ int i, j, k, dims = _data.cols, N = _data.rows;
+ const float* data = _data.ptr<float>(0);
+ int step = _data.step/sizeof(data[0]);
+ vector<int> _centers(K);
+ int* centers = &_centers[0];
+ vector<float> _dist(N*3);
+ float* dist = &_dist[0], *tdist = dist + N, *tdist2 = tdist + N;
+ double sum0 = 0;
+
+ centers[0] = (unsigned)rng % N;
+
+ for( i = 0; i < N; i++ )
+ {
+ dist[i] = distance(data + step*i, data + step*centers[0], dims);
+ sum0 += dist[i];
+ }
+
+ for( k = 1; k < K; k++ )
+ {
+ double bestSum = DBL_MAX;
+ int bestCenter = -1;
+
+ for( j = 0; j < trials; j++ )
+ {
+ double p = (double)rng*sum0, s = 0;
+ for( i = 0; i < N-1; i++ )
+ if( (p -= dist[i]) <= 0 )
+ break;
+ int ci = i;
+ for( i = 0; i < N; i++ )
+ {
+ tdist2[i] = std::min(distance(data + step*i, data + step*ci, dims), dist[i]);
+ s += tdist2[i];
+ }
+
+ if( s < bestSum )
+ {
+ bestSum = s;
+ bestCenter = ci;
+ std::swap(tdist, tdist2);
+ }
+ }
+ centers[k] = bestCenter;
+ sum0 = bestSum;
+ std::swap(dist, tdist);
+ }
+
+ for( k = 0; k < K; k++ )
+ {
+ const float* src = data + step*centers[k];
+ float* dst = _out_centers.ptr<float>(k);
+ for( j = 0; j < dims; j++ )
+ dst[j] = src[j];
+ }
+}
+
+double kmeans( const Mat& data, int K, Mat& best_labels,
+ TermCriteria criteria, int attempts,
+ int flags, Mat* _centers )
+{
+ const int SPP_TRIALS = 3;
+ int N = data.rows > 1 ? data.rows : data.cols;
+ int dims = (data.rows > 1 ? data.cols : 1)*data.channels();
+ int type = data.depth();
+
+ attempts = std::max(attempts, 1);
+ CV_Assert( type == CV_32F && K > 0 );
+
+ Mat _labels;
+ if( flags & CV_KMEANS_USE_INITIAL_LABELS )
+ {
+ CV_Assert( (best_labels.cols == 1 || best_labels.rows == 1) &&
+ best_labels.cols*best_labels.rows == N &&
+ best_labels.type() == CV_32S &&
+ best_labels.isContinuous());
+ best_labels.copyTo(_labels);
+ }
+ else
+ {
+ if( !((best_labels.cols == 1 || best_labels.rows == 1) &&
+ best_labels.cols*best_labels.rows == N &&
+ best_labels.type() == CV_32S &&
+ best_labels.isContinuous()))
+ best_labels.create(N, 1, CV_32S);
+ _labels.create(best_labels.size(), best_labels.type());
+ }
+ int* labels = _labels.ptr<int>();
+
+ Mat centers(K, dims, type), old_centers(K, dims, type);
+ vector<int> counters(K);
+ vector<Vec2f> _box(dims);
+ Vec2f* box = &_box[0];
+
+ double best_compactness = DBL_MAX, compactness = 0;
+ RNG& rng = theRNG();
+ int a, iter, i, j, k;
+
+ if( criteria.type & TermCriteria::EPS )
+ criteria.epsilon = std::max(criteria.epsilon, 0.);
+ else
+ criteria.epsilon = FLT_EPSILON;
+ criteria.epsilon *= criteria.epsilon;
+
+ if( criteria.type & TermCriteria::COUNT )
+ criteria.maxCount = std::min(std::max(criteria.maxCount, 2), 100);
+ else
+ criteria.maxCount = 100;
+
+ if( K == 1 )
+ {
+ attempts = 1;
+ criteria.maxCount = 2;
+ }
+
+ const float* sample = data.ptr<float>(0);
+ for( j = 0; j < dims; j++ )
+ box[j] = Vec2f(sample[j], sample[j]);
+
+ for( i = 1; i < N; i++ )
+ {
+ sample = data.ptr<float>(i);
+ for( j = 0; j < dims; j++ )
+ {
+ float v = sample[j];
+ box[j][0] = std::min(box[j][0], v);
+ box[j][1] = std::max(box[j][1], v);
+ }
+ }
+
+ for( a = 0; a < attempts; a++ )
+ {
+ double max_center_shift = DBL_MAX;
+ for( iter = 0; iter < criteria.maxCount && max_center_shift > criteria.epsilon; iter++ )
+ {
+ swap(centers, old_centers);
+
+ if( iter == 0 && (a > 0 || !(flags & KMEANS_USE_INITIAL_LABELS)) )
+ {
+ if( flags & KMEANS_PP_CENTERS )
+ generateCentersPP(data, centers, K, rng, SPP_TRIALS);
+ else
+ {
+ for( k = 0; k < K; k++ )
+ generateRandomCenter(_box, centers.ptr<float>(k), rng);
+ }
+ }
+ else
+ {
+ if( iter == 0 && a == 0 && (flags & KMEANS_USE_INITIAL_LABELS) )
+ {
+ for( i = 0; i < N; i++ )
+ CV_Assert( (unsigned)labels[i] < (unsigned)K );
+ }
+
+ // compute centers
+ centers = Scalar(0);
+ for( k = 0; k < K; k++ )
+ counters[k] = 0;
+
+ for( i = 0; i < N; i++ )
+ {
+ sample = data.ptr<float>(i);
+ k = labels[i];
+ float* center = centers.ptr<float>(k);
+ for( j = 0; j <= dims - 4; j += 4 )
+ {
+ float t0 = center[j] + sample[j];
+ float t1 = center[j+1] + sample[j+1];
+
+ center[j] = t0;
+ center[j+1] = t1;
+
+ t0 = center[j+2] + sample[j+2];
+ t1 = center[j+3] + sample[j+3];
+
+ center[j+2] = t0;
+ center[j+3] = t1;
+ }
+ for( ; j < dims; j++ )
+ center[j] += sample[j];
+ counters[k]++;
+ }
+
+ if( iter > 0 )
+ max_center_shift = 0;
+
+ for( k = 0; k < K; k++ )
+ {
+ float* center = centers.ptr<float>(k);
+ if( counters[k] != 0 )
+ {
+ float scale = 1.f/counters[k];
+ for( j = 0; j < dims; j++ )
+ center[j] *= scale;
+ }
+ else
+ generateRandomCenter(_box, center, rng);
+
+ if( iter > 0 )
+ {
+ double dist = 0;
+ const float* old_center = old_centers.ptr<float>(k);
+ for( j = 0; j < dims; j++ )
+ {
+ double t = center[j] - old_center[j];
+ dist += t*t;
+ }
+ max_center_shift = std::max(max_center_shift, dist);
+ }
+ }
+ }
+
+ // assign labels
+ compactness = 0;
+ for( i = 0; i < N; i++ )
+ {
+ sample = data.ptr<float>(i);
+ int k_best = 0;
+ double min_dist = DBL_MAX;
+
+ for( k = 0; k < K; k++ )
+ {
+ const float* center = centers.ptr<float>(k);
+ double dist = distance(sample, center, dims);
+
+ if( min_dist > dist )
+ {
+ min_dist = dist;
+ k_best = k;
+ }
+ }
+
+ compactness += min_dist;
+ labels[i] = k_best;
+ }
+ }
+
+ if( compactness < best_compactness )
+ {
+ best_compactness = compactness;
+ if( _centers )
+ centers.copyTo(*_centers);
+ _labels.copyTo(best_labels);
+ }
+ }
+
+ return best_compactness;
+}
+
+}
+
+
+CV_IMPL void cvSetIdentity( CvArr* arr, CvScalar value )
+{
+ cv::Mat m = cv::cvarrToMat(arr);
+ cv::setIdentity(m, value);
+}
+
+
+CV_IMPL CvScalar cvTrace( const CvArr* arr )
+{
+ return cv::trace(cv::cvarrToMat(arr));
+}
+
+
+CV_IMPL void cvTranspose( const CvArr* srcarr, CvArr* dstarr )
+{
+ cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
+
+ CV_Assert( src.rows == dst.cols && src.cols == dst.rows && src.type() == dst.type() );
+ transpose( src, dst );
+}
+
+
+CV_IMPL void cvCompleteSymm( CvMat* matrix, int LtoR )
+{
+ cv::Mat m(matrix);
+ cv::completeSymm( m, LtoR != 0 );
+}
+
+
+CV_IMPL void cvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr )
+{
+ cv::Mat srcA = cv::cvarrToMat(srcAarr), dst = cv::cvarrToMat(dstarr);
+
+ CV_Assert( srcA.size() == dst.size() && srcA.type() == dst.type() );
+ srcA.cross(cv::cvarrToMat(srcBarr)).copyTo(dst);
+}
+
+
+CV_IMPL void
+cvReduce( const CvArr* srcarr, CvArr* dstarr, int dim, int op )
+{
+ cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
+
+ if( dim < 0 )
+ dim = src.rows > dst.rows ? 0 : src.cols > dst.cols ? 1 : dst.cols == 1;
+
+ if( dim > 1 )
+ CV_Error( CV_StsOutOfRange, "The reduced dimensionality index is out of range" );
+
+ if( (dim == 0 && (dst.cols != src.cols || dst.rows != 1)) ||
+ (dim == 1 && (dst.rows != src.rows || dst.cols != 1)) )
+ CV_Error( CV_StsBadSize, "The output array size is incorrect" );
+
+ if( src.channels() != dst.channels() )
+ CV_Error( CV_StsUnmatchedFormats, "Input and output arrays must have the same number of channels" );
+
+ cv::reduce(src, dst, dim, op, dst.type());
+}
+
+
+CV_IMPL CvArr*
+cvRange( CvArr* arr, double start, double end )
+{
+ int ok = 0;
+
+ CvMat stub, *mat = (CvMat*)arr;
+ double delta;
+ int type, step;
+ double val = start;
+ int i, j;
+ int rows, cols;
+
+ if( !CV_IS_MAT(mat) )
+ mat = cvGetMat( mat, &stub);
+
+ rows = mat->rows;
+ cols = mat->cols;
+ type = CV_MAT_TYPE(mat->type);
+ delta = (end-start)/(rows*cols);
+
+ if( CV_IS_MAT_CONT(mat->type) )
+ {
+ cols *= rows;
+ rows = 1;
+ step = 1;
+ }
+ else
+ step = mat->step / CV_ELEM_SIZE(type);
+
+ if( type == CV_32SC1 )
+ {
+ int* idata = mat->data.i;
+ int ival = cvRound(val), idelta = cvRound(delta);
+
+ if( fabs(val - ival) < DBL_EPSILON &&
+ fabs(delta - idelta) < DBL_EPSILON )
+ {
+ for( i = 0; i < rows; i++, idata += step )
+ for( j = 0; j < cols; j++, ival += idelta )
+ idata[j] = ival;
+ }
+ else
+ {
+ for( i = 0; i < rows; i++, idata += step )
+ for( j = 0; j < cols; j++, val += delta )
+ idata[j] = cvRound(val);
+ }
+ }
+ else if( type == CV_32FC1 )
+ {
+ float* fdata = mat->data.fl;
+ for( i = 0; i < rows; i++, fdata += step )
+ for( j = 0; j < cols; j++, val += delta )
+ fdata[j] = (float)val;
+ }
+ else
+ CV_Error( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
+
+ ok = 1;
+ return ok ? arr : 0;
+}
+
+
+CV_IMPL void
+cvSort( const CvArr* _src, CvArr* _dst, CvArr* _idx, int flags )
+{
+ cv::Mat src = cv::cvarrToMat(_src), dst, idx;
+
+ if( _idx )
+ {
+ cv::Mat idx0 = cv::cvarrToMat(_idx), idx = idx0;
+ CV_Assert( src.size() == idx.size() && idx.type() == CV_32S && src.data != idx.data );
+ cv::sortIdx( src, idx, flags );
+ CV_Assert( idx0.data == idx.data );
+ }
+
+ if( _dst )
+ {
+ cv::Mat dst0 = cv::cvarrToMat(_dst), dst = dst0;
+ CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
+ cv::sort( src, dst, flags );
+ CV_Assert( dst0.data == dst.data );
+ }
+}
+
+
+CV_IMPL int
+cvKMeans2( const CvArr* _samples, int cluster_count, CvArr* _labels,
+ CvTermCriteria termcrit, int attempts, CvRNG*,
+ int flags, CvArr* _centers, double* _compactness )
+{
+ cv::Mat data = cv::cvarrToMat(_samples), labels = cv::cvarrToMat(_labels), centers;
+ if( _centers )
+ centers = cv::cvarrToMat(_centers);
+ CV_Assert( labels.isContinuous() && labels.type() == CV_32S &&
+ (labels.cols == 1 || labels.rows == 1) &&
+ labels.cols + labels.rows - 1 == data.rows );
+ double compactness = cv::kmeans(data, cluster_count, labels, termcrit, attempts,
+ flags, _centers ? ¢ers : 0 );
+ if( _compactness )
+ *_compactness = compactness;
+ return 1;
+}
+
+///////////////////////////// n-dimensional matrices ////////////////////////////
+
+namespace cv
+{
+
+//////////////////////////////// MatND ///////////////////////////////////
+
+MatND::MatND(const MatND& m, const Range* ranges)
+ : flags(MAGIC_VAL), dims(0), refcount(0), data(0), datastart(0), dataend(0)
+{
+ int i, j, d = m.dims;
+
+ CV_Assert(ranges);
+ for( i = 0; i < d; i++ )
+ {
+ Range r = ranges[i];
+ CV_Assert( r == Range::all() ||
+ (0 <= r.start && r.start < r.end && r.end <= m.size[i]) );
+ }
+ *this = m;
+ for( i = 0; i < d; i++ )
+ {
+ Range r = ranges[i];
+ if( r != Range::all() )
+ {
+ size[i] = r.end - r.start;
+ data += r.start*step[i];
+ }
+ }
+
+ for( i = 0; i < d; i++ )
+ {
+ if( size[i] != 1 )
+ break;
+ }
+
+ CV_Assert( step[d-1] == elemSize() );
+ for( j = d-1; j > i; j-- )
+ {
+ if( step[j]*size[j] < step[j-1] )
+ break;
+ }
+ flags = (flags & ~CONTINUOUS_FLAG) | (j <= i ? CONTINUOUS_FLAG : 0);
+}
+
+void MatND::create(int d, const int* _sizes, int _type)
+{
+ CV_Assert(d > 0 && _sizes);
+ int i;
+ _type = CV_MAT_TYPE(_type);
+ if( data && d == dims && _type == type() )
+ {
+ for( i = 0; i < d; i++ )
+ if( size[i] != _sizes[i] )
+ break;
+ if( i == d )
+ return;
+ }
+
+ release();
+
+ flags = (_type & CV_MAT_TYPE_MASK) | MAGIC_VAL | CONTINUOUS_FLAG;
+ size_t total = elemSize();
+ int64 total1;
+
+ for( i = d-1; i >= 0; i-- )
+ {
+ int sz = _sizes[i];
+ size[i] = sz;
+ step[i] = total;
+ total1 = (int64)total*sz;
+ CV_Assert( sz > 0 );
+ if( (uint64)total1 != (size_t)total1 )
+ CV_Error( CV_StsOutOfRange, "The total matrix size does not fit to \"size_t\" type" );
+ total = (size_t)total1;
+ }
+ total = alignSize(total, (int)sizeof(*refcount));
+ data = datastart = (uchar*)fastMalloc(total + (int)sizeof(*refcount));
+ dataend = datastart + step[0]*size[0];
+ refcount = (int*)(data + total);
+ *refcount = 1;
+ dims = d;
+}
+
+void MatND::copyTo( MatND& m ) const
+{
+ m.create( m.dims, m.size, type() );
+ NAryMatNDIterator it(*this, m);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ it.planes[0].copyTo(it.planes[1]);
+}
+
+void MatND::copyTo( MatND& m, const MatND& mask ) const
+{
+ m.create( dims, size, type() );
+ NAryMatNDIterator it(*this, m, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ it.planes[0].copyTo(it.planes[1], it.planes[2]);
+}
+
+void MatND::convertTo( MatND& m, int rtype, double alpha, double beta ) const
+{
+ rtype = rtype < 0 ? type() : CV_MAKETYPE(CV_MAT_DEPTH(rtype), channels());
+ m.create( dims, size, rtype );
+ NAryMatNDIterator it(*this, m);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ it.planes[0].convertTo(it.planes[1], rtype, alpha, beta);
+}
+
+MatND& MatND::operator = (const Scalar& s)
+{
+ NAryMatNDIterator it(*this);
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ it.planes[0] = s;
+
+ return *this;
+}
+
+MatND& MatND::setTo(const Scalar& s, const MatND& mask)
+{
+ NAryMatNDIterator it(*this, mask);
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ it.planes[0].setTo(s, it.planes[1]);
+
+ return *this;
+}
+
+MatND MatND::reshape(int, int, const int*) const
+{
+ CV_Error(CV_StsNotImplemented, "");
+ // TBD
+ return MatND();
+}
+
+MatND::operator Mat() const
+{
+ int i, d = dims, d1, rows, cols;
+ size_t _step = Mat::AUTO_STEP;
+
+ if( d <= 2 )
+ {
+ rows = size[0];
+ cols = d == 2 ? size[1] : 1;
+ if( d == 2 )
+ _step = step[0];
+ }
+ else
+ {
+ rows = 1;
+ cols = size[d-1];
+
+ for( d1 = 0; d1 < d; d1++ )
+ if( size[d1] > 1 )
+ break;
+
+ for( i = d-1; i > d1; i-- )
+ {
+ int64 cols1 = (int64)cols*size[i-1];
+ if( cols1 != (int)cols1 || size[i]*step[i] != step[i-1] )
+ break;
+ cols = (int)cols1;
+ }
+
+ if( i > d1 )
+ {
+ --i;
+ _step = step[i];
+ rows = size[i];
+ for( ; i > d1; i-- )
+ {
+ int64 rows1 = (int64)rows*size[i-1];
+ if( rows1 != (int)rows1 || size[i]*step[i] != step[i-1] )
+ break;
+ rows = (int)rows1;
+ }
+
+ if( i > d1 )
+ CV_Error( CV_StsBadArg,
+ "The nD matrix can not be represented as 2D matrix due "
+ "to its layout in memory; you may use (Mat)the_matnd.clone() instead" );
+ }
+ }
+
+ Mat m(rows, cols, type(), data, _step);
+ m.datastart = datastart;
+ m.dataend = dataend;
+ m.refcount = refcount;
+ m.addref();
+ return m;
+}
+
+MatND::operator CvMatND() const
+{
+ CvMatND mat;
+ cvInitMatNDHeader( &mat, dims, size, type(), data );
+ int i, d = dims;
+ for( i = 0; i < d; i++ )
+ mat.dim[i].step = (int)step[i];
+ mat.type |= flags & CONTINUOUS_FLAG;
+ return mat;
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND** _arrays, size_t count)
+{
+ init(_arrays, count);
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND* _arrays, size_t count)
+{
+ AutoBuffer<const MatND*, 32> buf(count);
+ for( size_t i = 0; i < count; i++ )
+ buf[i] = _arrays + i;
+ init(buf, count);
+}
+
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND& m1)
+{
+ const MatND* mm[] = {&m1};
+ init(mm, 1);
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2)
+{
+ const MatND* mm[] = {&m1, &m2};
+ init(mm, 2);
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2, const MatND& m3)
+{
+ const MatND* mm[] = {&m1, &m2, &m3};
+ init(mm, 3);
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2,
+ const MatND& m3, const MatND& m4)
+{
+ const MatND* mm[] = {&m1, &m2, &m3, &m4};
+ init(mm, 4);
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2,
+ const MatND& m3, const MatND& m4,
+ const MatND& m5)
+{
+ const MatND* mm[] = {&m1, &m2, &m3, &m4, &m5};
+ init(mm, 5);
+}
+
+NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2,
+ const MatND& m3, const MatND& m4,
+ const MatND& m5, const MatND& m6)
+{
+ const MatND* mm[] = {&m1, &m2, &m3, &m4, &m5, &m6};
+ init(mm, 6);
+}
+
+void NAryMatNDIterator::init(const MatND** _arrays, size_t count)
+{
+ CV_Assert( _arrays && count > 0 );
+ arrays.resize(count);
+ int i, j, d1=0, i0 = -1, d = -1, n = (int)count;
+ size_t esz = 0;
+
+ iterdepth = 0;
+
+ for( i = 0; i < n; i++ )
+ {
+ if( !_arrays[i] || !_arrays[i]->data )
+ {
+ arrays[i] = MatND();
+ continue;
+ }
+ const MatND& A = arrays[i] = *_arrays[i];
+
+ if( i0 < 0 )
+ {
+ i0 = i;
+ d = A.dims;
+ esz = A.elemSize();
+
+ // find the first dimensionality which is different from 1;
+ // in any of the arrays the first "d1" steps do not affect the continuity
+ for( d1 = 0; d1 < d; d1++ )
+ if( A.size[d1] > 1 )
+ break;
+ }
+ else
+ {
+ CV_Assert( A.dims == d );
+ for( j = 0; j < d; j++ )
+ CV_Assert( A.size[j] == arrays[i0].size[j] );
+ }
+
+ if( !A.isContinuous() )
+ {
+ CV_Assert( A.step[d-1] == esz );
+ for( j = d-1; j > d1; j-- )
+ if( A.step[j]*A.size[j] < A.step[j-1] )
+ break;
+ iterdepth = std::max(iterdepth, j);
+ }
+ }
+
+ if( i0 < 0 )
+ CV_Error( CV_StsBadArg, "All the input arrays are empty" );
+
+ int total = arrays[i0].size[d-1];
+ for( j = d-1; j > iterdepth; j-- )
+ {
+ int64 total1 = (int64)total*arrays[i0].size[j-1];
+ if( total1 != (int)total1 )
+ break;
+ total = (int)total1;
+ }
+
+ iterdepth = j;
+ if( iterdepth == d1 )
+ iterdepth = 0;
+
+ planes.resize(n);
+ for( i = 0; i < n; i++ )
+ {
+ if( !arrays[i].data )
+ {
+ planes[i] = Mat();
+ continue;
+ }
+ planes[i] = Mat( 1, total, arrays[i].type(), arrays[i].data );
+ planes[i].datastart = arrays[i].datastart;
+ planes[i].dataend = arrays[i].dataend;
+ planes[i].refcount = arrays[i].refcount;
+ planes[i].addref();
+ }
+
+ idx = 0;
+ nplanes = 1;
+ for( j = iterdepth-1; j >= 0; j-- )
+ nplanes *= arrays[i0].size[j];
+}
+
+
+NAryMatNDIterator& NAryMatNDIterator::operator ++()
+{
+ if( idx >= nplanes-1 )
+ return *this;
+ ++idx;
+
+ for( size_t i = 0; i < arrays.size(); i++ )
+ {
+ const MatND& A = arrays[i];
+ Mat& M = planes[i];
+ if( !A.data )
+ continue;
+ int _idx = idx;
+ uchar* data = A.data;
+ for( int j = iterdepth-1; j >= 0 && _idx > 0; j-- )
+ {
+ int szi = A.size[j], t = _idx/szi;
+ data += (_idx - t * szi)*A.step[j];
+ _idx = t;
+ }
+ M.data = data;
+ }
+
+ return *this;
+}
+
+NAryMatNDIterator NAryMatNDIterator::operator ++(int)
+{
+ NAryMatNDIterator it = *this;
+ ++*this;
+ return it;
+}
+
+void add(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ add( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
+}
+
+void subtract(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ subtract( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
+}
+
+void add(const MatND& a, const MatND& b, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ add( it.planes[0], it.planes[1], it.planes[2] );
+}
+
+
+void subtract(const MatND& a, const MatND& b, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ subtract( it.planes[0], it.planes[1], it.planes[2] );
+}
+
+void add(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ add( it.planes[0], s, it.planes[1], it.planes[2] );
+}
+
+void subtract(const Scalar& s, const MatND& a, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ subtract( s, it.planes[0], it.planes[1], it.planes[2] );
+}
+
+void multiply(const MatND& a, const MatND& b, MatND& c, double scale)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ multiply( it.planes[0], it.planes[1], it.planes[2], scale );
+}
+
+void divide(const MatND& a, const MatND& b, MatND& c, double scale)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ divide( it.planes[0], it.planes[1], it.planes[2], scale );
+}
+
+void divide(double scale, const MatND& b, MatND& c)
+{
+ c.create(b.dims, b.size, b.type());
+ NAryMatNDIterator it(b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ divide( scale, it.planes[0], it.planes[1] );
+}
+
+void scaleAdd(const MatND& a, double alpha, const MatND& b, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ scaleAdd( it.planes[0], alpha, it.planes[1], it.planes[2] );
+}
+
+void addWeighted(const MatND& a, double alpha, const MatND& b,
+ double beta, double gamma, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ addWeighted( it.planes[0], alpha, it.planes[1], beta, gamma, it.planes[2] );
+}
+
+Scalar sum(const MatND& m)
+{
+ NAryMatNDIterator it(m);
+ Scalar s;
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ s += sum(it.planes[0]);
+ return s;
+}
+
+int countNonZero( const MatND& m )
+{
+ NAryMatNDIterator it(m);
+ int nz = 0;
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ nz += countNonZero(it.planes[0]);
+ return nz;
+}
+
+Scalar mean(const MatND& m)
+{
+ NAryMatNDIterator it(m);
+ double total = 1;
+ for( int i = 0; i < m.dims; i++ )
+ total *= m.size[i];
+ return sum(m)*(1./total);
+}
+
+Scalar mean(const MatND& m, const MatND& mask)
+{
+ if( !mask.data )
+ return mean(m);
+ NAryMatNDIterator it(m, mask);
+ double total = 0;
+ Scalar s;
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ s += sum(it.planes[0]);
+ total += countNonZero(it.planes[1]);
+ }
+ return s *= std::max(total, 1.);
+}
+
+void meanStdDev(const MatND& m, Scalar& mean, Scalar& stddev, const MatND& mask)
+{
+ NAryMatNDIterator it(m, mask);
+ double total = 0;
+ Scalar s, sq;
+ int k, cn = m.channels();
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ Scalar _mean, _stddev;
+ meanStdDev(it.planes[0], _mean, _stddev, it.planes[1]);
+ double nz = mask.data ? countNonZero(it.planes[1]) :
+ (double)it.planes[0].rows*it.planes[0].cols;
+ for( k = 0; k < cn; k++ )
+ {
+ s[k] += _mean[k]*nz;
+ sq[k] += (_stddev[k]*_stddev[k] + _mean[k]*_mean[k])*nz;
+ }
+ total += nz;
+ }
+
+ mean = stddev = Scalar();
+ total = 1./std::max(total, 1.);
+ for( k = 0; k < cn; k++ )
+ {
+ mean[k] = s[k]*total;
+ stddev[k] = std::sqrt(std::max(sq[k]*total - mean[k]*mean[k], 0.));
+ }
+}
+
+double norm(const MatND& a, int normType, const MatND& mask)
+{
+ NAryMatNDIterator it(a, mask);
+ double total = 0;
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ double n = norm(it.planes[0], normType, it.planes[1]);
+ if( normType == NORM_INF )
+ total = std::max(total, n);
+ else if( normType == NORM_L1 )
+ total += n;
+ else
+ total += n*n;
+ }
+
+ return normType != NORM_L2 ? total : std::sqrt(total);
+}
+
+double norm(const MatND& a, const MatND& b,
+ int normType, const MatND& mask)
+{
+ bool isRelative = (normType & NORM_RELATIVE) != 0;
+ normType &= 7;
+
+ NAryMatNDIterator it(a, b, mask);
+ double num = 0, denom = 0;
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ double n = norm(it.planes[0], it.planes[1], normType, it.planes[2]);
+ double d = !isRelative ? 0 : norm(it.planes[1], normType, it.planes[2]);
+ if( normType == NORM_INF )
+ {
+ num = std::max(num, n);
+ denom = std::max(denom, d);
+ }
+ else if( normType == NORM_L1 )
+ {
+ num += n;
+ denom += d;
+ }
+ else
+ {
+ num += n*n;
+ denom += d*d;
+ }
+ }
+
+ if( normType == NORM_L2 )
+ {
+ num = std::sqrt(num);
+ denom = std::sqrt(denom);
+ }
+
+ return !isRelative ? num : num/std::max(denom,DBL_EPSILON);
+}
+
+void normalize( const MatND& src, MatND& dst, double a, double b,
+ int norm_type, int rtype, const MatND& mask )
+{
+ double scale = 1, shift = 0;
+ if( norm_type == CV_MINMAX )
+ {
+ double smin = 0, smax = 0;
+ double dmin = std::min( a, b ), dmax = std::max( a, b );
+ minMaxLoc( src, &smin, &smax, 0, 0, mask );
+ scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0);
+ shift = dmin - smin*scale;
+ }
+ else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
+ {
+ scale = norm( src, norm_type, mask );
+ scale = scale > DBL_EPSILON ? a/scale : 0.;
+ shift = 0;
+ }
+ else
+ CV_Error( CV_StsBadArg, "Unknown/unsupported norm type" );
+
+ if( !mask.data )
+ src.convertTo( dst, rtype, scale, shift );
+ else
+ {
+ MatND temp;
+ src.convertTo( temp, rtype, scale, shift );
+ temp.copyTo( dst, mask );
+ }
+}
+
+static void ofs2idx(const MatND& a, size_t ofs, int* idx)
+{
+ int i, d = a.dims;
+ for( i = 0; i < d; i++ )
+ {
+ idx[i] = (int)(ofs / a.step[i]);
+ ofs %= a.step[i];
+ }
+}
+
+
+void minMaxLoc(const MatND& a, double* minVal,
+ double* maxVal, int* minLoc, int* maxLoc,
+ const MatND& mask)
+{
+ NAryMatNDIterator it(a, mask);
+ double minval = DBL_MAX, maxval = -DBL_MAX;
+ size_t minofs = 0, maxofs = 0, esz = a.elemSize();
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ double val0 = 0, val1 = 0;
+ Point pt0, pt1;
+ minMaxLoc( it.planes[0], &val0, &val1, &pt0, &pt1, it.planes[1] );
+ if( val0 < minval )
+ {
+ minval = val0;
+ minofs = (it.planes[0].data - a.data) + pt0.x*esz;
+ }
+ if( val1 > minval )
+ {
+ maxval = val1;
+ maxofs = (it.planes[0].data - a.data) + pt1.x*esz;
+ }
+ }
+
+ if( minVal )
+ *minVal = minval;
+ if( maxVal )
+ *maxVal = maxval;
+ if( minLoc )
+ ofs2idx(a, minofs, minLoc);
+ if( maxLoc )
+ ofs2idx(a, maxofs, maxLoc);
+}
+
+void merge(const MatND* mv, size_t n, MatND& dst)
+{
+ size_t k;
+ CV_Assert( n > 0 );
+ vector<MatND> v(n + 1);
+ int total_cn = 0;
+ for( k = 0; k < n; k++ )
+ {
+ total_cn += mv[k].channels();
+ v[k] = mv[k];
+ }
+ dst.create( mv[0].dims, mv[0].size, CV_MAKETYPE(mv[0].depth(), total_cn) );
+ v[n] = dst;
+ NAryMatNDIterator it(&v[0], v.size());
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ merge( &it.planes[0], n, it.planes[n] );
+}
+
+void split(const MatND& m, MatND* mv)
+{
+ size_t k, n = m.channels();
+ CV_Assert( n > 0 );
+ vector<MatND> v(n + 1);
+ for( k = 0; k < n; k++ )
+ {
+ mv[k].create( m.dims, m.size, CV_MAKETYPE(m.depth(), 1) );
+ v[k] = mv[k];
+ }
+ v[n] = m;
+ NAryMatNDIterator it(&v[0], v.size());
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ split( it.planes[n], &it.planes[0] );
+}
+
+void mixChannels(const MatND* src, int nsrcs, MatND* dst, int ndsts,
+ const int* fromTo, size_t npairs)
+{
+ size_t k, m = nsrcs, n = ndsts;
+ CV_Assert( n > 0 && m > 0 );
+ vector<MatND> v(m + n);
+ for( k = 0; k < m; k++ )
+ v[k] = src[k];
+ for( k = 0; k < n; k++ )
+ v[m + k] = dst[k];
+ NAryMatNDIterator it(&v[0], v.size());
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ Mat* pptr = &it.planes[0];
+ mixChannels( pptr, m, pptr + m, n, fromTo, npairs );
+ }
+}
+
+void bitwise_and(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_and( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
+}
+
+void bitwise_or(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_or( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
+}
+
+void bitwise_xor(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_xor( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
+}
+
+void bitwise_and(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_and( it.planes[0], s, it.planes[1], it.planes[2] );
+}
+
+void bitwise_or(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_or( it.planes[0], s, it.planes[1], it.planes[2] );
+}
+
+void bitwise_xor(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c, mask);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_xor( it.planes[0], s, it.planes[1], it.planes[2] );
+}
+
+void bitwise_not(const MatND& a, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ bitwise_not( it.planes[0], it.planes[1] );
+}
+
+void absdiff(const MatND& a, const MatND& b, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ absdiff( it.planes[0], it.planes[1], it.planes[2] );
+}
+
+void absdiff(const MatND& a, const Scalar& s, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ absdiff( it.planes[0], s, it.planes[1] );
+}
+
+void inRange(const MatND& src, const MatND& lowerb,
+ const MatND& upperb, MatND& dst)
+{
+ dst.create(src.dims, src.size, CV_8UC1);
+ NAryMatNDIterator it(src, lowerb, upperb, dst);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ inRange( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
+}
+
+void inRange(const MatND& src, const Scalar& lowerb,
+ const Scalar& upperb, MatND& dst)
+{
+ dst.create(src.dims, src.size, CV_8UC1);
+ NAryMatNDIterator it(src, dst);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ inRange( it.planes[0], lowerb, upperb, it.planes[1] );
+}
+
+void compare(const MatND& a, const MatND& b, MatND& c, int cmpop)
+{
+ c.create(a.dims, a.size, CV_8UC1);
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ compare( it.planes[0], it.planes[1], it.planes[2], cmpop );
+}
+
+void compare(const MatND& a, double s, MatND& c, int cmpop)
+{
+ c.create(a.dims, a.size, CV_8UC1);
+ NAryMatNDIterator it(a, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ compare( it.planes[0], s, it.planes[1], cmpop );
+}
+
+void min(const MatND& a, const MatND& b, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ min( it.planes[0], it.planes[1], it.planes[2] );
+}
+
+void min(const MatND& a, double alpha, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ min( it.planes[0], alpha, it.planes[1] );
+}
+
+void max(const MatND& a, const MatND& b, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ max( it.planes[0], it.planes[1], it.planes[2] );
+}
+
+void max(const MatND& a, double alpha, MatND& c)
+{
+ c.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, c);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ max( it.planes[0], alpha, it.planes[1] );
+}
+
+void sqrt(const MatND& a, MatND& b)
+{
+ b.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ sqrt( it.planes[0], it.planes[1] );
+}
+
+void pow(const MatND& a, double power, MatND& b)
+{
+ b.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ pow( it.planes[0], power, it.planes[1] );
+}
+
+void exp(const MatND& a, MatND& b)
+{
+ b.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ exp( it.planes[0], it.planes[1] );
+}
+
+void log(const MatND& a, MatND& b)
+{
+ b.create(a.dims, a.size, a.type());
+ NAryMatNDIterator it(a, b);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ log( it.planes[0], it.planes[1] );
+}
+
+bool checkRange(const MatND& a, bool quiet, int*,
+ double minVal, double maxVal)
+{
+ NAryMatNDIterator it(a);
+
+ for( int i = 0; i < it.nplanes; i++, ++it )
+ {
+ Point pt;
+ if( !checkRange( it.planes[0], quiet, &pt, minVal, maxVal ))
+ {
+ // todo: set index properly
+ return false;
+ }
+ }
+ return true;
+}
+
+
+//////////////////////////////// SparseMat ////////////////////////////////
+
+template<typename T1, typename T2> void
+convertData_(const void* _from, void* _to, int cn)
+{
+ const T1* from = (const T1*)_from;
+ T2* to = (T2*)_to;
+ if( cn == 1 )
+ *to = saturate_cast<T2>(*from);
+ else
+ for( int i = 0; i < cn; i++ )
+ to[i] = saturate_cast<T2>(from[i]);
+}
+
+template<typename T1, typename T2> void
+convertScaleData_(const void* _from, void* _to, int cn, double alpha, double beta)
+{
+ const T1* from = (const T1*)_from;
+ T2* to = (T2*)_to;
+ if( cn == 1 )
+ *to = saturate_cast<T2>(*from*alpha + beta);
+ else
+ for( int i = 0; i < cn; i++ )
+ to[i] = saturate_cast<T2>(from[i]*alpha + beta);
+}
+
+ConvertData getConvertData(int fromType, int toType)
+{
+ static ConvertData tab[][8] =
+ {{ convertData_<uchar, uchar>, convertData_<uchar, schar>,
+ convertData_<uchar, ushort>, convertData_<uchar, short>,
+ convertData_<uchar, int>, convertData_<uchar, float>,
+ convertData_<uchar, double>, 0 },
+
+ { convertData_<schar, uchar>, convertData_<schar, schar>,
+ convertData_<schar, ushort>, convertData_<schar, short>,
+ convertData_<schar, int>, convertData_<schar, float>,
+ convertData_<schar, double>, 0 },
+
+ { convertData_<ushort, uchar>, convertData_<ushort, schar>,
+ convertData_<ushort, ushort>, convertData_<ushort, short>,
+ convertData_<ushort, int>, convertData_<ushort, float>,
+ convertData_<ushort, double>, 0 },
+
+ { convertData_<short, uchar>, convertData_<short, schar>,
+ convertData_<short, ushort>, convertData_<short, short>,
+ convertData_<short, int>, convertData_<short, float>,
+ convertData_<short, double>, 0 },
+
+ { convertData_<int, uchar>, convertData_<int, schar>,
+ convertData_<int, ushort>, convertData_<int, short>,
+ convertData_<int, int>, convertData_<int, float>,
+ convertData_<int, double>, 0 },
+
+ { convertData_<float, uchar>, convertData_<float, schar>,
+ convertData_<float, ushort>, convertData_<float, short>,
+ convertData_<float, int>, convertData_<float, float>,
+ convertData_<float, double>, 0 },
+
+ { convertData_<double, uchar>, convertData_<double, schar>,
+ convertData_<double, ushort>, convertData_<double, short>,
+ convertData_<double, int>, convertData_<double, float>,
+ convertData_<double, double>, 0 },
+
+ { 0, 0, 0, 0, 0, 0, 0, 0 }};
+
+ ConvertData func = tab[CV_MAT_DEPTH(fromType)][CV_MAT_DEPTH(toType)];
+ CV_Assert( func != 0 );
+ return func;
+}
+
+ConvertScaleData getConvertScaleData(int fromType, int toType)
+{
+ static ConvertScaleData tab[][8] =
+ {{ convertScaleData_<uchar, uchar>, convertScaleData_<uchar, schar>,
+ convertScaleData_<uchar, ushort>, convertScaleData_<uchar, short>,
+ convertScaleData_<uchar, int>, convertScaleData_<uchar, float>,
+ convertScaleData_<uchar, double>, 0 },
+
+ { convertScaleData_<schar, uchar>, convertScaleData_<schar, schar>,
+ convertScaleData_<schar, ushort>, convertScaleData_<schar, short>,
+ convertScaleData_<schar, int>, convertScaleData_<schar, float>,
+ convertScaleData_<schar, double>, 0 },
+
+ { convertScaleData_<ushort, uchar>, convertScaleData_<ushort, schar>,
+ convertScaleData_<ushort, ushort>, convertScaleData_<ushort, short>,
+ convertScaleData_<ushort, int>, convertScaleData_<ushort, float>,
+ convertScaleData_<ushort, double>, 0 },
+
+ { convertScaleData_<short, uchar>, convertScaleData_<short, schar>,
+ convertScaleData_<short, ushort>, convertScaleData_<short, short>,
+ convertScaleData_<short, int>, convertScaleData_<short, float>,
+ convertScaleData_<short, double>, 0 },
+
+ { convertScaleData_<int, uchar>, convertScaleData_<int, schar>,
+ convertScaleData_<int, ushort>, convertScaleData_<int, short>,
+ convertScaleData_<int, int>, convertScaleData_<int, float>,
+ convertScaleData_<int, double>, 0 },
+
+ { convertScaleData_<float, uchar>, convertScaleData_<float, schar>,
+ convertScaleData_<float, ushort>, convertScaleData_<float, short>,
+ convertScaleData_<float, int>, convertScaleData_<float, float>,
+ convertScaleData_<float, double>, 0 },
+
+ { convertScaleData_<double, uchar>, convertScaleData_<double, schar>,
+ convertScaleData_<double, ushort>, convertScaleData_<double, short>,
+ convertScaleData_<double, int>, convertScaleData_<double, float>,
+ convertScaleData_<double, double>, 0 },
+
+ { 0, 0, 0, 0, 0, 0, 0, 0 }};
+
+ ConvertScaleData func = tab[CV_MAT_DEPTH(fromType)][CV_MAT_DEPTH(toType)];
+ CV_Assert( func != 0 );
+ return func;
+}
+
+enum { HASH_SIZE0 = 8 };
+
+static inline void copyElem(const uchar* from, uchar* to, size_t elemSize)
+{
+ size_t i;
+ for( i = 0; i <= elemSize - sizeof(int); i += sizeof(int) )
+ *(int*)(to + i) = *(const int*)(from + i);
+ for( ; i < elemSize; i++ )
+ to[i] = from[i];
+}
+
+static inline bool isZeroElem(const uchar* data, size_t elemSize)
+{
+ size_t i;
+ for( i = 0; i <= elemSize - sizeof(int); i += sizeof(int) )
+ if( *(int*)(data + i) != 0 )
+ return false;
+ for( ; i < elemSize; i++ )
+ if( data[i] != 0 )
+ return false;
+ return true;
+}
+
+SparseMat::Hdr::Hdr( int _dims, const int* _sizes, int _type )
+{
+ refcount = 1;
+
+ dims = _dims;
+ valueOffset = (int)alignSize(sizeof(SparseMat::Node) +
+ sizeof(int)*(dims - CV_MAX_DIM), CV_ELEM_SIZE1(_type));
+ nodeSize = alignSize(valueOffset +
+ CV_ELEM_SIZE(_type), (int)sizeof(size_t));
+
+ int i;
+ for( i = 0; i < dims; i++ )
+ size[i] = _sizes[i];
+ for( ; i < CV_MAX_DIM; i++ )
+ size[i] = 0;
+ clear();
+}
+
+void SparseMat::Hdr::clear()
+{
+ hashtab.clear();
+ hashtab.resize(HASH_SIZE0);
+ pool.clear();
+ pool.resize(nodeSize);
+ nodeCount = freeList = 0;
+}
+
+
+SparseMat::SparseMat(const Mat& m, bool try1d)
+: flags(MAGIC_VAL), hdr(0)
+{
+ bool is1d = try1d && m.cols == 1;
+
+ if( is1d )
+ {
+ int i, M = m.rows;
+ const uchar* data = m.data;
+ size_t step = m.step, esz = m.elemSize();
+ create( 1, &M, m.type() );
+ for( i = 0; i < M; i++ )
+ {
+ const uchar* from = data + step*i;
+ if( isZeroElem(from, esz) )
+ continue;
+ uchar* to = newNode(&i, hash(i));
+ copyElem(from, to, esz);
+ }
+ }
+ else
+ {
+ int i, j, size[] = {m.rows, m.cols};
+ const uchar* data = m.data;
+ size_t step = m.step, esz = m.elemSize();
+ create( 2, size, m.type() );
+ for( i = 0; i < m.rows; i++ )
+ {
+ for( j = 0; j < m.cols; j++ )
+ {
+ const uchar* from = data + step*i + esz*j;
+ if( isZeroElem(from, esz) )
+ continue;
+ int idx[] = {i, j};
+ uchar* to = newNode(idx, hash(i, j));
+ copyElem(from, to, esz);
+ }
+ }
+ }
+}
+
+SparseMat::SparseMat(const MatND& m)
+: flags(MAGIC_VAL), hdr(0)
+{
+ create( m.dims, m.size, m.type() );
+
+ int i, idx[CV_MAX_DIM] = {0}, d = m.dims, lastSize = m.size[d - 1];
+ size_t esz = m.elemSize();
+ uchar* ptr = m.data;
+
+ for(;;)
+ {
+ for( i = 0; i < lastSize; i++, ptr += esz )
+ {
+ if( isZeroElem(ptr, esz) )
+ continue;
+ idx[d-1] = i;
+ uchar* to = newNode(idx, hash(idx));
+ copyElem( ptr, to, esz );
+ }
+
+ for( i = d - 2; i >= 0; i-- )
+ {
+ ptr += m.step[i] - m.size[i+1]*m.step[i+1];
+ if( ++idx[i] < m.size[i] )
+ break;
+ idx[i] = 0;
+ }
+ if( i < 0 )
+ break;
+ }
+}
+
+SparseMat::SparseMat(const CvSparseMat* m)
+: flags(MAGIC_VAL), hdr(0)
+{
+ CV_Assert(m);
+ create( m->dims, &m->size[0], m->type );
+
+ CvSparseMatIterator it;
+ CvSparseNode* n = cvInitSparseMatIterator(m, &it);
+ size_t esz = elemSize();
+
+ for( ; n != 0; n = cvGetNextSparseNode(&it) )
+ {
+ const int* idx = CV_NODE_IDX(m, n);
+ uchar* to = newNode(idx, hash(idx));
+ copyElem((const uchar*)CV_NODE_VAL(m, n), to, esz);
+ }
+}
+
+void SparseMat::create(int d, const int* _sizes, int _type)
+{
+ int i;
+ CV_Assert( _sizes && 0 < d && d <= CV_MAX_DIM );
+ for( i = 0; i < d; i++ )
+ CV_Assert( _sizes[i] > 0 );
+ _type = CV_MAT_TYPE(_type);
+ if( hdr && _type == type() && hdr->dims == d && hdr->refcount == 1 )
+ {
+ for( i = 0; i < d; i++ )
+ if( _sizes[i] != hdr->size[i] )
+ break;
+ if( i == d )
+ {
+ clear();
+ return;
+ }
+ }
+ release();
+ flags = MAGIC_VAL | _type;
+ hdr = new Hdr(d, _sizes, _type);
+}
+
+void SparseMat::copyTo( SparseMat& m ) const
+{
+ if( this == &m )
+ return;
+ if( !hdr )
+ {
+ m.release();
+ return;
+ }
+ m.create( m.hdr->dims, m.hdr->size, type() );
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount(), esz = elemSize();
+
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.newNode(n->idx, n->hashval);
+ copyElem( from.ptr, to, esz );
+ }
+}
+
+void SparseMat::copyTo( Mat& m ) const
+{
+ CV_Assert( hdr && hdr->dims <= 2 );
+ m.create( hdr->size[0], hdr->dims == 2 ? hdr->size[1] : 1, type() );
+ m = Scalar(0);
+
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount(), esz = elemSize();
+
+ if( hdr->dims == 2 )
+ {
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.data + m.step*n->idx[0] + esz*n->idx[1];
+ copyElem( from.ptr, to, esz );
+ }
+ }
+ else
+ {
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.data + esz*n->idx[0];
+ copyElem( from.ptr, to, esz );
+ }
+ }
+}
+
+void SparseMat::copyTo( MatND& m ) const
+{
+ CV_Assert( hdr );
+ m.create( dims(), hdr->size, type() );
+ m = Scalar(0);
+
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount(), esz = elemSize();
+
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ copyElem( from.ptr, m.ptr(n->idx), esz);
+ }
+}
+
+
+void SparseMat::convertTo( SparseMat& m, int rtype, double alpha ) const
+{
+ int cn = channels();
+ if( rtype < 0 )
+ rtype = type();
+ rtype = CV_MAKETYPE(rtype, cn);
+ if( this == &m && rtype != type() )
+ {
+ SparseMat temp;
+ convertTo(temp, rtype, alpha);
+ m = temp;
+ return;
+ }
+
+ CV_Assert(hdr != 0);
+ m.create( m.hdr->dims, m.hdr->size, rtype );
+
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount();
+
+ if( alpha == 1 )
+ {
+ ConvertData cvtfunc = getConvertData(type(), rtype);
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.newNode(n->idx, n->hashval);
+ cvtfunc( from.ptr, to, cn );
+ }
+ }
+ else
+ {
+ ConvertScaleData cvtfunc = getConvertScaleData(type(), rtype);
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.newNode(n->idx, n->hashval);
+ cvtfunc( from.ptr, to, cn, alpha, 0 );
+ }
+ }
+}
+
+
+void SparseMat::convertTo( Mat& m, int rtype, double alpha, double beta ) const
+{
+ int cn = channels();
+ if( rtype < 0 )
+ rtype = type();
+ rtype = CV_MAKETYPE(rtype, cn);
+
+ CV_Assert( hdr && hdr->dims <= 2 );
+ m.create( hdr->size[0], hdr->dims == 2 ? hdr->size[1] : 1, type() );
+ m = Scalar(beta);
+
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount(), esz = CV_ELEM_SIZE(rtype);
+
+ if( alpha == 1 && beta == 0 )
+ {
+ ConvertData cvtfunc = getConvertData(type(), rtype);
+
+ if( hdr->dims == 2 )
+ {
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.data + m.step*n->idx[0] + esz*n->idx[1];
+ cvtfunc( from.ptr, to, cn );
+ }
+ }
+ else
+ {
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.data + esz*n->idx[0];
+ cvtfunc( from.ptr, to, cn );
+ }
+ }
+ }
+ else
+ {
+ ConvertScaleData cvtfunc = getConvertScaleData(type(), rtype);
+
+ if( hdr->dims == 2 )
+ {
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.data + m.step*n->idx[0] + esz*n->idx[1];
+ cvtfunc( from.ptr, to, cn, alpha, beta );
+ }
+ }
+ else
+ {
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.data + esz*n->idx[0];
+ cvtfunc( from.ptr, to, cn, alpha, beta );
+ }
+ }
+ }
+}
+
+void SparseMat::convertTo( MatND& m, int rtype, double alpha, double beta ) const
+{
+ int cn = channels();
+ if( rtype < 0 )
+ rtype = type();
+ rtype = CV_MAKETYPE(rtype, cn);
+
+ CV_Assert( hdr );
+ m.create( dims(), hdr->size, rtype );
+ m = Scalar(beta);
+
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount();
+
+ if( alpha == 1 && beta == 0 )
+ {
+ ConvertData cvtfunc = getConvertData(type(), rtype);
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.ptr(n->idx);
+ cvtfunc( from.ptr, to, cn );
+ }
+ }
+ else
+ {
+ ConvertScaleData cvtfunc = getConvertScaleData(type(), rtype);
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = m.ptr(n->idx);
+ cvtfunc( from.ptr, to, cn, alpha, beta );
+ }
+ }
+}
+
+void SparseMat::clear()
+{
+ if( hdr )
+ hdr->clear();
+}
+
+SparseMat::operator CvSparseMat*() const
+{
+ if( !hdr )
+ return 0;
+ CvSparseMat* m = cvCreateSparseMat(hdr->dims, hdr->size, type());
+
+ SparseMatConstIterator from = begin();
+ size_t i, N = nzcount(), esz = elemSize();
+
+ for( i = 0; i < N; i++, ++from )
+ {
+ const Node* n = from.node();
+ uchar* to = cvPtrND(m, n->idx, 0, -2, 0);
+ copyElem(from.ptr, to, esz);
+ }
+ return m;
+}
+
+uchar* SparseMat::ptr(int i0, int i1, bool createMissing, size_t* hashval)
+{
+ CV_Assert( hdr && hdr->dims == 2 );
+ size_t h = hashval ? *hashval : hash(i0, i1);
+ size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
+ uchar* pool = &hdr->pool[0];
+ while( nidx != 0 )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ if( elem->hashval == h && elem->idx[0] == i0 && elem->idx[1] == i1 )
+ return &value<uchar>(elem);
+ nidx = elem->next;
+ }
+
+ if( createMissing )
+ {
+ int idx[] = { i0, i1 };
+ return newNode( idx, h );
+ }
+ return 0;
+}
+
+uchar* SparseMat::ptr(int i0, int i1, int i2, bool createMissing, size_t* hashval)
+{
+ CV_Assert( hdr && hdr->dims == 3 );
+ size_t h = hashval ? *hashval : hash(i0, i1, i2);
+ size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
+ uchar* pool = &hdr->pool[0];
+ while( nidx != 0 )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ if( elem->hashval == h && elem->idx[0] == i0 &&
+ elem->idx[1] == i1 && elem->idx[2] == i2 )
+ return &value<uchar>(elem);
+ nidx = elem->next;
+ }
+
+ if( createMissing )
+ {
+ int idx[] = { i0, i1, i2 };
+ return newNode( idx, h );
+ }
+ return 0;
+}
+
+uchar* SparseMat::ptr(const int* idx, bool createMissing, size_t* hashval)
+{
+ CV_Assert( hdr );
+ int i, d = hdr->dims;
+ size_t h = hashval ? *hashval : hash(idx);
+ size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
+ uchar* pool = &hdr->pool[0];
+ while( nidx != 0 )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ if( elem->hashval == h )
+ {
+ for( i = 0; i < d; i++ )
+ if( elem->idx[i] != idx[i] )
+ break;
+ if( i == d )
+ return &value<uchar>(elem);
+ }
+ nidx = elem->next;
+ }
+
+ return createMissing ? newNode(idx, h) : 0;
+}
+
+void SparseMat::erase(int i0, int i1, size_t* hashval)
+{
+ CV_Assert( hdr && hdr->dims == 2 );
+ size_t h = hashval ? *hashval : hash(i0, i1);
+ size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
+ uchar* pool = &hdr->pool[0];
+ while( nidx != 0 )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ if( elem->hashval == h && elem->idx[0] == i0 && elem->idx[1] == i1 )
+ break;
+ previdx = nidx;
+ nidx = elem->next;
+ }
+
+ if( nidx )
+ removeNode(hidx, nidx, previdx);
+}
+
+void SparseMat::erase(int i0, int i1, int i2, size_t* hashval)
+{
+ CV_Assert( hdr && hdr->dims == 3 );
+ size_t h = hashval ? *hashval : hash(i0, i1, i2);
+ size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
+ uchar* pool = &hdr->pool[0];
+ while( nidx != 0 )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ if( elem->hashval == h && elem->idx[0] == i0 &&
+ elem->idx[1] == i1 && elem->idx[2] == i2 )
+ break;
+ previdx = nidx;
+ nidx = elem->next;
+ }
+
+ if( nidx )
+ removeNode(hidx, nidx, previdx);
+}
+
+void SparseMat::erase(const int* idx, size_t* hashval)
+{
+ CV_Assert( hdr );
+ int i, d = hdr->dims;
+ size_t h = hashval ? *hashval : hash(idx);
+ size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
+ uchar* pool = &hdr->pool[0];
+ while( nidx != 0 )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ if( elem->hashval == h )
+ {
+ for( i = 0; i < d; i++ )
+ if( elem->idx[i] != idx[i] )
+ break;
+ if( i == d )
+ break;
+ }
+ previdx = nidx;
+ nidx = elem->next;
+ }
+
+ if( nidx )
+ removeNode(hidx, nidx, previdx);
+}
+
+void SparseMat::resizeHashTab(size_t newsize)
+{
+ newsize = std::max(newsize, (size_t)8);
+ if((newsize & (newsize-1)) != 0)
+ newsize = 1 << cvCeil(std::log((double)newsize)/CV_LOG2);
+
+ size_t i, hsize = hdr->hashtab.size();
+ vector<size_t> _newh(newsize);
+ size_t* newh = &_newh[0];
+ for( i = 0; i < newsize; i++ )
+ newh[i] = 0;
+ uchar* pool = &hdr->pool[0];
+ for( i = 0; i < hsize; i++ )
+ {
+ size_t nidx = hdr->hashtab[i];
+ while( nidx )
+ {
+ Node* elem = (Node*)(pool + nidx);
+ size_t next = elem->next;
+ size_t newhidx = elem->hashval & (newsize - 1);
+ elem->next = newh[newhidx];
+ newh[newhidx] = nidx;
+ nidx = next;
+ }
+ }
+ hdr->hashtab = _newh;
+}
+
+uchar* SparseMat::newNode(const int* idx, size_t hashval)
+{
+ const int HASH_MAX_FILL_FACTOR=3;
+ assert(hdr);
+ size_t hsize = hdr->hashtab.size();
+ if( ++hdr->nodeCount > hsize*HASH_MAX_FILL_FACTOR )
+ {
+ resizeHashTab(std::max(hsize*2, (size_t)8));
+ hsize = hdr->hashtab.size();
+ }
+
+ if( !hdr->freeList )
+ {
+ size_t i, nsz = hdr->nodeSize, psize = hdr->pool.size(),
+ newpsize = std::max(psize*2, 8*nsz);
+ hdr->pool.resize(newpsize);
+ uchar* pool = &hdr->pool[0];
+ hdr->freeList = std::max(psize, nsz);
+ for( i = hdr->freeList; i < newpsize - nsz; i += nsz )
+ ((Node*)(pool + i))->next = i + nsz;
+ ((Node*)(pool + i))->next = 0;
+ }
+ size_t nidx = hdr->freeList;
+ Node* elem = (Node*)&hdr->pool[nidx];
+ hdr->freeList = elem->next;
+ elem->hashval = hashval;
+ size_t hidx = hashval & (hsize - 1);
+ elem->next = hdr->hashtab[hidx];
+ hdr->hashtab[hidx] = nidx;
+
+ size_t i, d = hdr->dims;
+ for( i = 0; i < d; i++ )
+ elem->idx[i] = idx[i];
+ d = elemSize();
+ uchar* p = &value<uchar>(elem);
+ for( i = 0; i <= d - sizeof(int); i += sizeof(int) )
+ *(int*)(p + i) = 0;
+ for( ; i < d; i++ )
+ p[i] = 0;
+
+ return p;
+}
+
+
+void SparseMat::removeNode(size_t hidx, size_t nidx, size_t previdx)
+{
+ Node* n = node(nidx);
+ if( previdx )
+ {
+ Node* prev = node(previdx);
+ prev->next = n->next;
+ }
+ else
+ hdr->hashtab[hidx] = n->next;
+ n->next = hdr->freeList;
+ hdr->freeList = nidx;
+ --hdr->nodeCount;
+}
+
+
+SparseMatConstIterator::SparseMatConstIterator(const SparseMat* _m)
+: m((SparseMat*)_m), hashidx(0), ptr(0)
+{
+ if(!_m || !_m->hdr)
+ return;
+ SparseMat::Hdr& hdr = *m->hdr;
+ const vector<size_t>& htab = hdr.hashtab;
+ size_t i, hsize = htab.size();
+ for( i = 0; i < hsize; i++ )
+ {
+ size_t nidx = htab[i];
+ if( nidx )
+ {
+ hashidx = i;
+ ptr = &hdr.pool[nidx] + hdr.valueOffset;
+ return;
+ }
+ }
+}
+
+SparseMatConstIterator& SparseMatConstIterator::operator ++()
+{
+ if( !ptr || !m || !m->hdr )
+ return *this;
+ SparseMat::Hdr& hdr = *m->hdr;
+ size_t next = ((const SparseMat::Node*)(ptr - hdr.valueOffset))->next;
+ if( next )
+ {
+ ptr = &hdr.pool[next] + hdr.valueOffset;
+ return *this;
+ }
+ size_t i = hashidx + 1, sz = hdr.hashtab.size();
+ for( ; i < sz; i++ )
+ {
+ size_t nidx = hdr.hashtab[i];
+ if( nidx )
+ {
+ hashidx = i;
+ ptr = &hdr.pool[nidx] + hdr.valueOffset;
+ return *this;
+ }
+ }
+ hashidx = sz;
+ ptr = 0;
+ return *this;
+}
+
+
+double norm( const SparseMat& src, int normType )
+{
+ SparseMatConstIterator it;
+
+ size_t i, N = src.nzcount();
+ normType &= NORM_TYPE_MASK;
+ int type = src.type();
+ double result = 0;
+
+ CV_Assert( normType == NORM_INF || normType == NORM_L1 || normType == NORM_L2 );
+
+ if( type == CV_32F )
+ {
+ if( normType == NORM_INF )
+ for( i = 0; i < N; i++, ++it )
+ result = std::max(result, (double)*(const float*)it.ptr);
+ else if( normType == NORM_L1 )
+ for( i = 0; i < N; i++, ++it )
+ result += std::abs(*(const float*)it.ptr);
+ else
+ for( i = 0; i < N; i++, ++it )
+ {
+ double v = *(const float*)it.ptr;
+ result += v*v;
+ }
+ }
+ else if( type == CV_64F )
+ {
+ if( normType == NORM_INF )
+ for( i = 0; i < N; i++, ++it )
+ result = std::max(result, *(const double*)it.ptr);
+ else if( normType == NORM_L1 )
+ for( i = 0; i < N; i++, ++it )
+ result += std::abs(*(const double*)it.ptr);
+ else
+ for( i = 0; i < N; i++, ++it )
+ {
+ double v = *(const double*)it.ptr;
+ result += v*v;
+ }
+ }
+ else
+ CV_Error( CV_StsUnsupportedFormat, "Only 32f and 64f are supported" );
+
+ if( normType == NORM_L2 )
+ result = std::sqrt(result);
+ return result;
+}
+
+void minMaxLoc( const SparseMat& src, double* _minval, double* _maxval, int* _minidx, int* _maxidx )
+{
+ SparseMatConstIterator it;
+
+ size_t i, N = src.nzcount(), d = src.hdr ? src.hdr->dims : 0;
+ int type = src.type();
+ const int *minidx = 0, *maxidx = 0;
+
+ if( type == CV_32F )
+ {
+ float minval = FLT_MAX, maxval = -FLT_MAX;
+ for( i = 0; i < N; i++, ++it )
+ {
+ float v = *(const float*)it.ptr;
+ if( v < minval )
+ {
+ minval = v;
+ minidx = it.node()->idx;
+ }
+ if( v > maxval )
+ {
+ maxval = v;
+ maxidx = it.node()->idx;
+ }
+ }
+ if( _minval )
+ *_minval = minval;
+ if( _maxval )
+ *_maxval = maxval;
+ }
+ else if( type == CV_64F )
+ {
+ double minval = DBL_MAX, maxval = -DBL_MAX;
+ for( i = 0; i < N; i++, ++it )
+ {
+ double v = *(const double*)it.ptr;
+ if( v < minval )
+ {
+ minval = v;
+ minidx = it.node()->idx;
+ }
+ if( v > maxval )
+ {
+ maxval = v;
+ maxidx = it.node()->idx;
+ }
+ }
+ if( _minval )
+ *_minval = minval;
+ if( _maxval )
+ *_maxval = maxval;
+ }
+ else
+ CV_Error( CV_StsUnsupportedFormat, "Only 32f and 64f are supported" );
+
+ if( _minidx )
+ for( i = 0; i < d; i++ )
+ _minidx[i] = minidx[i];
+ if( _maxidx )
+ for( i = 0; i < d; i++ )
+ _maxidx[i] = maxidx[i];
+}
+
+
+void normalize( const SparseMat& src, SparseMat& dst, double a, int norm_type )
+{
+ double scale = 1;
+ if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
+ {
+ scale = norm( src, norm_type );
+ scale = scale > DBL_EPSILON ? a/scale : 0.;
+ }
+ else
+ CV_Error( CV_StsBadArg, "Unknown/unsupported norm type" );
+
+ src.convertTo( dst, -1, scale );
+}
+
+
+}
+
+/* End of file. */
projects / opencv / blobdiff