Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMatrix_covariance.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Covariance matrix computation.
32  *
33  * Authors:
34  * Aurelien Yol
35  *
36  *****************************************************************************/
37 
38 #include <limits> // numeric_limits
39 #include <cmath> // std::fabs()
40 
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>
47 
48 
60 {
61 // double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To consider OLS Estimate for sigma
62  double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma
63 
64  if(denom <= std::numeric_limits<double>::epsilon())
65  throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
66 
67 // double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be equivalent to line bellow.
68  double sigma2 = (b - (A * x)).t() * (b - (A * x));
69 
70  sigma2 /= denom;
71 
72  return (A.t()*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
73 }
74 
88 {
89  double denom = 0.0;
90  vpMatrix W2(W.getCols(),W.getCols());
91  for(unsigned int i = 0 ; i < W.getCols() ; i++){
92  denom += W[i][i];
93  W2[i][i] = W[i][i]*W[i][i];
94  }
95 
96  if(denom <= std::numeric_limits<double>::epsilon())
97  throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
98 
99 // double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should be equivalent to line bellow.
100  double sigma2 = (W * b - (W * A * x)).t() * (W*b - (W * A * x));
101  sigma2 /= denom;
102 
103  return (A.t()*(W2)*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
104 }
105 
116 vpMatrix
118 {
119  vpMatrix Js;
120  vpColVector deltaP;
121  vpMatrix::computeCovarianceMatrixVVS(cMo,deltaS,Ls,Js,deltaP);
122 
123  return vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS);
124 }
125 
138 vpMatrix
140 {
141  vpMatrix Js;
142  vpColVector deltaP;
143  vpMatrix::computeCovarianceMatrixVVS(cMo,deltaS,Ls,Js,deltaP);
144 
145  return vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS,W);
146 }
147 
148 void
149 vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, vpMatrix &Js, vpColVector &deltaP)
150 {
151  //building Lp
152  vpMatrix LpInv(6,6);
153  LpInv = 0;
154  LpInv[0][0] = -1.0;
155  LpInv[1][1] = -1.0;
156  LpInv[2][2] = -1.0;
157 
158  vpTranslationVector ctoInit;
159 
160  cMo.extract(ctoInit);
161  vpMatrix ctoInitSkew = ctoInit.skew();
162 
163  vpThetaUVector thetau;
164  cMo.extract(thetau);
165 
166  vpColVector tu(3);
167  for(unsigned int i = 0 ; i < 3 ; i++)
168  tu[i] = thetau[i];
169 
170  double theta = sqrt(tu.sumSquare()) ;
171 
172 // vpMatrix Lthetau(3,3);
173  vpMatrix LthetauInvAnalytic(3,3);
174  vpMatrix I3(3,3);
175  I3.eye();
176 // Lthetau = -I3;
177  LthetauInvAnalytic = -I3;
178 
179  if(theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon())
180  {
181  // Computing [theta/2 u]_x
182  vpColVector theta2u(3) ;
183  for (unsigned int i=0 ; i < 3 ; i++) {
184  theta2u[i] = tu[i]/2.0 ;
185  }
186  vpMatrix theta2u_skew = vpColVector::skew(theta2u);
187 
188  vpColVector u(3) ;
189  for (unsigned int i=0 ; i < 3 ; i++) {
190  u[i] = tu[i]/theta ;
191  }
192  vpMatrix u_skew = vpColVector::skew(u);
193 
194 // Lthetau += (theta2u_skew - (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew);
195  LthetauInvAnalytic += -(vpMath::sqr(vpMath::sinc(theta/2.0)) * theta2u_skew - (1.0-vpMath::sinc(theta))*u_skew*u_skew);
196  }
197 
198 // vpMatrix LthetauInv = Lthetau.inverseByLU();
199 
200  ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
201 
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];
205 
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];
209 
210  // Building Js
211  Js = Ls * LpInv;
212 
213  // building deltaP
214  deltaP = (Js).pseudoInverse(Js.getRows()*std::numeric_limits<double>::epsilon()) * deltaS;
215 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
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 getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
static double sinc(double x)
Definition: vpMath.cpp:169
static double sqr(double x)
Definition: vpMath.h:110
void extract(vpRotationMatrix &R) const
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vpMatrix t() const
Definition: vpMatrix.cpp:207
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
error that can be emited by the vpMatrix class and its derivates
static vpMatrix skew(const vpColVector &v)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.