void mrpt::vision::pnp::posit::POS()
{
Eigen::Vector3d I0, J0 , r1, r2, r3;
double I0_norm, J0_norm;
int i;
double scale;
for(i=0;i<3;i++)
{
I0(i)=obj_matrix.row(i).dot(img_vecs.col(0));
J0(i)=obj_matrix.row(i).dot(img_vecs.col(1));
}
I0_norm=I0.norm();
J0_norm=J0.norm();
scale=(I0_norm + J0_norm)/2;
/*Computing TRANSLATION */
t(0)=img_pts(0,0)/scale;
t(1)=img_pts(0,1)/scale;
t(2)=f/scale;
/* Computing ROTATION */
r1=I0/I0_norm;
r2=J0/J0_norm;
r3=r1.cross(r2);
R.row(0)=r1;
R.row(1)=r2;
R.row(2)=r3;
}
// This evaluates the error term and additionally computes
// the Jacobians in the minimal internal representation.
bool SpeedAndBiasError::EvaluateWithMinimalJacobians(
double const* const * parameters, double* residuals, double** jacobians,
double** jacobiansMinimal) const {
// compute error
Eigen::Map<const okvis::SpeedAndBias> estimate(parameters[0]);
okvis::SpeedAndBias error = measurement_ - estimate;
// weigh it
Eigen::Map<Eigen::Matrix<double, 9, 1> > weighted_error(residuals);
weighted_error = squareRootInformation_ * error;
// compute Jacobian - this is rather trivial in this case...
if (jacobians != NULL) {
if (jacobians[0] != NULL) {
Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor> > J0(
jacobians[0]);
J0 = -squareRootInformation_ * Eigen::Matrix<double, 9, 9>::Identity();
}
}
if (jacobiansMinimal != NULL) {
if (jacobiansMinimal[0] != NULL) {
Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor> > J0min(
jacobiansMinimal[0]);
J0min = -squareRootInformation_ * Eigen::Matrix<double, 9, 9>::Identity();
}
}
return true;
}
void Con2dP2::setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
{
// node references
Node2d &nr = nodes[ndr];
Matrix<double,3,1> &tr = nr.trans;
Node2d &n1 = nodes[nd1];
Matrix<double,3,1> &t1 = n1.trans;
// first get the second frame in first frame coords
Eigen::Matrix<double,2,1> pc = nr.w2n * t1;
// Jacobians wrt first frame parameters
// translational part of 0p1 wrt translational vars of p0
// this is just -R0' [from 0t1 = R0'(t1 - t0)]
J0.block<2,2>(0,0) = -nr.w2n.block<2,2>(0,0);
// translational part of 0p1 wrt rotational vars of p0
// dR'/dq * [pw - t]
Eigen::Matrix<double,2,1> pwt;
pwt = (t1-tr).head(2); // transform translations
// dx
Eigen::Matrix<double,2,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
J0.block<2,1>(0,2) = dp;
// rotational part of 0p1 wrt translation vars of p0 => zero
J0.block<1,2>(2,0).setZero();
// rotational part of 0p1 wrt rotational vars of p0
// from 0q1 = (q1-q0)
J0(2,2) = -1.0;
J0t = J0.transpose();
// cout << endl << "J0 " << ndr << endl << J0 << endl;
// Jacobians wrt second frame parameters
// translational part of 0p1 wrt translational vars of p1
// this is just R0' [from 0t1 = R0'(t1 - t0)]
J1.block<2,2>(0,0) = nr.w2n.block<2,2>(0,0);
// translational part of 0p1 wrt rotational vars of p1: zero
J1.block<2,1>(0,2).setZero();
// rotational part of 0p1 wrt translation vars of p0 => zero
J1.block<1,2>(2,0).setZero();
// rotational part of 0p1 wrt rotational vars of p0
// from 0q1 = (q1-q0)
J1(2,2) = 1.0;
J1t = J1.transpose();
// cout << endl << "J1 " << nd1 << endl << J1 << endl;
};
void
vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
#endif
unsigned int i,j,k ;
if (c3d !=NULL) delete []c3d ;
c3d = new vpPoint[npt] ;
vpPoint p0 = listP.front() ;
vpPoint P ;
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-1,3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
for (i=1 ; i < npt ; i++)
{
a[i-1][0]=c3d[i].get_oX();
a[i-1][1]=c3d[i].get_oY();
a[i-1][2]=c3d[i].get_oZ();
}
// calcul a^T a
vpMatrix ata ;
ata = a.t()*a ;
/* essai FC pour debug SVD */
/*
vpMatrix ata_old ;
ata_old = a.t()*a ;
vpMatrix ata((ata_old.getRows()-1),(ata_old.getCols()-1)) ;
for (i=0;i<ata.getRows();i++)
for (j=0;j<ata.getCols();j++) ata[i][j] = ata_old[i][j];
*/
vpMatrix ata_sav;
ata_sav = ata;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata<<std::endl ;
}
#endif
// calcul (a^T a)^-1
vpMatrix ata1(ata.getRows(),ata.getCols()) ;
vpMatrix v(ata.getRows(),ata.getCols());
vpColVector sv(ata.getRows());
// ata1 = ata.i() ;
unsigned int imin = 0;
double s = 0.0;
//calcul de ata^-1
ata.svd(sv,v) ;
unsigned int nc = sv.getRows() ;
for (i=0; i < nc ; i++)
if (sv[i] > s) s = sv[i];
s *= 0.0002;
int irank = 0;
for (i=0;i<nc;i++)
if (sv[i] > s ) irank++;
double svm = 100.0;
for (i = 0; i < nc; i++)
if (sv[i] < svm) { imin = i; svm = sv[i]; }
#if (DEBUG_LEVEL2)
{
std::cout << "rang: " << irank << std::endl ;;
std::cout <<"imin = " << imin << std::endl ;
std::cout << "sv " << sv.t() << std::endl ;
}
#endif
for (i=0 ; i < ata.getRows() ; i++)
for (j=0 ; j < ata.getCols() ; j++)
{
ata1[i][j] = 0.0;
for (k=0 ; k < nc ; k++)
if (sv[k] > s)
ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
}
vpMatrix b ; // b=(at a)^-1*at
b = ata1*a.t() ;
//calcul de U
vpColVector U(3) ;
U = ata.column(imin+1) ;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
std::cout << "ata1" << std::endl <<ata1<<std::endl ;
std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
std::cout << "b" << std::endl << b ;
std::cout << "U " << U.t() << std::endl ;
}
#endif
vpColVector xi(npt) ;
vpColVector yi(npt) ;
//calcul de la premiere solution
for (i = 0; i < npt; i++)
{
xi[i] = c3d[i].get_x() ;
yi[i] = c3d[i].get_y() ;
}
vpColVector I0(3) ; I0 = 0 ;
vpColVector J0(3) ; J0 = 0 ;
vpColVector I(3) ;
vpColVector J(3) ;
for (i=1;i<npt;i++)
{
I0[0] += b[0][i-1] * (xi[i]-xi[0]);
I0[1] += b[1][i-1] * (xi[i]-xi[0]);
I0[2] += b[2][i-1] * (xi[i]-xi[0]);
J0[0] += b[0][i-1] * (yi[i]-yi[0]);
J0[1] += b[1][i-1] * (yi[i]-yi[0]);
J0[2] += b[2][i-1] * (yi[i]-yi[0]);
}
#if (DEBUG_LEVEL2)
{
std::cout << "I0 "<<I0.t() ;
std::cout << "J0 "<<J0.t() ;
}
#endif
s = -2.0*vpColVector::dotProd(I0,J0);
double c = J0.sumSquare() - I0.sumSquare() ;
double r,theta,si,co ;
calculRTheta(s, c, r, theta);
co = cos(theta);
si = sin(theta);
// calcul de la premiere solution
I = I0 + U*r*co ;
J = J0 + U*r*si ;
vpHomogeneousMatrix cMo1f ;
calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
int erreur1 = calculArbreDementhon(b, U, cMo1f);
// calcul de la deuxieme solution
I = I0 - U*r*co ;
J = J0 - U*r*si ;
vpHomogeneousMatrix cMo2f;
calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
int erreur2 = calculArbreDementhon(b, U, cMo2f);
if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
if ((erreur1 == 0) && (erreur2 == 0))
{
double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
}
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 ;
#if (DEBUG_LEVEL1)
std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl ;
#endif
}
int
vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
#endif
unsigned int i, k;
int erreur = 0;
unsigned int cpt;
double s,c,si,co;
double smin,smin_old, s1,s2;
double r, theta;
vpHomogeneousMatrix cMo1,cMo2,cMo_old;
unsigned int iter_max = 20;
vpMatrix eps(iter_max+1,npt) ;
// on test si tous les points sont devant la camera
for(i = 0; i < npt; i++)
{
double z ;
z = cMo[2][0]*c3d[i].get_oX()+cMo[2][1]*c3d[i].get_oY()+cMo[2][2]*c3d[i].get_oZ() + cMo[2][3];
if (z <= 0.0) erreur = -1;
}
smin = sqrt(computeResidualDementhon(cMo)/npt) ;
vpColVector xi(npt) ;
vpColVector yi(npt) ;
if (erreur==0)
{
k=0;
for(i = 0; i < npt; i++)
{
xi[k] = c3d[i].get_x();
yi[k] = c3d[i].get_y();
if (k != 0)
{ // On ne prend pas le 1er point
eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
cMo[2][1]*c3d[i].get_oY() +
cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
}
k++;
}
vpColVector I0(3) ;
vpColVector J0(3) ;
vpColVector I(3) ;
vpColVector J(3) ;
vpHomogeneousMatrix cMo_old ;
smin_old = 2*smin ;
cpt = 0;
while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
{
#if (DEBUG_LEVEL2)
{
std::cout << "cpt " << cpt << std::endl ;
std::cout << "smin_old " << smin_old << std::endl ;
std::cout << "smin " << smin << std::endl ;
}
#endif
smin_old = smin;
cMo_old = cMo;
I0 = 0 ;
J0 = 0 ;
for (i=1;i<npt;i++)
{
s = (1.0+eps[cpt][i])*xi[i] - xi[0];
I0[0] += b[0][i-1] * s;
I0[1] += b[1][i-1] * s;
I0[2] += b[2][i-1] * s;
s = (1.0+eps[cpt][i])*yi[i] - yi[0];
J0[0] += b[0][i-1] * s;
J0[1] += b[1][i-1] * s;
J0[2] += b[2][i-1] * s;
}
s = -2.0*(vpColVector::dotProd(I0,J0));
c = J0.sumSquare() - I0.sumSquare() ;
calculRTheta(s,c,r,theta);
co = cos(theta);
si = sin(theta);
/* 1ere branche */
I = I0 + U*r*co ;
J = J0 + U*r*si ;
#if (DEBUG_LEVEL3)
{
std::cout << "I " << I.t() ;
std::cout << "J " << J.t() ;
}
#endif
calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
#if (DEBUG_LEVEL3)
std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
#endif
/* 2eme branche */
I = I0 - U*r*co ;
J = J0 - U*r*si ;
#if (DEBUG_LEVEL3)
{
std::cout << "I " << I.t() ;
std::cout << "J " << J.t() ;
}
#endif
calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
#if (DEBUG_LEVEL3)
std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
#endif
cpt ++;
if (s1 <= s2)
{
smin = s1;
k = 0;
for(i = 0; i < npt; i++)
{
if (k != 0) { // On ne prend pas le 1er point
eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
+ cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
}
k++;
}
cMo = cMo1 ;
}
else
{
smin = s2;
k = 0;
for(i = 0; i < npt; i++)
{
if (k != 0) { // On ne prend pas le 1er point
eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
+ cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
}
k++;
}
cMo = cMo2 ;
}
if (smin > smin_old)
{
#if (DEBUG_LEVEL2)
std::cout << "Divergence " << std::endl ;
#endif
cMo = cMo_old ;
}
#if (DEBUG_LEVEL2)
{
std::cout << "s1 = " << s1 << std::endl ;
std::cout << "s2 = " << s2 << std::endl ;
std::cout << "smin = " << smin << std::endl ;
std::cout << "smin_old = " << smin_old << std::endl ;
}
#endif
}
}
#if (DEBUG_LEVEL1)
std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
#endif
return erreur ;
}
