50 #include <visp/vpPose.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpException.h>
53 #include <visp/vpPoseException.h>
54 #include <visp/vpMeterPixelConversion.h>
55 #include <visp/vpCameraParameters.h>
56 #include <visp/vpDisplay.h>
57 #include <visp/vpMath.h>
62 #define DEBUG_LEVEL1 0
70 std::cout <<
"begin vpPose::Init() " << std::endl ;
83 computeCovariance =
false;
85 ransacMaxTrials = 1000;
86 ransacThreshold = 0.0001;
87 ransacNbInlierConsensus = 4;
90 std::cout <<
"end vpPose::Init() " << std::endl ;
98 std::cout <<
"begin vpPose::vpPose() " << std::endl ;
104 std::cout <<
"end vpPose::vpPose() " << std::endl ;
115 std::cout <<
"begin vpPose::~vpPose() " << std::endl ;
121 std::cout <<
"end vpPose::~vpPose() " << std::endl ;
131 std::cout <<
"begin vpPose::ClearPoint() " << std::endl ;
138 std::cout <<
"end vpPose::ClearPoint() " << std::endl ;
154 std::cout <<
"begin vpPose::AddPoint(Dot) " << std::endl ;
157 listP.push_back(newP);
161 std::cout <<
"end vpPose::AddPoint(Dot) " << std::endl ;
187 coplanar_plane_type = 0;
192 "Not enough points ")) ;
195 if (
npt ==3)
return true ;
197 double x1=0,x2=0,x3=0,y1=0,y2=0,y3=0,z1=0,z2=0,z3=0 ;
199 std::list<vpPoint>::const_iterator it =
listP.begin();
204 bool degenerate =
true;
205 bool not_on_origin =
true;
206 std::list<vpPoint>::const_iterator it_tmp;
208 std::list<vpPoint>::const_iterator it_i, it_j, it_k;
209 for (it_i=
listP.begin(); it_i !=
listP.end(); ++it_i) {
210 if (degenerate ==
false) {
216 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon())
217 && (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon())
218 && (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
219 not_on_origin =
false;
222 not_on_origin =
true;
225 it_tmp = it_i; it_tmp ++;
226 for (it_j=it_tmp; it_j !=
listP.end(); ++it_j) {
227 if (degenerate ==
false) {
232 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon())
233 && (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon())
234 && (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
235 not_on_origin =
false;
238 not_on_origin =
true;
241 it_tmp = it_j; it_tmp ++;
242 for (it_k=it_tmp; it_k !=
listP.end(); ++it_k) {
244 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon())
245 && (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon())
246 && (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
247 not_on_origin =
false;
250 not_on_origin =
true;
266 a_b[0] = x1-x2; a_b[1] = y1-y2; a_b[2] = z1-z2;
267 b_c[0] = x2-x3; b_c[1] = y2-y3; b_c[2] = z2-z3;
270 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon())
275 if (degenerate ==
false)
284 coplanar_plane_type = 4;
288 double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
289 double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
290 double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
291 double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
294 if (std::fabs(b) <= std::numeric_limits<double>::epsilon()
295 && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
296 coplanar_plane_type = 1;
297 }
else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
298 && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
299 coplanar_plane_type = 2;
300 }
else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
301 && std::fabs(b) <= std::numeric_limits<double>::epsilon() ) {
302 coplanar_plane_type = 3;
309 for(it=
listP.begin(); it !=
listP.end(); ++it)
344 for(std::list<vpPoint>::const_iterator it=
listP.begin(); it !=
listP.end(); ++it)
347 double x = P.
get_x() ;
348 double y = P.
get_y() ;
385 std::cout <<
"begin vpPose::ComputePose()" << std::endl ;
392 "No enough point ")) ;
404 vpERROR_TRACE(
"Dementhon method cannot be used in that case ") ;
408 "Not enough points ")) ;
412 int coplanar_plane_type = 0;
413 bool plan =
coplanar(coplanar_plane_type) ;
447 int coplanar_plane_type;
448 bool plan =
coplanar(coplanar_plane_type) ;
450 if (plan ==
true && coplanar_plane_type > 0)
453 if (coplanar_plane_type == 4)
455 vpERROR_TRACE(
"Lagrange method cannot be used in that case ") ;
458 "Points are collinear ")) ;
462 vpERROR_TRACE(
"Lagrange method cannot be used in that case ") ;
466 "Not enough points ")) ;
481 vpERROR_TRACE(
"Lagrange method cannot be used in that case ") ;
485 "Not enough points ")) ;
501 vpERROR_TRACE(
"Ransac method cannot be used in that case ") ;
505 "Not enough points ")) ;
560 std::cout <<
"end vpPose::ComputePose()" << std::endl ;
570 for(std::list<vpPoint>::const_iterator it=
listP.begin(); it !=
listP.end(); ++it)
574 std::cout <<
"3D oP " << P.
oP.
t() ;
575 std::cout <<
"3D cP " << P.
cP.
t() ;
576 std::cout <<
"2D " << P.
p.
t() ;
614 for(std::list<vpPoint>::const_iterator it=
listP.begin(); it !=
listP.end(); ++it)
637 for(std::list<vpPoint>::const_iterator it=
listP.begin(); it !=
listP.end(); ++it)
687 irectx[0]=(p1.
get_x()) ;
688 irecty[0]=(p1.
get_y()) ;
689 irectx[1]=(p2.
get_x()) ;
690 irecty[1]=(p2.
get_y()) ;
691 irectx[2]=(p3.
get_x()) ;
692 irecty[2]=(p3.
get_y()) ;
693 irectx[3]=(p4.
get_x()) ;
694 irecty[3]=(p4.
get_y()) ;
702 for (
unsigned int i=0 ; i < 3 ; i++)
703 for(
unsigned int j=0 ; j < 3 ; j++)
704 H[i][j] = hom[i][j] ;