Visual Servoing Platform  version 3.6.1 under development (2024-12-09)
vpPoseVirtualVisualServoing.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 
45 BEGIN_VISP_NAMESPACE
46 
48 {
49  try {
50  double residu_1 = 1e8;
51  double r = 1e8 - 1;
52  const unsigned int index_0 = 0;
53  const unsigned int index_1 = 1;
54  const unsigned int index_2 = 2;
55  const unsigned int index_3 = 3;
56  const unsigned int index_4 = 4;
57  const unsigned int index_5 = 5;
58  const unsigned int dim2DPoints = 2;
59 
60  // we stop the minimization when the error is bellow 1e-8
61 
62  int iter = 0;
63 
64  unsigned int nb = static_cast<unsigned int>(listP.size());
65  const unsigned int nbColsL = 6;
66  vpMatrix L(dim2DPoints * nb, nbColsL);
67  vpColVector err(dim2DPoints * nb);
68  vpColVector sd(dim2DPoints * nb), s(dim2DPoints * nb);
69  vpColVector v;
70 
71  vpPoint P;
72  std::list<vpPoint> lP;
73 
74  // create sd
75  unsigned int k = 0;
76  std::list<vpPoint>::const_iterator listp_end = listP.end();
77  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
78  P = *it;
79  sd[dim2DPoints * k] = P.get_x();
80  sd[(dim2DPoints * k) + 1] = P.get_y();
81  lP.push_back(P);
82  ++k;
83  }
84 
85  vpHomogeneousMatrix cMoPrev = cMo;
86  /*
87  // --comment: while((int)((residu_1 - r)*1e12) !=0)
88  // --comment: while(std::fabs((residu_1 - r)*1e12) >
89  // --comment: std::numeric_limits < double > :: epsilon())
90  */
91  bool iter_gt_vvsitermax = false;
92  while ((std::fabs(residu_1 - r) > vvsEpsilon) && (iter_gt_vvsitermax == false)) {
93  residu_1 = r;
94 
95  // Compute the interaction matrix and the error
96  k = 0;
97  std::list<vpPoint>::const_iterator lp_end = lP.end();
98  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lp_end; ++it) {
99  P = *it;
100  // forward projection of the 3D model for a given pose
101  // change frame coordinates
102  // perspective projection
103  P.track(cMo);
104 
105  double x = s[dim2DPoints * k] = P.get_x(); /* point projected from cMo */
106  double y = s[(dim2DPoints * k) + index_1] = P.get_y();
107  double Z = P.get_Z();
108  L[dim2DPoints * k][index_0] = -1 / Z;
109  L[dim2DPoints * k][index_1] = 0;
110  L[dim2DPoints * k][index_2] = x / Z;
111  L[dim2DPoints * k][index_3] = x * y;
112  L[dim2DPoints * k][index_4] = -(1 + (x * x));
113  L[dim2DPoints * k][index_5] = y;
114 
115  L[(dim2DPoints * k) + 1][index_0] = 0;
116  L[(dim2DPoints * k) + 1][index_1] = -1 / Z;
117  L[(dim2DPoints * k) + 1][index_2] = y / Z;
118  L[(dim2DPoints * k) + 1][index_3] = 1 + (y * y);
119  L[(dim2DPoints * k) + 1][index_4] = -x * y;
120  L[(dim2DPoints * k) + 1][index_5] = -x;
121 
122  k += 1;
123  }
124  err = s - sd;
125 
126  // compute the residual
127  r = err.sumSquare();
128 
129  // compute the pseudo inverse of the interaction matrix
130  vpMatrix Lp;
131  L.pseudoInverse(Lp, 1e-16);
132 
133  // compute the VVS control law
134  v = -m_lambda * Lp * err;
135 
136  // update the pose
137 
138  cMoPrev = cMo;
139  cMo = vpExponentialMap::direct(v).inverse() * cMo;
140 
141  if (iter> vvsIterMax) {
142  iter_gt_vvsitermax = true;
143  // break
144  }
145  else {
146  ++iter;
147  }
148  }
149 
150  if (computeCovariance) {
151  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L);
152  }
153  }
154  catch (...) {
155  throw(vpException(vpException::fatalError, "poseVirtualVS did not succeed"));
156  }
157 }
158 
160 {
161  double residu_1 = 1e8;
162  double r = 1e8 - 1;
163  const unsigned int index_0 = 0;
164  const unsigned int index_1 = 1;
165  const unsigned int index_2 = 2;
166  const unsigned int index_3 = 3;
167  const unsigned int index_4 = 4;
168  const unsigned int index_5 = 5;
169  const unsigned int dim2DPoints = 2;
170 
171  // we stop the minimization when the error is bellow 1e-8
172  vpMatrix W;
173  vpRobust robust;
174  robust.setMinMedianAbsoluteDeviation(0.00001);
175  vpColVector w, res;
176 
177  unsigned int nb = static_cast<unsigned int>(listP.size());
178  const unsigned int nbColsL = 6;
179  vpMatrix L(dim2DPoints * nb, nbColsL);
180  vpColVector error(dim2DPoints * nb);
181  vpColVector sd(dim2DPoints * nb), s(dim2DPoints * nb);
182  vpColVector v;
183 
184  vpPoint P;
185  std::list<vpPoint> lP;
186 
187  // create sd
188  unsigned int k_ = 0;
189  std::list<vpPoint>::const_iterator listp_end = listP.end();
190  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
191  P = *it;
192  sd[dim2DPoints * k_] = P.get_x();
193  sd[(dim2DPoints * k_) + 1] = P.get_y();
194  lP.push_back(P);
195  ++k_;
196  }
197  int iter = 0;
198  res.resize(s.getRows() / dim2DPoints);
199  w.resize(s.getRows() / dim2DPoints);
200  W.resize(s.getRows(), s.getRows());
201  w = 1;
202 
203  // --comment: while (residu_1 - r) times 1e12 diff 0
204  bool iter_gt_vvsitermax = false;
205  while ((std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) && (iter_gt_vvsitermax == false)) {
206  residu_1 = r;
207 
208  // Compute the interaction matrix and the error
209  k_ = 0;
210  std::list<vpPoint>::const_iterator lp_end = lP.end();
211  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lp_end; ++it) {
212  P = *it;
213  // forward projection of the 3D model for a given pose
214  // change frame coordinates
215  // perspective projection
216  P.track(cMo);
217 
218  double x = s[dim2DPoints * k_] = P.get_x(); // point projected from cMo
219  double y = s[(dim2DPoints * k_) + index_1] = P.get_y();
220  double Z = P.get_Z();
221  L[dim2DPoints * k_][index_0] = -1 / Z;
222  L[dim2DPoints * k_][index_1] = 0;
223  L[dim2DPoints * k_][index_2] = x / Z;
224  L[dim2DPoints * k_][index_3] = x * y;
225  L[dim2DPoints * k_][index_4] = -(1 + (x * x));
226  L[dim2DPoints * k_][index_5] = y;
227 
228  L[(dim2DPoints * k_) + index_1][index_0] = 0;
229  L[(dim2DPoints * k_) + index_1][index_1] = -1 / Z;
230  L[(dim2DPoints * k_) + index_1][index_2] = y / Z;
231  L[(dim2DPoints * k_) + index_1][index_3] = 1 + (y * y);
232  L[(dim2DPoints * k_) + index_1][index_4] = -x * y;
233  L[(dim2DPoints * k_) + index_1][index_5] = -x;
234 
235  ++k_;
236  }
237  error = s - sd;
238 
239  // compute the residual
240  r = error.sumSquare();
241 
242  unsigned int v_error_rows = error.getRows();
243  const unsigned int nbPts = v_error_rows / dim2DPoints;
244  for (unsigned int k = 0; k < nbPts; ++k) {
245  res[k] = vpMath::sqr(error[dim2DPoints * k]) + vpMath::sqr(error[(dim2DPoints * k) + 1]);
246  }
247  robust.MEstimator(vpRobust::TUKEY, res, w);
248 
249  // compute the pseudo inverse of the interaction matrix
250  unsigned int error_rows = error.getRows();
251  unsigned int nbErrors = error_rows / dim2DPoints;
252  for (unsigned int k = 0; k < nbErrors; ++k) {
253  W[dim2DPoints * k][dim2DPoints * k] = w[k];
254  W[(dim2DPoints * k) + 1][(dim2DPoints * k) + 1] = w[k];
255  }
256  // compute the pseudo inverse of the interaction matrix
257  vpMatrix Lp;
258  (W * L).pseudoInverse(Lp, 1e-6);
259 
260  // compute the VVS control law
261  v = -m_lambda * Lp * W * error;
262 
263  cMo = vpExponentialMap::direct(v).inverse() * cMo;
264  if (iter > vvsIterMax) {
265  iter_gt_vvsitermax = true;
266  // break
267  }
268  else {
269  ++iter;
270  }
271  }
272 
273  if (computeCovariance) {
274  covarianceMatrix =
275  vpMatrix::computeCovarianceMatrix(L, v, -m_lambda * error, W * W); // Remark: W*W = W*W.t() since the
276  // matrix is diagonale, but using W*W
277  // is more efficient.
278  }
279 }
280 
281 // Check if std:c++17 or higher
282 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
283 std::optional<vpHomogeneousMatrix> vpPose::poseVirtualVSWithDepth(const std::vector<vpPoint> &points, const vpHomogeneousMatrix &cMo)
284 {
285  auto residu_1 { 1e8 }, r { 1e8 - 1 };
286  const auto lambda { 0.9 }, vvsEpsilon { 1e-8 };
287  const unsigned int vvsIterMax { 200 };
288  const unsigned int index_0 = 0;
289  const unsigned int index_1 = 1;
290  const unsigned int index_2 = 2;
291  const unsigned int index_3 = 3;
292  const unsigned int index_4 = 4;
293  const unsigned int index_5 = 5;
294 
295  const unsigned int nb = static_cast<unsigned int>(points.size());
296  const unsigned int sizePoints = 3;
297  const unsigned int nbColsL = 6;
298  vpMatrix L(sizePoints * nb, nbColsL);
299  vpColVector err(sizePoints * nb);
300  vpColVector sd(sizePoints * nb), s(sizePoints * nb);
301 
302  // create sd
303  auto v_points_size = points.size();
304  for (auto i = 0u; i < v_points_size; ++i) {
305  sd[sizePoints * i] = points[i].get_x();
306  sd[(sizePoints * i) + index_1] = points[i].get_y();
307  sd[(sizePoints * i) + index_2] = points[i].get_Z();
308  }
309 
310  auto cMoPrev = cMo;
311  auto iter = 0u;
312  while (std::fabs(residu_1 - r) > vvsEpsilon) {
313  residu_1 = r;
314 
315  // Compute the interaction matrix and the error
316  auto points_size = points.size();
317  for (auto i = 0u; i < points_size; ++i) {
318  // forward projection of the 3D model for a given pose
319  // change frame coordinates
320  // perspective projection
321  vpColVector cP, p;
322  points.at(i).changeFrame(cMo, cP);
323  points.at(i).projection(cP, p);
324 
325  const auto x = s[sizePoints * i] = p[index_0];
326  const auto y = s[(sizePoints * i) + index_1] = p[index_1];
327  const auto Z = s[(sizePoints * i) + index_2] = cP[index_2];
328  L[sizePoints * i][index_0] = -1. / Z;
329  L[sizePoints * i][index_1] = 0;
330  L[sizePoints * i][index_2] = x / Z;
331  L[sizePoints * i][index_3] = x * y;
332  L[sizePoints * i][index_4] = -(1. + vpMath::sqr(x));
333  L[sizePoints * i][index_5] = y;
334 
335  L[(sizePoints * i) + index_1][index_0] = 0;
336  L[(sizePoints * i) + index_1][index_1] = -1. / Z;
337  L[(sizePoints * i) + index_1][index_2] = y / Z;
338  L[(sizePoints * i) + index_1][index_3] = 1. + vpMath::sqr(y);
339  L[(sizePoints * i) + index_1][index_4] = -x * y;
340  L[(sizePoints * i) + index_1][index_5] = -x;
341 
342  L[(sizePoints * i) + index_2][index_0] = 0;
343  L[(sizePoints * i) + index_2][index_1] = 0;
344  L[(sizePoints * i) + index_2][index_2] = -1.;
345  L[(sizePoints * i) + index_2][index_3] = -y * Z;
346  L[(sizePoints * i) + index_2][index_4] = x * Z;
347  L[(sizePoints * i) + index_2][index_5] = -0;
348  }
349  err = s - sd;
350 
351  // compute the residual
352  r = err.sumSquare();
353 
354  // compute the pseudo inverse of the interaction matrix
355  vpMatrix Lp;
356  L.pseudoInverse(Lp, 1e-16);
357 
358  // compute the VVS control law
359  const auto v = -lambda * Lp * err;
360 
361  // update the pose
362  cMoPrev = vpExponentialMap::direct(v).inverse() * cMoPrev;
363 
364  if (iter > vvsIterMax) {
365  return std::nullopt;
366  }
367  else {
368  ++iter;
369  }
370  }
371  return cMoPrev;
372 }
373 
374 #endif
375 
376 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
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:203
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:426
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:424
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:410
double m_lambda
Parameters use for the virtual visual servoing approach.
Definition: vpPose.h:768
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:114
void poseVirtualVS(vpHomogeneousMatrix &cMo)
void poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:89
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:130
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:136