41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpExponentialMap.h>
44 #include <visp3/core/vpMath.h>
45 #include <visp3/core/vpMatrixException.h>
46 #include <visp3/core/vpPixelMeterConversion.h>
47 #include <visp3/core/vpPolygon3D.h>
48 #include <visp3/core/vpTrackingException.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/mbt/vpMbEdgeTracker.h>
51 #include <visp3/mbt/vpMbtDistanceLine.h>
52 #include <visp3/mbt/vpMbtXmlGenericParser.h>
53 #include <visp3/vision/vpPose.h>
65 : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
66 percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
67 m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
68 m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
69 m_robust_edge(), m_featuresToBeDisplayedEdge()
87 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
89 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
97 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
106 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
132 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
134 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
139 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
145 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
164 double residu_1 = 1e3;
167 unsigned int iter = 0;
180 while (reloop ==
true && iter < 10) {
185 count = count / (double)nbrow;
217 while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter <
m_maxIter)) {
220 bool reStartFromLastIncrement =
false;
224 if (!reStartFromLastIncrement) {
232 if (!isoJoIdentity) {
238 double wi = 0.0, eri = 0.0;
239 double num = 0.0, den = 0.0;
241 for (
unsigned int i = 0; i < nbrow; i++) {
250 for (
unsigned int j = 0; j < 6; j++) {
256 for (
unsigned int i = 0; i < nbrow; i++) {
299 double e_prev = 0, e_cur, e_next;
301 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
302 if ((*it)->isTracked()) {
310 int index = *itindex;
322 std::list<vpMeSite>::const_iterator itListLine;
324 unsigned int indexFeature = 0;
326 for (
size_t a = 0; a < l->
meline.size(); a++) {
327 if (iter == 0 && l->
meline[a] !=
nullptr)
328 itListLine = l->
meline[a]->getMeList().begin();
330 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
331 for (
unsigned int j = 0; j < 6; j++) {
332 m_L_edge[n + i][j] = l->
L[indexFeature][j];
350 if (indexFeature == 0) {
353 e_next = l->
error[1];
365 e_cur = l->
error[indexFeature];
372 e_cur = l->
error[indexFeature];
373 e_next = l->
error[indexFeature + 1];
374 if (fabs(e_cur - e_prev) < limite) {
377 if (fabs(e_cur - e_next) < limite) {
389 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
391 if ((*it)->isTracked()) {
396 std::list<vpMeSite>::const_iterator itCyl1;
397 std::list<vpMeSite>::const_iterator itCyl2;
398 if (iter == 0 && (cy->
meline1 !=
nullptr || cy->
meline2 !=
nullptr)) {
399 itCyl1 = cy->
meline1->getMeList().begin();
400 itCyl2 = cy->
meline2->getMeList().begin();
403 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
404 for (
unsigned int j = 0; j < 6; j++) {
417 if (i < cy->nbFeaturel1) {
431 e_cur = cy->
error[0];
433 e_next = cy->
error[1];
443 e_cur = cy->
error[i];
445 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 !=
nullptr)) {
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];
533 e_cur = ci->
error[i];
540 e_cur = ci->
error[i];
541 e_next = ci->
error[i + 1];
542 if (fabs(e_cur - e_prev) < limite) {
545 if (fabs(e_cur - e_next) < limite) {
564 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
565 if ((*it)->isTracked()) {
572 int index = *itindex;
583 for (
size_t a = 0; a < l->
meline.size(); a++) {
584 std::list<vpMeSite>::const_iterator itListLine;
585 if (l->
meline[a] !=
nullptr) {
586 itListLine = l->
meline[a]->getMeList().begin();
588 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
601 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
603 if ((*it)->isTracked()) {
607 std::list<vpMeSite>::const_iterator itCyl1;
608 std::list<vpMeSite>::const_iterator itCyl2;
610 itCyl1 = cy->
meline1->getMeList().begin();
611 itCyl2 = cy->
meline2->getMeList().begin();
614 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
617 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++) {
674 for (
unsigned int i = 0; i < nerror; i++) {
692 unsigned int rank = (
m_L_edge * cVo).kernel(K);
701 isoJoIdentity =
false;
720 v = -0.7 * LVJTLVJ.
pseudoInverse(LVJTLVJ.
getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
730 unsigned int nbrow = 0;
731 unsigned int nberrors_lines = 0;
732 unsigned int nberrors_cylinders = 0;
733 unsigned int nberrors_circles = 0;
735 nbrow =
initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
739 "No data found to compute the interaction matrix...");
770 "computeVVSInteractionMatrixAndR"
771 "esidu() should not be called!");
781 unsigned int nlines = 0;
782 unsigned int ncylinders = 0;
783 unsigned int ncircles = 0;
787 if ((*it)->isTracked()) {
791 for (
unsigned int j = 0; j < 6; j++) {
802 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
804 if ((*it)->isTracked()) {
807 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
808 for (
unsigned int j = 0; j < 6; j++) {
820 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
822 if ((*it)->isTracked()) {
825 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
826 for (
unsigned int j = 0; j < 6; j++) {
844 if (nberrors_lines > 0)
846 if (nberrors_cylinders > 0)
848 if (nberrors_circles > 0)
867 unsigned int nbFeatures = 0;
872 for (
size_t a = 0; a < l->
meline.size(); a++) {
873 if (l->
meline[a] !=
nullptr) {
874 double lineNormGradient;
875 unsigned int lineNbFeatures;
880 nbFeatures += lineNbFeatures;
886 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
891 double cylinderNormGradient = 0;
892 unsigned int cylinderNbFeatures = 0;
897 nbFeatures += cylinderNbFeatures;
901 double cylinderNormGradient = 0;
902 unsigned int cylinderNbFeatures = 0;
907 nbFeatures += cylinderNbFeatures;
912 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
916 double circleNormGradient = 0;
917 unsigned int circleNbFeatures = 0;
922 nbFeatures += circleNbFeatures;
926 if (nbFeatures > 0) {
944 int nbExpectedPoint = 0;
952 for (
size_t a = 0; a < l->
meline.size(); a++) {
953 if (l->
meline[a] !=
nullptr) {
954 nbExpectedPoint += (int)l->
meline[a]->expecteddensity;
955 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
956 itme != l->
meline[a]->getMeList().end(); ++itme) {
968 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
972 nbExpectedPoint += (int)cy->
meline1->expecteddensity;
973 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
974 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
981 nbExpectedPoint += (int)cy->
meline2->expecteddensity;
982 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
983 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
993 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
997 nbExpectedPoint += ci->
meEllipse->getExpectedDensity();
998 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
999 itme != ci->
meEllipse->getMeList().end(); ++itme) {
1012 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1013 std::ostringstream oss;
1014 oss <<
"Not enough moving edges (" << nbGoodPoint <<
") to track the object: expected " << nb_min
1016 <<
" using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1032 unsigned int lvl = (
unsigned int)
scales.size();
1047 vpTRACE(
"Error in moving edge tracking");
1090 bool newvisibleface =
false;
1146 #ifdef VISP_HAVE_OGRE
1175 unsigned int i = (
unsigned int)
scales.size();
1230 #if defined(VISP_HAVE_PUGIXML)
1243 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1245 xmlp.
parse(configFile);
1301 bool displayFullModel)
1308 std::vector<std::vector<double> > models =
1311 for (
size_t i = 0; i < models.size(); i++) {
1319 double n20 = models[i][3];
1320 double n11 = models[i][4];
1321 double n02 = models[i][5];
1326 #ifdef VISP_HAVE_OGRE
1344 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
1351 std::vector<std::vector<double> > models =
1354 for (
size_t i = 0; i < models.size(); i++) {
1362 double n20 = models[i][3];
1363 double n11 = models[i][4];
1364 double n02 = models[i][5];
1369 #ifdef VISP_HAVE_OGRE
1377 std::vector<std::vector<double> > features;
1379 const unsigned int lvl = 0;
1380 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1384 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1388 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1393 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1397 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1401 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1426 bool displayFullModel)
1428 std::vector<std::vector<double> > models;
1430 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1434 std::vector<std::vector<double> > currentModel =
1435 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1436 models.insert(models.end(), currentModel.begin(), currentModel.end());
1439 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1441 std::vector<std::vector<double> > currentModel =
1442 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1443 models.insert(models.end(), currentModel.begin(), currentModel.end());
1446 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1448 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1449 if (!paramsCircle.empty()) {
1450 models.push_back(paramsCircle);
1540 const bool doNotTrack =
false;
1545 bool isvisible =
false;
1549 int index = *itindex;
1570 for (
size_t a = 0; a < l->
meline.size(); a++) {
1571 if (l->
meline[a] !=
nullptr)
1573 if (a < l->nbFeature.size())
1582 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1586 bool isvisible =
false;
1619 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1622 bool isvisible =
false;
1656 const bool doNotTrack =
false;
1669 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1680 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1702 if ((*it)->isTracked()) {
1712 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1714 if ((*it)->isTracked()) {
1724 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1726 if ((*it)->isTracked()) {
1743 if ((*it)->isTracked()) {
1745 unsigned int indexLine = 0;
1747 for (
size_t a = 0; a < l->
meline.size(); a++) {
1749 std::list<vpMeSite>::iterator itListLine;
1750 itListLine = l->
meline[a]->getMeList().begin();
1752 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
1755 if (
m_w_edge[n + indexLine] < 0.5) {
1782 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1784 if ((*it)->isTracked()) {
1787 std::list<vpMeSite>::iterator itListCyl1;
1788 std::list<vpMeSite>::iterator itListCyl2;
1791 itListCyl1 = cy->
meline1->getMeList().begin();
1792 itListCyl2 = cy->
meline2->getMeList().begin();
1794 for (
unsigned int i = 0; i < cy->
nbFeaturel1; i++) {
1819 for (
unsigned int i = cy->
nbFeaturel1; i < cy->nbFeature; i++) {
1848 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1850 if ((*it)->isTracked()) {
1853 std::list<vpMeSite>::iterator itListCir;
1856 itListCir = ci->
meEllipse->getMeList().begin();
1860 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
1902 if ((*it)->isTracked()) {
1910 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1912 if ((*it)->isTracked()) {
1920 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1922 if ((*it)->isTracked()) {
1935 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1937 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1938 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
1939 if ((*it)->meline[a] !=
nullptr) {
1940 delete (*it)->meline[a];
1941 (*it)->meline[a] =
nullptr;
1945 (*it)->meline.clear();
1946 (*it)->nbFeature.clear();
1947 (*it)->nbFeatureTotal = 0;
1950 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1952 if ((*it)->meline1 !=
nullptr) {
1953 delete (*it)->meline1;
1954 (*it)->meline1 =
nullptr;
1956 if ((*it)->meline2 !=
nullptr) {
1957 delete (*it)->meline2;
1958 (*it)->meline2 =
nullptr;
1961 (*it)->nbFeature = 0;
1962 (*it)->nbFeaturel1 = 0;
1963 (*it)->nbFeaturel2 = 0;
1966 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1967 if ((*it)->meEllipse !=
nullptr) {
1968 delete (*it)->meEllipse;
1969 (*it)->meEllipse =
nullptr;
1971 (*it)->nbFeature = 0;
1993 bool already_here =
false;
1996 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1999 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2003 already_here =
true;
2009 if (!already_here) {
2032 lines[i].push_back(l);
2049 for (
unsigned int i = 0; i <
scales.size(); i++) {
2051 for (std::list<vpMbtDistanceLine *>::iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2053 if (name.compare(l->
getName()) == 0) {
2073 const std::string &name)
2076 bool already_here =
false;
2079 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2082 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2091 if (!already_here) {
2134 bool already_here =
false;
2137 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2140 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2150 if (!already_here) {
2178 for (
unsigned int i = 0; i <
scales.size(); i++) {
2180 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2182 if (name.compare(cy->
getName()) == 0) {
2200 for (
unsigned int i = 0; i <
scales.size(); i++) {
2202 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2204 if (name.compare(ci->
getName()) == 0) {
2222 for (
unsigned int i = 0; i < nbpt - 1; i++)
2242 bool changed =
false;
2250 #ifdef VISP_HAVE_OGRE
2259 newvisibleline =
true;
2262 newvisibleline =
false;
2286 for (
unsigned int i = 0; i < nbpt - 1; i++)
2310 for (
unsigned int i = 0; i < nbpt - 1; i++)
2316 unsigned int &nberrors_circles)
2318 unsigned int nbrow = 0;
2320 nberrors_cylinders = 0;
2321 nberrors_circles = 0;
2335 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2346 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2372 const std::string &name)
2374 addCircle(p1, p2, p3, radius, (
int)idFace, name);
2388 const std::string &name)
2405 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2407 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2414 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2422 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2438 #ifdef VISP_HAVE_OGRE
2479 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2481 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2488 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2496 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2539 unsigned int nbGoodPoints = 0;
2541 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2544 for (
size_t a = 0; a < l->
meline.size(); a++) {
2546 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2547 itme != l->
meline[a]->getMeList().end(); ++itme) {
2556 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2560 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2561 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2565 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2566 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2574 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2577 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2578 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2585 return nbGoodPoints;
2611 unsigned int nbActivatedLevels = 0;
2612 for (
unsigned int i = 0; i < scale.size(); i++) {
2614 nbActivatedLevels++;
2618 if (scale.empty() || (nbActivatedLevels == 0)) {
2619 vpERROR_TRACE(
" !! WARNING : must use at least one level for the "
2620 "tracking. Use the global one");
2622 this->
scales.push_back(
true);
2636 lines.resize(scale.size());
2640 for (
unsigned int i = 0; i <
lines.size(); i++) {
2656 std::cerr <<
"Far clipping value cannot be inferior than near clipping "
2657 "value. Far clipping won't be considered."
2660 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping "
2661 "won't be considered."
2667 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2669 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2686 std::cerr <<
"Near clipping value cannot be superior than far clipping "
2687 "value. Near clipping won't be considered."
2690 std::cerr <<
"Near clipping value cannot be inferior than 0. Near "
2691 "clipping won't be considered."
2697 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2699 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2721 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2723 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2749 _pyramid.resize(
scales.size());
2755 _pyramid[0] =
nullptr;
2758 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2760 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2762 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2763 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2764 (*I)[k][l] = _I[ii][jj];
2770 _pyramid[i] =
nullptr;
2783 if (_pyramid.size() > 0) {
2784 _pyramid[0] =
nullptr;
2785 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2786 if (_pyramid[i] !=
nullptr) {
2788 _pyramid[i] =
nullptr;
2808 std::ostringstream oss;
2810 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2814 linesList =
lines[level];
2830 std::ostringstream oss;
2832 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2852 std::ostringstream oss;
2854 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2869 const double ratio = pow(2., (
int)_scale);
2890 const double ratio = pow(2., (
int)_scale);
2918 if ((*it)->isTracked()) {
2925 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2927 if ((*it)->isTracked()) {
2934 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2936 if ((*it)->isTracked()) {
2956 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2958 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2969 (*it)->setTracked(name, useEdgeTracking);
2972 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2974 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2975 (*it)->setTracked(useEdgeTracking);
2979 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2980 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2981 (*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 functionalities.
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 emitted 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
unsigned int getHeight() const
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 threshold=0.001)
static Type minimum(const Type &a, const Type &b)
static int sign(double x)
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)
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
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 computeVVSWeights()
vpColVector m_error_edge
(s - s*)
void downScale(const unsigned int _scale)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
virtual ~vpMbEdgeTracker() vp_override
void removeCylinder(const std::string &name)
virtual void setNearClippingDistance(const double &dist) vp_override
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.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") vp_override
virtual void setFarClippingDistance(const double &dist) vp_override
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="")
void resetTracker() vp_override
virtual void computeVVSInteractionMatrixAndResidu() vp_override
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)
virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override
void removeLine(const std::string &name)
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual void track(const vpImage< unsigned char > &I) vp_override
void setScales(const std::vector< bool > &_scales)
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual void setClipping(const unsigned int &flags) vp_override
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual unsigned int getNbPoints(unsigned int level=0) const
vpColVector m_weightedError_edge
Weighted error.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") vp_override
virtual void setCameraParameters(const vpCameraParameters &cam) vp_override
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
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) vp_override
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void reInitLevel(const unsigned int _lvl)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
virtual void computeVVSInit() vp_override
virtual void initFaceFromCorners(vpMbtPolygon &polygon) vp_override
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
void setMovingEdge(const vpMe &me)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) vp_override
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
virtual void testTracking() vp_override
vpColVector m_factor
Edge VVS variables.
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void init(const vpImage< unsigned char > &I) vp_override
void addPolygon(vpMbtPolygon &p)
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.
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.
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.
vpMatrix m_SobelX
Sobel kernel in X.
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
vpCameraParameters m_cam
The camera parameters.
bool useOgre
Use Ogre3d for visibility tests.
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
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.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
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=nullptr, vpColVector *const m_w_prev=nullptr)
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 m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
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 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.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
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)
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 reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
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.
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
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 reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
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'....
@ TOO_NEAR
Point not tracked anymore, since too near from its neighbor.
@ THRESHOLD
Point not tracked due to the likelihood that is below the threshold, but retained in the ME list.
@ CONTRAST
Point not tracked due to a contrast problem, but retained in the ME list.
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
@ NO_SUPPRESSION
Point successfully tracked.
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 emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)