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 = (
static_cast<double>(A.
getRows()));
65 if (denom <= std::numeric_limits<double>::epsilon()) {
67 "Impossible to compute covariance matrix: not enough data");
72 double sigma2 = (b - (A * x)).t() * (b - (A * x));
97 unsigned int w_cols = W.
getCols();
98 for (
unsigned int i = 0; i < w_cols; ++i) {
100 W2[i][i] = W[i][i] * W[i][i];
103 if (denom <= std::numeric_limits<double>::epsilon()) {
105 "Impossible to compute covariance matrix: not enough data");
110 double sigma2 = ((W * b) - (W * A * x)).
t() * ((W * b) - (W * A * x));
113 return (A.
t() * W2 * A).
pseudoInverse(A.
getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
181 for (
unsigned int i = 0; i < 3; ++i) {
185 double theta = sqrt(tu.sumSquare());
192 LthetauInvAnalytic = -I3;
194 if ((theta / (2.0 * M_PI)) > std::numeric_limits<double>::epsilon()) {
197 for (
unsigned int i = 0; i < 3; ++i) {
198 theta2u[i] = tu[i] / 2.0;
203 for (
unsigned int i = 0; i < 3; ++i) {
204 u[i] = tu[i] / theta;
208 LthetauInvAnalytic +=
214 ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
216 for (
unsigned int a = 0; a < 3; ++a) {
217 for (
unsigned int b = 0; b < 3; ++b) {
218 LpInv[a][b + 3] = ctoInitSkew[a][b];
222 for (
unsigned int a = 0; a < 3; ++a) {
223 for (
unsigned int b = 0; b < 3; ++b) {
224 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.