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 ; }