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__;
  }

void
cvInitUndistortRectifyMap( 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;
//.........这里部分代码省略.........

void
cvUndistortPoints( 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.));
//.........这里部分代码省略.........