Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpMatrix_covariance.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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 
60 {
61  // double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To
62  // consider OLS Estimate for sigma
63  double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma
64 
65  if (denom <= std::numeric_limits<double>::epsilon())
67  "Impossible to compute covariance matrix: not enough data");
68 
69  // double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be
70  // equivalent to line bellow.
71  double sigma2 = (b - (A * x)).t() * (b - (A * x));
72 
73  sigma2 /= denom;
74 
75  return (A.t() * A).pseudoInverse(A.getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
76 }
77 
92  const vpMatrix &W)
93 {
94  double denom = 0.0;
95  vpMatrix W2(W.getCols(), W.getCols());
96  for (unsigned int i = 0; i < W.getCols(); i++) {
97  denom += W[i][i];
98  W2[i][i] = W[i][i] * W[i][i];
99  }
100 
101  if (denom <= std::numeric_limits<double>::epsilon())
103  "Impossible to compute covariance matrix: not enough data");
104 
105  // double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should
106  // be equivalent to line bellow.
107  double sigma2 = (W * b - (W * A * x)).t() * (W * b - (W * A * x));
108  sigma2 /= denom;
109 
110  return (A.t() * (W2)*A).pseudoInverse(A.getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
111 }
112 
125  const vpMatrix &Ls)
126 {
127  vpMatrix Js;
128  vpColVector deltaP;
129  vpMatrix::computeCovarianceMatrixVVS(cMo, deltaS, Ls, Js, deltaP);
130 
131  return vpMatrix::computeCovarianceMatrix(Js, deltaP, deltaS);
132 }
133 
150  const vpMatrix &Ls, const vpMatrix &W)
151 {
152  vpMatrix Js;
153  vpColVector deltaP;
154  vpMatrix::computeCovarianceMatrixVVS(cMo, deltaS, Ls, Js, deltaP);
155 
156  return vpMatrix::computeCovarianceMatrix(Js, deltaP, deltaS, W);
157 }
158 
159 void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls,
160  vpMatrix &Js, vpColVector &deltaP)
161 {
162  // building Lp
163  vpMatrix LpInv(6, 6);
164  LpInv = 0;
165  LpInv[0][0] = -1.0;
166  LpInv[1][1] = -1.0;
167  LpInv[2][2] = -1.0;
168 
169  vpTranslationVector ctoInit;
170 
171  cMo.extract(ctoInit);
172  vpMatrix ctoInitSkew = ctoInit.skew();
173 
174  vpThetaUVector thetau;
175  cMo.extract(thetau);
176 
177  vpColVector tu(3);
178  for (unsigned int i = 0; i < 3; i++)
179  tu[i] = thetau[i];
180 
181  double theta = sqrt(tu.sumSquare());
182 
183  // vpMatrix Lthetau(3,3);
184  vpMatrix LthetauInvAnalytic(3, 3);
185  vpMatrix I3(3, 3);
186  I3.eye();
187  // Lthetau = -I3;
188  LthetauInvAnalytic = -I3;
189 
190  if (theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon()) {
191  // Computing [theta/2 u]_x
192  vpColVector theta2u(3);
193  for (unsigned int i = 0; i < 3; i++) {
194  theta2u[i] = tu[i] / 2.0;
195  }
196  vpMatrix theta2u_skew = vpColVector::skew(theta2u);
197 
198  vpColVector u(3);
199  for (unsigned int i = 0; i < 3; i++) {
200  u[i] = tu[i] / theta;
201  }
202  vpMatrix u_skew = vpColVector::skew(u);
203 
204  // Lthetau += (theta2u_skew -
205  // (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew);
206  LthetauInvAnalytic +=
207  -(vpMath::sqr(vpMath::sinc(theta / 2.0)) * theta2u_skew - (1.0 - vpMath::sinc(theta)) * u_skew * u_skew);
208  }
209 
210  // vpMatrix LthetauInv = Lthetau.inverseByLU();
211 
212  ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
213 
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];
217 
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];
221 
222  // Building Js
223  Js = Ls * LpInv;
224 
225  // building deltaP
226  deltaP = (Js).pseudoInverse(Js.getRows() * std::numeric_limits<double>::epsilon()) * deltaS;
227 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
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
Definition: vpArray2D.h:289
void extract(vpRotationMatrix &R) const
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
static double sinc(double x)
Definition: vpMath.cpp:210
unsigned int getCols() const
Definition: vpArray2D.h:279
vpMatrix t() const
Definition: vpMatrix.cpp:464
static double sqr(double x)
Definition: vpMath.h:116
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double sumSquare() const
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.
void eye()
Definition: vpMatrix.cpp:449
Implementation of a rotation vector as axis-angle minimal representation.