std::vector<vpImagePoint> getTemplateTrackerCorners(const vpTemplateTrackerZone &zone)
{
std::vector<vpImagePoint> corners_tracked;
// Parse all the triangles that describe the zone
for (int i=0; i < zone.getNbTriangle(); i++) {
vpTemplateTrackerTriangle triangle;
// Get a triangle
zone.getTriangle(i, triangle);
std::vector<vpImagePoint> corners;
// Get the 3 triangle corners
triangle.getCorners( corners );
if (i==0)
corners_tracked = corners;
else {
for(unsigned int m=0; m < corners.size(); m++) { // corners of the 2nd triangle
bool already_exists = false;
for(unsigned int n=0; n < corners_tracked.size(); n++) { // already registered corners from the 1st triangle
if (corners[m] == corners_tracked[n]) {
already_exists = true;
break;
}
}
if (! already_exists)
corners_tracked.push_back(corners[m]);
}
}
}
return corners_tracked;
}
void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out)
{
vpTemplateTrackerTriangle TR,TT;
out.clear();
for(unsigned int i=0;i<in.getNbTriangle();i++)
{
in.getTriangle(i,TR);
warpTriangle(TR,p,TT);
out.add(TT);
}
}
double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p)
{
unsigned int nb_corners = Z.getNbTriangle() * 3;
computeCoeff(p);
vpColVector X1(2),X2(2);
double res=0;
vpTemplateTrackerTriangle triangle;
for(unsigned int i=0;i<Z.getNbTriangle();i++)
{
Z.getTriangle(i, triangle);
for (unsigned int j=0; j<3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
computeDenom(X1,p);
warpX(X1,X2,p);
res+=sqrt((X2[0]-X1[0])*(X2[0]-X1[0])+(X2[1]-X1[1])*(X2[1]-X1[1]));
}
}
return res/nb_corners;
}
void vpTemplateTracker::initTracking(const vpImage<unsigned char>& I, vpTemplateTrackerZone &zone)
{
// std::cout<<"\tInitialise reference..."<<std::endl;
zoneTracked=&zone;
int largeur_im=(int)I.getWidth();
int hauteur_im=(int)I.getHeight();
unsigned int NbPointDsZone=0;
//double xtotal=0,ytotal=0;
int mod_fi,mod_fj;
mod_fi=mod_i;mod_fj=mod_i;
for(int i=0;i<hauteur_im;i+=mod_fi) {
for(int j=0;j<largeur_im;j+=mod_fj) {
if(zone.inZone(i,j)) {
NbPointDsZone++;
//xtotal+=j;
//ytotal+=i;
}
}
}
//Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
templateSize=NbPointDsZone;
ptTemplate = new vpTemplateTrackerPoint[templateSize];ptTemplateInit=true;
ptTemplateSelect = new bool[templateSize];ptTemplateSelectInit=true;
Hdesire.resize(nbParam,nbParam);
HLMdesire.resize(nbParam,nbParam);
vpTemplateTrackerPoint pt;
//vpTemplateTrackerZPoint ptZ;
vpImage<double> GaussI ;
vpImageFilter::filter(I, GaussI,fgG,taillef);
vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
unsigned int cpt_point=0;
templateSelectSize=0;
for(int i=0;i<hauteur_im;i+=mod_i)
{
//for(int j=minx_t;j<maxx_t;j++)
for(int j=0;j<largeur_im;j+=mod_j)
{
// if(i%mod_i ==0 && j%mod_j ==0)
if(zone.inZone(i,j))
{
pt.x=j;
pt.y=i;
pt.dx=dIx[i][j];
pt.dy=dIy[i][j];
if(pt.dx*pt.dx+pt.dy*pt.dy>thresholdGradient)
{
ptTemplateSelect[cpt_point]=true;
templateSelectSize++;
}
else
ptTemplateSelect[cpt_point]=false;
//ptTemplate_select[cpt_point]=true;
/*if(blur)
pt.val=GaussI[i][j];
else
pt.val=I[i][j];*/
pt.val=vpTemplateTrackerBSpline::getSubPixBspline4(GaussI,i,j);
//ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
ptTemplate[cpt_point]=pt;
cpt_point++;
}
}
}
// std::cout<<"\tNb pt template apres scale:"<<cpt_point<<std::endl;
// std::cout<<"utilisation de "<<taille_template_select<<"/"<<cpt_point<<" = "<<100.*taille_template_select/cpt_point<<"% pour calcul derivees"<<std::endl;
templateSize=cpt_point;
GaussI.destroy();
// std::cout<<"\tEnd of reference initialisation ..."<<std::endl;
}
