ViSP  2.10.0
vpPoseVirtualVisualServoing.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPoseVirtualVisualServoing.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Pose computation.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
47 #include <visp/vpPose.h>
48 #include <visp/vpPoint.h>
49 #include <visp/vpFeatureBuilder.h>
50 #include <visp/vpFeaturePoint.h>
51 #include <visp/vpExponentialMap.h>
52 #include <visp/vpRobust.h>
53 
61 void
63 {
64  try
65  {
66 
67  double residu_1 = 1e8 ;
68  double r =1e8-1;
69 
70  // we stop the minimization when the error is bellow 1e-8
71 
72  int iter = 0 ;
73 
74  unsigned int nb = (unsigned int)listP.size() ;
75  vpMatrix L(2*nb,6) ;
76  vpColVector err(2*nb) ;
77  vpColVector sd(2*nb),s(2*nb) ;
78  vpColVector v ;
79 
80  vpPoint P;
81  std::list<vpPoint> lP ;
82 
83  // create sd
84  unsigned int k =0 ;
85  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
86  {
87  P = *it;
88  sd[2*k] = P.get_x() ;
89  sd[2*k+1] = P.get_y() ;
90  lP.push_back(P);
91  k ++;
92  }
93 
94  while((int)((residu_1 - r)*1e12) !=0)
95  {
96  residu_1 = r ;
97 
98  // Compute the interaction matrix and the error
99  k =0 ;
100  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
101  {
102  P = *it;
103  // forward projection of the 3D model for a given pose
104  // change frame coordinates
105  // perspective projection
106  P.track(cMo) ;
107 
108  double x = s[2*k] = P.get_x(); /* point projected from cMo */
109  double y = s[2*k+1] = P.get_y();
110  double Z = P.get_Z() ;
111  L[2*k][0] = -1/Z ;
112  L[2*k][1] = 0 ;
113  L[2*k][2] = x/Z ;
114  L[2*k][3] = x*y ;
115  L[2*k][4] = -(1+x*x) ;
116  L[2*k][5] = y ;
117 
118  L[2*k+1][0] = 0 ;
119  L[2*k+1][1] = -1/Z ;
120  L[2*k+1][2] = y/Z ;
121  L[2*k+1][3] = 1+y*y ;
122  L[2*k+1][4] = -x*y ;
123  L[2*k+1][5] = -x ;
124 
125  k+=1 ;
126  }
127  err = s - sd ;
128 
129  // compute the residual
130  r = err.sumSquare() ;
131 
132  // compute the pseudo inverse of the interaction matrix
133  vpMatrix Lp ;
134  L.pseudoInverse(Lp,1e-16) ;
135 
136  // compute the VVS control law
137  v = -lambda*Lp*err ;
138 
139  //std::cout << "r=" << r <<std::endl ;
140  // update the pose
141 
142  cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
143  if (iter++>vvsIterMax) break ;
144  }
145 
146  if(computeCovariance)
147  covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
148  }
149 
150  catch(...)
151  {
152  vpERROR_TRACE(" ") ;
153  throw ;
154  }
155 }
156 
164 void
166 {
167  try{
168 
169  double residu_1 = 1e8 ;
170  double r =1e8-1;
171 
172  // we stop the minimization when the error is bellow 1e-8
173  vpMatrix W ;
174  vpRobust robust((unsigned int)(2*listP.size())) ;
175  robust.setThreshold(0.0000) ;
176  vpColVector w,res ;
177 
178  unsigned int nb = (unsigned int)listP.size() ;
179  vpMatrix L(2*nb,6) ;
180  vpColVector error(2*nb) ;
181  vpColVector sd(2*nb),s(2*nb) ;
182  vpColVector v ;
183 
184  listP.front() ;
185  vpPoint P;
186  std::list<vpPoint> lP ;
187 
188  // create sd
189  unsigned int k_ =0 ;
190  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
191  {
192  P = *it;
193  sd[2*k_] = P.get_x() ;
194  sd[2*k_+1] = P.get_y() ;
195  lP.push_back(P) ;
196  k_ ++;
197  }
198  int iter = 0 ;
199  res.resize(s.getRows()/2) ;
200  w.resize(s.getRows()/2) ;
201  W.resize(s.getRows(), s.getRows()) ;
202  w =1 ;
203 
204  while((int)((residu_1 - r)*1e12) !=0)
205  {
206  residu_1 = r ;
207 
208  // Compute the interaction matrix and the error
209  k_ =0 ;
210  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
211  {
212  P = *it;
213  // forward projection of the 3D model for a given pose
214  // change frame coordinates
215  // perspective projection
216  P.track(cMo) ;
217 
218  double x = s[2*k_] = P.get_x(); // point projected from cMo
219  double y = s[2*k_+1] = P.get_y();
220  double Z = P.get_Z() ;
221  L[2*k_][0] = -1/Z ;
222  L[2*k_][1] = 0 ;
223  L[2*k_][2] = x/Z ;
224  L[2*k_][3] = x*y ;
225  L[2*k_][4] = -(1+x*x) ;
226  L[2*k_][5] = y ;
227 
228  L[2*k_+1][0] = 0 ;
229  L[2*k_+1][1] = -1/Z ;
230  L[2*k_+1][2] = y/Z ;
231  L[2*k_+1][3] = 1+y*y ;
232  L[2*k_+1][4] = -x*y ;
233  L[2*k_+1][5] = -x ;
234 
235  k_ ++;
236 
237  }
238  error = s - sd ;
239 
240  // compute the residual
241  r = error.sumSquare() ;
242 
243  for(unsigned int k=0 ; k <error.getRows()/2 ; k++)
244  {
245  res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
246  }
247  robust.setIteration(0);
248  robust.MEstimator(vpRobust::TUKEY, res, w);
249 
250  // compute the pseudo inverse of the interaction matrix
251  for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
252  {
253  W[2*k][2*k] = w[k] ;
254  W[2*k+1][2*k+1] = w[k] ;
255  }
256  // compute the pseudo inverse of the interaction matrix
257  vpMatrix Lp ;
258  (W*L).pseudoInverse(Lp,1e-6) ;
259 
260  // compute the VVS control law
261  v = -lambda*Lp*W*error ;
262 
263  cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
264  if (iter++>vvsIterMax) break ;
265  }
266 
267  if(computeCovariance)
268  covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
269  }
270  catch(...)
271  {
272  vpERROR_TRACE(" ") ;
273  throw ;
274  }
275 
276 }
277 
278 
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:136
void poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
compute the pose using a robust virtual visual servoing approach
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
double sumSquare() const
Definition: vpMatrix.cpp:868
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:95
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 what is a point.
Definition: vpPoint.h:65
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:100
static double sqr(double x)
Definition: vpMath.h:106
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.h:122
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix direct(const vpColVector &v)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:63
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1932
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:128
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:122
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98