41 #include <visp3/core/vpConfig.h> 42 #include <visp3/core/vpMatrix.h> 43 #include <visp3/core/vpHomogeneousMatrix.h> 44 #include <visp3/core/vpColVector.h> 45 #include <visp3/core/vpTranslationVector.h> 46 #include <visp3/core/vpMatrixException.h> 62 double denom = ((double)(A.
getRows()));
64 if(denom <= std::numeric_limits<double>::epsilon())
68 double sigma2 = (b - (A * x)).t() * (b - (A * x));
91 for(
unsigned int i = 0 ; i < W.
getCols() ; i++){
93 W2[i][i] = W[i][i]*W[i][i];
96 if(denom <= std::numeric_limits<double>::epsilon())
100 double sigma2 = (W * b - (W * A * x)).t() * (W*b - (W * A * x));
103 return (A.
t()*(W2)*A).pseudoInverse(A.
getCols()*std::numeric_limits<double>::epsilon())*sigma2;
167 for(
unsigned int i = 0 ; i < 3 ; i++)
177 LthetauInvAnalytic = -I3;
179 if(theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon())
183 for (
unsigned int i=0 ; i < 3 ; i++) {
184 theta2u[i] = tu[i]/2.0 ;
189 for (
unsigned int i=0 ; i < 3 ; i++) {
200 ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
202 for(
unsigned int a = 0 ; a < 3 ; a++)
203 for(
unsigned int b = 0 ; b < 3 ; b++)
204 LpInv[a][b+3] = ctoInitSkew[a][b];
206 for(
unsigned int a = 0 ; a < 3 ; a++)
207 for(
unsigned int b = 0 ; b < 3 ; b++)
208 LpInv[a+3][b+3] = LthetauInvAnalytic[a][b];
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
void extract(vpRotationMatrix &R) const
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
static double sinc(double x)
unsigned int getCols() const
Return the number of columns of the 2D array.
static double sqr(double x)
Implementation of column vector and the associated operations.
error that can be emited by the vpMatrix class and its derivates
static vpMatrix skew(const vpColVector &v)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.