CvConnectedComp* MeanShift::track(const Image* image, const Mask* probabilisticMap)
{
if (!m_trackingHist || !image)
return NULL;
//Obtain probabilistic image from backProject of histogram
Mask* probimage = m_trackingHist->calcBackProjection(image);
if (probabilisticMap)
(*probimage) &= (*probabilisticMap);
if ((m_lastPostition.width < 0) || (m_lastPostition.height < 0)
|| (m_lastPostition.x > image->width()) || (m_lastPostition.y > image->height()))
return &m_components;
//cvTermCriteria with CV_TERMCRIT_ITER specifies number of iteratios until center of mass found
//cvTermCriteria with CV_TERMCRIT_EPS specifies the max error of the result
cvMeanShift(probimage->cvImage(),
m_lastPostition,
cvTermCriteria(CV_TERMCRIT_EPS, 256, .001l),
&m_components);
//Update lastPosition
m_lastPostition = m_components.rect;
delete probimage;
return &m_components;
}
int meanShift( const Mat& probImage, Rect& window, TermCriteria criteria )
{
CvConnectedComp comp;
CvMat _probImage = probImage;
int iters = cvMeanShift(&_probImage, window, (CvTermCriteria)criteria, &comp );
window = comp.rect;
return iters;
}
t_jit_err cv_jit_shift_matrix_calc(t_cv_jit_shift *x, void *inputs, void *outputs)
{
t_jit_err err=JIT_ERR_NONE;
long in_savelock = 0;
t_jit_matrix_info in_minfo;
void *in_matrix;
CvMat source;
CvRect rectangle;
CvBox2D box;
CvConnectedComp component;
CvPoint2D32f vertices[4];
float w,h,c,s;
//Get pointer to matrix
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
if (x&&in_matrix)
{
//Lock the matrix
in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1);
//Make sure input is of proper format
jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo);
if(in_minfo.dimcount != 2)
{
err = JIT_ERR_MISMATCH_DIM;
goto out;
}
if(in_minfo.planecount != 1)
{
err = JIT_ERR_MISMATCH_PLANE;
goto out;
}
if(in_minfo.type != _jit_sym_char)
{
err = JIT_ERR_MISMATCH_TYPE;
goto out;
}
//Don't process if image is too small
if((in_minfo.dim[0] < 2)||(in_minfo.dim[1] < 2))
goto out;
//Calculate start rectangle:
rectangle = cvRect(x->rect[0],x->rect[1],x->rect[2]-x->rect[0],x->rect[3]-x->rect[1]);
CLIP_ASSIGN(rectangle.x,0,in_minfo.dim[0]-1);
CLIP_ASSIGN(rectangle.y,0,in_minfo.dim[1]-1);
CLIP_ASSIGN(rectangle.width,1,in_minfo.dim[0]-rectangle.x);
CLIP_ASSIGN(rectangle.height,1,in_minfo.dim[1]-rectangle.y);
//Convert Jitter matrix to OpenCV matrix
cvJitter2CvMat(in_matrix, &source);
//Calculate camshift
if(x->mode == 1) //Use camshift
cvCamShift(&source, rectangle, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,(int)x->maxiters,x->epsilon), &component, &box );
else {
cvMeanShift(&source, rectangle, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,(int)x->maxiters,x->epsilon), &component);
box.angle = 90.f;
box.size = cvSize2D32f(component.rect.width, component.rect.height);
box.center = cvPoint2D32f((float)component.rect.x + (float)component.rect.width * 0.5f,(float)component.rect.y + (float)component.rect.height * 0.5f);
}
//Prepare output
//
jit_atom_setlong(&x->box[0],component.rect.x);
jit_atom_setlong(&x->box[1],component.rect.y);
jit_atom_setlong(&x->box[2],component.rect.x + component.rect.width);
jit_atom_setlong(&x->box[3],component.rect.y + component.rect.height);
x->rect[0]=component.rect.x;
x->rect[1]=component.rect.y;
x->rect[2]=component.rect.x + component.rect.width;
x->rect[3]=component.rect.y + component.rect.height;
//cvBoxPoints(box,vertices);
w = box.size.width * 0.5;
h = box.size.height * 0.5;
c = cos((box.angle - 90.f) * -0.01745329252);
s = sin((box.angle - 90.f) * -0.01745329252);
vertices[0].x = box.center.x - s*h - c*w;
vertices[0].y = box.center.y - c*h + s*w;
vertices[1].x = box.center.x - s*h + c*w;
vertices[1].y = box.center.y - c*h - s*w;
vertices[2].x = box.center.x + s*h + c*w;
vertices[2].y = box.center.y + c*h - s*w;
vertices[3].x = box.center.x + s*h - c*w;
vertices[3].y = box.center.y + c*h + s*w;
jit_atom_setlong(&x->frame[0],(long)vertices[0].x);
jit_atom_setlong(&x->frame[1],(long)vertices[0].y);
jit_atom_setlong(&x->frame[2],(long)vertices[1].x);
jit_atom_setlong(&x->frame[3],(long)vertices[1].y);
jit_atom_setlong(&x->frame[4],(long)vertices[2].x);
jit_atom_setlong(&x->frame[5],(long)vertices[2].y);
jit_atom_setlong(&x->frame[6],(long)vertices[3].x);
jit_atom_setlong(&x->frame[7],(long)vertices[3].y);
x->mass = (float)(component.area / 256.);
}
out:
jit_object_method(in_matrix,gensym("lock"),in_savelock);
return err;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvCamShift
// Purpose: CAMSHIFT algorithm
// Context:
// Parameters:
// imgProb - 2D object probability distribution
// windowIn - CvRect of CAMSHIFT Window intial size
// criteria - criteria of stop finding window
// windowOut - Location, height and width of converged CAMSHIFT window
// orientation - If != NULL, return distribution orientation
// len - If != NULL, return equivalent len
// width - If != NULL, return equivalent width
// area - sum of all elements in result window
// itersUsed - Returns number of iterations CAMSHIFT took to converge
// Returns:
// The function itself returns the area found
// Notes:
//F*/
CV_IMPL int
cvCamShift( const void* imgProb, CvRect windowIn,
CvTermCriteria criteria,
CvConnectedComp* _comp,
CvBox2D* box )
{
const int TOLERANCE = 10;
CvMoments moments;
double m00 = 0, m10, m01, mu20, mu11, mu02, inv_m00;
double a, b, c, xc, yc;
double rotate_a, rotate_c;
double theta = 0, square;
double cs, sn;
double length = 0, width = 0;
int itersUsed = 0;
CvConnectedComp comp;
CvMat cur_win, stub, *mat = (CvMat*)imgProb;
CV_FUNCNAME( "cvCamShift" );
comp.rect = windowIn;
__BEGIN__;
CV_CALL( mat = cvGetMat( mat, &stub ));
CV_CALL( itersUsed = cvMeanShift( mat, windowIn, criteria, &comp ));
windowIn = comp.rect;
windowIn.x -= TOLERANCE;
if( windowIn.x < 0 )
windowIn.x = 0;
windowIn.y -= TOLERANCE;
if( windowIn.y < 0 )
windowIn.y = 0;
windowIn.width += 2 * TOLERANCE;
if( windowIn.x + windowIn.width > mat->width )
windowIn.width = mat->width - windowIn.x;
windowIn.height += 2 * TOLERANCE;
if( windowIn.y + windowIn.height > mat->height )
windowIn.height = mat->height - windowIn.y;
CV_CALL( cvGetSubRect( mat, &cur_win, windowIn ));
/* Calculating moments in new center mass */
CV_CALL( cvMoments( &cur_win, &moments ));
m00 = moments.m00;
m10 = moments.m10;
m01 = moments.m01;
mu11 = moments.mu11;
mu20 = moments.mu20;
mu02 = moments.mu02;
if( fabs(m00) < DBL_EPSILON )
EXIT;
inv_m00 = 1. / m00;
xc = cvRound( m10 * inv_m00 + windowIn.x );
yc = cvRound( m01 * inv_m00 + windowIn.y );
a = mu20 * inv_m00;
b = mu11 * inv_m00;
c = mu02 * inv_m00;
/* Calculating width & height */
square = sqrt( 4 * b * b + (a - c) * (a - c) );
/* Calculating orientation */
theta = atan2( 2 * b, a - c + square );
/* Calculating width & length of figure */
cs = cos( theta );
sn = sin( theta );
rotate_a = cs * cs * mu20 + 2 * cs * sn * mu11 + sn * sn * mu02;
rotate_c = sn * sn * mu20 - 2 * cs * sn * mu11 + cs * cs * mu02;
length = sqrt( rotate_a * inv_m00 ) * 4;
width = sqrt( rotate_c * inv_m00 ) * 4;
/* In case, when tetta is 0 or 1.57... the Length & Width may be exchanged */
if( length < width )
{
double t;
CV_SWAP( length, width, t );
CV_SWAP( cs, sn, t );
theta = CV_PI*0.5 - theta;
}
/* Saving results */
if( _comp || box )
{
int t0, t1;
int _xc = cvRound( xc );
int _yc = cvRound( yc );
t0 = cvRound( fabs( length * cs ));
t1 = cvRound( fabs( width * sn ));
t0 = MAX( t0, t1 ) + 2;
comp.rect.width = MIN( t0, (mat->width - _xc) * 2 );
t0 = cvRound( fabs( length * sn ));
t1 = cvRound( fabs( width * cs ));
t0 = MAX( t0, t1 ) + 2;
comp.rect.height = MIN( t0, (mat->height - _yc) * 2 );
comp.rect.x = MAX( 0, _xc - comp.rect.width / 2 );
comp.rect.y = MAX( 0, _yc - comp.rect.height / 2 );
comp.rect.width = MIN( mat->width - comp.rect.x, comp.rect.width );
comp.rect.height = MIN( mat->height - comp.rect.y, comp.rect.height );
comp.area = (float) m00;
}
__END__;
if( _comp )
*_comp = comp;
if( box )
{
box->size.height = (float)length;
box->size.width = (float)width;
box->angle = (float) theta;
box->center = cvPoint2D32f( comp.rect.x + comp.rect.width*0.5f,
comp.rect.y + comp.rect.height*0.5f);
}
return itersUsed;
}
