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__;
}
CvContourPolygon *cvPolygonContourConvexHull(CvContourPolygon const *p)
{
CV_FUNCNAME("cvPolygonContourConvexHull");
__CV_BEGIN__;
{
CV_ASSERT(p!=NULL);
CV_ASSERT(p->size()>=2);
deque<CvPoint> dq;
if (cvCrossProductPoints((*p)[0], (*p)[1], (*p)[2])>0)
{
dq.push_back((*p)[0]);
dq.push_back((*p)[1]);
}
else
{
dq.push_back((*p)[1]);
dq.push_back((*p)[0]);
}
dq.push_back((*p)[2]);
dq.push_front((*p)[2]);
for (int i=3; i<p->size(); i++)
{
int s = dq.size();
if ((cvCrossProductPoints((*p)[i], dq.at(0), dq.at(1))>=0) && (cvCrossProductPoints(dq.at(s-2), dq.at(s-1), (*p)[i])>=0))
continue; // TODO Optimize.
while (cvCrossProductPoints(dq.at(s-2), dq.at(s-1), (*p)[i])<0)
{
dq.pop_back();
s = dq.size();
}
dq.push_back((*p)[i]);
while (cvCrossProductPoints((*p)[i], dq.at(0), dq.at(1))<0)
dq.pop_front();
dq.push_front((*p)[i]);
}
return new CvContourPolygon(dq.begin(), dq.end());
}
__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;
double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
mapx[j] = (float)u;
mapy[j] = (float)v;
}
}
__END__;
}
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;
}
double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
x = xx*ww;
y = yy*ww;
if( dtype == CV_32FC2 )
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
__END__;
}
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.));
}
}
__CV_END__;
}
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];
if ((nx<imgIn_width)&&(nx>=0)&&(ny<imgIn_height)&&(ny>=0))
{
if (imageIn(nx, ny))
{
found = true;
blob->contour.chainCode.push_back(movesE[direction][i][3]);
xx=nx;
yy=ny;
direction=movesE[direction][i][2];
break;
}
else
{
imageOut(nx, ny) = CV_BLOB_MAX_LABEL;
}
}
}
if (!found)
direction=(direction+1)%4;
else
{
if (imageOut(xx, yy) != label)
{
imageOut(xx, yy) = label;
numPixels++;
if (xx<blob->minx) blob->minx = xx;
else if (xx>blob->maxx) blob->maxx = xx;
if (yy<blob->miny) blob->miny = yy;
else if (yy>blob->maxy) blob->maxy = yy;
blob->area++;
blob->m10+=xx; blob->m01+=yy;
blob->m11+=xx*yy;
blob->m20+=xx*xx; blob->m02+=yy*yy;
}
break;
}
if (contourEnd = ((xx==x) && (yy==y) && (direction==1)))
break;
}
}
while (!contourEnd);
}
if ((y+1<imgIn_height)&&(!imageIn(x, y+1))&&(!imageOut(x, y+1)))
{
labeled = true;
// Label internal contour
CvLabel l;
CvBlob *blob = NULL;
if (!imageOut(x, y))
{
l = imageOut(x-1, y);
imageOut(x, y) = l;
numPixels++;
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
blob->area++;
blob->m10+=x; blob->m01+=y;
blob->m11+=x*y;
blob->m20+=x*x; blob->m02+=y*y;
}
else
{
l = imageOut(x, y);
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
}
imageOut(x, y+1) = CV_BLOB_MAX_LABEL;
CvContourChainCode *contour = new CvContourChainCode;
contour->startingPoint = cvPoint(x, y);
unsigned char direction = 3;
unsigned int xx = x;
unsigned int yy = y;
do
{
for (unsigned int numAttempts=0; numAttempts<3; numAttempts++)
{
bool found = false;
for (unsigned char i=0; i<3; i++)
{
int nx = xx+movesI[direction][i][0];
int ny = yy+movesI[direction][i][1];
if (imageIn(nx, ny))
{
found = true;
contour->chainCode.push_back(movesI[direction][i][3]);
xx=nx;
yy=ny;
direction=movesI[direction][i][2];
break;
}
else
{
imageOut(nx, ny) = CV_BLOB_MAX_LABEL;
}
}
if (!found)
direction=(direction+1)%4;
else
{
if (!imageOut(xx, yy))
{
imageOut(xx, yy) = l;
numPixels++;
blob->area++;
blob->m10+=xx; blob->m01+=yy;
blob->m11+=xx*yy;
blob->m20+=xx*xx; blob->m02+=yy*yy;
}
break;
}
}
}
while (!(xx==x && yy==y));
blob->internalContours.push_back(contour);
}
if (!labeled)
{
CvLabel l = imageOut(x-1, y);
imageOut(x, y) = l;
numPixels++;
CvBlob *blob = NULL;
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
blob->area++;
blob->m10+=x; blob->m01+=y;
blob->m11+=x*y;
blob->m20+=x*x; blob->m02+=y*y;
}
}
}
}
for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it)
{
cvCentroid((*it).second);
(*it).second->u11 = (*it).second->m11 - ((*it).second->m10*(*it).second->m01)/(*it).second->m00;
(*it).second->u20 = (*it).second->m20 - ((*it).second->m10*(*it).second->m10)/(*it).second->m00;
(*it).second->u02 = (*it).second->m02 - ((*it).second->m01*(*it).second->m01)/(*it).second->m00;
double m00_2 = (*it).second->m00 * (*it).second->m00;
(*it).second->n11 = (*it).second->u11 / m00_2;
(*it).second->n20 = (*it).second->u20 / m00_2;
(*it).second->n02 = (*it).second->u02 / m00_2;
(*it).second->p1 = (*it).second->n20 + (*it).second->n02;
double nn = (*it).second->n20 - (*it).second->n02;
(*it).second->p2 = nn*nn + 4.*((*it).second->n11*(*it).second->n11);
}
return numPixels;
}
__CV_END__;
}
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 void
cvRQDecomp3x3( 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;
}
else
{
// rotate around y for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, 1, 0],
// [ 0, 0, -1]
_R[0][0] *= -1;
_R[0][2] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
_Qy[0][0] *= -1;
_Qy[0][2] *= -1;
_Qy[2][0] *= -1;
_Qy[2][2] *= -1;
}
}
else if( _R[1][1] < 0 )
{
// ??? for some reason, we never get here ???
// rotate around x for 180 degree, i.e. a rotation matrix of
// [ 1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, -1]
_R[0][1] *= -1;
_R[0][2] *= -1;
_R[1][1] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
cvTranspose( &Qy, &Qy );
_Qx[1][1] *= -1;
_Qx[1][2] *= -1;
_Qx[2][1] *= -1;
_Qx[2][2] *= -1;
}
// calculate the euler angle
if( eulerAngles )
{
eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
}
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
/* Save R and Q matrices. */
cvConvert( &R, matrixR );
cvConvert( &Q, matrixQ );
if( matrixQx )
cvConvert(&Qx, matrixQx);
if( matrixQy )
cvConvert(&Qy, matrixQy);
if( matrixQz )
cvConvert(&Qz, matrixQz);
}
__END__;
}
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 );
}
for( ; e != 0; e = e->next )
{
negRun = T - e->tLastUpdate;
e->stale = MAX( e->stale, negRun );
}
if( !found )
{
if( !freeList )
{
freeList = (CvBGCodeBookElem*)cvMemStorageAlloc(model->storage,
nblocks*sizeof(*freeList));
for( i = 0; i < nblocks-1; i++ )
freeList[i].next = &freeList[i+1];
freeList[nblocks-1].next = 0;
}
e = freeList;
freeList = freeList->next;
e->learnMin[0] = l0; e->learnMax[0] = h0;
e->learnMin[1] = l1; e->learnMax[1] = h1;
e->learnMin[2] = l2; e->learnMax[2] = h2;
e->boxMin[0] = e->boxMax[0] = p0;
e->boxMin[1] = e->boxMax[1] = p1;
e->boxMin[2] = e->boxMax[2] = p2;
e->tLastUpdate = T;
e->stale = 0;
e->next = *cb;
*cb = e;
}
}
}
model->freeList = freeList;
__END__;
}
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];
int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 );
if( Enew < E )
{
nZeroExpansions = 0;
E = Enew;
}
else if( ++nZeroExpansions >= state->numberOfDisparities )
break;
}
}
if( dispLeft )
cvConvert( state->dispLeft, dispLeft );
if( dispRight )
cvConvert( state->dispRight, dispRight );
__END__;
cvFree( &state2.orphans );
}