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>
72 double residu_1 = 1e8 ;
79 unsigned int nb =
listP.size() ;
86 std::list<vpPoint> lP ;
90 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
94 sd[2*k+1] = P.
get_y() ;
99 while((
int)((residu_1 - r)*1e12) !=0)
105 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
113 double x = s[2*k] = P.
get_x();
114 double y = s[2*k+1] = P.
get_y();
115 double Z = P.
get_Z() ;
120 L[2*k][4] = -(1+x*x) ;
126 L[2*k+1][3] = 1+y*y ;
148 if (iter++>vvsIterMax) break ;
151 if(computeCovariance)
178 double residu_1 = 1e8 ;
187 unsigned int nb =
listP.size() ;
195 std::list<vpPoint> lP ;
199 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
202 sd[2*k] = P.
get_x() ;
203 sd[2*k+1] = P.
get_y() ;
213 while((
int)((residu_1 - r)*1e12) !=0)
220 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
228 double x = s[2*k] = P.
get_x();
229 double y = s[2*k+1] = P.
get_y();
230 double Z = P.
get_Z() ;
235 L[2*k][4] = -(1+x*x) ;
241 L[2*k+1][3] = 1+y*y ;
253 for(
unsigned int k=0 ; k <error.
getRows()/2 ; k++)
257 robust.setIteration(0);
261 for (
unsigned int k=0 ; k < error.
getRows()/2 ; k++)
264 W[2*k+1][2*k+1] = w[k] ;
268 (W*L).pseudoInverse(Lp,1e-6) ;
274 if (iter++>vvsIterMax) break ;
277 if(computeCovariance)