Visual Servoing Platform  version 3.6.1 under development (2024-06-15)
vpMatrix_covariance.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Covariance matrix computation.
33  *
34  * Authors:
35  * Aurelien Yol
36  *
37 *****************************************************************************/
38 
39 #include <cmath> // std::fabs()
40 #include <limits> // numeric_limits
41 
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>
48 
61 {
62  // double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To
63  // consider OLS Estimate for sigma
64  double denom = (static_cast<double>(A.getRows())); // To consider MLE Estimate for sigma
65 
66  if (denom <= std::numeric_limits<double>::epsilon()) {
68  "Impossible to compute covariance matrix: not enough data");
69  }
70 
71  // double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be
72  // equivalent to line bellow.
73  double sigma2 = (b - (A * x)).t() * (b - (A * x));
74 
75  sigma2 /= denom;
76 
77  return (A.t() * A).pseudoInverse(A.getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
78 }
79 
94  const vpMatrix &W)
95 {
96  double denom = 0.0;
97  vpMatrix W2(W.getCols(), W.getCols());
98  unsigned int w_cols = W.getCols();
99  for (unsigned int i = 0; i < w_cols; ++i) {
100  denom += W[i][i];
101  W2[i][i] = W[i][i] * W[i][i];
102  }
103 
104  if (denom <= std::numeric_limits<double>::epsilon()) {
106  "Impossible to compute covariance matrix: not enough data");
107  }
108 
109  // double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should
110  // be equivalent to line bellow.
111  double sigma2 = ((W * b) - (W * A * x)).t() * ((W * b) - (W * A * x));
112  sigma2 /= denom;
113 
114  return (A.t() * W2 * A).pseudoInverse(A.getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
115 }
116 
129  const vpMatrix &Ls)
130 {
131  vpMatrix Js;
132  vpColVector deltaP;
133  vpMatrix::computeCovarianceMatrixVVS(cMo, deltaS, Ls, Js, deltaP);
134 
135  return vpMatrix::computeCovarianceMatrix(Js, deltaP, deltaS);
136 }
137 
154  const vpMatrix &Ls, const vpMatrix &W)
155 {
156  vpMatrix Js;
157  vpColVector deltaP;
158  vpMatrix::computeCovarianceMatrixVVS(cMo, deltaS, Ls, Js, deltaP);
159 
160  return vpMatrix::computeCovarianceMatrix(Js, deltaP, deltaS, W);
161 }
162 
163 void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls,
164  vpMatrix &Js, vpColVector &deltaP)
165 {
166  // building Lp
167  vpMatrix LpInv(6, 6);
168  LpInv = 0;
169  LpInv[0][0] = -1.0;
170  LpInv[1][1] = -1.0;
171  LpInv[2][2] = -1.0;
172 
173  vpTranslationVector ctoInit;
174 
175  cMo.extract(ctoInit);
176  vpMatrix ctoInitSkew = ctoInit.skew();
177 
178  vpThetaUVector thetau;
179  cMo.extract(thetau);
180 
181  vpColVector tu(3);
182  for (unsigned int i = 0; i < 3; ++i) {
183  tu[i] = thetau[i];
184  }
185 
186  double theta = sqrt(tu.sumSquare());
187 
188  // --comment: declare variable Lthetau of three by three of type vpMatrix
189  vpMatrix LthetauInvAnalytic(3, 3);
190  vpMatrix I3(3, 3);
191  I3.eye();
192  // --comment: Lthetau equals -I3;
193  LthetauInvAnalytic = -I3;
194 
195  if ((theta / (2.0 * M_PI)) > std::numeric_limits<double>::epsilon()) {
196  // Computing [theta/2 u]_x
197  vpColVector theta2u(3);
198  for (unsigned int i = 0; i < 3; ++i) {
199  theta2u[i] = tu[i] / 2.0;
200  }
201  vpMatrix theta2u_skew = vpColVector::skew(theta2u);
202 
203  vpColVector u(3);
204  for (unsigned int i = 0; i < 3; ++i) {
205  u[i] = tu[i] / theta;
206  }
207  vpMatrix u_skew = vpColVector::skew(u);
208 
209  LthetauInvAnalytic +=
210  -((vpMath::sqr(vpMath::sinc(theta / 2.0)) * theta2u_skew) - ((1.0 - vpMath::sinc(theta)) * u_skew * u_skew));
211  }
212 
213  // --comment: vpMatrix LthetauInv equals Lthetau dot inverseByLU()
214 
215  ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
216 
217  for (unsigned int a = 0; a < 3; ++a) {
218  for (unsigned int b = 0; b < 3; ++b) {
219  LpInv[a][b + 3] = ctoInitSkew[a][b];
220  }
221  }
222 
223  for (unsigned int a = 0; a < 3; ++a) {
224  for (unsigned int b = 0; b < 3; ++b) {
225  LpInv[a + 3][b + 3] = LthetauInvAnalytic[a][b];
226  }
227  }
228 
229  // Building Js
230  Js = Ls * LpInv;
231 
232  // building deltaP
233  deltaP = Js.pseudoInverse(Js.getRows() * std::numeric_limits<double>::epsilon()) * deltaS;
234 }
unsigned int getCols() const
Definition: vpArray2D.h:330
unsigned int getRows() const
Definition: vpArray2D.h:340
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
static vpMatrix skew(const vpColVector &v)
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sinc(double x)
Definition: vpMath.cpp:270
static double sqr(double x)
Definition: vpMath.h:203
error that can be emitted by the vpMatrix class and its derivatives
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
vpMatrix t() const
Definition: vpMatrix.cpp:473
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2326
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.