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
49 /*!
50 Compute the covariance matrix of the parameters x from a least squares
51 minimisation defined as: Ax = b
52
53 \param A : Matrix A from Ax = b.
54
55 \param x : Vector x from Ax = b corresponding to the parameters to estimate.
56
57 \param b : Vector b from Ax = b.
58 */
computeCovarianceMatrix(const vpMatrix & A,const vpColVector & x,const vpColVector & b)59 vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
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())
66 throw vpMatrixException(vpMatrixException::divideByZeroError,
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
78 /*!
79 Compute the covariance matrix of the parameters x from a least squares
80 minimisation defined as: WAx = Wb
81
82 \param A : Matrix A from WAx = Wb.
83
84 \param x : Vector x from WAx = Wb corresponding to the parameters to
85 estimate.
86
87 \param b : Vector b from WAx = Wb.
88
89 \param W : Diagonal weigths matrix from WAx = Wb.
90 */
computeCovarianceMatrix(const vpMatrix & A,const vpColVector & x,const vpColVector & b,const vpMatrix & W)91 vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b,
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())
102 throw vpMatrixException(vpMatrixException::divideByZeroError,
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
113 /*!
114 Compute the covariance matrix of an image-based virtual visual servoing.
115 This assumes the optimization has been done via v = Ls.pseudoInverse() *
116 DeltaS.
117
118 \param cMo : Pose matrix that has been computed with the v.
119
120 \param deltaS : Error vector used in v = Ls.pseudoInverse() * DeltaS
121
122 \param Ls : interaction matrix used in v = Ls.pseudoInverse() * DeltaS
123 */
computeCovarianceMatrixVVS(const vpHomogeneousMatrix & cMo,const vpColVector & deltaS,const vpMatrix & Ls)124 vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS,
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
134 /*!
135 Compute the covariance matrix of an image-based virtual visual servoing.
136 This assumes the optimization has been done via v = (W * Ls).pseudoInverse()
137 * W * DeltaS.
138
139 \param cMo : Pose matrix that has been computed with the v.
140
141 \param deltaS : Error vector used in v = (W * Ls).pseudoInverse() * W *
142 DeltaS.
143
144 \param Ls : interaction matrix used in v = (W * Ls).pseudoInverse() * W *
145 DeltaS.
146
147 \param W : Weight matrix used in v = (W * Ls).pseudoInverse() * W * DeltaS.
148 */
computeCovarianceMatrixVVS(const vpHomogeneousMatrix & cMo,const vpColVector & deltaS,const vpMatrix & Ls,const vpMatrix & W)149 vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS,
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
computeCovarianceMatrixVVS(const vpHomogeneousMatrix & cMo,const vpColVector & deltaS,const vpMatrix & Ls,vpMatrix & Js,vpColVector & deltaP)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 }
228