static void lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) { if (DEBUG_LEVEL1) std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl; try{ int i,imin; vpMatrix ata ; // A^T A ata = a.t()*a ; vpMatrix btb ; // B^T B btb = b.t()*b ; vpMatrix bta ; // B^T A bta = b.t()*a ; vpMatrix btb1 ; // (B^T B)^(-1) if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ; else btb1 = btb.pseudoInverse(); if (DEBUG_LEVEL1) { std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl; std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl; } vpMatrix r ; // (B^T B)^(-1) B^T A r = btb1*bta ; vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A e = - (a.t()*b) *r ; e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A if (DEBUG_LEVEL1) { std::cout << " E :" << std::endl << e << std::endl; } // vpColVector sv ; // vpMatrix v ; e.svd(x1,ata) ;// destructif sur e // calcul du vecteur propre de E correspondant a la valeur propre min. /* calcul de SVmax */ imin = 0; // FC : Pourquoi calculer SVmax ?????? // double svm = 0.0; // for (i=0;i<x1.getRows();i++) // { // if (x1[i] > svm) { svm = x1[i]; imin = i; } // } // svm *= EPS; /* pour le rang */ for (i=0;i<x1.getRows();i++) if (x1[i] < x1[imin]) imin = i; if (DEBUG_LEVEL1) { printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]); std::cout << " i_min " << imin << std::endl; } for (i=0;i<x1.getRows();i++) x1[i] = ata[i][imin]; x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1 if (DEBUG_LEVEL1) { std::cout << " X1 : " << x1.t() << std::endl; std::cout << " V : " << std::endl << ata << std::endl; } } catch(...) { vpERROR_TRACE(" ") ; throw ; } if (DEBUG_LEVEL1) std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl; }
/*! Compute the covariance matrix of the parameters x from a least squares minimisation defined as: WAx = Wb \param A : Matrix A from WAx = Wb. \param x : Vector x from WAx = Wb corresponding to the parameters to estimate. \param b : Vector b from WAx = Wb. \param W : Diagonal weigths matrix from WAx = Wb. */ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &W) { double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); return (A.t()*W*A).pseudoInverse()*sigma2; }