void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompInverse(I);
double erreur=0;
int Nbpoint=0;
double i2,j2;
double Tij;
double IW;
int cr,ct;
double er,et;
int i,j;
Nbpoint=0;
erreur=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
zeroProbabilities();
Warp->computeCoeff(p);
// AY : Optim
// unsigned int totParam = (bspline * bspline)*(1+nbParam+nbParam*nbParam);
// unsigned int size = (1 + nbParam + nbParam*nbParam)*bspline;
// double *ptb;
for(unsigned int point=0;point<templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Nbpoint++;
Tij=ptTemplate[point].val;
if(blur)
IW=BI.getValue(i2,j2);
else
IW=I.getValue(i2,j2);
ct=ptTemplateSupp[point].ct;
et=ptTemplateSupp[point].et;
cr=(int)((IW*(Nc-1))/255.);
er=((double)IW*(Nc-1))/255.-cr;
//calcul de l'erreur
erreur+=(Tij-IW)*(Tij-IW);
if( ApproxHessian==HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect) )
{
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
}
else if ((ApproxHessian==HESSIAN_0||ApproxHessian==HESSIAN_NEW) && (ptTemplateSelect[point] || !useTemplateSelect))
{
if(bspline==3){
vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
// {
// if(et>0.5){ct++;}
// if(er>0.5){cr++;}
// int index = (cr*Nc+ct)*totParam;
// double *ptb = &PrtTout[index];
// vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
// }
}
else{
vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
// {
// // ################### AY : Optim
// unsigned int index = (cr*Nc+ct)*totParam;
// ptb = &PrtTout[index];
// vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb, er, ptTemplateSupp[point].BtInit, size);
// // ###################
// }
}
}
else if (ptTemplateSelect[point] || !useTemplateSelect)
vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc,nbParam, bspline);
}
}
double MI;
computeProba(Nbpoint);
computeMI(MI);
computeHessien(Hdesire);
lambda=lambdaDep;
vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
HLMdesireInverse=HLMdesire.inverseByLU();
KQuasiNewton=HLMdesireInverse;
}
void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
if(!CompoInitialised)
std::cout<<"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
dW=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
int Nbpoint=0;
lambda=lambdaDep;
double MI=0,MIprec=-1000;
vpColVector p_avant_estimation;p_avant_estimation=p;
MI_preEstimation=-getCost(I,p);
NMI_preEstimation=-getNormalizedCost(I,p);
// std::cout << "MI avant: " << MI_preEstimation << std::endl;
// std::cout << "NMI avant: " << NMI_preEstimation << std::endl;
initPosEvalRMS(p);
vpColVector dpinv(nbParam);
double alpha=2.;
unsigned int iteration=0;
//unsigned int bspline_ = (unsigned int) bspline;
//unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);
vpMatrix Hnorm(nbParam,nbParam);
do
{
Nbpoint=0;
MIprec=MI;
MI=0;
zeroProbabilities();
Warp->computeCoeff(p);
{
for(int point=0;point<(int)templateSize;point++)
{
vpColVector x1(2),x2(2);
double i2,j2;
double IW;
int cr,ct;
double er,et;
x1[0]=(double)ptTemplate[point].x;
x1[1]=(double)ptTemplate[point].y;
Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
Warp->warpX(x1,x2,p);
j2=x2[0];
i2=x2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
//if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
{
Nbpoint++;
if(!blur)
IW=(double)I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
ct=ptTemplateSupp[point].ct;
et=ptTemplateSupp[point].et;
double tmp = IW*(((double)Nc)-1.f)/255.f;
cr=(int)tmp;
er=tmp-(double)cr;
if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
{
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
}
else if (ptTemplateSelect[point] || !useTemplateSelect)
{
if(bspline==3){
vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
}
else{
vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
}
}
else{
vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
}
}
}
}
}
if(Nbpoint==0)
{
diverge=true;
MI=0;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
else
{
// computeProba(Nbpoint);
unsigned int indd, indd2;
indd = indd2 = 0;
unsigned int Ncb_ = (unsigned int)Ncb;
for(unsigned int i=0;i<Ncb_*Ncb_;i++){
Prt[i]=Prt[i]/Nbpoint;
for(unsigned int j=0;j<nbParam;j++){
dPrt[indd]=dPrt[indd]/Nbpoint;
indd++;
for(unsigned int k=0;k<nbParam;k++){
d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
indd2++;
}
}
}
computeMI(MI);
if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
computeHessienNormalized(Hnorm);
computeHessien(H);
}
computeGradient();
vpMatrix::computeHLM(H,lambda,HLM);
try
{
switch(hessianComputation)
{
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
dp=gain*HLMdesireInverse*G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
if(HLM.cond()>HLMdesire.cond())
dp=gain*HLMdesireInverse*G;
else
dp=gain*0.2*HLM.inverseByLU()*G;
break;
default:
dp=gain*0.2*HLM.inverseByLU()*G;
break;
}
}
catch(vpException &e)
{
//std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
}
switch(minimizationMethod)
{
case vpTemplateTrackerMIInverseCompositional::USE_LMA:
{
vpColVector dp_test_LMA(nbParam);
vpColVector dpinv_test_LMA(nbParam);
vpColVector p_test_LMA(nbParam);
if(ApproxHessian==HESSIAN_NONSECOND)
dp_test_LMA=-100000.1*dp;
else
dp_test_LMA=1.*dp;
Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);
MI=-getCost(I,p);
double MI_LMA=-getCost(I,p_test_LMA);
if(MI_LMA>MI)
{
dp=dp_test_LMA;
lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
}
else
{
dp=0;
lambda=(lambda*10.<1e6)?1e6:lambda*10.;
}
}
break;
case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
dp=-gain*0.3*G*20;
break;
case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
{
double s_scal_y;
if(iterationGlobale!=0)
{
vpColVector s_quasi=p-p_prec;
vpColVector y_quasi=G-G_prec;
s_scal_y=s_quasi.t()*y_quasi;
//std::cout<<"mise a jour K"<<std::endl;
/*if(s_scal_y!=0)//BFGS
KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
//if(s_scal_y!=0)//DFP
if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
{
KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
//std::cout<<"mise a jour K"<<std::endl;
}
}
dp=gain*KQuasiNewton*G;
//std::cout<<KQuasiNewton<<std::endl<<std::endl;
p_prec=p;
G_prec=G;
//p-=1.01*dp;
}
break;
default:
{
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
dp=alpha*dp;
}
if(ApproxHessian==HESSIAN_NONSECOND)
dp=-1.*dp;
break;
}
}
Warp->getParamInverse(dp,dpinv);
Warp->pRondp(p,dpinv,p);
iteration++;
iterationGlobale++;
computeEvalRMS(p);
// std::cout << p.t() << std::endl;
}
while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
//while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
nbIteration=iteration;
if(diverge)
{
if(computeCovariance){
covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
covarianceMatrix = -1;
MI_postEstimation = -1;
NMI_postEstimation = -1;
}
deletePosEvalRMS();
// throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
}
else
{
MI_postEstimation=-getCost(I,p);
NMI_postEstimation=-getNormalizedCost(I,p);
// std::cout << "MI apres: " << MI_postEstimation << std::endl;
// std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
if(MI_preEstimation>MI_postEstimation)
{
p=p_avant_estimation;
MI_postEstimation = MI_preEstimation;
NMI_postEstimation = NMI_preEstimation;
covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
covarianceMatrix = -1;
}
deletePosEvalRMS();
if(computeCovariance){
try{
covarianceMatrix = (-H).inverseByLU();
// covarianceMatrix = (-Hnorm).inverseByLU();
}
catch(...){
covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
covarianceMatrix = -1;
MI_postEstimation = -1;
NMI_postEstimation = -1;
deletePosEvalRMS();
}
}
}
}
/*!
Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
\param I : The image used to store the result.
\param cam : The parameters of the virtual camera.
\param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
*/
void
vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
vpMatrix &zBuffer)
{
if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
if (cleanPrevImage)
{
for (unsigned int i = 0; i < I.getHeight(); i++)
{
for (unsigned int j = 0; j < I.getWidth(); j++)
{
I[i][j] = bgColor;
}
}
}
if(visible)
{
if(!needClipping)
getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
else
getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
double right= rect.getRight();
vpRGBa *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
int nb_point_dessine = 0;
for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
{
for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
{
double x=0,y=0;
ip.set_ij(i,j);
vpPixelMeterConversion::convertPoint(cam,ip, x,y);
ip.set_ij(y,x);
if (colorI == GRAY_SCALED)
{
unsigned char Ipixelplan;
if(getPixel(ip,Ipixelplan))
{
if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
{
vpRGBa pixelcolor;
pixelcolor.R = Ipixelplan;
pixelcolor.G = Ipixelplan;
pixelcolor.B = Ipixelplan;
*(bitmap+i*width+j) = pixelcolor;
nb_point_dessine++;
zBuffer[i][j] = Xinter_optim[2];
}
}
}
else if (colorI == COLORED)
{
vpRGBa Ipixelplan;
if(getPixel(ip,Ipixelplan))
{
if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
{
*(bitmap+i*width+j) = Ipixelplan;
nb_point_dessine++;
zBuffer[i][j] = Xinter_optim[2];
}
}
}
}
}
}
}
/*!
Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
The following example shows how to use the method:
\code
#include <list>
#include <visp3/core/vpImage.h>
#include <visp3/robot/vpImageSimulator.h>
int main()
{
vpImage<vpRGBa> Icamera(480,640,0);
vpImage<vpRGBa> Iimage(60,60);
// Initialise the image which will be projected into the image Icamera
vpRGBa colorb(0,0,255);
vpRGBa colorw(255,255,255);
vpRGBa colorr(255,0,0);
for(int i = 0; i < 60; i++)
{
for(int j = 0; j < 20; j++)
Iimage[i][j] = colorb;
for(int j = 20; j < 40; j++)
Iimage[i][j] = colorw;
for(int j = 40; j < 60; j++)
Iimage[i][j] = colorr;
}
// Initialise the 3D coordinates of the Iimage corners
vpColVector X[4];
for (int i = 0; i < 4; i++) X[i].resize(3);
// Top left corner
X[0][0] = -1;
X[0][1] = -1;
X[0][2] = 1;
// Top right corner
X[1][0] = 1;
X[1][1] = -1;
X[1][2] = 1;
// Bottom right corner
X[2][0] = 1;
X[2][1] = 1;
X[2][2] = 1;
//Bottom left corner
X[3][0] = -1;
X[3][1] = 1;
X[3][2] = 1;
vpImageSimulator sim;
sim.init(Iimage, X);
// Top left corner
X[0][0] = -1;
X[0][1] = -1;
X[0][2] = 1;
// Top right corner
X[1][0] = 1;
X[1][1] = -1;
X[1][2] = 1;
// Bottom right corner
X[2][0] = 1;
X[2][1] = 1;
X[2][2] = 1;
//Bottom left corner
X[3][0] = -1;
X[3][1] = 1;
X[3][2] = 1;
vpImageSimulator sim2;
sim2.init(Iimage, X);
sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
std::list<vpImageSimulator> listSim;
listSim.addRight(sim);
listSim.addRight(sim2);
sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
vpCameraParameters cam(868.0, 869.0, 320, 240);
vpImageSimulator::getImage(Icamera,listSim,cam);
return 0;
}
\endcode
\param I : The image used to store the result
\param list : List of vpImageSimulator to project
\param cam : The parameters of the virtual camera
*/
void
vpImageSimulator::getImage(vpImage<vpRGBa> &I,
std::list<vpImageSimulator> &list,
const vpCameraParameters &cam)
{
unsigned int width = I.getWidth();
unsigned int height = I.getHeight();
unsigned int nbsimList = (unsigned int)list.size();
if (nbsimList < 1)
return;
vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
double topFinal = height+1;;
double bottomFinal = -1;
double leftFinal = width+1;
double rightFinal = -1;
unsigned int unvisible = 0;
unsigned int indexSimu = 0;
for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
vpImageSimulator* sim = &(*it);
if (sim->visible)
simList[indexSimu] = sim;
else
unvisible++;
}
nbsimList = nbsimList - unvisible;
if (nbsimList < 1)
{
delete[] simList;
return;
}
for (unsigned int i = 0; i < nbsimList; i++)
{
if(!simList[i]->needClipping)
simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
else
simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
}
double zmin = -1;
int indice = -1;
vpRGBa *bitmap = I.bitmap;
vpImagePoint ip;
for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
{
for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
{
zmin = -1;
double x=0,y=0;
ip.set_ij(i,j);
vpPixelMeterConversion::convertPoint(cam,ip, x,y);
ip.set_ij(y,x);
for (int k = 0; k < (int)nbsimList; k++)
{
double z = 0;
if(simList[k]->getPixelDepth(ip,z))
{
if (z < zmin || zmin < 0)
{
zmin = z;
indice = k;
}
}
}
if (indice >= 0)
{
if (simList[indice]->colorI == GRAY_SCALED)
{
unsigned char Ipixelplan = 255;
simList[indice]->getPixel(ip,Ipixelplan);
vpRGBa pixelcolor;
pixelcolor.R = Ipixelplan;
pixelcolor.G = Ipixelplan;
pixelcolor.B = Ipixelplan;
*(bitmap+i*width+j) = pixelcolor;
}
else if (simList[indice]->colorI == COLORED)
{
vpRGBa Ipixelplan(255,255,255);
simList[indice]->getPixel(ip,Ipixelplan);
//unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B;
*(bitmap+i*width+j)=Ipixelplan;
}
}
}
}
delete[] simList;
}
/*!
Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
\param I : The image used to store the result.
\param cam : The parameters of the virtual camera.
*/
void
vpImageSimulator::getImage(vpImage<unsigned char> &I,
const vpCameraParameters &cam)
{
if (setBackgroundTexture)
// The Ig has been set to a previously defined background texture
I = Ig;
else
{
if (cleanPrevImage)
{
unsigned char col = (unsigned char) (0.2126 * bgColor.R
+ 0.7152 * bgColor.G + 0.0722 * bgColor.B);
for (unsigned int i = 0; i < I.getHeight(); i++)
{
for (unsigned int j = 0; j < I.getWidth(); j++)
{
I[i][j] = col;
}
}
}
}
if(visible)
{
if(!needClipping)
getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
else
getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
double right= rect.getRight();
unsigned char *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
int nb_point_dessine = 0;
for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
{
for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
{
double x=0,y=0;
ip.set_ij(i,j);
vpPixelMeterConversion::convertPoint(cam,ip, x,y);
ip.set_ij(y,x);
if (colorI == GRAY_SCALED)
{
unsigned char Ipixelplan = 0;
if(getPixel(ip,Ipixelplan))
{
*(bitmap+i*width+j)=Ipixelplan;
nb_point_dessine++;
}
}
else if (colorI == COLORED)
{
vpRGBa Ipixelplan;
if(getPixel(ip,Ipixelplan))
{
unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
*(bitmap+i*width+j)=pixelgrey;
nb_point_dessine++;
}
}
}
}
}
}
/*!
Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
\param I : The image used to store the result.
\param cam : The parameters of the virtual camera.
*/
void
vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam)
{
if (cleanPrevImage)
{
for (unsigned int i = 0; i < I.getHeight(); i++)
{
for (unsigned int j = 0; j < I.getWidth(); j++)
{
I[i][j] = bgColor;
}
}
}
if(visible)
{
if(!needClipping)
getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
else
getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
double right= rect.getRight();
vpRGBa *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
int nb_point_dessine = 0;
for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
{
for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
{
double x=0,y=0;
ip.set_ij(i,j);
vpPixelMeterConversion::convertPoint(cam,ip, x,y);
ip.set_ij(y,x);
if (colorI == GRAY_SCALED)
{
unsigned char Ipixelplan;
if(getPixel(ip,Ipixelplan))
{
vpRGBa pixelcolor;
pixelcolor.R = Ipixelplan;
pixelcolor.G = Ipixelplan;
pixelcolor.B = Ipixelplan;
*(bitmap+i*width+j) = pixelcolor;
nb_point_dessine++;
}
}
else if (colorI == COLORED)
{
vpRGBa Ipixelplan;
if(getPixel(ip,Ipixelplan))
{
*(bitmap+i*width+j) = Ipixelplan;
nb_point_dessine++;
}
}
}
}
}
}
/*!
Perform the tracking of a dot by connex components.
\param mean_value : Threshold to use for the next call to track()
and corresponding to the mean value of the dot intensity.
\warning The content of the image is modified thanks to
setGrayLevelOut() called before. This method choose a gray level
(default is 0) used to modify the "in" dot level in "out" dot
level. This gray level is here needed to stop the recursivity . The
pixels of the dot are set to this new gray level "\out\".
\return vpDot::out if an error occurs, vpDot::in otherwise.
\sa setGrayLevelOut()
*/
int
vpDot::connexe(vpImage<unsigned char>& I, int u, int v,
unsigned int gray_level_min, unsigned int gray_level_max,
double &mean_value, double &u_cog, double &v_cog, double &n)
{
int width = I.getWidth();
int height= I.getHeight();
// Test if we are in the image
if ( (u < 0) || (v < 0) || (u >= width) || (v >= height) ) {
return vpDot::out ;
}
if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
{
vpImagePoint ip;
ip.set_u(u);
ip.set_v(v);
if (graphics==true)
{
// printf("u %d v %d\n", u, v);
vpDisplay::displayPoint(I, ip, vpColor::green) ;
//vpDisplay::flush(I);
}
ip_edges_list += ip;
u_cog += u ;
v_cog += v ;
n+=1 ;
if (n > nbMaxPoint) {
vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
"This threshold can be modified using the setMaxDotSize() "
"method.",
n, n / (I.getWidth() * I.getHeight()),
nbMaxPoint, maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to big")) ;
}
// Bounding box update
if (u < this->u_min) this->u_min = u;
if (u > this->u_max) this->u_max = u;
if (v < this->v_min) this->v_min = v;
if (v > this->v_max) this->v_max = v;
// if (graphics==true)
// {
// vpRect r(this->u_min, this->v_min,
// this->u_max - this->u_min + 1,
// this->v_max - this->v_min + 1);
// vpDisplay::displayRectangle(I, r, vpColor::white) ;
// }
// Mean value of the dot intensities
mean_value = (mean_value *(n-1) + I[v][u]) / n;
if (compute_moment==true)
{
m00++ ;
m10 += u ;
m01 += v ;
m11 += (u*v) ;
m20 += u*u ;
m02 += v*v ;
}
I[v][u] = this->gray_level_out ;
}
else
{
return vpDot::out ;
}
if ( u-1 >= 0)
{
if (I[v][u-1] >= gray_level_min && I[v][u-1] <= gray_level_max)
connexe(I,u-1,v, gray_level_min, gray_level_max, mean_value,
u_cog,v_cog, n) ;
}
if (u+1 < width)
{
if (I[v][u+1] >= gray_level_min && I[v][u+1] <= gray_level_max)
connexe(I,u+1,v,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if (v-1 >= 0)
{
if (I[v-1][u] >=gray_level_min && I[v-1][u] <= gray_level_max)
connexe(I,u, v-1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if (v+1 < height)
{
if (I[v+1][u] >=gray_level_min && I[v+1][u] <= gray_level_max)
connexe(I,u,v+1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if (connexity == CONNEXITY_8) {
if ( (u-1 >= 0) && (v-1 >= 0) )
{
if (I[v-1][u-1] >=gray_level_min && I[v-1][u-1] <= gray_level_max)
connexe(I,u-1,v-1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if ( (u+1 < width) && (v-1 >= 0 ) )
{
if (I[v-1][u+1] >=gray_level_min && I[v-1][u+1] <= gray_level_max)
connexe(I,u+1,v-1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if ( (v+1 < height) && (u-1 >= 0) )
{
if (I[v+1][u-1] >=gray_level_min && I[v+1][u-1] <= gray_level_max)
connexe(I,u-1,v+1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if ( (v+1 < height) && (u+1 < width) )
{
if (I[v+1][u+1] >=gray_level_min && I[v+1][u+1] <= gray_level_max)
connexe(I,u+1,v+1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
}
return vpDot::in ;
}
/*!
Compute the center of gravity (COG) of the dot using connex
components. We assume the origin pixel (u, v) is in the dot. If
not, the dot is seach arround this origin using a spiral search.
\param I : Image to process.
\param u : Starting pixel coordinate along the columns from where the
dot is searched .
\param v : Starting pixel coordinate along the rows from where the
dot is searched .
\warning The content of the image is modified.
\exception vpTrackingException::featureLostError : If the tracking fails.
\sa connexe()
*/
void
vpDot::COG(vpImage<unsigned char> &I, double& u, double& v)
{
// Set the maximal number of points considering the maximal dot size
// image percentage
nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;
// segmentation de l'image apres seuillage
// (etiquetage des composante connexe)
if (compute_moment)
m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;
double u_cog = 0 ;
double v_cog = 0 ;
double npoint = 0 ;
this->mean_gray_level = 0 ;
ip_edges_list.kill() ;
// Initialise the boundig box
this->u_min = I.getWidth();
this->u_max = 0;
this->v_min = I.getHeight();
this->v_max = 0;
#if 0
// Original version
if ( connexe(I, (unsigned int)u, (unsigned int)v,
gray_level_min, gray_level_max,
mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
{
bool sol = false ;
unsigned int pas ;
for (pas = 2 ; pas <= 25 ; pas ++ )if (sol==false)
{
for (int k=-1 ; k <=1 ; k++) if (sol==false)
for (int l=-1 ; l <=1 ; l++) if (sol==false)
{
u_cog = 0 ;
v_cog = 0 ;
ip_edges_list.kill() ;
this->mean_gray_level = 0 ;
if (connexe(I, (unsigned int)(u+k*pas),(unsigned int)(v+l*pas),
gray_level_min, gray_level_max,
mean_gray_level, u_cog, v_cog, npoint) != vpDot::out)
{
sol = true ; u += k*pas ; v += l*pas ;
}
}
}
if (sol == false)
{
vpERROR_TRACE("Dot has been lost") ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot has been lost")) ;
}
}
#else
// If the dot is not found, search around using a spiral
if ( connexe(I,(unsigned int)u,(unsigned int)v,
gray_level_min, gray_level_max,
mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
{
bool sol = false ;
unsigned int right = 1;
unsigned int botom = 1;
unsigned int left = 2;
unsigned int up = 2;
double u_ = u, v_ = v;
unsigned int k;
// Spiral search from the center to find the nearest dot
while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
for (k=1; k <= right; k++) if(sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_edges_list.kill() ;
this->mean_gray_level = 0 ;
if (connexe(I, (unsigned int)u_+k, (unsigned int)(v_),
gray_level_min, gray_level_max, mean_gray_level,
u_cog, v_cog, npoint) != vpDot::out) {
sol = true; u = u_+k; v = v_;
}
}
u_ += k;
right += 2;
for (k=1; k <= botom; k++) if (sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_edges_list.kill() ;
this->mean_gray_level = 0 ;
if (connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),
gray_level_min, gray_level_max, mean_gray_level,
u_cog, v_cog, npoint)
!= vpDot::out) {
sol = true; u = u_; v = v_+k;
}
}
v_ += k;
botom += 2;
for (k=1; k <= left; k++) if (sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_edges_list.kill() ;
this->mean_gray_level = 0 ;
if (connexe(I, (unsigned int)(u_-k), (unsigned int)(v_),
gray_level_min, gray_level_max, mean_gray_level,
u_cog, v_cog, npoint)
!= vpDot::out) {
sol = true ; u = u_-k; v = v_;
}
}
u_ -= k;
left += 2;
for (k=1; k <= up; k++) if(sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_edges_list.kill() ;
this->mean_gray_level = 0 ;
if (connexe(I, (unsigned int)(u_), (unsigned int)(v_-k),
gray_level_min, gray_level_max, mean_gray_level,
u_cog, v_cog, npoint)
!= vpDot::out) {
sol = true ; u = u_; v = v_-k;
}
}
v_ -= k;
up += 2;
}
if (sol == false) {
vpERROR_TRACE("Dot has been lost") ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot has been lost")) ;
}
}
#endif
ip_edges_list.front() ;
vpImagePoint ip;
unsigned int i, j;
while (! ip_edges_list.outside()) {
ip = ip_edges_list.value();
i = (unsigned int) ip.get_i();
j = (unsigned int) ip.get_j();
I[i][j] = 255 ;
ip_edges_list.next() ;
}
u_cog = u_cog/npoint ;
v_cog = v_cog/npoint ;
u = u_cog ;
v = v_cog ;
// Initialize the threshold for the next call to track()
double Ip = pow((double)this->mean_gray_level/255,1/gamma);
if(Ip - (1 - grayLevelPrecision)<0){
gray_level_min = 0 ;
}
else{
gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
if (gray_level_max > 255)
gray_level_max = 255;
//vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
//vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;
if (npoint < 5)
{
vpERROR_TRACE("Dot to small") ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to small")) ;
}
if (npoint > nbMaxPoint)
{
vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf (%lf%%). This threshold can be modified using the setMaxDotSize() method.",
npoint, npoint / (I.getWidth() * I.getHeight()),
nbMaxPoint, maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to big")) ;
}
}
void MSRCR(vpImage<vpRGBa> &I, const int _scale, const int scaleDiv,
const int level, const double dynamic, const int _kernelSize) {
//Calculate the scales of filtering according to the number of filter and their distribution.
std::vector<double> retinexScales = retinexScalesDistribution(scaleDiv, level, _scale);
//Filtering according to the various scales.
//Summarize the results of the various filters according to a specific weight(here equivalent for all).
double weight = 1.0 / (double) scaleDiv;
std::vector<vpImage<double> > doubleRGB(3);
std::vector<vpImage<double> > doubleResRGB(3);
unsigned int size = I.getSize();
int kernelSize = _kernelSize;
if(kernelSize == -1) {
//Compute the kernel size from the input image size
kernelSize = (int) (std::min(I.getWidth(), I.getHeight()) / 2.0);
kernelSize = (kernelSize - kernelSize%2) + 1;
}
for(int channel = 0; channel < 3; channel++) {
doubleRGB[(size_t) channel] = vpImage<double>(I.getHeight(), I.getWidth());
doubleResRGB[(size_t) channel] = vpImage<double>(I.getHeight(), I.getWidth());
for(unsigned int cpt = 0; cpt < size; cpt++) {
//Shift the pixel values by 1 to avoid problem with log(0)
switch(channel) {
case 0:
doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].R + 1.0;
break;
case 1:
doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].G + 1.0;
break;
case 2:
doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].B + 1.0;
break;
default:
break;
}
}
for (int sc = 0; sc < scaleDiv; sc++) {
vpImage<double> blurImage;
double sigma = retinexScales[(size_t) sc];
vpImageFilter::gaussianBlur(doubleRGB[(size_t) channel], blurImage, (unsigned int) kernelSize, sigma);
for(unsigned int cpt = 0; cpt < size; cpt++) {
//Summarize the filtered values.
//In fact one calculates a ratio between the original values and the filtered values.
doubleResRGB[(size_t) channel].bitmap[cpt] += weight * (std::log(doubleRGB[(size_t) channel].bitmap[cpt])
- std::log(blurImage.bitmap[cpt]));
}
}
}
std::vector<double> dest(size*3);
const double gain = 1.0, alpha = 128.0, offset = 0.0;
for(unsigned int cpt = 0; cpt < size; cpt++) {
double logl = std::log( (double) (I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0) );
dest[cpt*3] = gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt] + offset;
dest[cpt*3 + 1] = gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt] + offset;
dest[cpt*3 + 2] = gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt] + offset;
}
double sum = std::accumulate(dest.begin(), dest.end(), 0.0);
double mean = sum / dest.size();
std::vector<double> diff(dest.size());
std::transform(dest.begin(), dest.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
double stdev = std::sqrt(sq_sum / dest.size());
double mini = mean - dynamic*stdev;
double maxi = mean + dynamic*stdev;
double range = maxi - mini;
if(vpMath::nul(range)) {
range = 1.0;
}
for(unsigned int cpt = 0; cpt < size; cpt++) {
I.bitmap[cpt].R = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 0] - mini) / range));
I.bitmap[cpt].G = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 1] - mini) / range));
I.bitmap[cpt].B = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 2] - mini) / range));
}
}
/*!
\brief Constructor : initialize a display to visualize a RGBa image
(32 bits).
\param I : Image to be displayed (note that image has to be initialized).
\param winx, winy : The window is set at position x,y (column index, row index).
\param title : Window's title.
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
*/
vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I, vpScaleType scaleType)
: vpDisplayWin32(new vpD3DRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
init(I);
}
void vpTemplateTrackerMIForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
{
//std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
dW=0;
int Nbpoint=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
double i2,j2;
double Tij;
double IW,dx,dy;
int cr,ct;
double er,et;
int i,j;
Nbpoint=0;
zeroProbabilities();
Warp->computeCoeff(p);
for(unsigned int point=0;point<templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
X2[0]=j;X2[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Nbpoint++;
Tij=ptTemplate[point].val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
ct=(int)((IW*(Nc-1))/255.);
cr=(int)((Tij*(Nc-1))/255.);
et=(IW*(Nc-1))/255.-ct;
er=((double)Tij*(Nc-1))/255.-cr;
//std::cout<<"test"<<std::endl;
Warp->dWarp(X1,X2,p,dW);
double *tptemp=new double[nbParam];
for(unsigned int it=0;it<nbParam;it++)
tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
if(ApproxHessian==HESSIAN_NONSECOND)
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
delete[] tptemp;
}
}
if(Nbpoint>0)
{
double MI;
computeProba(Nbpoint);
computeMI(MI);
computeHessien(Hdesire);
// double conditionnement=GetConditionnement(Hdesire);
// std::cout<<"conditionnement : "<<conditionnement<<std::endl;
vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
try
{
HLMdesireInverse=HLMdesire.inverseByLU();
}
catch(vpException &e)
{
//std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
//std::cout<<"Hdesire = "<<Hdesire<<std::endl;
//std::cout<<"\tEnd initialisation..."<<std::endl;
}
}
void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
dW=0;
double erreur=0;
int Nbpoint=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
double MI=0,MIprec=-1000;
MI_preEstimation=-getCost(I,p);
double i2,j2;
double Tij;
double IW,dx,dy;
//unsigned
int cr,ct;
double er,et;
double alpha=2.;
int i,j;
unsigned int iteration=0;
initPosEvalRMS(p);
do
{
if(iteration%5==0)
initHessienDesired(I);
Nbpoint=0;
MIprec=MI;
MI=0;
erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
int nthreads = omp_get_num_procs() ;
//std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
omp_set_num_threads(nthreads);
#pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
#endif
for(int point=0;point<(int)templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Nbpoint++;
Tij=ptTemplate[point].val;
//Tij=Iterateurvecteur->val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
ct=(int)((IW*(Nc-1))/255.);
cr=(int)((Tij*(Nc-1))/255.);
et=(IW*(Nc-1))/255.-ct;
er=((double)Tij*(Nc-1))/255.-cr;
//calcul de l'erreur
erreur+=(Tij-IW)*(Tij-IW);
//Calcul de l'histogramme joint par interpolation bilinÃaire (Bspline ordre 1)
Warp->dWarp(X1,X2,p,dW);
//double *tptemp=temp;
double *tptemp=new double[nbParam];;
for(unsigned int it=0;it<nbParam;it++)
tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
//*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
//std::cout<<cr<<" "<<ct<<" ; ";
if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
delete[] tptemp;
}
}
if(Nbpoint==0)
{
//std::cout<<"plus de point dans template suivi"<<std::endl;
diverge=true;
MI=0;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
else
{
computeProba(Nbpoint);
computeMI(MI);
//std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
computeHessien(H);
computeGradient();
vpMatrix::computeHLM(H,lambda,HLM);
try
{
switch(hessianComputation)
{
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
dp=gain*HLMdesireInverse*G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
if(HLM.cond()>HLMdesire.cond())
dp=gain*HLMdesireInverse*G;
else
dp=gain*0.2*HLM.inverseByLU()*G;
break;
default:
dp=gain*0.2*HLM.inverseByLU()*G;
break;
}
}
catch(vpException &e)
{
//std::cerr<<"probleme inversion"<<std::endl;
deletePosEvalRMS();
throw(e);
}
}
switch(minimizationMethod)
{
case vpTemplateTrackerMIForwardAdditional::USE_LMA:
{
vpColVector p_test_LMA(nbParam);
if(ApproxHessian==HESSIAN_NONSECOND)
p_test_LMA=p-100000.1*dp;
else
p_test_LMA=p+1.*dp;
MI=-getCost(I,p);
double MI_LMA=-getCost(I,p_test_LMA);
if(MI_LMA>MI)
{
p=p_test_LMA;
lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
}
else
{
lambda=(lambda*10.<1e6)?1e6:lambda*10.;
}
}
break;
case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
{
dp=-gain*6.0*G;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
dp=alpha*dp;
}
p+=1.*dp;
break;
}
case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
{
double s_scal_y;
if(iterationGlobale!=0)
{
vpColVector s_quasi=p-p_prec;
vpColVector y_quasi=G-G_prec;
s_scal_y=s_quasi.t()*y_quasi;
//if(s_scal_y!=0)//BFGS
// KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
//if(s_scal_y!=0)//DFP
if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
}
dp=-KQuasiNewton*G;
p_prec=p;
G_prec=G;
p-=1.01*dp;
}
break;
default:
{
if(ApproxHessian==HESSIAN_NONSECOND)
dp=-0.1*dp;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
//std::cout<<alpha<<std::endl;
dp=alpha*dp;
}
p+=1.*dp;
break;
}
}
computeEvalRMS(p);
iteration++;
iterationGlobale++;
}
while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
//while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
if(Nbpoint==0) {
//std::cout<<"plus de point dans template suivi"<<std::endl;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
nbIteration=iteration;
MI_postEstimation=-getCost(I,p);
if(MI_preEstimation>MI_postEstimation)
{
MI_postEstimation = -1;
}
deletePosEvalRMS();
}
/*!
Check if the polygon is visible in the image and if the angle between the normal
to the face and the line vector going from the optical center to the cog of the face is below
the given threshold.
To do that, the polygon is projected into the image thanks to the camera pose.
\param cMo : The pose of the camera.
\param alpha : Maximum angle to detect if the face is visible (in rad).
\param modulo : Indicates if the test should also consider faces that are not oriented
counter clockwise. If true, the orientation of the face is without importance.
\param cam : Camera parameters (intrinsics parameters)
\param I : Image used to consider level of detail.
\return Return true if the polygon is visible.
*/
bool
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
const vpCameraParameters &cam, const vpImage<unsigned char> &I)
{
// std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;
changeFrame(cMo);
if(nbpt <= 2) {
//Level of detail (LOD)
if(useLod) {
vpCameraParameters c = cam;
if(clippingFlag > 3) { // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
}
computePolygonClipped(c);
std::vector<vpImagePoint> roiImagePoints;
getRoiClipped(c, roiImagePoints);
if (roiImagePoints.size() == 2) {
double x1 = roiImagePoints[0].get_u();
double y1 = roiImagePoints[0].get_v();
double x2 = roiImagePoints[1].get_u();
double y2 = roiImagePoints[1].get_v();
double length = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
// std::cout << "Index=" << index << " ; Line length=" << length << " ; clippingFlag=" << clippingFlag << std::endl;
// vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index, length, minLineLengthThresh);
if (length < minLineLengthThresh) {
isvisible = false;
isappearing = false;
return false;
}
}
}
/* a line is always visible when LOD is not used */
isvisible = true;
isappearing = false;
return true ;
}
// If the polygon has no orientation, the angle check visibility is always valid.
// Feature mainly used for cylinders.
if(!hasOrientation)
{
isvisible = true;
isappearing = false;
return true;
}
//Check visibility from normal
//Newell's Method for calculating the normal of an arbitrary 3D polygon
//https://www.opengl.org/wiki/Calculating_a_Surface_Normal
vpColVector faceNormal(3);
vpColVector currentVertex, nextVertex;
for(unsigned int i = 0; i<nbpt; i++) {
currentVertex = p[i].cP;
nextVertex = p[(i+1) % nbpt].cP;
faceNormal[0] += (currentVertex[1] - nextVertex[1]) * (currentVertex[2] + nextVertex[2]);
faceNormal[1] += (currentVertex[2] - nextVertex[2]) * (currentVertex[0] + nextVertex[0]);
faceNormal[2] += (currentVertex[0] - nextVertex[0]) * (currentVertex[1] + nextVertex[1]);
}
faceNormal.normalize();
vpColVector e4(3) ;
vpPoint pt;
for (unsigned int i = 0; i < nbpt; i += 1){
pt.set_X(pt.get_X() + p[i].get_X());
pt.set_Y(pt.get_Y() + p[i].get_Y());
pt.set_Z(pt.get_Z() + p[i].get_Z());
}
e4[0] = -pt.get_X() / (double)nbpt;
e4[1] = -pt.get_Y() / (double)nbpt;
e4[2] = -pt.get_Z() / (double)nbpt;
e4.normalize();
double angle = acos(vpColVector::dotProd(e4, faceNormal));
// vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" << vpMath::deg(alpha) << std::endl;
if( angle < alpha || (modulo && (M_PI - angle) < alpha)) {
isvisible = true;
isappearing = false;
if (useLod) {
vpCameraParameters c = cam;
if(clippingFlag > 3) { // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
}
computePolygonClipped(c);
std::vector<vpImagePoint> roiImagePoints;
getRoiClipped(c, roiImagePoints);
vpPolygon roiPolygon(roiImagePoints);
double area = roiPolygon.getArea();
// std::cout << "After normal test ; Index=" << index << " ; area=" << area << " ; clippingFlag="
// << clippingFlag << std::endl;
if (area < minPolygonAreaThresh) {
isappearing = false;
isvisible = false;
return false;
}
}
return true;
}
if (angle < alpha+vpMath::rad(1) ){
isappearing = true;
}
else if (modulo && (M_PI - angle) < alpha+vpMath::rad(1) ){
isappearing = true;
}
else {
isappearing = false;
}
isvisible = false ;
return false ;
}
bool vpBlobsTargetTracker::track(const cv::Mat &cvI, const vpImage<unsigned char> &I )
{
if (m_state == detection || m_force_detection) {
//std::cout << "STATE: DETECTION "<< std::endl;
bool obj_found = false;
if (!m_manual_blob_init && !m_full_manual)
obj_found = m_colBlob.detect(cvI);
// Delete previuos list of blobs
m_blob_list.clear();
m_blob_list.resize(4);
m_initPose = true;
m_target_found = false;
if (obj_found || m_manual_blob_init || m_full_manual) {
try{
if (m_full_manual)
{
std::cout << "Full manual" << std::endl;
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Click on the 4 blobs", vpColor::red);
vpDisplay::flush(I);
for(std::list<vpDot2>::iterator it=m_blob_list.begin(); it != m_blob_list.end(); ++it)
{
(*it).setGraphics(true);
(*it).setGraphicsThickness(1);
(*it).initTracking(I);
(*it).track(I);
vpDisplay::flush(I);
}
m_state = tracking;
m_force_detection = false;
}
else {
// std::cout << "TARGET FOUND" << std::endl;
vpDot2 blob;
blob.setGraphics(true);
blob.setGraphicsThickness(1);
blob.setEllipsoidShapePrecision(0.9);
if (m_manual_blob_init)
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Click on the colored blob", vpColor::red);
vpDisplay::flush(I);
blob.initTracking(I);
m_manual_blob_init = false;
}
else
{
vpImagePoint cog = m_colBlob.getCog(0);
vpDisplay::displayCross(I,cog,10, vpColor::red,2 );
blob.initTracking(I,cog);
}
blob.track(I);
// printf("Dot characteristics: \n");
// printf(" width : %lf\n", blob.getWidth());
// printf(" height: %lf\n", blob.getHeight());
// printf(" area: %lf\n", blob.getArea());
// printf(" gray level min: %d\n", blob.getGrayLevelMin());
// printf(" gray level max: %d\n", blob.getGrayLevelMax());
// printf(" grayLevelPrecision: %lf\n", blob.getGrayLevelPrecision());
// printf(" sizePrecision: %lf\n", blob.getSizePrecision());
// printf(" ellipsoidShapePrecision: %lf\n", blob.getEllipsoidShapePrecision());
vpDot2 black_blob = blob;
black_blob.setGrayLevelMax(m_grayLevelMaxBlob);
black_blob.setGrayLevelMin(m_grayLevelMinBlob);
int i,j,aj,ai;
if(m_left_hand_target)
{
i = blob.getCog().get_i()-blob.getHeight()*2.3;
j = blob.getCog().get_j()-blob.getWidth()*3.3;
ai = blob.getHeight()*5;
aj = blob.getWidth()*5;
}
else
{
i = blob.getCog().get_i()-blob.getHeight();
j = blob.getCog().get_j()-blob.getWidth()*2.0;
ai = blob.getHeight()*4.5;
aj = blob.getWidth()*4;
}
//search similar blobs in the image and store them in blob_list
//black_blob.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), m_blob_list);
//vpDisplay::displayRectangle(I, i, j, ai, aj, vpColor::red, false, 1);
black_blob.searchDotsInArea(I, j, i, ai, aj, m_blob_list);
// vpDisplay::flush(I);
// vpDisplay::getClick(I,true);
m_blob_list.insert(m_blob_list.begin(),blob);
std::cout << "SIZE: " << m_blob_list.size() << std::endl;
if(m_blob_list.size() == m_numBlobs)
{
for(std::list<vpDot2>::iterator it = m_blob_list.begin(); it != m_blob_list.end(); ++it)
{
//it->setEllipsoidShapePrecision(0.8);
it->setEllipsoidShapePrecision(0.75);
}
if (1){
for(std::list<vpDot2>::iterator it = m_blob_list.begin(); it != m_blob_list.end(); ++it)
{
it->initTracking(I, it->getCog());
}
}
m_state = tracking;
m_force_detection = false;
}
else
std::cout << "Number blobs found is "<< m_blob_list.size() << ". Expected number: " << m_numBlobs << std::endl;
}
}
catch(vpException &e) {
std::cout << "Exception tracking detection: " << e.getStringMessage() << std::endl;
m_target_found = false;
}
}
}
else if (m_state == tracking) {
// std::cout << "STATE: TRACKING "<< std::endl;
try {
m_cog.set_uv(0.0,0.0);
for(std::list<vpDot2>::iterator it = m_blob_list.begin(); it != m_blob_list.end(); ++it)
{
it->track(I);
m_cog += it->getCog();
}
m_cog /= m_blob_list.size();
// Display the ACTUAL center of gravity of the object
//vpDisplay::displayCross(I,cog_tot,10, vpColor::blue,2 );
// std::vector<vpImagePoint> corners(m_blob_list.begin(),m_blob_list.end());
// Now we create a map of VpPoint in order to order the points
std::map< double,vpImagePoint> poly_verteces;
double theta;
for(std::list<vpDot2>::iterator it=m_blob_list.begin(); it != m_blob_list.end(); ++it)
{
// Get the cog of the blob
vpImagePoint cog = it->getCog();
theta = atan2(cog.get_v() - m_cog.get_v(), cog.get_u() - m_cog.get_u());
// Insert the vertexes in the map (ordered)
poly_verteces.insert ( std::pair<double,vpImagePoint>(theta,cog) );
}
// Now we create a Vector containing the ordered vertexes
std::vector<vpImagePoint> poly_vert;
int index_first= 0;
unsigned int count = 0;
for( std::map<double,vpImagePoint>::iterator it = poly_verteces.begin(); it!=poly_verteces.end(); ++it )
{
poly_vert.push_back( it->second );
if (m_blob_list.front().getCog() == it->second )
index_first = count;
count++;
}
std::rotate(poly_vert.begin(), poly_vert.begin() + index_first, poly_vert.end());
//std::cout << "---------------------------------------" << std::endl;
for(unsigned int j = 0; j<poly_vert.size();j++)
{
std::ostringstream s;
s << j;
vpDisplay::displayText(I, poly_vert[j], s.str(), vpColor::green);
//std::cout << "Cog blob " << j << " :" << poly_vert[j] << std::endl;
}
computePose(m_P, poly_vert, m_cam, m_initPose, m_cMo);
bool duplicate = false;
for(unsigned int i = 0; i < poly_vert.size()-1; i++)
{
for(unsigned int j = i+1; j < poly_vert.size(); j++)
{
//std::cout << "Distance " << i <<"-" << j << " :" << vpImagePoint::sqrDistance(poly_vert[i],poly_vert[j] )<< std::endl;
if (vpImagePoint::sqrDistance(poly_vert[i],poly_vert[j]) < 5.0)
duplicate = true;
}
}
//std::cout << "---------------------------------------" << std::endl;
// std::cout << "Number blobs found is "<< poly_vert.size() << ". Expected number: " << m_numBlobs << std::endl;
// int i = m_blob_list.front().getCog().get_i()-m_blob_list.front().getHeight()*2.3;
// int j = m_blob_list.front().getCog().get_j()-m_blob_list.front().getWidth()*3.3;
// unsigned int ai = m_blob_list.front().getHeight()*5;
// unsigned int aj =m_blob_list.front().getWidth()*5;
// vpDisplay::displayRectangle(I,i, j, ai,aj, vpColor::red,false,1);
if (duplicate)
{
m_target_found = false;
m_state = detection;
std::cout << "PROBLEM: tracking failed " << m_numBlobs << std::endl;
}
else if (poly_vert.size() != m_numBlobs)
{
m_target_found = false;
m_state = detection;
std::cout << "PROBLEM: Expected number: " << m_numBlobs << std::endl;
}
else
m_target_found = true;
}
catch(vpException &e) {
std::cout << "Exception tracking: " << e.getStringMessage() << std::endl;
m_state = detection;
m_target_found = false;
}
}
return m_target_found;
}
/*!
Computes the SURF points in the current image I and try to matched
them with the points in the reference list. Only the matched points
are stored.
\param I : The gray scaled image where the points are computed.
\return the number of point which have been matched.
*/
unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
{
IplImage* currentImage = NULL;
if((I.getWidth() % 8) == 0){
int height = (int)I.getHeight();
int width = (int)I.getWidth();
CvSize size = cvSize(width, height);
currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
currentImage->imageData = (char*)I.bitmap;
}else{
vpImageConvert::convert(I,currentImage);
}
/* we release the memory storage for the current points (it has to be kept
allocated for the get descriptor points, ...) */
if(storage_cur != NULL){
cvReleaseMemStorage(&storage_cur);
storage_cur = NULL;
}
storage_cur = cvCreateMemStorage(0);
cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
storage_cur, params );
CvSeqReader reader, kreader;
cvStartReadSeq( ref_keypoints, &kreader );
cvStartReadSeq( ref_descriptors, &reader );
std::list<int> indexImagePair;
std::list<int> indexReferencePair;
unsigned int nbrPair = 0;
for(int i = 0; i < ref_descriptors->total; i++ )
{
const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
const float* descriptor = (const float*)reader.ptr;
CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
int nearest_neighbor = naiveNearestNeighbor( descriptor,
kp->laplacian,
image_keypoints,
image_descriptors );
if( nearest_neighbor >= 0 )
{
indexReferencePair.push_back(i);
indexImagePair.push_back(nearest_neighbor);
nbrPair++;
}
}
std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
matchedReferencePoints.resize(0);
if (nbrPair == 0)
return (0);
currentImagePointsList.resize(nbrPair);
matchedReferencePoints.resize(nbrPair);
for (unsigned int i = 0; i < nbrPair; i++)
{
int index = *indexImagePairIter;
CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
currentImagePointsList[i].set_i(r1->pt.y);
currentImagePointsList[i].set_j(r1->pt.x);
matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
++indexImagePairIter;
++indexReferencePairIter;
}
if((I.getWidth() % 8) == 0){
currentImage->imageData = NULL;
cvReleaseImageHeader(¤tImage);
}else{
cvReleaseImage(¤tImage);
}
return nbrPair;
}