51 #include <visp/vpDebug.h>
52 #include <visp/vpPose.h>
53 #include <visp/vpExponentialMap.h>
54 #include <visp/vpPixelMeterConversion.h>
55 #include <visp/vpImageIo.h>
56 #include <visp/vpRobust.h>
57 #include <visp/vpDisplayOpenCV.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGDI.h>
60 #include <visp/vpMatrixException.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpException.h>
63 #include <visp/vpTrackingException.h>
64 #include <visp/vpMbEdgeTracker.h>
65 #include <visp/vpMbtDistanceLine.h>
66 #include <visp/vpMbtXmlParser.h>
67 #include <visp/vpMbtPolygon.h>
114 for (
unsigned int i = 0; i <
lines.size(); i += 1){
116 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
124 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
149 for (
unsigned int i = 0; i <
scales.size(); i += 1){
151 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
152 (*it)->setMovingEdge(&(this->me)) ;
156 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
176 #ifndef VISP_HAVE_OGRE
178 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
194 double residu_1 =1e3;
207 unsigned int iter = 0;
210 unsigned int nbrow = 0;
211 unsigned int nberrors_lines = 0;
212 unsigned int nberrors_cylinders = 0;
229 vpERROR_TRACE(
"\n\t\t Error-> not enough data in the interaction matrix...") ;
237 unsigned int nerror = error.
getRows();
244 double e_prev = 0, e_cur, e_next;
249 while ( reloop ==
true && iter<10)
253 weighted_error.
resize(nerror) ;
286 std::list<vpMeSite>::const_iterator itListLine;
287 if (iter == 0 && l->
meline != NULL)
290 for (
unsigned int i=0 ; i < l->
nbFeature ; i++)
292 for (
unsigned int j=0; j < 6 ; j++)
294 L[n+i][j] = l->
L[i][j];
296 error[n+i] = l->
error[i];
298 if (error[n+i] <= limite) count = count+1.0;
316 e_next = l->
error[1];
339 e_next = l->
error[i+1];
340 if ( fabs(e_cur - e_prev) < limite )
344 if ( fabs(e_cur - e_next) < limite )
361 std::list<vpMeSite>::const_iterator itCyl1;
362 std::list<vpMeSite>::const_iterator itCyl2;
368 for(
unsigned int i=0 ; i < cy->
nbFeature ; i++){
369 for(
unsigned int j=0; j < 6 ; j++){
370 L[n+i][j] = cy->
L[i][j];
372 error[n+i] = cy->
error[i];
374 if (error[n+i] <= limite) count = count+1.0;
382 if(i<cy->nbFeaturel1) {
396 e_cur = cy->
error[0];
399 e_next = cy->
error[1];
410 e_cur = cy->
error[i];
413 e_next = cy->
error[i+1];
426 e_cur = cy->
error[i];
435 e_cur = cy->
error[i];
444 e_cur = cy->
error[i];
445 e_next = cy->
error[i+1];
446 if ( fabs(e_cur - e_prev) < limite ){
449 if ( fabs(e_cur - e_next) < limite ){
459 count = count / (double)nbrow;
467 double wi ;
double eri ;
468 for(
unsigned int i = 0; i < nerror; i++){
474 weighted_error[i] = wi*eri ;
478 for (
unsigned int i=0 ; i < nerror ; i++){
479 for (
unsigned int j=0 ; j < 6 ; j++){
480 L[i][j] = w[i]*factor[i]*L[i][j] ;
496 vpRobust robust_lines(nberrors_lines);
497 vpRobust robust_cylinders(nberrors_cylinders);
510 while ( ((
int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
513 unsigned int nlines = 0;
514 unsigned int ncylinders = 0;
518 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
519 for (
unsigned int j=0; j < 6 ; j++){
520 L[n+i][j] = l->
L[i][j];
521 error[n+i] = l->
error[i];
522 error_lines[nlines+i] = error[n+i];
532 for(
unsigned int i=0 ; i < cy->
nbFeature ; i++){
533 for(
unsigned int j=0; j < 6 ; j++){
534 L[n+i][j] = cy->
L[i][j];
535 error[n+i] = cy->
error[i];
536 error_cylinders[ncylinders+i] = error[n+i];
546 weighted_error.
resize(nerror);
549 w_lines.
resize(nberrors_lines);
551 w_cylinders.
resize(nberrors_cylinders);
556 if(nberrors_lines > 0)
558 if(nberrors_cylinders > 0){
566 if(nberrors_lines > 0)
568 if(nberrors_cylinders > 0){
573 unsigned int cpt = 0;
575 if(cpt<nberrors_lines){
576 w[cpt] = w_lines[cpt];
579 w[cpt] = w_cylinders[cpt-nberrors_lines];
594 for(
unsigned int i=0; i<nerror; i++){
601 weighted_error[i] = wi*eri ;
607 for (
unsigned int i=0 ; i < nerror ; i++){
608 for (
unsigned int j=0 ; j < 6 ; j++){
609 L[i][j] = w[i]*factor[i]*L[i][j];
633 std::list<vpMeSite>::iterator itListLine;
636 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
666 std::list<vpMeSite>::iterator itListCyl1;
667 std::list<vpMeSite>::iterator itListCyl2;
698 for(
unsigned int i=cy->
nbFeaturel1 ; i < cy->nbFeature ; i++){
733 int nbExpectedPoint = 0;
775 || nbExpectedPoint < 2)
795 unsigned int lvl = (
unsigned int)
scales.size();
810 vpTRACE(
"Error in moving edge tracking") ;
817 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
825 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
836 vpTRACE(
"Error in computeVVS") ;
852 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
859 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
872 vpTRACE(
"Error in moving edge updating") ;
877 bool newvisibleface = false ;
912 #ifdef VISP_HAVE_OGRE
923 unsigned int i = (
unsigned int)
scales.size();
1036 #ifdef VISP_HAVE_XML2
1045 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1046 xmlp.
parse(configFile);
1056 xmlp.
getMe(meParser);
1072 vpTRACE(
"You need the libXML2 to read the config file %s", configFile);
1090 const unsigned int thickness,
const bool displayFullModel)
1094 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1098 l->
display(I,cMo, cam, col, thickness, displayFullModel);
1102 (*it)->display(I, cMo, cam, col, thickness);
1109 #ifdef VISP_HAVE_OGRE
1128 const unsigned int thickness,
const bool displayFullModel)
1132 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1136 l->
display(I, cMo, cam, col, thickness, displayFullModel) ;
1140 (*it)->display(I, cMo, cam, col, thickness) ;
1147 #ifdef VISP_HAVE_OGRE
1169 bool isvisible = false ;
1173 if (index ==-1) isvisible =true ;
1306 bool samePoint(
const vpPoint &P1,
const vpPoint &P2,
double threshold=1e-5)
1333 bool already_here = false ;
1336 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1339 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1341 if((samePoint(*(l->
p1),P1) && samePoint(*(l->
p2),P2)) ||
1342 (samePoint(*(l->
p1),P2) && samePoint(*(l->
p2),P1)) ){
1343 already_here = true ;
1370 lines[i].push_back(l);
1387 for(
unsigned int i=0; i<
scales.size(); i++){
1389 for(std::list<vpMbtDistanceLine*>::iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1391 if (name.compare(l->
getName()) == 0){
1414 bool already_here = false ;
1417 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1420 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
1422 if((samePoint(*(cy->
p1),P1) && samePoint(*(cy->
p2),P2)) ||
1423 (samePoint(*(cy->
p1),P2) && samePoint(*(cy->
p2),P1)) ){
1455 for(
unsigned int i=0; i<
scales.size(); i++){
1457 for(std::list<vpMbtDistanceCylinder*>::iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
1459 if (name.compare(cy->
getName()) == 0){
1481 for (
unsigned int i=0 ; i < nbpt-1 ; i++)
1490 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1510 #ifdef VISP_HAVE_OGRE
1511 bool changed =
false;
1522 newvisibleline = true ;
1525 newvisibleline = false ;
1529 #endif //VISP_BUILD_DEPRECATED_FUNCTIONS
1546 bool changed =
false;
1551 #ifdef VISP_HAVE_OGRE
1562 newvisibleline = true ;
1565 newvisibleline = false ;
1582 std::string model(file);
1613 polygon->
setNbPoint((
unsigned int)_corners.size());
1614 polygon->
setIndex((
int)_indexFace);
1615 for(
unsigned int j = 0; j < _corners.size(); j++) {
1636 if(_indexCylinder != 0){
1654 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1656 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1658 if (l!=NULL)
delete l ;
1662 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
1664 if (cy!=NULL)
delete cy;
1724 unsigned int nbGoodPoints = 0;
1726 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[level].begin(); it!=
lines[level].end(); ++it){
1737 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[level].begin(); it!=
cylinders[level].end(); ++it){
1750 return nbGoodPoints;
1766 if(index >= static_cast<unsigned int>(
faces.
size()) ){
1770 return faces[index];
1781 return static_cast<unsigned int>(
faces.
size());
1807 unsigned int nbActivatedLevels = 0;
1808 for (
unsigned int i = 0; i < scales.size(); i += 1){
1810 nbActivatedLevels++;
1813 if((scales.size() < 1) || (nbActivatedLevels == 0)){
1814 vpERROR_TRACE(
" !! WARNING : must use at least one level for the tracking. Use the global one");
1815 this->scales.resize(0);
1816 this->scales.push_back(
true);
1824 lines.resize(scales.size());
1826 for (
unsigned int i = 0; i <
lines.size(); i += 1){
1842 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
1843 else if ( dist < 0 )
1844 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
1850 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1852 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1870 vpTRACE(
"Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
1871 else if ( dist < 0 )
1872 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
1878 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1880 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1903 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1905 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1930 _pyramid.resize(
scales.size());
1939 for(
unsigned int i=1; i<_pyramid.size(); i += 1){
1941 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
1943 #ifdef VISP_HAVE_OPENCV
1944 IplImage* vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (int)_I.
getHeight()), IPL_DEPTH_8U, 1);
1945 vpI0->imageData = (
char*)(_I.
bitmap);
1946 IplImage* vpI = cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
1947 cvResize(vpI0, vpI, CV_INTER_NN);
1949 cvReleaseImage(&vpI);
1950 vpI0->imageData = NULL;
1951 cvReleaseImageHeader(&vpI0);
1953 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale){
1954 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale){
1955 (*I)[k][l] = _I[ii][jj];
1976 if(_pyramid.size() > 0){
1978 for (
unsigned int i = 1; i < _pyramid.size(); i += 1){
1979 if(_pyramid[i] != NULL){
2002 std::ostringstream oss;
2004 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2008 linesList =
lines[level];
2026 std::ostringstream oss;
2028 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2045 const double ratio = pow(2., (
int)_scale);
2067 const double ratio = pow(2., (
int)_scale);