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>
67 double residu_1 = 1e8 ;
74 unsigned int nb = (
unsigned int)
listP.size() ;
81 std::list<vpPoint> lP ;
85 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
89 sd[2*k+1] = P.
get_y() ;
94 while((
int)((residu_1 - r)*1e12) !=0)
100 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
108 double x = s[2*k] = P.
get_x();
109 double y = s[2*k+1] = P.
get_y();
110 double Z = P.
get_Z() ;
115 L[2*k][4] = -(1+x*x) ;
121 L[2*k+1][3] = 1+y*y ;
143 if (iter++>vvsIterMax) break ;
146 if(computeCovariance)
169 double residu_1 = 1e8 ;
178 unsigned int nb = (
unsigned int)
listP.size() ;
186 std::list<vpPoint> lP ;
190 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
193 sd[2*k] = P.
get_x() ;
194 sd[2*k+1] = P.
get_y() ;
204 while((
int)((residu_1 - r)*1e12) !=0)
211 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
219 double x = s[2*k] = P.
get_x();
220 double y = s[2*k+1] = P.
get_y();
221 double Z = P.
get_Z() ;
226 L[2*k][4] = -(1+x*x) ;
232 L[2*k+1][3] = 1+y*y ;
244 for(
unsigned int k=0 ; k <error.
getRows()/2 ; k++)
252 for (
unsigned int k=0 ; k < error.
getRows()/2 ; k++)
255 W[2*k+1][2*k+1] = w[k] ;
259 (W*L).pseudoInverse(Lp,1e-6) ;
265 if (iter++>vvsIterMax) break ;
268 if(computeCovariance)