46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpExponentialMap.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMatrixException.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPolygon3D.h>
53 #include <visp3/core/vpTrackingException.h>
54 #include <visp3/core/vpVelocityTwistMatrix.h>
55 #include <visp3/mbt/vpMbEdgeTracker.h>
56 #include <visp3/mbt/vpMbtDistanceLine.h>
57 #include <visp3/mbt/vpMbtXmlGenericParser.h>
58 #include <visp3/vision/vpPose.h>
70 : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
71 percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
72 m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
73 m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
74 m_robust_edge(), m_featuresToBeDisplayedEdge()
92 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
94 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
102 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
111 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
137 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
139 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
144 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
150 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
169 double residu_1 = 1e3;
172 unsigned int iter = 0;
185 while (reloop ==
true && iter < 10) {
190 count = count / (double)nbrow;
222 while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter <
m_maxIter)) {
225 bool reStartFromLastIncrement =
false;
229 if (!reStartFromLastIncrement) {
237 if (!isoJoIdentity_) {
243 double wi = 0.0, eri = 0.0;
244 double num = 0.0, den = 0.0;
246 for (
unsigned int i = 0; i < nbrow; i++) {
255 for (
unsigned int j = 0; j < 6; j++) {
260 for (
unsigned int i = 0; i < nbrow; i++) {
303 double e_prev = 0, e_cur, e_next;
305 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
306 if ((*it)->isTracked()) {
314 int index = *itindex;
326 std::list<vpMeSite>::const_iterator itListLine;
328 unsigned int indexFeature = 0;
330 for (
size_t a = 0; a < l->
meline.size(); a++) {
331 if (iter == 0 && l->
meline[a] != NULL)
332 itListLine = l->
meline[a]->getMeList().begin();
334 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
335 for (
unsigned int j = 0; j < 6; j++) {
336 m_L_edge[n + i][j] = l->
L[indexFeature][j];
354 if (indexFeature == 0) {
357 e_next = l->
error[1];
368 e_cur = l->
error[indexFeature];
375 e_cur = l->
error[indexFeature];
376 e_next = l->
error[indexFeature + 1];
377 if (fabs(e_cur - e_prev) < limite) {
380 if (fabs(e_cur - e_next) < limite) {
392 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
394 if ((*it)->isTracked()) {
399 std::list<vpMeSite>::const_iterator itCyl1;
400 std::list<vpMeSite>::const_iterator itCyl2;
402 itCyl1 = cy->
meline1->getMeList().begin();
403 itCyl2 = cy->
meline2->getMeList().begin();
406 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
407 for (
unsigned int j = 0; j < 6; j++) {
420 if (i < cy->nbFeaturel1) {
433 e_cur = cy->
error[0];
435 e_next = cy->
error[1];
444 e_cur = cy->
error[i];
446 e_next = cy->
error[i + 1];
457 e_cur = cy->
error[i];
464 e_cur = cy->
error[i];
471 e_cur = cy->
error[i];
472 e_next = cy->
error[i + 1];
473 if (fabs(e_cur - e_prev) < limite) {
476 if (fabs(e_cur - e_next) < limite) {
487 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
488 if ((*it)->isTracked()) {
493 std::list<vpMeSite>::const_iterator itCir;
494 if (iter == 0 && (ci->
meEllipse != NULL)) {
495 itCir = ci->
meEllipse->getMeList().begin();
498 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
499 for (
unsigned int j = 0; j < 6; j++) {
519 e_cur = ci->
error[0];
521 e_next = ci->
error[1];
532 e_cur = ci->
error[i];
539 e_cur = ci->
error[i];
540 e_next = ci->
error[i + 1];
541 if (fabs(e_cur - e_prev) < limite) {
544 if (fabs(e_cur - e_next) < limite) {
563 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
564 if ((*it)->isTracked()) {
571 int index = *itindex;
582 unsigned int indexFeature = 0;
583 for (
size_t a = 0; a < l->
meline.size(); a++) {
584 std::list<vpMeSite>::const_iterator itListLine;
585 if (l->
meline[a] != NULL) {
586 itListLine = l->
meline[a]->getMeList().begin();
588 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
602 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
604 if ((*it)->isTracked()) {
608 std::list<vpMeSite>::const_iterator itCyl1;
609 std::list<vpMeSite>::const_iterator itCyl2;
611 itCyl1 = cy->
meline1->getMeList().begin();
612 itCyl2 = cy->
meline2->getMeList().begin();
615 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
618 if (i < cy->nbFeaturel1) {
633 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
634 if ((*it)->isTracked()) {
638 std::list<vpMeSite>::const_iterator itCir;
640 itCir = ci->
meEllipse->getMeList().begin();
643 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
662 for (
unsigned int i = 0; i < nerror; i++) {
668 for (
unsigned int j = 0; j < 6; j++) {
673 for (
unsigned int i = 0; i < nerror; i++) {
687 if (isoJoIdentity_) {
691 unsigned int rank = (
m_L_edge * cVo).kernel(K);
700 isoJoIdentity_ =
false;
708 if (isoJoIdentity_) {
718 v = -0.7 * LVJTLVJ.
pseudoInverse(LVJTLVJ.
getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
728 unsigned int nbrow = 0;
729 unsigned int nberrors_lines = 0;
730 unsigned int nberrors_cylinders = 0;
731 unsigned int nberrors_circles = 0;
733 nbrow =
initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
737 "No data found to compute the interaction matrix...");
768 "computeVVSInteractionMatrixAndR"
769 "esidu() should not be called!");
779 unsigned int nlines = 0;
780 unsigned int ncylinders = 0;
781 unsigned int ncircles = 0;
785 if ((*it)->isTracked()) {
789 for (
unsigned int j = 0; j < 6; j++) {
800 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
802 if ((*it)->isTracked()) {
805 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
806 for (
unsigned int j = 0; j < 6; j++) {
818 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
820 if ((*it)->isTracked()) {
823 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
824 for (
unsigned int j = 0; j < 6; j++) {
842 if (nberrors_lines > 0)
844 if (nberrors_cylinders > 0)
846 if (nberrors_circles > 0)
865 unsigned int nbFeatures = 0;
870 for (
size_t a = 0; a < l->
meline.size(); a++) {
871 if (l->
meline[a] != NULL) {
872 double lineNormGradient;
873 unsigned int lineNbFeatures;
878 nbFeatures += lineNbFeatures;
884 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
889 double cylinderNormGradient = 0;
890 unsigned int cylinderNbFeatures = 0;
895 nbFeatures += cylinderNbFeatures;
899 double cylinderNormGradient = 0;
900 unsigned int cylinderNbFeatures = 0;
905 nbFeatures += cylinderNbFeatures;
910 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
914 double circleNormGradient = 0;
915 unsigned int circleNbFeatures = 0;
920 nbFeatures += circleNbFeatures;
924 if (nbFeatures > 0) {
941 int nbExpectedPoint = 0;
949 for (
size_t a = 0; a < l->
meline.size(); a++) {
950 if (l->
meline[a] != NULL) {
951 nbExpectedPoint += (int)l->
meline[a]->expecteddensity;
952 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
953 itme != l->
meline[a]->getMeList().end(); ++itme) {
965 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
969 nbExpectedPoint += (int)cy->
meline1->expecteddensity;
970 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
971 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
978 nbExpectedPoint += (int)cy->
meline2->expecteddensity;
979 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
980 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
990 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
994 nbExpectedPoint += ci->
meEllipse->getExpectedDensity();
995 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
996 itme != ci->
meEllipse->getMeList().end(); ++itme) {
1010 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1011 std::ostringstream oss;
1012 oss <<
"Not enough moving edges (" << nbGoodPoint <<
") to track the object: expected " << nb_min
1014 <<
" using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1030 unsigned int lvl = (
unsigned int)
scales.size();
1044 vpTRACE(
"Error in moving edge tracking");
1086 bool newvisibleface =
false;
1140 #ifdef VISP_HAVE_OGRE
1169 unsigned int i = (
unsigned int)
scales.size();
1236 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1238 xmlp.
parse(configFile);
1288 bool displayFullModel)
1297 for (
size_t i = 0; i < models.size(); i++) {
1304 double n20 = models[i][3];
1305 double n11 = models[i][4];
1306 double n02 = models[i][5];
1311 #ifdef VISP_HAVE_OGRE
1330 bool displayFullModel)
1339 for (
size_t i = 0; i < models.size(); i++) {
1346 double n20 = models[i][3];
1347 double n11 = models[i][4];
1348 double n02 = models[i][5];
1353 #ifdef VISP_HAVE_OGRE
1361 std::vector<std::vector<double> > features;
1363 const unsigned int lvl = 0;
1364 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1368 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1372 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1377 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1381 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1385 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1409 bool displayFullModel)
1411 std::vector<std::vector<double> > models;
1413 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1417 std::vector<std::vector<double> > currentModel =
1418 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1419 models.insert(models.end(), currentModel.begin(), currentModel.end());
1422 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1424 std::vector<std::vector<double> > currentModel =
1425 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1426 models.insert(models.end(), currentModel.begin(), currentModel.end());
1429 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1431 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1432 if (!paramsCircle.empty()) {
1433 models.push_back(paramsCircle);
1523 const bool doNotTrack =
false;
1528 bool isvisible =
false;
1532 int index = *itindex;
1552 for (
size_t a = 0; a < l->
meline.size(); a++) {
1553 if (l->
meline[a] != NULL)
1555 if (a < l->nbFeature.size())
1564 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1568 bool isvisible =
false;
1600 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1603 bool isvisible =
false;
1636 const bool doNotTrack =
false;
1649 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1660 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1682 if ((*it)->isTracked()) {
1692 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1694 if ((*it)->isTracked()) {
1704 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1706 if ((*it)->isTracked()) {
1723 if ((*it)->isTracked()) {
1725 unsigned int indexLine = 0;
1727 for (
size_t a = 0; a < l->
meline.size(); a++) {
1729 std::list<vpMeSite>::iterator itListLine;
1730 itListLine = l->
meline[a]->getMeList().begin();
1732 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
1735 if (
m_w_edge[n + indexLine] < 0.5) {
1762 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1764 if ((*it)->isTracked()) {
1767 std::list<vpMeSite>::iterator itListCyl1;
1768 std::list<vpMeSite>::iterator itListCyl2;
1771 itListCyl1 = cy->
meline1->getMeList().begin();
1772 itListCyl2 = cy->
meline2->getMeList().begin();
1774 for (
unsigned int i = 0; i < cy->
nbFeaturel1; i++) {
1799 for (
unsigned int i = cy->
nbFeaturel1; i < cy->nbFeature; i++) {
1828 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1830 if ((*it)->isTracked()) {
1833 std::list<vpMeSite>::iterator itListCir;
1836 itListCir = ci->
meEllipse->getMeList().begin();
1840 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
1882 if ((*it)->isTracked()) {
1890 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1892 if ((*it)->isTracked()) {
1900 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1902 if ((*it)->isTracked()) {
1915 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1917 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1918 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
1919 if ((*it)->meline[a] != NULL) {
1920 delete (*it)->meline[a];
1921 (*it)->meline[a] = NULL;
1925 (*it)->meline.clear();
1926 (*it)->nbFeature.clear();
1927 (*it)->nbFeatureTotal = 0;
1930 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1932 if ((*it)->meline1 != NULL) {
1933 delete (*it)->meline1;
1934 (*it)->meline1 = NULL;
1936 if ((*it)->meline2 != NULL) {
1937 delete (*it)->meline2;
1938 (*it)->meline2 = NULL;
1941 (*it)->nbFeature = 0;
1942 (*it)->nbFeaturel1 = 0;
1943 (*it)->nbFeaturel2 = 0;
1946 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1947 if ((*it)->meEllipse != NULL) {
1948 delete (*it)->meEllipse;
1949 (*it)->meEllipse = NULL;
1951 (*it)->nbFeature = 0;
1973 bool already_here =
false;
1976 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1979 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1983 already_here =
true;
1989 if (!already_here) {
2012 lines[i].push_back(l);
2029 for (
unsigned int i = 0; i <
scales.size(); i++) {
2031 for (std::list<vpMbtDistanceLine *>::iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2033 if (name.compare(l->
getName()) == 0) {
2053 const std::string &name)
2056 bool already_here =
false;
2059 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2062 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2071 if (!already_here) {
2112 const std::string &name)
2115 bool already_here =
false;
2118 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2121 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2131 if (!already_here) {
2159 for (
unsigned int i = 0; i <
scales.size(); i++) {
2161 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2163 if (name.compare(cy->
getName()) == 0) {
2181 for (
unsigned int i = 0; i <
scales.size(); i++) {
2183 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2185 if (name.compare(ci->
getName()) == 0) {
2203 for (
unsigned int i = 0; i < nbpt - 1; i++)
2221 bool &newvisibleline)
2224 bool changed =
false;
2231 #ifdef VISP_HAVE_OGRE
2240 newvisibleline =
true;
2242 newvisibleline =
false;
2266 for (
unsigned int i = 0; i < nbpt - 1; i++)
2290 for (
unsigned int i = 0; i < nbpt - 1; i++)
2296 unsigned int &nberrors_circles)
2298 unsigned int nbrow = 0;
2300 nberrors_cylinders = 0;
2301 nberrors_circles = 0;
2315 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2326 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2352 int idFace,
const std::string &name)
2354 addCircle(p1, p2, p3, radius, (
int)idFace, name);
2368 const std::string &name)
2385 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2387 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2394 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2402 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2418 #ifdef VISP_HAVE_OGRE
2460 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2462 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2469 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2477 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2520 unsigned int nbGoodPoints = 0;
2522 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2525 for (
size_t a = 0; a < l->
meline.size(); a++) {
2527 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2528 itme != l->
meline[a]->getMeList().end(); ++itme) {
2537 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2541 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2542 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2546 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2547 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2555 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2558 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2559 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2566 return nbGoodPoints;
2592 unsigned int nbActivatedLevels = 0;
2593 for (
unsigned int i = 0; i < scale.size(); i++) {
2595 nbActivatedLevels++;
2599 if (scale.empty() || (nbActivatedLevels == 0)) {
2600 vpERROR_TRACE(
" !! WARNING : must use at least one level for the "
2601 "tracking. Use the global one");
2603 this->
scales.push_back(
true);
2616 lines.resize(scale.size());
2620 for (
unsigned int i = 0; i <
lines.size(); i++) {
2636 std::cerr <<
"Far clipping value cannot be inferior than near clipping "
2637 "value. Far clipping won't be considered."
2640 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping "
2641 "won't be considered."
2647 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2649 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2666 std::cerr <<
"Near clipping value cannot be superior than far clipping "
2667 "value. Near clipping won't be considered."
2670 std::cerr <<
"Near clipping value cannot be inferior than 0. Near "
2671 "clipping won't be considered."
2677 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2679 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2701 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2703 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2729 _pyramid.resize(
scales.size());
2737 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2739 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2741 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2742 IplImage *vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (
int)_I.
getHeight()), IPL_DEPTH_8U, 1);
2743 vpI0->imageData = (
char *)(_I.
bitmap);
2745 cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
2746 cvResize(vpI0, vpI, CV_INTER_NN);
2748 cvReleaseImage(&vpI);
2749 vpI0->imageData = NULL;
2750 cvReleaseImageHeader(&vpI0);
2752 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2753 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2754 (*I)[k][l] = _I[ii][jj];
2773 if (_pyramid.size() > 0) {
2775 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2776 if (_pyramid[i] != NULL) {
2798 std::ostringstream oss;
2800 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2804 linesList =
lines[level];
2820 std::ostringstream oss;
2822 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2842 std::ostringstream oss;
2844 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2859 const double ratio = pow(2., (
int)_scale);
2880 const double ratio = pow(2., (
int)_scale);
2908 if ((*it)->isTracked()) {
2915 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2917 if ((*it)->isTracked()) {
2924 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2926 if ((*it)->isTracked()) {
2946 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2948 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2959 (*it)->setTracked(name, useEdgeTracking);
2962 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2964 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2965 (*it)->setTracked(useEdgeTracking);
2969 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2970 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2971 (*it)->setTracked(useEdgeTracking);
void setWindowName(const Ogre::String &n)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
void initFromCalibrationMatrix(const vpMatrix &_K)
Implementation of column vector and the associated operations.
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionnalities.
static const vpColor cyan
static const vpColor blue
static const vpColor purple
static const vpColor yellow
static const vpColor green
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
static int() sign(double x)
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static bool equal(double x, double y, double s=0.001)
static Type minimum(const Type &a, const Type &b)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
void removeCircle(const std::string &name)
void computeVVS(const vpImage< unsigned char > &_I, unsigned int lvl)
vpColVector m_errorCircles
void updateMovingEdgeWeights()
vpColVector m_w_edge
Robust weights.
void upScale(const unsigned int _scale)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setNearClippingDistance(const double &dist)
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
virtual void setFarClippingDistance(const double &dist)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMe me
The moving edges parameters.
void displayFeaturesOnImage(const vpImage< unsigned char > &I)
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, unsigned int level=0) const
void computeProjectionError(const vpImage< unsigned char > &_I)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void track(const vpImage< unsigned char > &I)
virtual void computeVVSWeights()
vpColVector m_error_edge
(s - s*)
void downScale(const unsigned int _scale)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
void removeCylinder(const std::string &name)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void computeVVSFirstPhase(const vpImage< unsigned char > &I, unsigned int iter, double &count, unsigned int lvl=0)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_)
virtual void setClipping(const unsigned int &flags)
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, unsigned int level=0) const
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void removeLine(const std::string &name)
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
void setScales(const std::vector< bool > &_scales)
void trackMovingEdge(const vpImage< unsigned char > &I)
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual unsigned int getNbPoints(unsigned int level=0) const
vpColVector m_weightedError_edge
Weighted error.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
void updateMovingEdge(const vpImage< unsigned char > &I)
vpMatrix m_L_edge
Interaction matrix.
vpRobust m_robustCylinders
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void reInitLevel(const unsigned int _lvl)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void setMovingEdge(const vpMe &me)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
vpColVector m_factor
Edge VVS variables.
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
void addPolygon(vpMbtPolygon &p)
virtual ~vpMbEdgeTracker()
virtual void init(const vpImage< unsigned char > &I)
virtual void testTracking()
vpColVector m_errorCylinders
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool isAppearing(unsigned int i)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void displayOgre(const vpHomogeneousMatrix &cMo)
void setOgreShowConfigDialog(bool showConfigDialog)
double m_lambda
Gain of the virtual visual servoing stage.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
bool m_computeInteraction
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
vpMatrix covarianceMatrix
Covariance matrix.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
vpHomogeneousMatrix m_cMo
The current pose.
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix <L, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
vpMatrix m_SobelX
Sobel kernel in X.
vpCameraParameters m_cam
The camera parameters.
bool useOgre
Use Ogre3d for visibility tests.
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
bool displayFeatures
If true, the features are displayed.
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool useScanLine
Use Scanline for visibility tests.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
vpMatrix m_SobelY
Sobel kernel in Y.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
double distNearClip
Distance for near clipping.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
bool ogreShowConfigDialog
unsigned int clippingFlag
Flags specifying which clipping to used.
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void setMovingEdge(vpMe *Me)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getFeaturesForDisplay()
void setCameraParameters(const vpCameraParameters &camera)
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void setIndex(unsigned int i)
std::string getName() const
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
void setMeanWeight(double _wmean)
bool Reinit
Indicates if the circle has to be reinitialized.
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
void initInteractionMatrixError()
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Manage a cylinder used in the model-based tracker.
void setMeanWeight1(double wmean)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
void initInteractionMatrixError()
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setMovingEdge(vpMe *Me)
void setMeanWeight2(double wmean)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
std::string getName() const
std::vector< std::vector< double > > getFeaturesForDisplay()
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setIndex(unsigned int i)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpPoint * p2
The second extremity.
void initInteractionMatrixError()
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
std::string getName() const
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getFeaturesForDisplay()
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setMeanWeight(double w_mean)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
void trackMovingEdge(const vpImage< unsigned char > &I)
vpMbtPolygon & getPolygon()
Implementation of a polygon of the model used by the model-based tracker.
std::string getName() const
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void getCameraParameters(vpCameraParameters &cam) const
double getAngleAppear() const
void setEdgeMe(const vpMe &ecm)
void getEdgeMe(vpMe &ecm) const
double getLodMinLineLengthThreshold() const
void setAngleDisappear(const double &adisappear)
double getAngleDisappear() const
void setAngleAppear(const double &aappear)
void parse(const std::string &filename)
double getNearClippingDistance() const
bool hasNearClippingDistance() const
bool hasFarClippingDistance() const
void setCameraParameters(const vpCameraParameters &cam)
double getFarClippingDistance() const
bool getFovClipping() const
double getLodMinPolygonAreaThreshold() const
void setVerbose(bool verbose)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
@ CONSTRAST
Point removed due to a contrast problem.
@ TOO_NEAR
Point removed because too near image borders.
@ THRESHOLD
Point removed due to a threshold problem.
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
@ NO_SUPPRESSION
Point used by the tracker.
vpMeSiteState getState() const
void setState(const vpMeSiteState &flag)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setFarClippingDistance(const double &dist)
unsigned int getNbPoint() const
void setNearClippingDistance(const double &dist)
vpPoint * p
corners in the object frame
void setClipping(const unsigned int &flags)
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
void setMinMedianAbsoluteDeviation(double mad_min)
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)