Visual Servoing Platform version 3.5.0
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);
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}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:116
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:110
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:115
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