C++ Mat::isContinuous方法代码示例

double calcGlobalOrientation( InputArray _orientation, InputArray _mask,
                                  InputArray _mhi, double /*timestamp*/,
                                  double duration )
{
    Mat orient = _orientation.getMat(), mask = _mask.getMat(), mhi = _mhi.getMat();
    Size size = mhi.size();

    CV_Assert( mask.type() == CV_8U && orient.type() == CV_32F && mhi.type() == CV_32F );
    CV_Assert( mask.size() == size && orient.size() == size );
    CV_Assert( duration > 0 );

    int histSize = 12;
    float _ranges[] = { 0.f, 360.f };
    const float* ranges = _ranges;
    Mat hist;

    calcHist(&orient, 1, 0, mask, hist, 1, &histSize, &ranges);

    // find the maximum index (the dominant orientation)
    Point baseOrientPt;
    minMaxLoc(hist, 0, 0, 0, &baseOrientPt);
    float fbaseOrient = (baseOrientPt.x + baseOrientPt.y)*360.f/histSize;

    // override timestamp with the maximum value in MHI
    double timestamp = 0;
    minMaxLoc( mhi, 0, &timestamp, 0, 0, mask );

    // find the shift relative to the dominant orientation as weighted sum of relative angles
    float a = (float)(254. / 255. / duration);
    float b = (float)(1. - timestamp * a);
    float delbound = (float)(timestamp - duration);

    if( mhi.isContinuous() && mask.isContinuous() && orient.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
    }

    /*
     a = 254/(255*dt)
     b = 1 - t*a = 1 - 254*t/(255*dur) =
     (255*dt - 254*t)/(255*dt) =
     (dt - (t - dt)*254)/(255*dt);
     --------------------------------------------------------
     ax + b = 254*x/(255*dt) + (dt - (t - dt)*254)/(255*dt) =
     (254*x + dt - (t - dt)*254)/(255*dt) =
     ((x - (t - dt))*254 + dt)/(255*dt) =
     (((x - (t - dt))/dt)*254 + 1)/255 = (((x - low_time)/dt)*254 + 1)/255
     */
    float shiftOrient = 0, shiftWeight = 0;
    for( int y = 0; y < size.height; y++ )
    {
        const float* mhiptr = mhi.ptr<float>(y);
        const float* oriptr = orient.ptr<float>(y);
        const uchar* maskptr = mask.ptr<uchar>(y);

        for( int x = 0; x < size.width; x++ )
        {
            if( maskptr[x] != 0 && mhiptr[x] > delbound )
            {
                /*
                 orient in 0..360, base_orient in 0..360
                 -> (rel_angle = orient - base_orient) in -360..360.
                 rel_angle is translated to -180..180
                 */
                float weight = mhiptr[x] * a + b;
                float relAngle = oriptr[x] - fbaseOrient;

                relAngle += (relAngle < -180 ? 360 : 0);
                relAngle += (relAngle > 180 ? -360 : 0);

                if( fabs(relAngle) < 45 )
                {
                    shiftOrient += weight * relAngle;
                    shiftWeight += weight;
                }
            }
        }
    }

    // add the dominant orientation and the relative shift
    if( shiftWeight == 0 )
        shiftWeight = 0.01f;

    fbaseOrient += shiftOrient / shiftWeight;
    fbaseOrient -= (fbaseOrient < 360 ? 0 : 360);
    fbaseOrient += (fbaseOrient >= 0 ? 0 : 360);

    return fbaseOrient;
}

bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
                        InputArray _cameraMatrix, InputArray _distCoeffs,
                        OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
                        int iterationsCount, float reprojectionError, double confidence,
                        OutputArray _inliers, int flags)
{

    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();

    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );

    CV_Assert(opoints.isContinuous());
    CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
    CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
    CV_Assert(ipoints.isContinuous());
    CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
    CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);

    _rvec.create(3, 1, CV_64FC1);
    _tvec.create(3, 1, CV_64FC1);

    Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
    Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();

    Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
    cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec);

    int model_points = 4;                             // minimum of number of model points
    if( flags == cv::SOLVEPNP_ITERATIVE ) model_points = 6;
    else if( flags == cv::SOLVEPNP_UPNP ) model_points = 6;
    else if( flags == cv::SOLVEPNP_EPNP ) model_points = 5;

    double param1 = reprojectionError;                // reprojection error
    double param2 = confidence;                       // confidence
    int param3 = iterationsCount;                     // number maximum iterations

    cv::Mat _local_model(3, 2, CV_64FC1);
    cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);

    // call Ransac
    int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);

    if( result <= 0 || _local_model.rows <= 0)
    {
        _rvec.assign(rvec);    // output rotation vector
        _tvec.assign(tvec);    // output translation vector

        if( _inliers.needed() )
            _inliers.release();

        return false;
    }
    else
    {
        _rvec.assign(_local_model.col(0));    // output rotation vector
        _tvec.assign(_local_model.col(1));    // output translation vector
    }

    if(_inliers.needed())
    {
        Mat _local_inliers;
        int count = 0;
        for (int i = 0; i < _mask_local_inliers.rows; ++i)
        {
            if((int)_mask_local_inliers.at<uchar>(i) == 1) // inliers mask
            {
                _local_inliers.push_back(count);    // output inliers vector
                count++;
            }
        }
        _local_inliers.copyTo(_inliers);
    }
    return true;
}

int RegionSaliency::Quantize(const Mat& img3f, Mat &idx1i, Mat &_color3f, Mat &_colorNum, double ratio)
{
	static const int clrNums[3] = {12, 12, 12};
	static const float clrTmp[3] = {clrNums[0] - 0.0001f, clrNums[1] - 0.0001f, clrNums[2] - 0.0001f};
	static const int w[3] = {clrNums[1] * clrNums[2], clrNums[2], 1};

	CV_Assert(img3f.data != NULL);
	idx1i = Mat::zeros(img3f.size(), CV_32S);
	int rows = img3f.rows, cols = img3f.cols;
	if (img3f.isContinuous() && idx1i.isContinuous())
	{
		cols *= rows;
		rows = 1;
	}

	// Build color pallet
	map<int, int> pallet;
	for (int y = 0; y < rows; y++)
	{
		const float* imgData = img3f.ptr<float>(y);
		int* idx = idx1i.ptr<int>(y);
		for (int x = 0; x < cols; x++, imgData += 3)
		{
			idx[x] = (int)(imgData[0]*clrTmp[0])*w[0] + (int)(imgData[1]*clrTmp[1])*w[1] + (int)(imgData[2]*clrTmp[2]);
			pallet[idx[x]] ++;
		}
	}

	// Find significant colors
	int maxNum = 0;
	{
		int count = 0;
		vector<pair<int, int> > num; // (num, color) pairs in num
		num.reserve(pallet.size());
		for (map<int, int>::iterator it = pallet.begin(); it != pallet.end(); it++)
			num.push_back(pair<int, int>(it->second, it->first)); // (color, num) pairs in pallet
		sort(num.begin(), num.end(), std::greater<pair<int, int> >());

		maxNum = (int)num.size();
		int maxDropNum = cvRound(rows * cols * (1-ratio));
		for (int crnt = num[maxNum-1].first; crnt < maxDropNum && maxNum > 1; maxNum--)
			crnt += num[maxNum - 2].first;
		maxNum = min(maxNum, 256); // To avoid very rarely case
		if (maxNum < 10)
			maxNum = min((int)pallet.size(), 100);
		pallet.clear();
		for (int i = 0; i < maxNum; i++)
			pallet[num[i].second] = i; 

		vector<Vec3i> color3i(num.size());
		for (unsigned int i = 0; i < num.size(); i++)
		{
			color3i[i][0] = num[i].second / w[0];
			color3i[i][1] = num[i].second % w[0] / w[1];
			color3i[i][2] = num[i].second % w[1];
		}

		for (unsigned int i = maxNum; i < num.size(); i++)
		{
			int simIdx = 0, simVal = INT_MAX;
			for (int j = 0; j < maxNum; j++)
			{
				int d_ij = vecSqrDist3(color3i[i], color3i[j]);
				if (d_ij < simVal)
					simVal = d_ij, simIdx = j;
			}
			pallet[num[i].second] = pallet[num[simIdx].second];
		}
	}

	_color3f = Mat::zeros(1, maxNum, CV_32FC3);
	_colorNum = Mat::zeros(_color3f.size(), CV_32S);

	Vec3f* color = (Vec3f*)(_color3f.data);
	int* colorNum = (int*)(_colorNum.data);
	for (int y = 0; y < rows; y++) 
	{
		const Vec3f* imgData = img3f.ptr<Vec3f>(y);
		int* idx = idx1i.ptr<int>(y);
		for (int x = 0; x < cols; x++)
		{
			idx[x] = pallet[idx[x]];
			color[idx[x]] += imgData[x];
			colorNum[idx[x]] ++;
		}
	}
	for (int i = 0; i < _color3f.cols; i++)
  {
    color[i] *= (1.0f/((float)colorNum[i]));
 		//color[i] /= ((float)colorNum[i]); // original code that caused trouble with newer versions; it seems like the division operator is ill defined
  }

	return _color3f.cols;
}

static double
getThreshVal_Otsu_8u( const Mat& _src )
{
    Size size = _src.size();
    int step = (int) _src.step;
    if( _src.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
        step = size.width;
    }

#if IPP_VERSION_X100 >= 801 && !defined(HAVE_IPP_ICV_ONLY)
    CV_IPP_CHECK()
    {
        IppiSize srcSize = { size.width, size.height };
        Ipp8u thresh;
        CV_SUPPRESS_DEPRECATED_START
        IppStatus ok = ippiComputeThreshold_Otsu_8u_C1R(_src.ptr(), step, srcSize, &thresh);
        CV_SUPPRESS_DEPRECATED_END
        if (ok >= 0)
        {
            CV_IMPL_ADD(CV_IMPL_IPP);
            return thresh;
        }
        setIppErrorStatus();
    }
#endif

    const int N = 256;
    int i, j, h[N] = {0};
    for( i = 0; i < size.height; i++ )
    {
        const uchar* src = _src.ptr() + step*i;
        j = 0;
        #if CV_ENABLE_UNROLLED
        for( ; j <= size.width - 4; j += 4 )
        {
            int v0 = src[j], v1 = src[j+1];
            h[v0]++; h[v1]++;
            v0 = src[j+2]; v1 = src[j+3];
            h[v0]++; h[v1]++;
        }
        #endif
        for( ; j < size.width; j++ )
            h[src[j]]++;
    }

    double mu = 0, scale = 1./(size.width*size.height);
    for( i = 0; i < N; i++ )
        mu += i*(double)h[i];

    mu *= scale;
    double mu1 = 0, q1 = 0;
    double max_sigma = 0, max_val = 0;

    for( i = 0; i < N; i++ )
    {
        double p_i, q2, mu2, sigma;

        p_i = h[i]*scale;
        mu1 *= q1;
        q1 += p_i;
        q2 = 1. - q1;

        if( std::min(q1,q2) < FLT_EPSILON || std::max(q1,q2) > 1. - FLT_EPSILON )
            continue;

        mu1 = (mu1 + i*p_i)/q1;
        mu2 = (mu - q1*mu1)/q2;
        sigma = q1*q2*(mu1 - mu2)*(mu1 - mu2);
        if( sigma > max_sigma )
        {
            max_sigma = sigma;
            max_val = i;
        }
    }

    return max_val;
}

void MyOpenCV::cvtColor(const Mat &matFrom, Mat &matTo, int code)
{
    CvSize s = matFrom.size();
    
    matTo = cvCreateMat(s.height, s.width, CV_8UC3);
    int w = s.width;
    int h = s.height;
    double  S, V, L;
    double R, G, B, M, m, C, H=0;
    int channels = matFrom.channels();
    switch(code)
    {
        case BGR2HSV:
            if(matFrom.isContinuous() && matTo.isContinuous())
            {
                int size;
                uchar *to = matTo.ptr(0);
                const uchar *from = matFrom.ptr(0);
                size = w*h*channels;
                for(int i = 0; i < size; i += channels)
                {
                    B = from[i]/255.;
                    G = from[i+1]/255.;
                    R = from[i+2]/255.;
                    M = max(max(R,G),B);
                    m = min(min(R,G),B);
                    C = M - m;
                    V = M;
                    if(C == 0)
                    {
                        H = 0;
                        S = 0;
                    }
                    else
                    {
                        if(M == R) H = fmod((G-B)/C,6);
                        if(M == G) H = (B-R)/C+2;
                        if(M == B) H = (R-G)/C+4;
                        
                        S = C/V;
                    }
                    
                    H = H*255/6;
                    S = S*255;
                    V = V*255;
                    to[i] = H;
                    to[i+1] = S;
                    to[i+2] = V;
                    
                }
            }
            else
            {
                for(int i = 0; i < h; i++)
                    for(int j = 0; j < w; j ++)
                    {
                        R = matFrom.ptr<Vec3b>(i)[j][2]/255.;
                        G = matFrom.ptr<Vec3b>(i)[j][1]/255.;
                        B = matFrom.ptr<Vec3b>(i)[j][0]/255.;
                        M = max(max(R,G),B);
                        m = min(min(R,G),B);
                        C = M - m;
                        V = M;
                        if(C == 0)
                        {
                            H = 0;
                            S = 0;
                        }
                        else
                        {
                            if(M == R) H = fmod((G-B)/C,6);
                            if(M == G) H = (B-R)/C+2;
                            if(M == B) H = (R-G)/C+4;
                            
                            S = C/V;
                        }
                        
                        H = H*255/6;
                        S = S*255;
                        V = V*255;
                        matTo.ptr<Vec3b>(i)[j][0] = H;
                        matTo.ptr<Vec3b>(i)[j][1] = S;
                        matTo.ptr<Vec3b>(i)[j][2] = V;
                    }
            }
            break;
        case BGR2HSL:
            for(int i = 0; i < h; i++)
                for(int j = 0; j < w; j ++)
                {
                    R = matFrom.ptr<Vec3b>(i)[j][2]/255.;
                    G = matFrom.ptr<Vec3b>(i)[j][1]/255.;
                    B = matFrom.ptr<Vec3b>(i)[j][0]/255.;
                    M = max(max(R,G),B);
                    m = min(min(R,G),B);
                    C = M - m;
                    L = (M+m)/2;
                    if(C == 0)
                    {
                        H = 0;
//.........这里部分代码省略.........

static void convertFromCCS( const Mat& _src0, const Mat& _src1, Mat& _dst, int flags )
{
    if( _dst.rows > 1 && (_dst.cols > 1 || (flags & DFT_ROWS)) )
    {
        int i, count = _dst.rows, len = _dst.cols;
        bool is2d = (flags & DFT_ROWS) == 0;
        Mat src0row, src1row, dstrow;
        for( i = 0; i < count; i++ )
        {
            int j = !is2d || i == 0 ? i : count - i;
            src0row = _src0.row(i);
            src1row = _src1.row(j);
            dstrow = _dst.row(i);
            convertFromCCS( src0row, src1row, dstrow, 0 );
        }

        if( is2d )
        {
            src0row = _src0.col(0);
            dstrow = _dst.col(0);
            convertFromCCS( src0row, src0row, dstrow, 0 );
            if( (len & 1) == 0 )
            {
                src0row = _src0.col(_src0.cols - 1);
                dstrow = _dst.col(len/2);
                convertFromCCS( src0row, src0row, dstrow, 0 );
            }
        }
    }
    else
    {
        int i, n = _dst.cols + _dst.rows - 1, n2 = (n+1) >> 1;
        int cn = _src0.channels();
        int srcstep = cn, dststep = 1;

        if( !_dst.isContinuous() )
            dststep = (int)(_dst.step/_dst.elemSize());

        if( !_src0.isContinuous() )
            srcstep = (int)(_src0.step/_src0.elemSize1());

        if( _dst.depth() == CV_32F )
        {
            Complexf* dst = _dst.ptr<Complexf>();
            const float* src0 = _src0.ptr<float>();
            const float* src1 = _src1.ptr<float>();
            int delta0, delta1;

            dst->re = src0[0];
            dst->im = 0;

            if( (n & 1) == 0 )
            {
                dst[n2*dststep].re = src0[(cn == 1 ? n-1 : n2)*srcstep];
                dst[n2*dststep].im = 0;
            }

            delta0 = srcstep;
            delta1 = delta0 + (cn == 1 ? srcstep : 1);
            if( cn == 1 )
                srcstep *= 2;

            for( i = 1; i < n2; i++, delta0 += srcstep, delta1 += srcstep )
            {
                float t0 = src0[delta0];
                float t1 = src0[delta1];

                dst[i*dststep].re = t0;
                dst[i*dststep].im = t1;

                t0 = src1[delta0];
                t1 = -src1[delta1];

                dst[(n-i)*dststep].re = t0;
                dst[(n-i)*dststep].im = t1;
            }
        }
        else
        {
            Complexd* dst = _dst.ptr<Complexd>();
            const double* src0 = _src0.ptr<double>();
            const double* src1 = _src1.ptr<double>();
            int delta0, delta1;

            dst->re = src0[0];
            dst->im = 0;

            if( (n & 1) == 0 )
            {
                dst[n2*dststep].re = src0[(cn == 1 ? n-1 : n2)*srcstep];
                dst[n2*dststep].im = 0;
            }

            delta0 = srcstep;
            delta1 = delta0 + (cn == 1 ? srcstep : 1);
            if( cn == 1 )
                srcstep *= 2;

            for( i = 1; i < n2; i++, delta0 += srcstep, delta1 += srcstep )
            {
//.........这里部分代码省略.........

static void
thresh_16s( const Mat& _src, Mat& _dst, short thresh, short maxval, int type )
{
    int i, j;
    Size roi = _src.size();
    roi.width *= _src.channels();
    const short* src = _src.ptr<short>();
    short* dst = _dst.ptr<short>();
    size_t src_step = _src.step/sizeof(src[0]);
    size_t dst_step = _dst.step/sizeof(dst[0]);

#if CV_SSE2
    volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE);
#endif

    if( _src.isContinuous() && _dst.isContinuous() )
    {
        roi.width *= roi.height;
        roi.height = 1;
        src_step = dst_step = roi.width;
    }

#ifdef HAVE_TEGRA_OPTIMIZATION
    if (tegra::thresh_16s(_src, _dst, roi.width, roi.height, thresh, maxval, type))
        return;
#endif

#if defined(HAVE_IPP)
    CV_IPP_CHECK()
    {
        IppiSize sz = { roi.width, roi.height };
        CV_SUPPRESS_DEPRECATED_START
        switch( type )
        {
        case THRESH_TRUNC:
#ifndef HAVE_IPP_ICV_ONLY
            if (_src.data == _dst.data && ippiThreshold_GT_16s_C1IR(dst, (int)dst_step*sizeof(dst[0]), sz, thresh) >= 0)
            {
                CV_IMPL_ADD(CV_IMPL_IPP);
                return;
            }
#endif
            if (ippiThreshold_GT_16s_C1R(src, (int)src_step*sizeof(src[0]), dst, (int)dst_step*sizeof(dst[0]), sz, thresh) >= 0)
            {
                CV_IMPL_ADD(CV_IMPL_IPP);
                return;
            }
            setIppErrorStatus();
            break;
        case THRESH_TOZERO:
#ifndef HAVE_IPP_ICV_ONLY
            if (_src.data == _dst.data && ippiThreshold_LTVal_16s_C1IR(dst, (int)dst_step*sizeof(dst[0]), sz, thresh + 1, 0) >= 0)
            {
                CV_IMPL_ADD(CV_IMPL_IPP);
                return;
            }
#endif
            if (ippiThreshold_LTVal_16s_C1R(src, (int)src_step*sizeof(src[0]), dst, (int)dst_step*sizeof(dst[0]), sz, thresh+1, 0) >= 0)
            {
                CV_IMPL_ADD(CV_IMPL_IPP);
                return;
            }
            setIppErrorStatus();
            break;
        case THRESH_TOZERO_INV:
#ifndef HAVE_IPP_ICV_ONLY
            if (_src.data == _dst.data && ippiThreshold_GTVal_16s_C1IR(dst, (int)dst_step*sizeof(dst[0]), sz, thresh, 0) >= 0)
            {
                CV_IMPL_ADD(CV_IMPL_IPP);
                return;
            }
#endif
            if (ippiThreshold_GTVal_16s_C1R(src, (int)src_step*sizeof(src[0]), dst, (int)dst_step*sizeof(dst[0]), sz, thresh, 0) >= 0)
            {
                CV_IMPL_ADD(CV_IMPL_IPP);
                return;
            }
            setIppErrorStatus();
            break;
        }
        CV_SUPPRESS_DEPRECATED_END
    }
#endif

    switch( type )
    {
    case THRESH_BINARY:
        for( i = 0; i < roi.height; i++, src += src_step, dst += dst_step )
        {
            j = 0;
        #if CV_SSE2
            if( useSIMD )
            {
                __m128i thresh8 = _mm_set1_epi16(thresh), maxval8 = _mm_set1_epi16(maxval);
                for( ; j <= roi.width - 16; j += 16 )
                {
                    __m128i v0, v1;
                    v0 = _mm_loadu_si128( (const __m128i*)(src + j) );
                    v1 = _mm_loadu_si128( (const __m128i*)(src + j + 8) );
                    v0 = _mm_cmpgt_epi16( v0, thresh8 );
//.........这里部分代码省略.........

void HDF5Impl::dsinsert( InputArray Array, const String& dslabel,
             const int* dims_offset, const int* dims_counts ) const
{
    // only Mat support
    CV_Assert( Array.isMat() );

    // check dataset exists
    if ( hlexists( dslabel ) == false )
      CV_Error_(Error::StsInternal, ("Dataset '%s' does not exist.", dslabel.c_str()));

    Mat matrix = Array.getMat();

    // memory array should be compact
    CV_Assert( matrix.isContinuous() );

    int n_dims = matrix.dims;
    int channs = matrix.channels();

    hsize_t *dsdims = new hsize_t[n_dims];
    hsize_t *offset = new hsize_t[n_dims];
    // replicate Mat dimensions
    for ( int d = 0; d < n_dims; d++ )
    {
      offset[d] = 0;
      dsdims[d] = matrix.size[d];
    }

    // set custom amount of data
    if ( dims_counts != NULL )
    {
      for ( int d = 0; d < n_dims; d++ )
      {
        CV_Assert( dims_counts[d] <= matrix.size[d] );
        dsdims[d] = dims_counts[d];
      }
    }

    // open dataset
    hid_t dsdata = H5Dopen( m_h5_file_id, dslabel.c_str(), H5P_DEFAULT );

    // create input data space
    hid_t dspace = H5Screate_simple( n_dims, dsdims, NULL );

    // set custom offsets
    if ( dims_offset != NULL )
    {
      for ( int d = 0; d < n_dims; d++ )
        offset[d] = dims_offset[d];
    }

    // get actual file space and dims
    hid_t fspace = H5Dget_space( dsdata );
    int f_dims = H5Sget_simple_extent_ndims( fspace );
    hsize_t *fsdims = new hsize_t[f_dims];
    H5Sget_simple_extent_dims( fspace, fsdims, NULL );
    H5Sclose( fspace );

    CV_Assert( f_dims == n_dims );

    // compute new extents
    hsize_t *nwdims = new hsize_t[n_dims];
    for ( int d = 0; d < n_dims; d++ )
    {
      // init
      nwdims[d] = 0;
      // add offset
      if ( dims_offset != NULL )
        nwdims[d] += dims_offset[d];
      // add counts or matrix size
      if ( dims_counts != NULL )
        nwdims[d] += dims_counts[d];
      else
        nwdims[d] += matrix.size[d];

      // clamp back if smaller
      if ( nwdims[d] < fsdims[d] )
        nwdims[d] = fsdims[d];
    }

    // extend dataset
    H5Dextend( dsdata, nwdims );

    // get the extended data space
    fspace = H5Dget_space( dsdata );

    H5Sselect_hyperslab( fspace, H5S_SELECT_SET,
                         offset, NULL, dsdims, NULL );

    // convert type
    hid_t dstype = GetH5type( matrix.type() );

    // expand channs
    if ( matrix.channels() > 1 )
    {
      hsize_t adims[1] = { (hsize_t)channs };
      dstype = H5Tarray_create( dstype, 1, adims );
    }

    // write into dataset
    H5Dwrite( dsdata, dstype, dspace, fspace,
//.........这里部分代码省略.........

void HDF5Impl::dswrite( InputArray Array, const String& dslabel,
             const int* dims_offset, const int* dims_counts ) const
{
    // only Mat support
    CV_Assert( Array.isMat() );

    Mat matrix = Array.getMat();

    // memory array should be compact
    CV_Assert( matrix.isContinuous() );

    int n_dims = matrix.dims;
    int channs = matrix.channels();

    int *dsizes = new int[n_dims];
    hsize_t *dsdims = new hsize_t[n_dims];
    hsize_t *offset = new hsize_t[n_dims];
    // replicate Mat dimensions
    for ( int d = 0; d < n_dims; d++ )
    {
      offset[d] = 0;
      dsizes[d] = matrix.size[d];
      dsdims[d] = matrix.size[d];
    }

    // FixMe: If one of the groups the dataset belongs to does not exist,
    // FixMe: dscreate() will fail!
    // FixMe: It should be an error if the specified dataset has not been created instead of trying to create it
    // pre-create dataset if needed
    if ( hlexists( dslabel ) == false )
      dscreate( n_dims, dsizes, matrix.type(), dslabel );

    // set custom amount of data
    if ( dims_counts != NULL )
    {
      for ( int d = 0; d < n_dims; d++ )
        dsdims[d] = dims_counts[d];
    }

    // open dataset
    hid_t dsdata = H5Dopen( m_h5_file_id, dslabel.c_str(), H5P_DEFAULT );

    // create input data space
    hid_t dspace = H5Screate_simple( n_dims, dsdims, NULL );

    // set custom offsets
    if ( dims_offset != NULL )
    {
      for ( int d = 0; d < n_dims; d++ )
        offset[d] = dims_offset[d];
    }

    // create offset write window space
    hid_t fspace = H5Dget_space( dsdata );
    H5Sselect_hyperslab( fspace, H5S_SELECT_SET,
                         offset, NULL, dsdims, NULL );

    // convert type
    hid_t dstype = GetH5type( matrix.type() );

    // expand channs
    if ( matrix.channels() > 1 )
    {
      hsize_t adims[1] = { (hsize_t)channs };
      dstype = H5Tarray_create( dstype, 1, adims );
    }

    // write into dataset
    H5Dwrite( dsdata, dstype, dspace, fspace,
              H5P_DEFAULT, matrix.data );

    if ( matrix.channels() > 1 )
      H5Tclose( dstype );

    delete [] dsizes;
    delete [] dsdims;
    delete [] offset;

    H5Sclose( dspace );
    H5Sclose( fspace );
    H5Dclose( dsdata );
}

static void build3dmodel( const Ptr<FeatureDetector>& detector,
                         const Ptr<DescriptorExtractor>& descriptorExtractor,
                         const vector<Point3f>& /*modelBox*/,
                         const vector<string>& imageList,
                         const vector<Rect>& roiList,
                         const vector<Vec6f>& poseList,
                         const Mat& cameraMatrix,
                         PointModel& model )
{
    int progressBarSize = 10;
    
    const double Feps = 5;
    const double DescriptorRatio = 0.7;
    
    vector<vector<KeyPoint> > allkeypoints;
    vector<int> dstart;
    vector<float> alldescriptorsVec;
    vector<Vec2i> pairwiseMatches;
    vector<Mat> Rs, ts;
    int descriptorSize = 0;
    Mat descriptorbuf;
    Set2i pairs, keypointsIdxMap;
    
    model.points.clear();
    model.didx.clear();
    
    dstart.push_back(0);
    
    size_t nimages = imageList.size();
    size_t nimagePairs = (nimages - 1)*nimages/2 - nimages;
    
    printf("\nComputing descriptors ");
    
    // 1. find all the keypoints and all the descriptors
    for( size_t i = 0; i < nimages; i++ )
    {
        vector<KeyPoint> keypoints;
        
        std::stringstream descriptorsPath;
        string algStringCode = "SURF";
        descriptorsPath << imageList[i] << "." << algStringCode << ".xml";
        cv::FileStorage fs(descriptorsPath.str().c_str(), FileStorage::READ);
        
        if( fs.isOpened())
        {
            fs["descriptors"] >> descriptorbuf;
            cv::read( fs["keypoints"], keypoints);
            
        }
        fs.release();
        
        //Point2f roiofs = roiList[i].tl(); 
        //for( size_t k = 0; k < keypoints.size(); k++ )
        //    keypoints[k].pt += roiofs;
        allkeypoints.push_back(keypoints);
        
        
        //        Mat img = imread(imageList[i], 1), gray;
        //        cvtColor(img, gray, CV_BGR2GRAY);
        //        
        //        vector<KeyPoint> keypoints;
        //        detector->detect(gray, keypoints);
        //        descriptorExtractor->compute(gray, keypoints, descriptorbuf);
        //        Point2f roiofs = roiList[i].tl(); 
        //        for( size_t k = 0; k < keypoints.size(); k++ )
        //            keypoints[k].pt += roiofs;
        //        allkeypoints.push_back(keypoints);
        //        
        Mat buf = descriptorbuf;
        if( !buf.isContinuous() || buf.type() != CV_32F )
        {
            buf.release();
            descriptorbuf.convertTo(buf, CV_32F);
        }
        descriptorSize = buf.cols;
        
        size_t prev = alldescriptorsVec.size();
        size_t delta = buf.rows*buf.cols;
        alldescriptorsVec.resize(prev + delta); 
        std::copy(buf.ptr<float>(), buf.ptr<float>() + delta,
                  alldescriptorsVec.begin() + prev);
        dstart.push_back(dstart.back() + keypoints.size());
        
        //Mat R, t;
        //unpackPose(poseList[i], R, t);
        //Rs.push_back(R);
        //ts.push_back(t);
        
        if( (i+1)*progressBarSize/nimages > i*progressBarSize/nimages )
        {
            putchar('.');
            fflush(stdout);
        }
    }

void cv::meanStdDev( InputArray _src, OutputArray _mean, OutputArray _sdv, InputArray _mask )
{
    Mat src = _src.getMat(), mask = _mask.getMat();
    CV_Assert( mask.empty() || mask.type() == CV_8U );
    
    int k, cn = src.channels(), depth = src.depth();
    SumSqrFunc func = sumSqrTab[depth];
    
    CV_Assert( func != 0 );
    
    const Mat* arrays[] = {&src, &mask, 0};
    uchar* ptrs[2];
    NAryMatIterator it(arrays, ptrs);
    int total = (int)it.size, blockSize = total, intSumBlockSize = 0;
    int j, count = 0, nz0 = 0;
    AutoBuffer<double> _buf(cn*4);
    double *s = (double*)_buf, *sq = s + cn;
    int *sbuf = (int*)s, *sqbuf = (int*)sq;
    bool blockSum = depth <= CV_16S, blockSqSum = depth <= CV_8S;
    size_t esz = 0;
    
    for( k = 0; k < cn; k++ )
        s[k] = sq[k] = 0;
    
    if( blockSum )
    {
        intSumBlockSize = 1 << 15;
        blockSize = std::min(blockSize, intSumBlockSize);
        sbuf = (int*)(sq + cn);
        if( blockSqSum )
            sqbuf = sbuf + cn;
        for( k = 0; k < cn; k++ )
            sbuf[k] = sqbuf[k] = 0;
        esz = src.elemSize();
    }
    
    for( size_t i = 0; i < it.nplanes; i++, ++it )
    {
        for( j = 0; j < total; j += blockSize )
        {
            int bsz = std::min(total - j, blockSize);
            int nz = func( ptrs[0], ptrs[1], (uchar*)sbuf, (uchar*)sqbuf, bsz, cn );
            count += nz;
            nz0 += nz;
            if( blockSum && (count + blockSize >= intSumBlockSize || (i+1 >= it.nplanes && j+bsz >= total)) )
            {
                for( k = 0; k < cn; k++ )
                {
                    s[k] += sbuf[k];
                    sbuf[k] = 0;
                }
                if( blockSqSum )
                {
                    for( k = 0; k < cn; k++ )
                    {
                        sq[k] += sqbuf[k];
                        sqbuf[k] = 0;
                    }
                }
                count = 0;
            }
            ptrs[0] += bsz*esz;
            if( ptrs[1] )
                ptrs[1] += bsz;
        }
    }
    
    double scale = nz0 ? 1./nz0 : 0.;
    for( k = 0; k < cn; k++ )
    {
        s[k] *= scale;
        sq[k] = std::sqrt(std::max(sq[k]*scale - s[k]*s[k], 0.));
    }
    
    for( j = 0; j < 2; j++ )
    {
        const double* sptr = j == 0 ? s : sq;
        _OutputArray _dst = j == 0 ? _mean : _sdv;
        if( !_dst.needed() )
            continue;

        if( !_dst.fixedSize() )
            _dst.create(cn, 1, CV_64F, -1, true);
        Mat dst = _dst.getMat();
        int dcn = (int)dst.total();
        CV_Assert( dst.type() == CV_64F && dst.isContinuous() &&
                  (dst.cols == 1 || dst.rows == 1) && dcn >= cn );
        double* dptr = dst.ptr<double>();
        for( k = 0; k < cn; k++ )
            dptr[k] = sptr[k];
        for( ; k < dcn; k++ )
            dptr[k] = 0;
    }
}

void divSpectrums( InputArray _srcA, InputArray _srcB, OutputArray _dst, int flags, bool conjB)
{
    Mat srcA = _srcA.getMat(), srcB = _srcB.getMat();
    int depth = srcA.depth(), cn = srcA.channels(), type = srcA.type();
    int rows = srcA.rows, cols = srcA.cols;
    int j, k;

    CV_Assert( type == srcB.type() && srcA.size() == srcB.size() );
    CV_Assert( type == CV_32FC1 || type == CV_32FC2 || type == CV_64FC1 || type == CV_64FC2 );

    _dst.create( srcA.rows, srcA.cols, type );
    Mat dst = _dst.getMat();

    bool is_1d = (flags & DFT_ROWS) || (rows == 1 || (cols == 1 &&
             srcA.isContinuous() && srcB.isContinuous() && dst.isContinuous()));

    if( is_1d && !(flags & DFT_ROWS) )
        cols = cols + rows - 1, rows = 1;

    int ncols = cols*cn;
    int j0 = cn == 1;
    int j1 = ncols - (cols % 2 == 0 && cn == 1);

    if( depth == CV_32F )
    {
        const float* dataA = srcA.ptr<float>();
        const float* dataB = srcB.ptr<float>();
        float* dataC = dst.ptr<float>();
        float eps = FLT_EPSILON; // prevent div0 problems

        size_t stepA = srcA.step/sizeof(dataA[0]);
        size_t stepB = srcB.step/sizeof(dataB[0]);
        size_t stepC = dst.step/sizeof(dataC[0]);

        if( !is_1d && cn == 1 )
        {
            for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
            {
                if( k == 1 )
                    dataA += cols - 1, dataB += cols - 1, dataC += cols - 1;
                dataC[0] = dataA[0] / (dataB[0] + eps);
                if( rows % 2 == 0 )
                    dataC[(rows-1)*stepC] = dataA[(rows-1)*stepA] / (dataB[(rows-1)*stepB] + eps);
                if( !conjB )
                    for( j = 1; j <= rows - 2; j += 2 )
                    {
                        double denom = (double)dataB[j*stepB]*dataB[j*stepB] +
                                       (double)dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + (double)eps;

                        double re = (double)dataA[j*stepA]*dataB[j*stepB] +
                                    (double)dataA[(j+1)*stepA]*dataB[(j+1)*stepB];

                        double im = (double)dataA[(j+1)*stepA]*dataB[j*stepB] -
                                    (double)dataA[j*stepA]*dataB[(j+1)*stepB];

                        dataC[j*stepC] = (float)(re / denom);
                        dataC[(j+1)*stepC] = (float)(im / denom);
                    }
                else
                    for( j = 1; j <= rows - 2; j += 2 )
                    {

                        double denom = (double)dataB[j*stepB]*dataB[j*stepB] +
                                       (double)dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + (double)eps;

                        double re = (double)dataA[j*stepA]*dataB[j*stepB] -
                                    (double)dataA[(j+1)*stepA]*dataB[(j+1)*stepB];

                        double im = (double)dataA[(j+1)*stepA]*dataB[j*stepB] +
                                    (double)dataA[j*stepA]*dataB[(j+1)*stepB];

                        dataC[j*stepC] = (float)(re / denom);
                        dataC[(j+1)*stepC] = (float)(im / denom);
                    }
                if( k == 1 )
                    dataA -= cols - 1, dataB -= cols - 1, dataC -= cols - 1;
            }
        }

        for( ; rows--; dataA += stepA, dataB += stepB, dataC += stepC )
        {
            if( is_1d && cn == 1 )
            {
                dataC[0] = dataA[0] / (dataB[0] + eps);
                if( cols % 2 == 0 )
                    dataC[j1] = dataA[j1] / (dataB[j1] + eps);
            }

            if( !conjB )
                for( j = j0; j < j1; j += 2 )
                {
                    double denom = (double)(dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps);
                    double re = (double)(dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1]);
                    double im = (double)(dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1]);
                    dataC[j] = (float)(re / denom);
                    dataC[j+1] = (float)(im / denom);
                }
            else
                for( j = j0; j < j1; j += 2 )
                {
//.........这里部分代码省略.........

static bool ipp_norm(Mat &src, int normType, Mat &mask, double &result)
{
    CV_INSTRUMENT_REGION_IPP();

#if IPP_VERSION_X100 >= 700
    size_t total_size = src.total();
    int rows = src.size[0], cols = rows ? (int)(total_size/rows) : 0;

    if( (src.dims == 2 || (src.isContinuous() && mask.isContinuous()))
        && cols > 0 && (size_t)rows*cols == total_size )
    {
        if( !mask.empty() )
        {
            IppiSize sz = { cols, rows };
            int type = src.type();

            typedef IppStatus (CV_STDCALL* ippiMaskNormFuncC1)(const void *, int, const void *, int, IppiSize, Ipp64f *);
            ippiMaskNormFuncC1 ippiNorm_C1MR =
                normType == NORM_INF ?
                (type == CV_8UC1 ? (ippiMaskNormFuncC1)ippiNorm_Inf_8u_C1MR :
                type == CV_16UC1 ? (ippiMaskNormFuncC1)ippiNorm_Inf_16u_C1MR :
                type == CV_32FC1 ? (ippiMaskNormFuncC1)ippiNorm_Inf_32f_C1MR :
                0) :
            normType == NORM_L1 ?
                (type == CV_8UC1 ? (ippiMaskNormFuncC1)ippiNorm_L1_8u_C1MR :
                type == CV_16UC1 ? (ippiMaskNormFuncC1)ippiNorm_L1_16u_C1MR :
                type == CV_32FC1 ? (ippiMaskNormFuncC1)ippiNorm_L1_32f_C1MR :
                0) :
            normType == NORM_L2 || normType == NORM_L2SQR ?
                (type == CV_8UC1 ? (ippiMaskNormFuncC1)ippiNorm_L2_8u_C1MR :
                type == CV_16UC1 ? (ippiMaskNormFuncC1)ippiNorm_L2_16u_C1MR :
                type == CV_32FC1 ? (ippiMaskNormFuncC1)ippiNorm_L2_32f_C1MR :
                0) : 0;
            if( ippiNorm_C1MR )
            {
                Ipp64f norm;
                if( CV_INSTRUMENT_FUN_IPP(ippiNorm_C1MR, src.ptr(), (int)src.step[0], mask.ptr(), (int)mask.step[0], sz, &norm) >= 0 )
                {
                    result = (normType == NORM_L2SQR ? (double)(norm * norm) : (double)norm);
                    return true;
                }
            }
            typedef IppStatus (CV_STDCALL* ippiMaskNormFuncC3)(const void *, int, const void *, int, IppiSize, int, Ipp64f *);
            ippiMaskNormFuncC3 ippiNorm_C3CMR =
                normType == NORM_INF ?
                (type == CV_8UC3 ? (ippiMaskNormFuncC3)ippiNorm_Inf_8u_C3CMR :
                type == CV_16UC3 ? (ippiMaskNormFuncC3)ippiNorm_Inf_16u_C3CMR :
                type == CV_32FC3 ? (ippiMaskNormFuncC3)ippiNorm_Inf_32f_C3CMR :
                0) :
            normType == NORM_L1 ?
                (type == CV_8UC3 ? (ippiMaskNormFuncC3)ippiNorm_L1_8u_C3CMR :
                type == CV_16UC3 ? (ippiMaskNormFuncC3)ippiNorm_L1_16u_C3CMR :
                type == CV_32FC3 ? (ippiMaskNormFuncC3)ippiNorm_L1_32f_C3CMR :
                0) :
            normType == NORM_L2 || normType == NORM_L2SQR ?
                (type == CV_8UC3 ? (ippiMaskNormFuncC3)ippiNorm_L2_8u_C3CMR :
                type == CV_16UC3 ? (ippiMaskNormFuncC3)ippiNorm_L2_16u_C3CMR :
                type == CV_32FC3 ? (ippiMaskNormFuncC3)ippiNorm_L2_32f_C3CMR :
                0) : 0;
            if( ippiNorm_C3CMR )
            {
                Ipp64f norm1, norm2, norm3;
                if( CV_INSTRUMENT_FUN_IPP(ippiNorm_C3CMR, src.data, (int)src.step[0], mask.data, (int)mask.step[0], sz, 1, &norm1) >= 0 &&
                    CV_INSTRUMENT_FUN_IPP(ippiNorm_C3CMR, src.data, (int)src.step[0], mask.data, (int)mask.step[0], sz, 2, &norm2) >= 0 &&
                    CV_INSTRUMENT_FUN_IPP(ippiNorm_C3CMR, src.data, (int)src.step[0], mask.data, (int)mask.step[0], sz, 3, &norm3) >= 0)
                {
                    Ipp64f norm =
                        normType == NORM_INF ? std::max(std::max(norm1, norm2), norm3) :
                        normType == NORM_L1 ? norm1 + norm2 + norm3 :
                        normType == NORM_L2 || normType == NORM_L2SQR ? std::sqrt(norm1 * norm1 + norm2 * norm2 + norm3 * norm3) :
                        0;
                    result = (normType == NORM_L2SQR ? (double)(norm * norm) : (double)norm);
                    return true;
                }
            }
        }
        else
        {
            IppiSize sz = { cols*src.channels(), rows };
            int type = src.depth();

            typedef IppStatus (CV_STDCALL* ippiNormFuncHint)(const void *, int, IppiSize, Ipp64f *, IppHintAlgorithm hint);
            typedef IppStatus (CV_STDCALL* ippiNormFuncNoHint)(const void *, int, IppiSize, Ipp64f *);
            ippiNormFuncHint ippiNormHint =
                normType == NORM_L1 ?
                (type == CV_32FC1 ? (ippiNormFuncHint)ippiNorm_L1_32f_C1R :
                0) :
                normType == NORM_L2 || normType == NORM_L2SQR ?
                (type == CV_32FC1 ? (ippiNormFuncHint)ippiNorm_L2_32f_C1R :
                0) : 0;
            ippiNormFuncNoHint ippiNorm =
                normType == NORM_INF ?
                (type == CV_8UC1 ? (ippiNormFuncNoHint)ippiNorm_Inf_8u_C1R :
                type == CV_16UC1 ? (ippiNormFuncNoHint)ippiNorm_Inf_16u_C1R :
                type == CV_16SC1 ? (ippiNormFuncNoHint)ippiNorm_Inf_16s_C1R :
                type == CV_32FC1 ? (ippiNormFuncNoHint)ippiNorm_Inf_32f_C1R :
                0) :
                normType == NORM_L1 ?
                (type == CV_8UC1 ? (ippiNormFuncNoHint)ippiNorm_L1_8u_C1R :
                type == CV_16UC1 ? (ippiNormFuncNoHint)ippiNorm_L1_16u_C1R :
//.........这里部分代码省略.........

void updateMotionHistory( InputArray _silhouette, InputOutputArray _mhi,
                              double timestamp, double duration )
{
    CV_Assert( _silhouette.type() == CV_8UC1 && _mhi.type() == CV_32FC1 );
    CV_Assert( _silhouette.sameSize(_mhi) );

    float ts = (float)timestamp;
    float delbound = (float)(timestamp - duration);

    CV_OCL_RUN(_mhi.isUMat() && _mhi.dims() <= 2,
               ocl_updateMotionHistory(_silhouette, _mhi, ts, delbound))

    Mat silh = _silhouette.getMat(), mhi = _mhi.getMat();
    Size size = silh.size();
#if defined(HAVE_IPP)
    int silhstep = (int)silh.step, mhistep = (int)mhi.step;
#endif

    if( silh.isContinuous() && mhi.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
#if defined(HAVE_IPP)
        silhstep = (int)silh.total();
        mhistep = (int)mhi.total() * sizeof(Ipp32f);
#endif
    }

#if defined(HAVE_IPP)
    IppStatus status = ippiUpdateMotionHistory_8u32f_C1IR((const Ipp8u *)silh.data, silhstep, (Ipp32f *)mhi.data, mhistep,
                                                          ippiSize(size.width, size.height), (Ipp32f)timestamp, (Ipp32f)duration);
    if (status >= 0)
        return;
#endif

#if CV_SSE2
    volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
#endif

    for(int y = 0; y < size.height; y++ )
    {
        const uchar* silhData = silh.ptr<uchar>(y);
        float* mhiData = mhi.ptr<float>(y);
        int x = 0;

#if CV_SSE2
        if( useSIMD )
        {
            __m128 ts4 = _mm_set1_ps(ts), db4 = _mm_set1_ps(delbound);
            for( ; x <= size.width - 8; x += 8 )
            {
                __m128i z = _mm_setzero_si128();
                __m128i s = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(silhData + x)), z);
                __m128 s0 = _mm_cvtepi32_ps(_mm_unpacklo_epi16(s, z)), s1 = _mm_cvtepi32_ps(_mm_unpackhi_epi16(s, z));
                __m128 v0 = _mm_loadu_ps(mhiData + x), v1 = _mm_loadu_ps(mhiData + x + 4);
                __m128 fz = _mm_setzero_ps();

                v0 = _mm_and_ps(v0, _mm_cmpge_ps(v0, db4));
                v1 = _mm_and_ps(v1, _mm_cmpge_ps(v1, db4));

                __m128 m0 = _mm_and_ps(_mm_xor_ps(v0, ts4), _mm_cmpneq_ps(s0, fz));
                __m128 m1 = _mm_and_ps(_mm_xor_ps(v1, ts4), _mm_cmpneq_ps(s1, fz));

                v0 = _mm_xor_ps(v0, m0);
                v1 = _mm_xor_ps(v1, m1);

                _mm_storeu_ps(mhiData + x, v0);
                _mm_storeu_ps(mhiData + x + 4, v1);
            }
        }
#endif

        for( ; x < size.width; x++ )
        {
            float val = mhiData[x];
            val = silhData[x] ? ts : val < delbound ? 0 : val;
            mhiData[x] = val;
        }
    }
}

void cv::filterSpeckles( Mat& img, uchar newVal, int maxSpeckleSize, uchar maxDiff, Mat& _buf)
{
    int MaxD = 1024;
    int WinSz = 64;

    int bufSize0 = (MaxD + 2)*sizeof(int) + (img.rows+WinSz+2)*MaxD*sizeof(int) +
        (img.rows + WinSz + 2)*sizeof(int) +
        (img.rows+WinSz+2)*MaxD*(WinSz+1)*sizeof(uchar) + 256;
    int bufSize1 = (img.cols + 9 + 2) * sizeof(int) + 256;
    int bufSz = max(bufSize0 * 1, bufSize1 * 2);

    _buf.create(1, bufSz, CV_8U);

    CV_Assert( img.type() == CV_8U );

    int width = img.cols, height = img.rows, npixels = width*height;
    size_t bufSize = npixels*(int)(sizeof(Point2s) + sizeof(int) + sizeof(uchar));
    if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize )
        _buf.create(1, bufSize, CV_8U);

    uchar* buf = _buf.data;
    int i, j, dstep = img.step/sizeof(uchar);
    int* labels = (int*)buf;
    buf += npixels*sizeof(labels[0]);
    Point2s* wbuf = (Point2s*)buf;
    buf += npixels*sizeof(wbuf[0]);
    uchar* rtype = (uchar*)buf;
    int curlabel = 0;

    // clear out label assignments
    memset(labels, 0, npixels*sizeof(labels[0]));

    for( i = 0; i < height; i++ )
    {
        uchar* ds = img.ptr<uchar>(i);
        int* ls = labels + width*i;

        for( j = 0; j < width; j++ )
        {
            if( ds[j] != newVal )	// not a bad disparity
            {
                if( ls[j] )		// has a label, check for bad label
                {  
                    if( rtype[ls[j]] ) // small region, zero out disparity
                        ds[j] = (uchar)newVal;
                }
                // no label, assign and propagate
                else
                {
                    Point2s* ws = wbuf;	// initialize wavefront
                    Point2s p((short)j, (short)i);	// current pixel
                    curlabel++;	// next label
                    int count = 0;	// current region size
                    ls[j] = curlabel;

                    // wavefront propagation
                    while( ws >= wbuf ) // wavefront not empty
                    {
                        count++;
                        // put neighbors onto wavefront
                        uchar* dpp = &img.at<uchar>(p.y, p.x);
                        uchar dp = *dpp;
                        int* lpp = labels + width*p.y + p.x;

                        if( p.x < width-1 && !lpp[+1] && dpp[+1] != newVal && std::abs(dp - dpp[+1]) <= maxDiff )
                        {
                            lpp[+1] = curlabel;
                            *ws++ = Point2s(p.x+1, p.y);
                        }

                        if( p.x > 0 && !lpp[-1] && dpp[-1] != newVal && std::abs(dp - dpp[-1]) <= maxDiff )
                        {
                            lpp[-1] = curlabel;
                            *ws++ = Point2s(p.x-1, p.y);
                        }

                        if( p.y < height-1 && !lpp[+width] && dpp[+dstep] != newVal && std::abs(dp - dpp[+dstep]) <= maxDiff )
                        {
                            lpp[+width] = curlabel;
                            *ws++ = Point2s(p.x, p.y+1);
                        }

                        if( p.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff )
                        {
                            lpp[-width] = curlabel;
                            *ws++ = Point2s(p.x, p.y-1);
                        }

                        // pop most recent and propagate
                        // NB: could try least recent, maybe better convergence
                        p = *--ws;
                    }

                    // assign label type
                    if( count <= maxSpeckleSize )	// speckle region
                    {
                        //printf("count = %d\n", count);
                        rtype[ls[j]] = 1;	// small region label
                        ds[j] = (uchar)newVal;
                    }
//.........这里部分代码省略.........