Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseVirtualVisualServoing.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
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 https://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 * Pose computation.
32 */
33
39#include <visp3/core/vpConfig.h>
40#include <visp3/core/vpExponentialMap.h>
41#include <visp3/core/vpPoint.h>
42#include <visp3/core/vpRobust.h>
43#include <visp3/vision/vpPose.h>
44
52{
53 try {
54
55 double residu_1 = 1e8;
56 double r = 1e8 - 1;
57
58 // we stop the minimization when the error is bellow 1e-8
59
60 int iter = 0;
61
62 unsigned int nb = (unsigned int)listP.size();
63 vpMatrix L(2 * nb, 6);
64 vpColVector err(2 * nb);
65 vpColVector sd(2 * nb), s(2 * nb);
67
68 vpPoint P;
69 std::list<vpPoint> lP;
70
71 // create sd
72 unsigned int k = 0;
73 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
74 P = *it;
75 sd[2 * k] = P.get_x();
76 sd[2 * k + 1] = P.get_y();
77 lP.push_back(P);
78 k++;
79 }
80
81 vpHomogeneousMatrix cMoPrev = cMo;
82 // while((int)((residu_1 - r)*1e12) !=0)
83 // while(std::fabs((residu_1 - r)*1e12) >
84 // std::numeric_limits<double>::epsilon())
85 while (std::fabs(residu_1 - r) > vvsEpsilon) {
86 residu_1 = r;
87
88 // Compute the interaction matrix and the error
89 k = 0;
90 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
91 P = *it;
92 // forward projection of the 3D model for a given pose
93 // change frame coordinates
94 // perspective projection
95 P.track(cMo);
96
97 double x = s[2 * k] = P.get_x(); /* point projected from cMo */
98 double y = s[2 * k + 1] = P.get_y();
99 double Z = P.get_Z();
100 L[2 * k][0] = -1 / Z;
101 L[2 * k][1] = 0;
102 L[2 * k][2] = x / Z;
103 L[2 * k][3] = x * y;
104 L[2 * k][4] = -(1 + x * x);
105 L[2 * k][5] = y;
106
107 L[2 * k + 1][0] = 0;
108 L[2 * k + 1][1] = -1 / Z;
109 L[2 * k + 1][2] = y / Z;
110 L[2 * k + 1][3] = 1 + y * y;
111 L[2 * k + 1][4] = -x * y;
112 L[2 * k + 1][5] = -x;
113
114 k += 1;
115 }
116 err = s - sd;
117
118 // compute the residual
119 r = err.sumSquare();
120
121 // compute the pseudo inverse of the interaction matrix
122 vpMatrix Lp;
123 L.pseudoInverse(Lp, 1e-16);
124
125 // compute the VVS control law
126 v = -lambda * Lp * err;
127
128 // std::cout << "r=" << r <<std::endl ;
129 // update the pose
130
131 cMoPrev = cMo;
132 cMo = vpExponentialMap::direct(v).inverse() * cMo;
133
134 if (iter++ > vvsIterMax) {
135 break;
136 }
137 }
138
139 if (computeCovariance)
140 covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L);
141 }
142
143 catch (...) {
144 vpERROR_TRACE(" ");
145 throw;
146 }
147}
148
157{
158 try {
159
160 double residu_1 = 1e8;
161 double r = 1e8 - 1;
162
163 // we stop the minimization when the error is bellow 1e-8
164 vpMatrix W;
165 vpRobust robust;
166 robust.setMinMedianAbsoluteDeviation(0.00001);
167 vpColVector w, res;
168
169 unsigned int nb = (unsigned int)listP.size();
170 vpMatrix L(2 * nb, 6);
171 vpColVector error(2 * nb);
172 vpColVector sd(2 * nb), s(2 * nb);
173 vpColVector v;
174
175 vpPoint P;
176 std::list<vpPoint> lP;
177
178 // create sd
179 unsigned int k_ = 0;
180 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
181 P = *it;
182 sd[2 * k_] = P.get_x();
183 sd[2 * k_ + 1] = P.get_y();
184 lP.push_back(P);
185 k_++;
186 }
187 int iter = 0;
188 res.resize(s.getRows() / 2);
189 w.resize(s.getRows() / 2);
190 W.resize(s.getRows(), s.getRows());
191 w = 1;
192
193 // while((int)((residu_1 - r)*1e12) !=0)
194 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
195 residu_1 = r;
196
197 // Compute the interaction matrix and the error
198 k_ = 0;
199 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
200 P = *it;
201 // forward projection of the 3D model for a given pose
202 // change frame coordinates
203 // perspective projection
204 P.track(cMo);
205
206 double x = s[2 * k_] = P.get_x(); // point projected from cMo
207 double y = s[2 * k_ + 1] = P.get_y();
208 double Z = P.get_Z();
209 L[2 * k_][0] = -1 / Z;
210 L[2 * k_][1] = 0;
211 L[2 * k_][2] = x / Z;
212 L[2 * k_][3] = x * y;
213 L[2 * k_][4] = -(1 + x * x);
214 L[2 * k_][5] = y;
215
216 L[2 * k_ + 1][0] = 0;
217 L[2 * k_ + 1][1] = -1 / Z;
218 L[2 * k_ + 1][2] = y / Z;
219 L[2 * k_ + 1][3] = 1 + y * y;
220 L[2 * k_ + 1][4] = -x * y;
221 L[2 * k_ + 1][5] = -x;
222
223 k_++;
224 }
225 error = s - sd;
226
227 // compute the residual
228 r = error.sumSquare();
229
230 for (unsigned int k = 0; k < error.getRows() / 2; k++) {
231 res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
232 }
233 robust.MEstimator(vpRobust::TUKEY, res, w);
234
235 // compute the pseudo inverse of the interaction matrix
236 for (unsigned int k = 0; k < error.getRows() / 2; k++) {
237 W[2 * k][2 * k] = w[k];
238 W[2 * k + 1][2 * k + 1] = w[k];
239 }
240 // compute the pseudo inverse of the interaction matrix
241 vpMatrix Lp;
242 (W * L).pseudoInverse(Lp, 1e-6);
243
244 // compute the VVS control law
245 v = -lambda * Lp * W * error;
246
247 cMo = vpExponentialMap::direct(v).inverse() * cMo;
248 if (iter++ > vvsIterMax)
249 break;
250 }
251
252 if (computeCovariance)
253 covarianceMatrix =
254 vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
255 // matrix is diagonale, but using W*W
256 // is more efficient.
257 } catch (...) {
258 vpERROR_TRACE(" ");
259 throw;
260 }
261}
262
263#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
264 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
274std::optional<vpHomogeneousMatrix> vpPose::poseVirtualVSWithDepth(const std::vector<vpPoint> &points, const vpHomogeneousMatrix &cMo)
275{
276 auto residu_1{1e8}, r{1e8 - 1};
277 const auto lambda{0.9}, vvsEpsilon{1e-8};
278 const unsigned int vvsIterMax{200};
279
280 const unsigned int nb = static_cast<unsigned int>(points.size());
281 vpMatrix L(3 * nb, 6);
282 vpColVector err(3 * nb);
283 vpColVector sd(3 * nb), s(3 * nb);
284
285 // create sd
286 for (auto i = 0u; i < points.size(); i++) {
287 sd[3 * i] = points[i].get_x();
288 sd[3 * i + 1] = points[i].get_y();
289 sd[3 * i + 2] = points[i].get_Z();
290 }
291
292 auto cMoPrev = cMo;
293 auto iter = 0u;
294 while (std::fabs(residu_1 - r) > vvsEpsilon) {
295 residu_1 = r;
296
297 // Compute the interaction matrix and the error
298 for (auto i = 0u; i < points.size(); i++) {
299 // forward projection of the 3D model for a given pose
300 // change frame coordinates
301 // perspective projection
302 vpColVector cP, p;
303 points.at(i).changeFrame(cMo, cP);
304 points.at(i).projection(cP, p);
305
306 const auto x = s[3 * i] = p[0];
307 const auto y = s[3 * i + 1] = p[1];
308 const auto Z = s[3 * i + 2] = cP[2];
309 L[3 * i][0] = -1 / Z;
310 L[3 * i][1] = 0;
311 L[3 * i][2] = x / Z;
312 L[3 * i][3] = x * y;
313 L[3 * i][4] = -(1 + vpMath::sqr(x));
314 L[3 * i][5] = y;
315
316 L[3 * i + 1][0] = 0;
317 L[3 * i + 1][1] = -1 / Z;
318 L[3 * i + 1][2] = y / Z;
319 L[3 * i + 1][3] = 1 + vpMath::sqr(y);
320 L[3 * i + 1][4] = -x * y;
321 L[3 * i + 1][5] = -x;
322
323 L[3 * i + 2][0] = 0;
324 L[3 * i + 2][1] = 0;
325 L[3 * i + 2][2] = -1;
326 L[3 * i + 2][3] = -y * Z;
327 L[3 * i + 2][4] = x * Z;
328 L[3 * i + 2][5] = -0;
329 }
330 err = s - sd;
331
332 // compute the residual
333 r = err.sumSquare();
334
335 // compute the pseudo inverse of the interaction matrix
336 vpMatrix Lp;
337 L.pseudoInverse(Lp, 1e-16);
338
339 // compute the VVS control law
340 const auto v = -lambda * Lp * err;
341
342 // update the pose
343 cMoPrev = vpExponentialMap::direct(v).inverse() * cMoPrev;
344
345 if (iter++ > vvsIterMax) {
346 return std::nullopt;
347 }
348 }
349 return cMoPrev;
350}
351
352#endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
double sumSquare() const
void resize(unsigned int i, bool flagNullify=true)
static vpHomogeneousMatrix direct(const vpColVector &v)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:453
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition vpPose.h:115
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
double lambda
parameters use for the virtual visual servoing approach
Definition vpPose.h:120
void poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach and a robust control law.
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
#define vpERROR_TRACE
Definition vpDebug.h:388