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