自学教程：C++ CV_ASSERT函数代码示例

  CvLabel cvGetLabel(IplImage const *img, unsigned int x, unsigned int y)  {    CV_FUNCNAME("cvGetLabel");    __CV_BEGIN__;    {      CV_ASSERT(img&&(img->depth==IPL_DEPTH_LABEL)&&(img->nChannels==1));      int step = img->widthStep / (img->depth / 8);      int img_width = 0;      int img_height= 0;      int img_offset = 0;      if(img->roi)      {	img_width = img->roi->width;	img_height = img->roi->height;	img_offset = img->roi->xOffset + (img->roi->yOffset * step);      }      else      {	img_width = img->width;	img_height= img->height;      }      CV_ASSERT((x>=0)&&(x<img_width)&&(y>=0)&&(y<img_height));      return ((CvLabel *)(img->imageData + img_offset))[x + y*step];    }    __CV_END__;  }

  void cvRenderBlobs(const IplImage *imgLabel, CvBlobs &blobs, IplImage *imgSource, IplImage *imgDest, unsigned short mode, double alpha)  {    CV_FUNCNAME("cvRenderBlobs");    __CV_BEGIN__;    {      CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1));      CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));      Palete pal;      if (mode&CV_BLOB_RENDER_COLOR)      {	unsigned int colorCount = 0;	for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)	{	  CvLabel label = (*it).second->label;	  double r, g, b;	  _HSV2RGB_((double)((colorCount*77)%360), .5, 1., r, g, b);	  colorCount++;	  pal[label] = CV_RGB(r, g, b);	}      }      for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it)	cvRenderBlob(imgLabel, (*it).second, imgSource, imgDest, mode, pal[(*it).second->label], alpha);    }    __CV_END__;  }

  CvContourPolygon *cvSimplifyPolygon(CvContourPolygon const *p, double const delta)  {    CV_FUNCNAME("cvSimplifyPolygon");    __CV_BEGIN__;    {      CV_ASSERT(p!=NULL);      CV_ASSERT(p->size()>2);      double furtherDistance=0.;      unsigned int furtherIndex=0;      CvContourPolygon::const_iterator it=p->begin();      ++it;      for (unsigned int i=1; it!=p->end(); ++it, i++)      {	double d = cvDistancePointPoint(*it, p->front());	if (d>furtherDistance)	{	  furtherDistance = d;	  furtherIndex = i;	}      }      if (furtherDistance<delta)      {	CvContourPolygon *result = new CvContourPolygon;	result->push_back(p->front());	return result;      }      bool *pnUseFlag = new bool[p->size()];      for (int i=1; i<p->size(); i++) pnUseFlag[i] = false;      pnUseFlag[0] = pnUseFlag[furtherIndex] = true;      simplifyPolygonRecursive(p, 0, furtherIndex, pnUseFlag, delta);      simplifyPolygonRecursive(p, furtherIndex, -1, pnUseFlag, delta);      CvContourPolygon *result = new CvContourPolygon;      for (int i=0; i<p->size(); i++)	if (pnUseFlag[i])	  result->push_back((*p)[i]);      delete[] pnUseFlag;      return result;    }    __CV_END__;  }

  void cvFilterLabels(IplImage *imgIn, IplImage *imgOut, const CvBlobs &blobs)  {    CV_FUNCNAME("cvFilterLabels");    __CV_BEGIN__;    {      CV_ASSERT(imgIn&&(imgIn->depth==IPL_DEPTH_LABEL)&&(imgIn->nChannels==1));      CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_8U)&&(imgOut->nChannels==1));      int stepIn = imgIn->widthStep / (imgIn->depth / 8);      int stepOut = imgOut->widthStep / (imgOut->depth / 8);      int imgIn_width = imgIn->width;      int imgIn_height = imgIn->height;      int imgIn_offset = 0;      int imgOut_width = imgOut->width;      int imgOut_height = imgOut->height;      int imgOut_offset = 0;      if(imgIn->roi)      {	imgIn_width = imgIn->roi->width;	imgIn_height = imgIn->roi->height;	imgIn_offset = imgIn->roi->xOffset + (imgIn->roi->yOffset * stepIn);      }      if(imgOut->roi)      {	imgOut_width = imgOut->roi->width;	imgOut_height = imgOut->roi->height;	imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut);      }      char *imgDataOut=imgOut->imageData + imgOut_offset;      CvLabel *imgDataIn=(CvLabel *)imgIn->imageData + imgIn_offset;      for (unsigned int r=0;r<(unsigned int)imgIn_height;r++,	  imgDataIn+=stepIn,imgDataOut+=stepOut)      {	for (unsigned int c=0;c<(unsigned int)imgIn_width;c++)	{	  if (imgDataIn[c])	  {	    if (blobs.find(imgDataIn[c])==blobs.end()) imgDataOut[c]=0x00;	    else imgDataOut[c]=(char)0xff;	  }	  else	    imgDataOut[c]=0x00;	}      }    }    __CV_END__;  }

  void cvRenderContourPolygon(CvContourPolygon const *contour, IplImage *img, CvScalar const &color)  {    CV_FUNCNAME("cvRenderContourPolygon");    __CV_BEGIN__;    {      CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==3));      CvContourPolygon::const_iterator it=contour->begin();      if (it!=contour->end())      {	unsigned int fx, x, fy, y;	fx = x = it->x;	fy = y = it->y;	for (; it!=contour->end(); ++it)	{	  cvLine(img, cvPoint(x, y), cvPoint(it->x, it->y), color, 1);	  x = it->x;	  y = it->y;	}	cvLine(img, cvPoint(x, y), cvPoint(fx, fy), color, 1);      }    }    __CV_END__;  }

  double cvContourPolygonArea(CvContourPolygon const *p)  {    CV_FUNCNAME("cvContourPolygonArea");    __CV_BEGIN__;    {      CV_ASSERT(p!=NULL);      if (p->size()<=2)	return 1.;      CvContourPolygon::const_iterator it=p->begin();      CvPoint lastPoint = p->back();      double a = 0.;      for (; it!=p->end(); ++it)      {	a += lastPoint.x*it->y - lastPoint.y*it->x;	lastPoint = *it;      }      return a*0.5;    }    __CV_END__;  }

  double cvContourPolygonPerimeter(CvContourPolygon const *p)  {    CV_FUNCNAME("cvContourPolygonPerimeter");    __CV_BEGIN__;    {      CV_ASSERT(p!=NULL);      CV_ASSERT(p->size()>2);      double perimeter = cvDistancePointPoint((*p)[p->size()-1], (*p)[0]);      for (int i=0; i<p->size()-1; i++)	perimeter+=cvDistancePointPoint((*p)[i], (*p)[i+1]);      return perimeter;    }    __CV_END__;  }

//! assuming row vectors (a row is a sample)void cvSoftmax(CvMat * src, CvMat * dst){  CV_FUNCNAME("cvSoftmax");  __BEGIN__;  CV_ASSERT(cvCountNAN(src)<1);  cvExp(src,dst);  CV_ASSERT(cvCountNAN(dst)<1);  const int dtype = CV_MAT_TYPE(src->type);  CvMat * sum = cvCreateMat(src->rows,1,dtype);  CvMat * sum_repeat = cvCreateMat(src->rows,src->cols,dtype);  cvReduce(dst,sum,-1,CV_REDUCE_SUM);  CV_ASSERT(cvCountNAN(sum)<1);  cvRepeat(sum,sum_repeat);  cvDiv(dst,sum_repeat,dst);  cvReleaseMat(&sum);  cvReleaseMat(&sum_repeat);  __END__;}

  // Returns radians  double cvAngle(CvBlob *blob)  {    CV_FUNCNAME("cvAngle");    __CV_BEGIN__;    CV_ASSERT(blob->centralMoments);    return .5*atan2(2.*blob->u11,(blob->u20-blob->u02));    __CV_END__;  }

  void cvRenderBlobs(const IplImage *imgLabel, CvBlob blob, IplImage *imgSource, IplImage *imgDest, unsigned short mode, double alpha)  {    CV_FUNCNAME("cvRenderBlobs");    __CV_BEGIN__;    {      CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1));      CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));      Palete pal;      if (mode&CV_BLOB_RENDER_COLOR)      {	  CvLabel label = blob.label;	  pal[label] = CV_RGB(255, 255, 255);      }	cvRenderBlob(imgLabel, &blob, imgSource, imgDest, mode, pal[blob.label], alpha);    }    __CV_END__;  }

  double cvContourPolygonCircularity(const CvContourPolygon *p)  {    CV_FUNCNAME("cvContourPolygonCircularity");    __CV_BEGIN__;    {      CV_ASSERT(p!=NULL);      double l = cvContourPolygonPerimeter(p);      double c = (l*l/cvContourPolygonArea(p)) - 4.*pi;      if (c>=0.)        return c;      else // This could happen if the blob it's only a pixel: the perimeter will be 0. Another solution would be to force "cvContourPolygonPerimeter" to be 1 or greater.        return 0.;    }    __CV_END__;  }

  void cvCentralMoments(CvBlob *blob, const IplImage *img)  {    CV_FUNCNAME("cvCentralMoments");    __CV_BEGIN__;    if (!blob->centralMoments)    {      CV_ASSERT(img&&(img->depth==IPL_DEPTH_LABEL)&&(img->nChannels==1));      //cvCentroid(blob); // Here?      blob->u11=blob->u20=blob->u02=0.;      // Only in the bounding box      int stepIn = img->widthStep / (img->depth / 8);      int img_width = img->width;      int img_height = img->height;      int img_offset = 0;      if(0 != img->roi)      {	img_width = img->roi->width;	img_height = img->roi->height;	img_offset = img->roi->xOffset + (img->roi->yOffset * stepIn);      }      CvLabel *imgData=(CvLabel *)img->imageData + (blob->miny * stepIn) + img_offset;      for (unsigned int r=blob->miny;	  r<blob->maxy;	  r++,imgData+=stepIn)	for (unsigned int c=blob->minx;c<blob->maxx;c++)	  if (imgData[c]==blob->label)	  {	    double tx=(c-blob->centroid.x);	    double ty=(r-blob->centroid.y);	    blob->u11+=tx*ty;	    blob->u20+=tx*tx;	    blob->u02+=ty*ty;	  }      blob->centralMoments = true;    }    __CV_END__;  }

  double cvContourChainCodePerimeter(CvContourChainCode const *c)  {    CV_FUNCNAME("cvContourChainCodePerimeter");    __CV_BEGIN__;    {      CV_ASSERT(c!=NULL);      double perimeter = 0.;      for(CvChainCodes::const_iterator it=c->chainCode.begin(); it!=c->chainCode.end(); ++it)      {	if ((*it)%2)	  perimeter+=sqrt(1.+1.);	else	  perimeter+=1.;      }      return perimeter;    }    __CV_END__;  }

  CvContourPolygon *cvConvertChainCodesToPolygon(CvContourChainCode const *cc)  {    CV_FUNCNAME("cvConvertChainCodesToPolygon");    __CV_BEGIN__;    {      CV_ASSERT(cc!=NULL);      CvContourPolygon *contour = new CvContourPolygon;      unsigned int x = cc->startingPoint.x;      unsigned int y = cc->startingPoint.y;      contour->push_back(cvPoint(x, y));      if (cc->chainCode.size())      {        CvChainCodes::const_iterator it=cc->chainCode.begin();        CvChainCode lastCode = *it;        x += cvChainCodeMoves[*it][0];        y += cvChainCodeMoves[*it][1];        ++it;        for (; it!=cc->chainCode.end(); ++it)        {          if (lastCode!=*it)          {            contour->push_back(cvPoint(x, y));            lastCode=*it;          }          x += cvChainCodeMoves[*it][0];          y += cvChainCodeMoves[*it][1];        }      }      return contour;    }    __CV_END__;  }

CvMat * icvConvertLabels(CvMat * src){  CV_FUNCNAME("icvConvertLabels");  CvMat * dst = 0;  __BEGIN__;  const int nsamples = src->rows;  const int ntargets = 5;  const int nclasses = 10; // decimal  const int nparams = 4;  CV_ASSERT(CV_MAT_TYPE(src->type)==CV_32S);  dst = cvCreateMat(nsamples,(ntargets+1)*nclasses,CV_32F); cvZero(dst);  for (int ii=0;ii<nsamples;ii++){  int nlabels = CV_MAT_ELEM(*src,int,ii,0);  CV_MAT_ELEM(*dst,float,ii,nlabels)=1;  for (int jj=0;jj<nlabels;jj++){    int label = CV_MAT_ELEM(*src,int,ii,1+nparams*jj+3); // label    CV_MAT_ELEM(*dst,float,ii,10+nclasses*jj+label)=1;  }  }  __END__;  return dst;}

  void cvRenderContourChainCode(CvContourChainCode const *contour, IplImage const *img, CvScalar const &color)  {    CV_FUNCNAME("cvRenderContourChainCode");    __CV_BEGIN__;    {      CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==3));      int stepDst = img->widthStep/(img->depth/8);      int img_width = img->width;      int img_height = img->height;      int img_offset = 0;      if(img->roi)      {	img_width = img->roi->width;	img_height = img->roi->height;	img_offset = (img->nChannels * img->roi->xOffset) + (img->roi->yOffset * stepDst);      }      unsigned char *imgData = (unsigned char *)img->imageData + img_offset;      unsigned int x = contour->startingPoint.x;      unsigned int y = contour->startingPoint.y;      for (CvChainCodes::const_iterator it=contour->chainCode.begin(); it!=contour->chainCode.end(); ++it)      {	imgData[img->nChannels*x+img->widthStep*y+0] = (unsigned char)(color.val[0]); // Blue	imgData[img->nChannels*x+img->widthStep*y+1] = (unsigned char)(color.val[1]); // Green	imgData[img->nChannels*x+img->widthStep*y+2] = (unsigned char)(color.val[2]); // Red	x += cvChainCodeMoves[*it][0];	y += cvChainCodeMoves[*it][1];      }    }    __CV_END__;  }

voidcvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs,    const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr ){    CV_FUNCNAME( "cvInitUndistortMap" );    __BEGIN__;        double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0};    int coi1 = 0, coi2 = 0;    CvMat mapxstub, *_mapx = (CvMat*)mapxarr;    CvMat mapystub, *_mapy = (CvMat*)mapyarr;    CvMat _a = cvMat( 3, 3, CV_64F, a );    CvMat _k = cvMat( 4, 1, CV_64F, k );    CvMat _ar = cvMat( 3, 3, CV_64F, ar );    CvMat _r = cvMat( 3, 3, CV_64F, r );    CvMat _ir = cvMat( 3, 3, CV_64F, ir );    int i, j;    double fx, fy, u0, v0, k1, k2, k3, p1, p2;    CvSize size;    CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));    CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));    if( coi1 != 0 || coi2 != 0 )        CV_ERROR( CV_BadCOI, "The function does not support COI" );    if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )        CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );    if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))        CV_ERROR( CV_StsUnmatchedFormats, "" );    if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))        CV_ERROR( CV_StsUnmatchedSizes, "" );    if( A )    {        if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3  ||            (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) )            CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );        cvConvert( A, &_a );    }    else        cvSetIdentity( &_a );    if( Ar )    {        CvMat Ar33;        if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) ||            (CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) )            CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" );        cvGetCols( Ar, &Ar33, 0, 3 );        cvConvert( &Ar33, &_ar );    }    else        cvSetIdentity( &_ar );    if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3  ||        (CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) )        CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" );    if( distCoeffs )    {        CV_ASSERT( CV_IS_MAT(distCoeffs) &&            (distCoeffs->rows == 1 || distCoeffs->cols == 1) &&            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 ||            distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) &&            (CV_MAT_DEPTH(distCoeffs->type) == CV_64F ||            CV_MAT_DEPTH(distCoeffs->type) == CV_32F) );        _k = cvMat( distCoeffs->rows, distCoeffs->cols,                CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k );        cvConvert( distCoeffs, &_k );    }    else        cvZero( &_k );        cvConvert( R, &_r );    // rectification matrix    cvMatMul( &_ar, &_r, &_r ); // Ar*R    cvInvert( &_r, &_ir );  // inverse: R^-1*Ar^-1    u0 = a[2]; v0 = a[5];    fx = a[0]; fy = a[4];    k1 = k[0]; k2 = k[1]; k3 = k[4];    p1 = k[2]; p2 = k[3];    size = cvGetMatSize(_mapx);    for( i = 0; i < size.height; i++ )    {        float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i);        float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i);        double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];        for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )        {            double w = 1./_w, x = _x*w, y = _y*w;            double x2 = x*x, y2 = y*y;            double r2 = x2 + y2, _2xy = 2*x*y;            double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;//.........这里部分代码省略.........

voidcvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,                   const CvMat* _distCoeffs,                   const CvMat* _R, const CvMat* _P ){    CV_FUNCNAME( "cvUndistortPoints" );    __BEGIN__;    double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;    CvMat _A=cvMat(3, 3, CV_64F, A), _Dk;    CvMat _RR=cvMat(3, 3, CV_64F, RR);    const CvPoint2D32f* srcf;    const CvPoint2D64f* srcd;    CvPoint2D32f* dstf;    CvPoint2D64f* dstd;    int stype, dtype;    int sstep, dstep;    int i, j, n;    CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&        (_src->rows == 1 || _src->cols == 1) &&        (_dst->rows == 1 || _dst->cols == 1) &&        CV_ARE_SIZES_EQ(_src, _dst) &&        (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&        (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));    CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) &&        _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 &&        (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&        (_distCoeffs->rows*_distCoeffs->cols == 4 ||        _distCoeffs->rows*_distCoeffs->cols == 5) );    _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,        CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);    cvConvert( _cameraMatrix, &_A );    cvConvert( _distCoeffs, &_Dk );    if( _R )    {        CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 );        cvConvert( _R, &_RR );    }    else        cvSetIdentity(&_RR);    if( _P )    {        double PP[3][3];        CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);        CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4));        cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP );        cvMatMul( &_PP, &_RR, &_RR );    }        srcf = (const CvPoint2D32f*)_src->data.ptr;    srcd = (const CvPoint2D64f*)_src->data.ptr;    dstf = (CvPoint2D32f*)_dst->data.ptr;    dstd = (CvPoint2D64f*)_dst->data.ptr;    stype = CV_MAT_TYPE(_src->type);    dtype = CV_MAT_TYPE(_dst->type);    sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);    dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);        n = _src->rows + _src->cols - 1;    fx = A[0][0];    fy = A[1][1];    ifx = 1./fx;    ify = 1./fy;    cx = A[0][2];    cy = A[1][2];    for( i = 0; i < n; i++ )    {        double x, y, x0, y0;        if( stype == CV_32FC2 )        {            x = srcf[i*sstep].x;            y = srcf[i*sstep].y;        }        else        {            x = srcd[i*sstep].x;            y = srcd[i*sstep].y;        }                x0 = x = (x - cx)*ifx;        y0 = y = (y - cy)*ify;        // compensate distortion iteratively        for( j = 0; j < 5; j++ )        {            double r2 = x*x + y*y;            double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);            double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);            double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;            x = (x0 - deltaX)*icdist;            y = (y0 - deltaY)*icdist;        }        //.........这里部分代码省略.........

void CvEM::init_em( const CvVectors& train_data ){    CvMat *w = 0, *u = 0, *tcov = 0;    CV_FUNCNAME( "CvEM::init_em" );    __BEGIN__;    double maxval = 0;    int i, force_symm_plus = 0;    int nclusters = params.nclusters, nsamples = train_data.count, dims = train_data.dims;    if( params.start_step == START_AUTO_STEP || nclusters == 1 || nclusters == nsamples )        init_auto( train_data );    else if( params.start_step == START_M_STEP )    {        for( i = 0; i < nsamples; i++ )        {            CvMat prob;            cvGetRow( params.probs, &prob, i );            cvMaxS( &prob, 0., &prob );            cvMinMaxLoc( &prob, 0, &maxval );            if( maxval < FLT_EPSILON )                cvSet( &prob, cvScalar(1./nclusters) );            else                cvNormalize( &prob, &prob, 1., 0, CV_L1 );        }        EXIT; // do not preprocess covariation matrices,              // as in this case they are initialized at the first iteration of EM    }    else    {        CV_ASSERT( params.start_step == START_E_STEP && params.means );        if( params.weights && params.covs )        {            cvConvert( params.means, means );            cvReshape( weights, weights, 1, params.weights->rows );            cvConvert( params.weights, weights );            cvReshape( weights, weights, 1, 1 );            cvMaxS( weights, 0., weights );            cvMinMaxLoc( weights, 0, &maxval );            if( maxval < FLT_EPSILON )                cvSet( weights, cvScalar(1./nclusters) );            cvNormalize( weights, weights, 1., 0, CV_L1 );            for( i = 0; i < nclusters; i++ )                CV_CALL( cvConvert( params.covs[i], covs[i] ));            force_symm_plus = 1;        }        else            init_auto( train_data );    }    CV_CALL( tcov = cvCreateMat( dims, dims, CV_64FC1 ));    CV_CALL( w = cvCreateMat( dims, dims, CV_64FC1 ));    if( params.cov_mat_type == COV_MAT_GENERIC )        CV_CALL( u = cvCreateMat( dims, dims, CV_64FC1 ));    for( i = 0; i < nclusters; i++ )    {        if( force_symm_plus )        {            cvTranspose( covs[i], tcov );            cvAddWeighted( covs[i], 0.5, tcov, 0.5, 0, tcov );        }        else            cvCopy( covs[i], tcov );        cvSVD( tcov, w, u, 0, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );        if( params.cov_mat_type == COV_MAT_SPHERICAL )            cvSetIdentity( covs[i], cvScalar(cvTrace(w).val[0]/dims) );        else if( params.cov_mat_type == COV_MAT_DIAGONAL )            cvCopy( w, covs[i] );        else        {            // generic case: covs[i] = (u')'*max(w,0)*u'            cvGEMM( u, w, 1, 0, 0, tcov, CV_GEMM_A_T );            cvGEMM( tcov, u, 1, 0, 0, covs[i], 0 );        }    }    __END__;    cvReleaseMat( &w );    cvReleaseMat( &u );    cvReleaseMat( &tcov );}

  void cvRenderBlob(const IplImage *imgLabel, CvBlob *blob, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvScalar const &color, double alpha)  {    CV_FUNCNAME("cvRenderBlob");    __CV_BEGIN__;    CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1));    CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));    if (mode&CV_BLOB_RENDER_COLOR)    {      int stepLbl = imgLabel->widthStep/(imgLabel->depth/8);      int stepSrc = imgSource->widthStep/(imgSource->depth/8);      int stepDst = imgDest->widthStep/(imgDest->depth/8);      int imgLabel_width = imgLabel->width;      int imgLabel_height = imgLabel->height;      int imgLabel_offset = 0;      int imgSource_width = imgSource->width;      int imgSource_height = imgSource->height;      int imgSource_offset = 0;      int imgDest_width = imgDest->width;      int imgDest_height = imgDest->height;      int imgDest_offset = 0;      if(imgLabel->roi)      {	imgLabel_width = imgLabel->roi->width;	imgLabel_height = imgLabel->roi->height;	imgLabel_offset = (imgLabel->nChannels * imgLabel->roi->xOffset) + (imgLabel->roi->yOffset * stepLbl);      }      if(imgSource->roi)      {	imgSource_width = imgSource->roi->width;	imgSource_height = imgSource->roi->height;	imgSource_offset = (imgSource->nChannels * imgSource->roi->xOffset) + (imgSource->roi->yOffset * stepSrc);      }      if(imgDest->roi)      {	imgDest_width = imgDest->roi->width;	imgDest_height = imgDest->roi->height;	imgDest_offset = (imgDest->nChannels * imgDest->roi->xOffset) + (imgDest->roi->yOffset * stepDst);      }      CvLabel *labels = (CvLabel *)imgLabel->imageData + imgLabel_offset + (blob->miny * stepLbl);      unsigned char *source = (unsigned char *)imgSource->imageData + imgSource_offset + (blob->miny * stepSrc);      unsigned char *imgData = (unsigned char *)imgDest->imageData + imgDest_offset + (blob->miny * stepDst);      for (unsigned int r=blob->miny; r<blob->maxy; r++, labels+=stepLbl, source+=stepSrc, imgData+=stepDst)	for (unsigned int c=blob->minx; c<blob->maxx; c++)	{	  if (labels[c]==blob->label)	  {	    imgData[imgDest->nChannels*c+0] = (unsigned char)((1.-alpha)*source[imgSource->nChannels*c+0]+alpha*color.val[0]);	    imgData[imgDest->nChannels*c+1] = (unsigned char)((1.-alpha)*source[imgSource->nChannels*c+1]+alpha*color.val[1]);	    imgData[imgDest->nChannels*c+2] = (unsigned char)((1.-alpha)*source[imgSource->nChannels*c+2]+alpha*color.val[2]);	  }	}    }    if (mode)    {      if (mode&CV_BLOB_RENDER_TO_LOG)      {	std::clog << "Blob " << blob->label << std::endl;	std::clog << " - Bounding box: (" << blob->minx << ", " << blob->miny << ") - (" << blob->maxx << ", " << blob->maxy << ")" << std::endl;	std::clog << " - Bounding box area: " << (1 + blob->maxx - blob->minx) * (1 + blob->maxy - blob->miny) << std::endl;	std::clog << " - Area: " << blob->area << std::endl;	std::clog << " - Centroid: (" << blob->centroid.x << ", " << blob->centroid.y << ")" << std::endl;	std::clog << std::endl;      }      if (mode&CV_BLOB_RENDER_TO_STD)      {	std::cout << "Blob " << blob->label << std::endl;	std::cout << " - Bounding box: (" << blob->minx << ", " << blob->miny << ") - (" << blob->maxx << ", " << blob->maxy << ")" << std::endl;	std::cout << " - Bounding box area: " << (1 + blob->maxx - blob->minx) * (1 + blob->maxy - blob->miny) << std::endl;	std::cout << " - Area: " << blob->area << std::endl;	std::cout << " - Centroid: (" << blob->centroid.x << ", " << blob->centroid.y << ")" << std::endl;	std::cout << std::endl;      }      if (mode&CV_BLOB_RENDER_BOUNDING_BOX)	cvRectangle(imgDest, cvPoint(blob->minx, blob->miny), cvPoint(blob->maxx-1, blob->maxy-1), CV_RGB(255., 0., 0.));      if (mode&CV_BLOB_RENDER_ANGLE)      {	double angle = cvAngle(blob);	double x1,y1,x2,y2;	double lengthLine = MAX(blob->maxx-blob->minx, blob->maxy-blob->miny)/2.;	x1=blob->centroid.x-lengthLine*cos(angle);	y1=blob->centroid.y-lengthLine*sin(angle);	x2=blob->centroid.x+lengthLine*cos(angle);	y2=blob->centroid.y+lengthLine*sin(angle);	cvLine(imgDest,cvPoint(int(x1),int(y1)),cvPoint(int(x2),int(y2)),CV_RGB(0.,255.,0.));      }      if (mode&CV_BLOB_RENDER_CENTROID)      {	cvLine(imgDest,cvPoint(int(blob->centroid.x)-3,int(blob->centroid.y)),cvPoint(int(blob->centroid.x)+3,int(blob->centroid.y)),CV_RGB(0.,0.,255.));	cvLine(imgDest,cvPoint(int(blob->centroid.x),int(blob->centroid.y)-3),cvPoint(int(blob->centroid.x),int(blob->centroid.y)+3),CV_RGB(0.,0.,255.));//.........这里部分代码省略.........

  CvScalar cvBlobMeanColor(CvBlob const *blob, IplImage const *imgLabel, IplImage const *img)  {    CV_FUNCNAME("cvBlobMeanColor");    __CV_BEGIN__;    {      CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1));      CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==3));      int stepLbl = imgLabel->widthStep/(imgLabel->depth/8);      int stepImg = img->widthStep/(img->depth/8);      int imgLabel_width = imgLabel->width;      int imgLabel_height = imgLabel->height;      int imgLabel_offset = 0;      int img_width = img->width;      int img_height = img->height;      int img_offset = 0;      if(imgLabel->roi)      {        imgLabel_width = imgLabel->roi->width;        imgLabel_height = imgLabel->roi->height;        imgLabel_offset = (imgLabel->nChannels * imgLabel->roi->xOffset) + (imgLabel->roi->yOffset * stepLbl);      }      if(img->roi)      {        img_width = img->roi->width;        img_height = img->roi->height;        img_offset = (img->nChannels * img->roi->xOffset) + (img->roi->yOffset * stepImg);      }      CvLabel *labels = (CvLabel *)imgLabel->imageData + imgLabel_offset;      unsigned char *imgData = (unsigned char *)img->imageData + img_offset;      double mb = 0;      double mg = 0;      double mr = 0;      double pixels = (double)blob->area;      for (unsigned int r=0; r<(unsigned int)imgLabel_height; r++, labels+=stepLbl, imgData+=stepImg)        for (unsigned int c=0; c<(unsigned int)imgLabel_width; c++)        {          if (labels[c]==blob->label)          {            mb += ((double)imgData[img->nChannels*c+0])/pixels; // B            mg += ((double)imgData[img->nChannels*c+1])/pixels; // G            mr += ((double)imgData[img->nChannels*c+2])/pixels; // R          }        }        /*double mb = 0;        double mg = 0;        double mr = 0;        double pixels = (double)blob->area;        for (unsigned int y=0; y<imgLabel->height; y++)        for (unsigned int x=0; x<imgLabel->width; x++)        {        if (cvGetLabel(imgLabel, x, y)==blob->label)        {        CvScalar color = cvGet2D(img, y, x);        mb += color.val[0]/pixels;        mg += color.val[1]/pixels;        mr += color.val[2]/pixels;        }        }*/        return cvScalar(mr, mg, mb);    }    __CV_END__;  }

  unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs)  {    CV_FUNCNAME("cvLabel");    __CV_BEGIN__;    {      CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==1));      CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_LABEL)&&(imgOut->nChannels==1));      unsigned int numPixels=0;      cvSetZero(imgOut);      CvLabel label=0;      cvReleaseBlobs(blobs);      unsigned int stepIn = img->widthStep / (img->depth / 8);      unsigned int stepOut = imgOut->widthStep / (imgOut->depth / 8);      unsigned int imgIn_width = img->width;      unsigned int imgIn_height = img->height;      unsigned int imgIn_offset = 0;      unsigned int imgOut_width = imgOut->width;      unsigned int imgOut_height = imgOut->height;      unsigned int imgOut_offset = 0;      if(img->roi)      {	imgIn_width = img->roi->width;	imgIn_height = img->roi->height;	imgIn_offset = img->roi->xOffset + (img->roi->yOffset * stepIn);      }      if(imgOut->roi)      {	imgOut_width = imgOut->roi->width;	imgOut_height = imgOut->roi->height;	imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut);      }      unsigned char *imgDataIn = (unsigned char *)img->imageData + imgIn_offset;      CvLabel *imgDataOut = (CvLabel *)imgOut->imageData + imgOut_offset;#define imageIn(X, Y) imgDataIn[(X) + (Y)*stepIn]#define imageOut(X, Y) imgDataOut[(X) + (Y)*stepOut]      CvLabel lastLabel = 0;      CvBlob *lastBlob = NULL;      for (unsigned int y=0; y<imgIn_height; y++)      {	for (unsigned int x=0; x<imgIn_width; x++)	{	  if (imageIn(x, y))	  {	    bool labeled = imageOut(x, y);	    if ((!imageOut(x, y))&&((y==0)||(!imageIn(x, y-1))))	    {	      labeled = true;	      // Label contour.	      label++;	      CV_ASSERT(label!=CV_BLOB_MAX_LABEL);	      imageOut(x, y) = label;	      numPixels++;	      if (y>0)		imageOut(x, y-1) = CV_BLOB_MAX_LABEL;	      CvBlob *blob = new CvBlob;	      blob->label = label;	      blob->area = 1;	      blob->minx = x; blob->maxx = x;	      blob->miny = y; blob->maxy = y;	      blob->m10=x; blob->m01=y;	      blob->m11=x*y;	      blob->m20=x*x; blob->m02=y*y;	      blob->internalContours.clear();	      blobs.insert(CvLabelBlob(label,blob));              lastLabel = label;	      lastBlob = blob;	      blob->contour.startingPoint = cvPoint(x, y);	      unsigned char direction=1;	      unsigned int xx = x;	      unsigned int yy = y;	      bool contourEnd = false;	      do	      {		for (unsigned int numAttempts=0; numAttempts<3; numAttempts++)		{		  bool found = false;		  for (unsigned char i=0; i<3; i++)		  {		    int nx = xx+movesE[direction][i][0];		    int ny = yy+movesE[direction][i][1];//.........这里部分代码省略.........

void cvBGCodeBookClearStale( CvBGCodeBookModel* model, int staleThresh,                             CvRect roi, const CvArr* _mask ){    CV_FUNCNAME( "cvBGCodeBookClearStale" );    __BEGIN__;    CvMat mstub, *mask = _mask ? cvGetMat( _mask, &mstub ) : 0;    int x, y, T;    CvBGCodeBookElem* freeList;    CV_ASSERT( model && (!mask || (CV_IS_MASK_ARR(mask) &&        mask->cols == model->size.width && mask->rows == model->size.height)) );    if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )    {        roi.width = model->size.width;        roi.height = model->size.height;    }    else        CV_ASSERT( (unsigned)roi.x < (unsigned)mask->cols &&                   (unsigned)roi.y < (unsigned)mask->rows &&                   roi.width >= 0 && roi.height >= 0 &&                   roi.x + roi.width <= mask->cols &&                   roi.y + roi.height <= mask->rows );    icvInitSatTab();    freeList = model->freeList;    T = model->t;    for( y = 0; y < roi.height; y++ )    {        const uchar* m = mask ? mask->data.ptr + mask->step*(y + roi.y) + roi.x : 0;        CvBGCodeBookElem** cb = model->cbmap + model->size.width*(y + roi.y) + roi.x;        for( x = 0; x < roi.width; x++, cb++ )        {            CvBGCodeBookElem *e, first, *prev = &first;            if( m && m[x] == 0 )                continue;            for( first.next = e = *cb; e != 0; e = prev->next )            {                if( e->stale > staleThresh )                {                    prev->next = e->next;                    e->next = freeList;                    freeList = e;                }                else                {                    e->stale = 0;                    e->tLastUpdate = T;                    prev = e;                }            }            *cb = first.next;        }    }    model->freeList = freeList;    __END__;}

int cvBGCodeBookDiff( const CvBGCodeBookModel* model, const CvArr* _image,                      CvArr* _fgmask, CvRect roi ){    int maskCount = -1;    CV_FUNCNAME( "cvBGCodeBookDiff" );    __BEGIN__;    CvMat stub, *image = cvGetMat( _image, &stub );    CvMat mstub, *mask = cvGetMat( _fgmask, &mstub );    int x, y;    uchar m0, m1, m2, M0, M1, M2;    CV_ASSERT( model && CV_MAT_TYPE(image->type) == CV_8UC3 &&        image->cols == model->size.width && image->rows == model->size.height &&        CV_IS_MASK_ARR(mask) && CV_ARE_SIZES_EQ(image, mask) );    if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )    {        roi.width = image->cols;        roi.height = image->rows;    }    else        CV_ASSERT( (unsigned)roi.x < (unsigned)image->cols &&                   (unsigned)roi.y < (unsigned)image->rows &&                   roi.width >= 0 && roi.height >= 0 &&                   roi.x + roi.width <= image->cols &&                   roi.y + roi.height <= image->rows );    m0 = model->modMin[0]; M0 = model->modMax[0];    m1 = model->modMin[1]; M1 = model->modMax[1];    m2 = model->modMin[2]; M2 = model->modMax[2];    maskCount = roi.height*roi.width;    for( y = 0; y < roi.height; y++ )    {        const uchar* p = image->data.ptr + image->step*(y + roi.y) + roi.x*3;        uchar* m = mask->data.ptr + mask->step*(y + roi.y) + roi.x;        CvBGCodeBookElem** cb = model->cbmap + image->cols*(y + roi.y) + roi.x;        for( x = 0; x < roi.width; x++, p += 3, cb++ )        {            CvBGCodeBookElem *e;            uchar p0 = p[0], p1 = p[1], p2 = p[2];            int l0 = p0 + m0, l1 = p1 + m1, l2 = p2 + m2;            int h0 = p0 - M0, h1 = p1 - M1, h2 = p2 - M2;            m[x] = (uchar)255;            for( e = *cb; e != 0; e = e->next )            {                if( e->boxMin[0] <= l0 && h0 <= e->boxMax[0] &&                    e->boxMin[1] <= l1 && h1 <= e->boxMax[1] &&                    e->boxMin[2] <= l2 && h2 <= e->boxMax[2] )                {                    m[x] = 0;                    maskCount--;                    break;                }            }        }    }    __END__;    return maskCount;}

  void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvFont *font)  {    CV_FUNCNAME("cvRenderTracks");    __CV_BEGIN__;    CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));    if ((mode&CV_TRACK_RENDER_ID)&&(!font))    {      if (!defaultFont)      {	font = defaultFont = new CvFont;	hudFont = new CvFont;	cvInitFont(font, CV_FONT_HERSHEY_DUPLEX, 0.5, 0.5, 0, 1);	cvInitFont(hudFont, CV_FONT_HERSHEY_PLAIN, 0.8, 0.8 ,0.5);	// Other fonts:	//   CV_FONT_HERSHEY_SIMPLEX, CV_FONT_HERSHEY_PLAIN,	//   CV_FONT_HERSHEY_DUPLEX, CV_FONT_HERSHEY_COMPLEX,	//   CV_FONT_HERSHEY_TRIPLEX, CV_FONT_HERSHEY_COMPLEX_SMALL,	//   CV_FONT_HERSHEY_SCRIPT_SIMPLEX, CV_FONT_HERSHEY_SCRIPT_COMPLEX      }      else	font = defaultFont;    }    if (mode)    {      for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)      {	if (mode&CV_TRACK_RENDER_ID)	  if (!it->second->inactive)	  {	    stringstream buffer, mouseloc;	    buffer << it->first;	    mouseloc << "Mouse Location : " << "(" << (int)it->second->centroid.x << " , " << (int)it->second->centroid.y << ")";       // // Mouse Location String    //my << it->second->centroid.y;	    //cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.,255.,0.));	    // Drawing functions for the Draw-conture/** C++: void circle(Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0) **//** C: void cvCircle(CvArr* img, CvPoint center, int radius, CvScalar color, int thickness=1, int line_type=8, int shift=0 ) **/	    //cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.0, 0.0, 0.0));	    cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->maxx, (int)it->second->miny), font, CV_RGB(255.0, 255.0, 0.0));   // // puts the blob id on the largest blob	    //cout << buffer.str().c_str() << endl;	    //cvPutText(imgDest, mouseloc.str().c_str(), cvPoint(0, 9), hudFont, CV_RGB(0.0, 230.0, 230.0));	    cvCircle(imgDest, cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), 13, CV_RGB(0.0, 255.0, 255.0), 4, CV_AA, 0);   // // Draws the origin-Circle            cvCircle(imgDest, cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), 4, CV_RGB(130.815, 0.0, 255.0), 1, CV_AA, 0);	    //cout << "Blob minx = " << it->second->minx << endl;	  }	if (mode&CV_TRACK_RENDER_BOUNDING_BOX)	  if (it->second->inactive)	    cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 50.));	  else	    cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 255.));	if (mode&CV_TRACK_RENDER_TO_LOG)	{	  clog << "Track " << it->second->id << endl;	  if (it->second->inactive)	    clog << " - Inactive for " << it->second->inactive << " frames" << endl;	  else        clog << " - Associated with blob " << it->second->label << endl;	  clog << " - Lifetime " << it->second->lifetime << endl;	  clog << " - Active " << it->second->active << endl;	  clog << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;	  clog << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;	  clog << endl;	}	if (mode&CV_TRACK_RENDER_TO_STD)	{	  cout << "Track " << it->second->id << endl;	  if (it->second->inactive)	    cout << " - Inactive for " << it->second->inactive << " frames" << endl;	  else	    cout << " - Associated with blobs " << it->second->label << endl;	  cout << " - Lifetime " << it->second->lifetime << endl;	  cout << " - Active " << it->second->active << endl;	  cout << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;	  cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;	  cout << endl;	}      }    }    __CV_END__;  }

CV_IMPL voidcvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,               CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,               CvPoint3D64f *eulerAngles){    CV_FUNCNAME("cvRQDecomp3x3");    __BEGIN__;    double _M[3][3], _R[3][3], _Q[3][3];    CvMat M = cvMat(3, 3, CV_64F, _M);    CvMat R = cvMat(3, 3, CV_64F, _R);    CvMat Q = cvMat(3, 3, CV_64F, _Q);    double z, c, s;    /* Validate parameters. */    CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&        matrixM->cols == 3 && matrixM->rows == 3 &&        CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));    cvConvert(matrixM, &M);    {    /* Find Givens rotation Q_x for x axis (left multiplication). */    /*         ( 1  0  0 )    Qx = ( 0  c  s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)         ( 0 -s  c )    */    s = _M[2][1];    c = _M[2][2];    z = 1./sqrt(c * c + s * s + DBL_EPSILON);    c *= z;    s *= z;    double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };    CvMat Qx = cvMat(3, 3, CV_64F, _Qx);    cvMatMul(&M, &Qx, &R);    assert(fabs(_R[2][1]) < FLT_EPSILON);    _R[2][1] = 0;    /* Find Givens rotation for y axis. */    /*         ( c  0  s )    Qy = ( 0  1  0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2)         (-s  0  c )    */    s = _R[2][0];    c = _R[2][2];    z = 1./sqrt(c * c + s * s + DBL_EPSILON);    c *= z;    s *= z;    double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} };    CvMat Qy = cvMat(3, 3, CV_64F, _Qy);    cvMatMul(&R, &Qy, &M);    assert(fabs(_M[2][0]) < FLT_EPSILON);    _M[2][0] = 0;    /* Find Givens rotation for z axis. */    /*         ( c  s  0 )    Qz = (-s  c  0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)         ( 0  0  1 )    */    s = _M[1][0];    c = _M[1][1];    z = 1./sqrt(c * c + s * s + DBL_EPSILON);    c *= z;    s *= z;    double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };    CvMat Qz = cvMat(3, 3, CV_64F, _Qz);    cvMatMul(&M, &Qz, &R);    assert(fabs(_R[1][0]) < FLT_EPSILON);    _R[1][0] = 0;    // Solve the decomposition ambiguity.    // Diagonal entries of R, except the last one, shall be positive.    // Further rotate R by 180 degree if necessary    if( _R[0][0] < 0 )    {        if( _R[1][1] < 0 )        {            // rotate around z for 180 degree, i.e. a rotation matrix of            // [-1,  0,  0],            // [ 0, -1,  0],            // [ 0,  0,  1]            _R[0][0] *= -1;            _R[0][1] *= -1;            _R[1][1] *= -1;            _Qz[0][0] *= -1;            _Qz[0][1] *= -1;            _Qz[1][0] *= -1;            _Qz[1][1] *= -1;        }//.........这里部分代码省略.........

  void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvFont *font)  {    CV_FUNCNAME("cvRenderTracks");    __CV_BEGIN__;    CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));    if ((mode&CV_TRACK_RENDER_ID)&&(!font))    {      if (!defaultFont)      {	font = defaultFont = new CvFont;	cvInitFont(font, CV_FONT_HERSHEY_DUPLEX, 0.5, 0.5, 0, 1);	// Other fonts:	//   CV_FONT_HERSHEY_SIMPLEX, CV_FONT_HERSHEY_PLAIN,	//   CV_FONT_HERSHEY_DUPLEX, CV_FONT_HERSHEY_COMPLEX,	//   CV_FONT_HERSHEY_TRIPLEX, CV_FONT_HERSHEY_COMPLEX_SMALL,	//   CV_FONT_HERSHEY_SCRIPT_SIMPLEX, CV_FONT_HERSHEY_SCRIPT_COMPLEX      }      else	font = defaultFont;    }    if (mode)    {      for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)      {	if (mode&CV_TRACK_RENDER_ID)	  if (!it->second->inactive)	  {	    stringstream buffer;	    buffer << it->first;	    cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.,255.,0.));	  }	if (mode&CV_TRACK_RENDER_BOUNDING_BOX)	  if (it->second->inactive)	    cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 50.));	  else	    cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 255.));	if (mode&CV_TRACK_RENDER_TO_LOG)	{	  clog << "Track " << it->second->id << endl;	  if (it->second->inactive)	    clog << " - Inactive for " << it->second->inactive << " frames" << endl;	  else	    clog << " - Associated with blob " << it->second->label << endl;	  clog << " - Lifetime " << it->second->lifetime << endl;	  clog << " - Active " << it->second->active << endl;	  clog << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;	  clog << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;	  clog << endl;	}	if (mode&CV_TRACK_RENDER_TO_STD)	{	  cout << "Track " << it->second->id << endl;	  if (it->second->inactive)	    cout << " - Inactive for " << it->second->inactive << " frames" << endl;	  else	    cout << " - Associated with blobs " << it->second->label << endl;	  cout << " - Lifetime " << it->second->lifetime << endl;	  cout << " - Active " << it->second->active << endl;	  cout << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;	  cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;	  cout << endl;	}      }    }    __CV_END__;  }

void cvBGCodeBookUpdate( CvBGCodeBookModel* model, const CvArr* _image,                         CvRect roi, const CvArr* _mask ){    CV_FUNCNAME( "cvBGCodeBookUpdate" );    __BEGIN__;    CvMat stub, *image = cvGetMat( _image, &stub );    CvMat mstub, *mask = _mask ? cvGetMat( _mask, &mstub ) : 0;    int i, x, y, T;    int nblocks;    uchar cb0, cb1, cb2;    CvBGCodeBookElem* freeList;    CV_ASSERT( model && CV_MAT_TYPE(image->type) == CV_8UC3 &&        (!mask || (CV_IS_MASK_ARR(mask) && CV_ARE_SIZES_EQ(image, mask))) );    if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )    {        roi.width = image->cols;        roi.height = image->rows;    }    else        CV_ASSERT( (unsigned)roi.x < (unsigned)image->cols &&                   (unsigned)roi.y < (unsigned)image->rows &&                   roi.width >= 0 && roi.height >= 0 &&                   roi.x + roi.width <= image->cols &&                   roi.y + roi.height <= image->rows );    if( image->cols != model->size.width || image->rows != model->size.height )    {        cvClearMemStorage( model->storage );        model->freeList = 0;        cvFree( &model->cbmap );        int bufSz = image->cols*image->rows*sizeof(model->cbmap[0]);        model->cbmap = (CvBGCodeBookElem**)cvAlloc(bufSz);        memset( model->cbmap, 0, bufSz );        model->size = cvSize(image->cols, image->rows);    }    icvInitSatTab();    cb0 = model->cbBounds[0];    cb1 = model->cbBounds[1];    cb2 = model->cbBounds[2];    T = ++model->t;    freeList = model->freeList;    nblocks = (int)((model->storage->block_size - sizeof(CvMemBlock))/sizeof(*freeList));    nblocks = MIN( nblocks, 1024 );    CV_ASSERT( nblocks > 0 );    for( y = 0; y < roi.height; y++ )    {        const uchar* p = image->data.ptr + image->step*(y + roi.y) + roi.x*3;        const uchar* m = mask ? mask->data.ptr + mask->step*(y + roi.y) + roi.x : 0;        CvBGCodeBookElem** cb = model->cbmap + image->cols*(y + roi.y) + roi.x;        for( x = 0; x < roi.width; x++, p += 3, cb++ )        {            CvBGCodeBookElem *e, *found = 0;            uchar p0, p1, p2, l0, l1, l2, h0, h1, h2;            int negRun;            if( m && m[x] == 0 )                continue;            p0 = p[0]; p1 = p[1]; p2 = p[2];            l0 = SAT_8U(p0 - cb0); l1 = SAT_8U(p1 - cb1); l2 = SAT_8U(p2 - cb2);            h0 = SAT_8U(p0 + cb0); h1 = SAT_8U(p1 + cb1); h2 = SAT_8U(p2 + cb2);            for( e = *cb; e != 0; e = e->next )            {                if( e->learnMin[0] <= p0 && p0 <= e->learnMax[0] &&                    e->learnMin[1] <= p1 && p1 <= e->learnMax[1] &&                    e->learnMin[2] <= p2 && p2 <= e->learnMax[2] )                {                    e->tLastUpdate = T;                    e->boxMin[0] = MIN(e->boxMin[0], p0);                    e->boxMax[0] = MAX(e->boxMax[0], p0);                    e->boxMin[1] = MIN(e->boxMin[1], p1);                    e->boxMax[1] = MAX(e->boxMax[1], p1);                    e->boxMin[2] = MIN(e->boxMin[2], p2);                    e->boxMax[2] = MAX(e->boxMax[2], p2);                    // no need to use SAT_8U for updated learnMin[i] & learnMax[i] here,                    // as the bounding li & hi are already within 0..255.                    if( e->learnMin[0] > l0 ) e->learnMin[0]--;                    if( e->learnMax[0] < h0 ) e->learnMax[0]++;                    if( e->learnMin[1] > l1 ) e->learnMin[1]--;                    if( e->learnMax[1] < h1 ) e->learnMax[1]++;                    if( e->learnMin[2] > l2 ) e->learnMin[2]--;                    if( e->learnMax[2] < h2 ) e->learnMax[2]++;                    found = e;                    break;                }                negRun = T - e->tLastUpdate;                e->stale = MAX( e->stale, negRun );            }//.........这里部分代码省略.........

CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right,    CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess ){    CvStereoGCState2 state2;    state2.orphans = 0;    state2.maxOrphans = 0;    CV_FUNCNAME( "cvFindStereoCorrespondenceGC" );    __BEGIN__;    CvMat lstub, *left = cvGetMat( _left, &lstub );    CvMat rstub, *right = cvGetMat( _right, &rstub );    CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub );    CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub );    CvSize size;    int iter, i, nZeroExpansions = 0;    CvRNG rng = cvRNG(-1);    int* disp;    CvMat _disp;    int64 E;    CV_ASSERT( state != 0 );    CV_ASSERT( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&               CV_MAT_TYPE(left->type) == CV_8UC1 );    CV_ASSERT( !dispLeft ||        (CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) );    CV_ASSERT( !dispRight ||        (CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) );    size = cvGetSize(left);    if( !state->left || state->left->width != size.width || state->left->height != size.height )    {        int pcn = (int)(sizeof(GCVtx*)/sizeof(int));        int vcn = (int)(sizeof(GCVtx)/sizeof(int));        int ecn = (int)(sizeof(GCEdge)/sizeof(int));        cvReleaseMat( &state->left );        cvReleaseMat( &state->right );        cvReleaseMat( &state->ptrLeft );        cvReleaseMat( &state->ptrRight );        cvReleaseMat( &state->dispLeft );        cvReleaseMat( &state->dispRight );        state->left = cvCreateMat( size.height, size.width, CV_8UC3 );        state->right = cvCreateMat( size.height, size.width, CV_8UC3 );        state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 );        state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 );        state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) );        state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) );        state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) );        state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) );    }    if( !useDisparityGuess )    {        cvSet( state->dispLeft, cvScalarAll(OCCLUDED));        cvSet( state->dispRight, cvScalarAll(OCCLUDED));    }    else    {        CV_ASSERT( dispLeft && dispRight );        cvConvert( dispLeft, state->dispLeft );        cvConvert( dispRight, state->dispRight );    }    state2.Ithreshold = state->Ithreshold;    state2.interactionRadius = state->interactionRadius;    state2.lambda = cvRound(state->lambda*DENOMINATOR);    state2.lambda1 = cvRound(state->lambda1*DENOMINATOR);    state2.lambda2 = cvRound(state->lambda2*DENOMINATOR);    state2.K = cvRound(state->K*DENOMINATOR);    icvInitStereoConstTabs();    icvInitGraySubpix( left, right, state->left, state->right );    disp = (int*)cvStackAlloc( state->numberOfDisparities*sizeof(disp[0]) );    _disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp );    cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities );    cvRandShuffle( &_disp, &rng );    if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) )    {        float L = icvComputeK(state)*0.2f;        state2.lambda = cvRound(L*DENOMINATOR);    }    if( state2.K < 0 )        state2.K = state2.lambda*5;    if( state2.lambda1 < 0 )        state2.lambda1 = state2.lambda*3;    if( state2.lambda2 < 0 )        state2.lambda2 = state2.lambda;    icvInitStereoTabs( &state2 );    E = icvComputeEnergy( state, &state2, !useDisparityGuess );    for( iter = 0; iter < state->maxIters; iter++ )    {        for( i = 0; i < state->numberOfDisparities; i++ )        {            int alpha = disp[i];//.........这里部分代码省略.........