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