void vpTemplateTrackerWarp::findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p)
{
vpMatrix dW_(2,nbParam);
vpMatrix dX(2,1);
vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
vpMatrix G(nbParam,1);
int cpt=0;
vpColVector X1(2);
vpColVector fX1(2);
vpColVector X2(2);
double erreur=0;
double erreur_prec;
double lambda=0.01;
do
{
erreur_prec=erreur;
H=0;
G=0;
erreur=0;
computeCoeff(p);
for(int i=0;i<nb_pt;i++)
{
X1[0]=ut0[i];
X1[1]=vt0[i];
computeDenom(X1,p);
warpX(X1,fX1,p);
dWarp(X1,fX1,p,dW_);
H+=dW_.AtA();
X2[0]=u[i];
X2[1]=v[i];
dX=X2-fX1;
G+=dW_.t()*dX;
erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
}
vpMatrix::computeHLM(H, lambda, HLM);
try{
p+=HLM.inverseByLU()*G;
}
catch(vpException &e) {
//std::cout<<"Cannot inverse the matrix by LU " << std::endl;
throw(e);
}
cpt++;
}
while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
//std::cout<<"erreur apres transformation="<<erreur<<std::endl;
}
void vpTemplateTrackerWarp::warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v)
{
computeCoeff(p);
vpColVector X1(2),X2(2);
for(int i=0;i<nb_pt;i++)
{
X1[0]=ut0[i];
X1[1]=vt0[i];
computeDenom(X1,p);
warpX(X1,X2,p);
u[i]=X2[0];
v[i]=X2[1];
//std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
}
}
void Bezier::computePoint( float t, int &x, int &y ) const
{
// See http://astronomy.swin.edu.au/~pbourke/curves/bezier/ for a simple
// explanation of the algorithm
float xPos = 0;
float yPos = 0;
float coeff;
for( int i = 0; i < m_nbCtrlPt; i++ )
{
coeff = computeCoeff( i, m_nbCtrlPt - 1, t );
xPos += m_ptx[i] * coeff;
yPos += m_pty[i] * coeff;
}
x = lrintf(xPos);
y = lrintf(yPos);
}
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;
}
