float *RK2(float told,float xold,float yold,float h)
{
float xp1;
float yp1;
float tmi;
float xmi;
float ymi;
float xp2;
float yp2;
float *pos;
float tnew;
float xnew;
float ynew;
pos=malloc(sizeof(float)*2);
xp1=xprim(xold,yold);
yp1=yprim(xold,yold);
tmi=told+h/2;
xmi=xold+(h/2)*xp1;
ymi=yold+(h/2)*yp1;
xp2=xprim(xmi,ymi);
yp2=yprim(xmi,ymi);
tnew=told+h;
xnew=xold+h*xp2;
ynew=yold+h*yp2;
pos[0]=tnew;
pos[1]=xnew;
pos[2]=ynew;
return pos;
}
void
vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
{
double normI = 0., normJ = 0.;
double Z0 = 0.;
double seuil=1.0;
double f=1.;
// CPoint c3d[npt] ;
if (c3d !=NULL) delete [] c3d ;
c3d = new vpPoint[npt] ;
vpPoint p0 = listP.front() ;
vpPoint P ;
int i=0;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it ;
c3d[i] = P ;
c3d[i].set_oX(P.get_oX()-p0.get_oX()) ;
c3d[i].set_oY(P.get_oY()-p0.get_oY()) ;
c3d[i].set_oZ(P.get_oZ()-p0.get_oZ()) ;
++i;
}
vpMatrix a ;
try{
a.resize(npt,3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
for (unsigned int i=0 ; i < npt ; i++)
{
a[i][0]=c3d[i].get_oX();
a[i][1]=c3d[i].get_oY();
a[i][2]=c3d[i].get_oZ();
}
//std::cout << a << std::endl ;
// calcul a^T a
vpMatrix ata ;
ata = a.t()*a ;
// calcul (a^T a)^-1 par decomposition LU
vpMatrix ata1 ;
ata1 = ata.pseudoInverse(1e-6) ; //InverseByLU() ;
vpMatrix b ;
b = (a*ata1).t() ;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata<<std::endl ;
std::cout << "ata1" << std::endl <<ata1<<std::endl ;
std::cout<< " ata*ata1" << std::endl << ata*ata1 ;
std::cout<< " b" << std::endl << (a*ata1).t() ;
}
#endif
// calcul de la premiere solution
vpColVector eps(npt) ;
eps =0 ;
int cpt = 0 ;
vpColVector I, J, k ;
try{
I.resize(3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
try{
J.resize(3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
try {
k.resize(3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
while(cpt < 20)
{
I = 0 ;
J = 0 ;
vpColVector xprim(npt) ;
vpColVector yprim(npt) ;
for (unsigned int i=0;i<npt;i++)
{
xprim[i]=(1+ eps[i])*c3d[i].get_x() - c3d[0].get_x();
yprim[i]=(1+ eps[i])*c3d[i].get_y() - c3d[0].get_y();
}
I = b*xprim ;
J = b*yprim ;
normI = sqrt(I.sumSquare()) ;
normJ = sqrt(J.sumSquare()) ;
I = I/normI ;
J = J/normJ ;
if (normI+normJ < 1e-10)
{
vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
throw(vpException(vpException::divideByZeroError,
"division by zero ")) ;
}
k = vpColVector::cross(I,J) ;
Z0=2*f/(normI+normJ);
cpt=cpt+1; seuil=0.0;
for (unsigned int i=0; i<npt; i++)
{
double epsi_1 = eps[i] ;
eps[i]=(c3d[i].get_oX()*k[0]+c3d[i].get_oY()*k[1]+c3d[i].get_oZ()*k[2])/Z0;
seuil+=fabs(eps[i]-epsi_1);
}
if (npt==0)
{
vpERROR_TRACE( " npt = 0, division par zero ");
throw(vpException(vpException::divideByZeroError,
"division by zero ")) ;
}
seuil/=npt;
}
k.normalize();
J = vpColVector::cross(k,I) ;
/*matrice de passage*/
cMo[0][0]=I[0];
cMo[0][1]=I[1];
cMo[0][2]=I[2];
cMo[0][3]=c3d[0].get_x()*2/(normI+normJ);
cMo[1][0]=J[0];
cMo[1][1]=J[1];
cMo[1][2]=J[2];
cMo[1][3]=c3d[0].get_y()*2/(normI+normJ);
cMo[2][0]=k[0];
cMo[2][1]=k[1];
cMo[2][2]=k[2];
cMo[2][3]=Z0;
cMo[0][3] -= (p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2]);
cMo[1][3] -= (p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2]);
cMo[2][3] -= (p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2]);
delete [] c3d ; c3d = NULL ;
}
