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