Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpPoseVirtualVisualServoing.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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/vpExponentialMap.h>
45 #include <visp3/core/vpPoint.h>
46 #include <visp3/core/vpRobust.h>
47 #include <visp3/vision/vpPose.h>
48 
57 {
58  try {
59 
60  double residu_1 = 1e8;
61  double r = 1e8 - 1;
62 
63  // we stop the minimization when the error is bellow 1e-8
64 
65  int iter = 0;
66 
67  unsigned int nb = (unsigned int)listP.size();
68  vpMatrix L(2 * nb, 6);
69  vpColVector err(2 * nb);
70  vpColVector sd(2 * nb), s(2 * nb);
71  vpColVector v;
72 
73  vpPoint P;
74  std::list<vpPoint> lP;
75 
76  // create sd
77  unsigned int k = 0;
78  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
79  P = *it;
80  sd[2 * k] = P.get_x();
81  sd[2 * k + 1] = P.get_y();
82  lP.push_back(P);
83  k++;
84  }
85 
86  vpHomogeneousMatrix cMoPrev = cMo;
87  // while((int)((residu_1 - r)*1e12) !=0)
88  // while(std::fabs((residu_1 - r)*1e12) >
89  // std::numeric_limits<double>::epsilon())
90  while (std::fabs(residu_1 - r) > vvsEpsilon) {
91  residu_1 = r;
92 
93  // Compute the interaction matrix and the error
94  k = 0;
95  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
96  P = *it;
97  // forward projection of the 3D model for a given pose
98  // change frame coordinates
99  // perspective projection
100  P.track(cMo);
101 
102  double x = s[2 * k] = P.get_x(); /* point projected from cMo */
103  double y = s[2 * k + 1] = P.get_y();
104  double Z = P.get_Z();
105  L[2 * k][0] = -1 / Z;
106  L[2 * k][1] = 0;
107  L[2 * k][2] = x / Z;
108  L[2 * k][3] = x * y;
109  L[2 * k][4] = -(1 + x * x);
110  L[2 * k][5] = y;
111 
112  L[2 * k + 1][0] = 0;
113  L[2 * k + 1][1] = -1 / Z;
114  L[2 * k + 1][2] = y / Z;
115  L[2 * k + 1][3] = 1 + y * y;
116  L[2 * k + 1][4] = -x * y;
117  L[2 * k + 1][5] = -x;
118 
119  k += 1;
120  }
121  err = s - sd;
122 
123  // compute the residual
124  r = err.sumSquare();
125 
126  // compute the pseudo inverse of the interaction matrix
127  vpMatrix Lp;
128  L.pseudoInverse(Lp, 1e-16);
129 
130  // compute the VVS control law
131  v = -lambda * Lp * err;
132 
133  // std::cout << "r=" << r <<std::endl ;
134  // update the pose
135 
136  cMoPrev = cMo;
137  cMo = vpExponentialMap::direct(v).inverse() * cMo;
138 
139  if (iter++ > vvsIterMax) {
140  break;
141  }
142  }
143 
144  if (computeCovariance)
145  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L);
146  }
147 
148  catch (...) {
149  vpERROR_TRACE(" ");
150  throw;
151  }
152 }
153 
162 {
163  try {
164 
165  double residu_1 = 1e8;
166  double r = 1e8 - 1;
167 
168  // we stop the minimization when the error is bellow 1e-8
169  vpMatrix W;
170  vpRobust robust;
171  robust.setMinMedianAbsoluteDeviation(0.00001);
172  vpColVector w, res;
173 
174  unsigned int nb = (unsigned int)listP.size();
175  vpMatrix L(2 * nb, 6);
176  vpColVector error(2 * nb);
177  vpColVector sd(2 * nb), s(2 * nb);
178  vpColVector v;
179 
180  vpPoint P;
181  std::list<vpPoint> lP;
182 
183  // create sd
184  unsigned int k_ = 0;
185  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
186  P = *it;
187  sd[2 * k_] = P.get_x();
188  sd[2 * k_ + 1] = P.get_y();
189  lP.push_back(P);
190  k_++;
191  }
192  int iter = 0;
193  res.resize(s.getRows() / 2);
194  w.resize(s.getRows() / 2);
195  W.resize(s.getRows(), s.getRows());
196  w = 1;
197 
198  // while((int)((residu_1 - r)*1e12) !=0)
199  while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
200  residu_1 = r;
201 
202  // Compute the interaction matrix and the error
203  k_ = 0;
204  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
205  P = *it;
206  // forward projection of the 3D model for a given pose
207  // change frame coordinates
208  // perspective projection
209  P.track(cMo);
210 
211  double x = s[2 * k_] = P.get_x(); // point projected from cMo
212  double y = s[2 * k_ + 1] = P.get_y();
213  double Z = P.get_Z();
214  L[2 * k_][0] = -1 / Z;
215  L[2 * k_][1] = 0;
216  L[2 * k_][2] = x / Z;
217  L[2 * k_][3] = x * y;
218  L[2 * k_][4] = -(1 + x * x);
219  L[2 * k_][5] = y;
220 
221  L[2 * k_ + 1][0] = 0;
222  L[2 * k_ + 1][1] = -1 / Z;
223  L[2 * k_ + 1][2] = y / Z;
224  L[2 * k_ + 1][3] = 1 + y * y;
225  L[2 * k_ + 1][4] = -x * y;
226  L[2 * k_ + 1][5] = -x;
227 
228  k_++;
229  }
230  error = s - sd;
231 
232  // compute the residual
233  r = error.sumSquare();
234 
235  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
236  res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
237  }
238  robust.MEstimator(vpRobust::TUKEY, res, w);
239 
240  // compute the pseudo inverse of the interaction matrix
241  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
242  W[2 * k][2 * k] = w[k];
243  W[2 * k + 1][2 * k + 1] = w[k];
244  }
245  // compute the pseudo inverse of the interaction matrix
246  vpMatrix Lp;
247  (W * L).pseudoInverse(Lp, 1e-6);
248 
249  // compute the VVS control law
250  v = -lambda * Lp * W * error;
251 
252  cMo = vpExponentialMap::direct(v).inverse() * cMo;
253  if (iter++ > vvsIterMax)
254  break;
255  }
256 
257  if (computeCovariance)
258  covarianceMatrix =
259  vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
260  // matrix is diagonale, but using W*W
261  // is more efficient.
262  } catch (...) {
263  vpERROR_TRACE(" ");
264  throw;
265  }
266 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
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
unsigned int getRows() const
Definition: vpArray2D.h:289
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
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:115
static double sqr(double x)
Definition: vpMath.h:116
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