42 #include <visp3/core/vpColVector.h>
43 #include <visp3/core/vpConfig.h>
44 #include <visp3/core/vpHomogeneousMatrix.h>
45 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpMatrixException.h>
47 #include <visp3/core/vpTranslationVector.h>
63 double denom = ((double)(A.
getRows()));
65 if (denom <= std::numeric_limits<double>::epsilon())
67 "Impossible to compute covariance matrix: not enough data");
71 double sigma2 = (b - (A * x)).t() * (b - (A * x));
96 for (
unsigned int i = 0; i < W.
getCols(); i++) {
98 W2[i][i] = W[i][i] * W[i][i];
101 if (denom <= std::numeric_limits<double>::epsilon())
103 "Impossible to compute covariance matrix: not enough data");
107 double sigma2 = (W * b - (W * A * x)).t() * (W * b - (W * A * x));
110 return (A.
t() * (W2)*A).pseudoInverse(A.
getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
178 for (
unsigned int i = 0; i < 3; i++)
181 double theta = sqrt(tu.sumSquare());
188 LthetauInvAnalytic = -I3;
190 if (theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon()) {
193 for (
unsigned int i = 0; i < 3; i++) {
194 theta2u[i] = tu[i] / 2.0;
199 for (
unsigned int i = 0; i < 3; i++) {
200 u[i] = tu[i] / theta;
206 LthetauInvAnalytic +=
212 ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
214 for (
unsigned int a = 0; a < 3; a++)
215 for (
unsigned int b = 0; b < 3; b++)
216 LpInv[a][b + 3] = ctoInitSkew[a][b];
218 for (
unsigned int a = 0; a < 3; a++)
219 for (
unsigned int b = 0; b < 3; b++)
220 LpInv[a + 3][b + 3] = LthetauInvAnalytic[a][b];
unsigned int getCols() const
unsigned int getRows() const
Implementation of column vector and the associated operations.
static vpMatrix skew(const vpColVector &v)
@ divideByZeroError
Division by zero.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sinc(double x)
static double sqr(double x)
error that can be emitted by the vpMatrix class and its derivatives
Implementation of a matrix and operations on matrices.
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.