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>
113 for (
unsigned int i = 0; i <
lines.size(); i += 1){
115 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
123 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
148 for (
unsigned int i = 0; i <
scales.size(); i += 1){
150 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
151 (*it)->setMovingEdge(&(this->me)) ;
155 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
175 #ifndef VISP_HAVE_OGRE
177 std::cout <<
"WARNING: ViSP dosen't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
193 double residu_1 =1e3;
206 unsigned int iter = 0;
209 unsigned int nbrow = 0;
210 unsigned int nberrors_lines = 0;
211 unsigned int nberrors_cylinders = 0;
228 vpERROR_TRACE(
"\n\t\t Error-> not enough data in the interaction matrix...") ;
236 unsigned int nerror = error.
getRows();
243 double e_prev = 0, e_cur, e_next;
248 while ( reloop ==
true && iter<10)
252 weighted_error.
resize(nerror) ;
285 std::list<vpMeSite>::const_iterator itListLine;
286 if (iter == 0 && l->
meline != NULL)
289 for (
unsigned int i=0 ; i < l->
nbFeature ; i++)
291 for (
unsigned int j=0; j < 6 ; j++)
293 L[n+i][j] = l->
L[i][j];
295 error[n+i] = l->
error[i];
297 if (error[n+i] <= limite) count = count+1.0;
315 e_next = l->
error[1];
338 e_next = l->
error[i+1];
339 if ( fabs(e_cur - e_prev) < limite )
343 if ( fabs(e_cur - e_next) < limite )
360 std::list<vpMeSite>::const_iterator itCyl1;
361 std::list<vpMeSite>::const_iterator itCyl2;
367 for(
unsigned int i=0 ; i < cy->
nbFeature ; i++){
368 for(
unsigned int j=0; j < 6 ; j++){
369 L[n+i][j] = cy->
L[i][j];
371 error[n+i] = cy->
error[i];
373 if (error[n+i] <= limite) count = count+1.0;
381 if(i<cy->nbFeaturel1) {
395 e_cur = cy->
error[0];
398 e_next = cy->
error[1];
409 e_cur = cy->
error[i];
412 e_next = cy->
error[i+1];
425 e_cur = cy->
error[i];
434 e_cur = cy->
error[i];
443 e_cur = cy->
error[i];
444 e_next = cy->
error[i+1];
445 if ( fabs(e_cur - e_prev) < limite ){
448 if ( fabs(e_cur - e_next) < limite ){
458 count = count / (double)nbrow;
466 double wi ;
double eri ;
467 for(
unsigned int i = 0; i < nerror; i++){
473 weighted_error[i] = wi*eri ;
477 for (
unsigned int i=0 ; i < nerror ; i++){
478 for (
unsigned int j=0 ; j < 6 ; j++){
479 L[i][j] = w[i]*factor[i]*L[i][j] ;
495 vpRobust robust_lines(nberrors_lines);
496 vpRobust robust_cylinders(nberrors_cylinders);
509 while ( ((
int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
512 unsigned int nlines = 0;
513 unsigned int ncylinders = 0;
517 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
518 for (
unsigned int j=0; j < 6 ; j++){
519 L[n+i][j] = l->
L[i][j];
520 error[n+i] = l->
error[i];
521 error_lines[nlines+i] = error[n+i];
531 for(
unsigned int i=0 ; i < cy->
nbFeature ; i++){
532 for(
unsigned int j=0; j < 6 ; j++){
533 L[n+i][j] = cy->
L[i][j];
534 error[n+i] = cy->
error[i];
535 error_cylinders[ncylinders+i] = error[n+i];
545 weighted_error.
resize(nerror);
548 w_lines.
resize(nberrors_lines);
550 w_cylinders.
resize(nberrors_cylinders);
555 if(nberrors_lines > 0)
557 if(nberrors_cylinders > 0){
565 if(nberrors_lines > 0)
567 if(nberrors_cylinders > 0){
572 unsigned int cpt = 0;
574 if(cpt<nberrors_lines){
575 w[cpt] = w_lines[cpt];
578 w[cpt] = w_cylinders[cpt-nberrors_lines];
593 for(
unsigned int i=0; i<nerror; i++){
600 weighted_error[i] = wi*eri ;
606 for (
unsigned int i=0 ; i < nerror ; i++){
607 for (
unsigned int j=0 ; j < 6 ; j++){
608 L[i][j] = w[i]*factor[i]*L[i][j];
632 std::list<vpMeSite>::iterator itListLine;
635 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
665 std::list<vpMeSite>::iterator itListCyl1;
666 std::list<vpMeSite>::iterator itListCyl2;
697 for(
unsigned int i=cy->
nbFeaturel1 ; i < cy->nbFeature ; i++){
732 int nbExpectedPoint = 0;
774 || nbExpectedPoint < 2)
794 unsigned int lvl =
scales.size();
809 vpTRACE(
"Error in moving edge tracking") ;
816 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
824 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
835 vpTRACE(
"Error in computeVVS") ;
851 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
858 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
871 vpTRACE(
"Error in moving edge updating") ;
876 bool newvisibleface = false ;
911 #ifdef VISP_HAVE_OGRE
921 unsigned int i=
scales.size();
1029 #ifdef VISP_HAVE_XML2
1038 std::cout <<
" *********** Parsing XML for MbEdge Tracker ************ " << std::endl;
1039 xmlp.
parse(configFile);
1049 xmlp.
getMe(meParser);
1056 vpTRACE(
"You need the libXML2 to read the config file %s", configFile);
1074 const unsigned int thickness,
const bool displayFullModel)
1078 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1082 l->
display(I,cMo, cam, col, thickness, displayFullModel);
1086 (*it)->display(I, cMo, cam, col, thickness);
1093 #ifdef VISP_HAVE_OGRE
1112 const unsigned int thickness,
const bool displayFullModel)
1116 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1120 l->
display(I, cMo, cam, col, thickness, displayFullModel) ;
1124 (*it)->display(I, cMo, cam, col, thickness) ;
1131 #ifdef VISP_HAVE_OGRE
1153 bool isvisible = false ;
1157 if (index ==-1) isvisible =true ;
1290 bool samePoint(
const vpPoint &P1,
const vpPoint &P2,
double threshold=1e-5)
1317 bool already_here = false ;
1320 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1323 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1325 if((samePoint(*(l->
p1),P1) && samePoint(*(l->
p2),P2)) ||
1326 (samePoint(*(l->
p1),P2) && samePoint(*(l->
p2),P1)) ){
1327 already_here = true ;
1344 lines[i].push_back(l);
1361 for(
unsigned int i=0; i<
scales.size(); i++){
1363 for(std::list<vpMbtDistanceLine*>::iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1365 if (name.compare(l->
getName()) == 0){
1388 bool already_here = false ;
1391 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1394 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
1396 if((samePoint(*(cy->
p1),P1) && samePoint(*(cy->
p2),P2)) ||
1397 (samePoint(*(cy->
p1),P2) && samePoint(*(cy->
p2),P1)) ){
1429 for(
unsigned int i=0; i<
scales.size(); i++){
1431 for(std::list<vpMbtDistanceCylinder*>::iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
1433 if (name.compare(cy->
getName()) == 0){
1455 for (
unsigned int i=0 ; i < nbpt-1 ; i++)
1481 #ifdef VISP_HAVE_OGRE
1482 bool changed =
false;
1493 newvisibleline = true ;
1496 newvisibleline = false ;
1513 #ifdef VISP_HAVE_OGRE
1523 #ifdef VISP_HAVE_OGRE
1524 bool changed =
false;
1535 newvisibleline = true ;
1538 newvisibleline = false ;
1555 std::string model(file);
1587 polygon->
setIndex((
int)_indexFace);
1588 for(
unsigned int j = 0; j < _corners.size(); j++) {
1609 if(_indexCylinder != 0){
1627 for (
unsigned int i = 0; i <
scales.size(); i += 1){
1629 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[i].begin(); it!=
lines[i].end(); ++it){
1631 if (l!=NULL)
delete l ;
1635 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[i].begin(); it!=
cylinders[i].end(); ++it){
1637 if (cy!=NULL)
delete cy;
1693 unsigned int nbGoodPoints = 0;
1695 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[level].begin(); it!=
lines[level].end(); ++it){
1706 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[level].begin(); it!=
cylinders[level].end(); ++it){
1719 return nbGoodPoints;
1735 if(index >= static_cast<unsigned int>(
faces.
size()) ){
1739 return faces[index];
1750 return static_cast<unsigned int>(
faces.
size());
1776 unsigned int nbActivatedLevels = 0;
1777 for (
unsigned int i = 0; i < scales.size(); i += 1){
1779 nbActivatedLevels++;
1782 if((scales.size() < 1) || (nbActivatedLevels == 0)){
1783 vpERROR_TRACE(
" !! WARNING : must use at least one level for the tracking. Use the global one");
1784 this->scales.resize(0);
1785 this->scales.push_back(
true);
1793 lines.resize(scales.size());
1795 for (
unsigned int i = 0; i <
lines.size(); i += 1){
1819 _pyramid.resize(
scales.size());
1828 for(
unsigned int i=1; i<_pyramid.size(); i += 1){
1830 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
1832 #ifdef VISP_HAVE_OPENCV
1833 IplImage* vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (int)_I.
getHeight()), IPL_DEPTH_8U, 1);
1834 vpI0->imageData = (
char*)(_I.
bitmap);
1835 IplImage* vpI = cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
1836 cvResize(vpI0, vpI, CV_INTER_NN);
1838 cvReleaseImage(&vpI);
1839 vpI0->imageData = NULL;
1840 cvReleaseImageHeader(&vpI0);
1842 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale){
1843 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale){
1844 (*I)[k][l] = _I[ii][jj];
1865 if(_pyramid.size() > 0){
1867 for (
unsigned int i = 1; i < _pyramid.size(); i += 1){
1868 if(_pyramid[i] != NULL){
1891 std::ostringstream oss;
1893 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
1897 linesList =
lines[level];
1915 std::ostringstream oss;
1917 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
1934 const double ratio = pow(2., (
int)_scale);
1956 const double ratio = pow(2., (
int)_scale);