Visual Servoing Platform  version 3.6.1 under development (2023-11-30)
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 = (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  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
68  P = *it;
69  sd[2 * k] = P.get_x();
70  sd[2 * k + 1] = P.get_y();
71  lP.push_back(P);
72  k++;
73  }
74 
75  vpHomogeneousMatrix cMoPrev = cMo;
76  // while((int)((residu_1 - r)*1e12) !=0)
77  // while(std::fabs((residu_1 - r)*1e12) >
78  // std::numeric_limits<double>::epsilon())
79  while (std::fabs(residu_1 - r) > vvsEpsilon) {
80  residu_1 = r;
81 
82  // Compute the interaction matrix and the error
83  k = 0;
84  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
85  P = *it;
86  // forward projection of the 3D model for a given pose
87  // change frame coordinates
88  // perspective projection
89  P.track(cMo);
90 
91  double x = s[2 * k] = P.get_x(); /* point projected from cMo */
92  double y = s[2 * k + 1] = P.get_y();
93  double Z = P.get_Z();
94  L[2 * k][0] = -1 / Z;
95  L[2 * k][1] = 0;
96  L[2 * k][2] = x / Z;
97  L[2 * k][3] = x * y;
98  L[2 * k][4] = -(1 + x * x);
99  L[2 * k][5] = y;
100 
101  L[2 * k + 1][0] = 0;
102  L[2 * k + 1][1] = -1 / Z;
103  L[2 * k + 1][2] = y / Z;
104  L[2 * k + 1][3] = 1 + y * y;
105  L[2 * k + 1][4] = -x * y;
106  L[2 * k + 1][5] = -x;
107 
108  k += 1;
109  }
110  err = s - sd;
111 
112  // compute the residual
113  r = err.sumSquare();
114 
115  // compute the pseudo inverse of the interaction matrix
116  vpMatrix Lp;
117  L.pseudoInverse(Lp, 1e-16);
118 
119  // compute the VVS control law
120  v = -m_lambda * Lp * err;
121 
122  // std::cout << "r=" << r <<std::endl ;
123  // update the pose
124 
125  cMoPrev = cMo;
126  cMo = vpExponentialMap::direct(v).inverse() * cMo;
127 
128  if (iter++ > vvsIterMax) {
129  break;
130  }
131  }
132 
133  if (computeCovariance)
134  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L);
135  }
136 
137  catch (...) {
138  vpERROR_TRACE(" ");
139  throw;
140  }
141 }
142 
144 {
145  try {
146 
147  double residu_1 = 1e8;
148  double r = 1e8 - 1;
149 
150  // we stop the minimization when the error is bellow 1e-8
151  vpMatrix W;
152  vpRobust robust;
153  robust.setMinMedianAbsoluteDeviation(0.00001);
154  vpColVector w, res;
155 
156  unsigned int nb = (unsigned int)listP.size();
157  vpMatrix L(2 * nb, 6);
158  vpColVector error(2 * nb);
159  vpColVector sd(2 * nb), s(2 * nb);
160  vpColVector v;
161 
162  vpPoint P;
163  std::list<vpPoint> lP;
164 
165  // create sd
166  unsigned int k_ = 0;
167  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
168  P = *it;
169  sd[2 * k_] = P.get_x();
170  sd[2 * k_ + 1] = P.get_y();
171  lP.push_back(P);
172  k_++;
173  }
174  int iter = 0;
175  res.resize(s.getRows() / 2);
176  w.resize(s.getRows() / 2);
177  W.resize(s.getRows(), s.getRows());
178  w = 1;
179 
180  // while((int)((residu_1 - r)*1e12) !=0)
181  while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
182  residu_1 = r;
183 
184  // Compute the interaction matrix and the error
185  k_ = 0;
186  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
187  P = *it;
188  // forward projection of the 3D model for a given pose
189  // change frame coordinates
190  // perspective projection
191  P.track(cMo);
192 
193  double x = s[2 * k_] = P.get_x(); // point projected from cMo
194  double y = s[2 * k_ + 1] = P.get_y();
195  double Z = P.get_Z();
196  L[2 * k_][0] = -1 / Z;
197  L[2 * k_][1] = 0;
198  L[2 * k_][2] = x / Z;
199  L[2 * k_][3] = x * y;
200  L[2 * k_][4] = -(1 + x * x);
201  L[2 * k_][5] = y;
202 
203  L[2 * k_ + 1][0] = 0;
204  L[2 * k_ + 1][1] = -1 / Z;
205  L[2 * k_ + 1][2] = y / Z;
206  L[2 * k_ + 1][3] = 1 + y * y;
207  L[2 * k_ + 1][4] = -x * y;
208  L[2 * k_ + 1][5] = -x;
209 
210  k_++;
211  }
212  error = s - sd;
213 
214  // compute the residual
215  r = error.sumSquare();
216 
217  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
218  res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
219  }
220  robust.MEstimator(vpRobust::TUKEY, res, w);
221 
222  // compute the pseudo inverse of the interaction matrix
223  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
224  W[2 * k][2 * k] = w[k];
225  W[2 * k + 1][2 * k + 1] = w[k];
226  }
227  // compute the pseudo inverse of the interaction matrix
228  vpMatrix Lp;
229  (W * L).pseudoInverse(Lp, 1e-6);
230 
231  // compute the VVS control law
232  v = -m_lambda * Lp * W * error;
233 
234  cMo = vpExponentialMap::direct(v).inverse() * cMo;
235  if (iter++ > vvsIterMax)
236  break;
237  }
238 
239  if (computeCovariance)
240  covarianceMatrix =
241  vpMatrix::computeCovarianceMatrix(L, v, -m_lambda * error, W * W); // Remark: W*W = W*W.t() since the
242  // matrix is diagonale, but using W*W
243  // is more efficient.
244  }
245  catch (...) {
246  vpERROR_TRACE(" ");
247  throw;
248  }
249 }
250 
251 // Check if std:c++17 or higher
252 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
253 std::optional<vpHomogeneousMatrix> vpPose::poseVirtualVSWithDepth(const std::vector<vpPoint> &points, const vpHomogeneousMatrix &cMo)
254 {
255  auto residu_1 { 1e8 }, r { 1e8 - 1 };
256  const auto lambda { 0.9 }, vvsEpsilon { 1e-8 };
257  const unsigned int vvsIterMax { 200 };
258 
259  const unsigned int nb = static_cast<unsigned int>(points.size());
260  vpMatrix L(3 * nb, 6);
261  vpColVector err(3 * nb);
262  vpColVector sd(3 * nb), s(3 * nb);
263 
264  // create sd
265  for (auto i = 0u; i < points.size(); i++) {
266  sd[3 * i] = points[i].get_x();
267  sd[3 * i + 1] = points[i].get_y();
268  sd[3 * i + 2] = points[i].get_Z();
269  }
270 
271  auto cMoPrev = cMo;
272  auto iter = 0u;
273  while (std::fabs(residu_1 - r) > vvsEpsilon) {
274  residu_1 = r;
275 
276  // Compute the interaction matrix and the error
277  for (auto i = 0u; i < points.size(); i++) {
278  // forward projection of the 3D model for a given pose
279  // change frame coordinates
280  // perspective projection
281  vpColVector cP, p;
282  points.at(i).changeFrame(cMo, cP);
283  points.at(i).projection(cP, p);
284 
285  const auto x = s[3 * i] = p[0];
286  const auto y = s[3 * i + 1] = p[1];
287  const auto Z = s[3 * i + 2] = cP[2];
288  L[3 * i][0] = -1 / Z;
289  L[3 * i][1] = 0;
290  L[3 * i][2] = x / Z;
291  L[3 * i][3] = x * y;
292  L[3 * i][4] = -(1 + vpMath::sqr(x));
293  L[3 * i][5] = y;
294 
295  L[3 * i + 1][0] = 0;
296  L[3 * i + 1][1] = -1 / Z;
297  L[3 * i + 1][2] = y / Z;
298  L[3 * i + 1][3] = 1 + vpMath::sqr(y);
299  L[3 * i + 1][4] = -x * y;
300  L[3 * i + 1][5] = -x;
301 
302  L[3 * i + 2][0] = 0;
303  L[3 * i + 2][1] = 0;
304  L[3 * i + 2][2] = -1;
305  L[3 * i + 2][3] = -y * Z;
306  L[3 * i + 2][4] = x * Z;
307  L[3 * i + 2][5] = -0;
308  }
309  err = s - sd;
310 
311  // compute the residual
312  r = err.sumSquare();
313 
314  // compute the pseudo inverse of the interaction matrix
315  vpMatrix Lp;
316  L.pseudoInverse(Lp, 1e-16);
317 
318  // compute the VVS control law
319  const auto v = -lambda * Lp * err;
320 
321  // update the pose
322  cMoPrev = vpExponentialMap::direct(v).inverse() * cMoPrev;
323 
324  if (iter++ > vvsIterMax) {
325  return std::nullopt;
326  }
327  }
328  return cMoPrev;
329 }
330 
331 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:282
unsigned int getRows() const
Definition: vpArray2D.h:267
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:1049
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:461
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:459
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:445
double m_lambda
Parameters use for the virtual visual servoing approach.
Definition: vpPose.h:766
void poseVirtualVS(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:116
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:134
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:154
#define vpERROR_TRACE
Definition: vpDebug.h:388