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 for (
size_t a = 0; a < l->
meline.size(); a++) {
583 std::list<vpMeSite>::const_iterator itListLine;
584 if (l->
meline[a] != NULL) {
585 itListLine = l->
meline[a]->getMeList().begin();
587 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
600 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
602 if ((*it)->isTracked()) {
606 std::list<vpMeSite>::const_iterator itCyl1;
607 std::list<vpMeSite>::const_iterator itCyl2;
609 itCyl1 = cy->
meline1->getMeList().begin();
610 itCyl2 = cy->
meline2->getMeList().begin();
613 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
616 if (i < cy->nbFeaturel1) {
631 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
632 if ((*it)->isTracked()) {
636 std::list<vpMeSite>::const_iterator itCir;
638 itCir = ci->
meEllipse->getMeList().begin();
641 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
660 for (
unsigned int i = 0; i < nerror; i++) {
666 for (
unsigned int j = 0; j < 6; j++) {
671 for (
unsigned int i = 0; i < nerror; i++) {
689 unsigned int rank = (
m_L_edge * cVo).kernel(K);
698 isoJoIdentity =
false;
716 v = -0.7 * LVJTLVJ.
pseudoInverse(LVJTLVJ.
getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
726 unsigned int nbrow = 0;
727 unsigned int nberrors_lines = 0;
728 unsigned int nberrors_cylinders = 0;
729 unsigned int nberrors_circles = 0;
731 nbrow =
initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
735 "No data found to compute the interaction matrix...");
766 "computeVVSInteractionMatrixAndR"
767 "esidu() should not be called!");
777 unsigned int nlines = 0;
778 unsigned int ncylinders = 0;
779 unsigned int ncircles = 0;
783 if ((*it)->isTracked()) {
787 for (
unsigned int j = 0; j < 6; j++) {
798 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
800 if ((*it)->isTracked()) {
803 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
804 for (
unsigned int j = 0; j < 6; j++) {
816 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
818 if ((*it)->isTracked()) {
821 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
822 for (
unsigned int j = 0; j < 6; j++) {
840 if (nberrors_lines > 0)
842 if (nberrors_cylinders > 0)
844 if (nberrors_circles > 0)
863 unsigned int nbFeatures = 0;
868 for (
size_t a = 0; a < l->
meline.size(); a++) {
869 if (l->
meline[a] != NULL) {
870 double lineNormGradient;
871 unsigned int lineNbFeatures;
876 nbFeatures += lineNbFeatures;
882 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
887 double cylinderNormGradient = 0;
888 unsigned int cylinderNbFeatures = 0;
893 nbFeatures += cylinderNbFeatures;
897 double cylinderNormGradient = 0;
898 unsigned int cylinderNbFeatures = 0;
903 nbFeatures += cylinderNbFeatures;
908 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
912 double circleNormGradient = 0;
913 unsigned int circleNbFeatures = 0;
918 nbFeatures += circleNbFeatures;
922 if (nbFeatures > 0) {
939 int nbExpectedPoint = 0;
947 for (
size_t a = 0; a < l->
meline.size(); a++) {
948 if (l->
meline[a] != NULL) {
949 nbExpectedPoint += (int)l->
meline[a]->expecteddensity;
950 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
951 itme != l->
meline[a]->getMeList().end(); ++itme) {
963 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
967 nbExpectedPoint += (int)cy->
meline1->expecteddensity;
968 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
969 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
976 nbExpectedPoint += (int)cy->
meline2->expecteddensity;
977 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
978 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
988 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
992 nbExpectedPoint += ci->
meEllipse->getExpectedDensity();
993 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
994 itme != ci->
meEllipse->getMeList().end(); ++itme) {
1008 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1009 std::ostringstream oss;
1010 oss <<
"Not enough moving edges (" << nbGoodPoint <<
") to track the object: expected " << nb_min
1012 <<
" using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1028 unsigned int lvl = (
unsigned int)
scales.size();
1042 vpTRACE(
"Error in moving edge tracking");
1084 bool newvisibleface =
false;
1138 #ifdef VISP_HAVE_OGRE
1167 unsigned int i = (
unsigned int)
scales.size();
1234 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1236 xmlp.
parse(configFile);
1286 bool displayFullModel)
1293 std::vector<std::vector<double> > models =
1296 for (
size_t i = 0; i < models.size(); i++) {
1303 double n20 = models[i][3];
1304 double n11 = models[i][4];
1305 double n02 = models[i][5];
1310 #ifdef VISP_HAVE_OGRE
1328 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
1335 std::vector<std::vector<double> > models =
1338 for (
size_t i = 0; i < models.size(); i++) {
1345 double n20 = models[i][3];
1346 double n11 = models[i][4];
1347 double n02 = models[i][5];
1352 #ifdef VISP_HAVE_OGRE
1360 std::vector<std::vector<double> > features;
1362 const unsigned int lvl = 0;
1363 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1367 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1371 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1376 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1380 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1384 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) {
2114 bool already_here =
false;
2117 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2120 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2130 if (!already_here) {
2158 for (
unsigned int i = 0; i <
scales.size(); i++) {
2160 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2162 if (name.compare(cy->
getName()) == 0) {
2180 for (
unsigned int i = 0; i <
scales.size(); i++) {
2182 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2184 if (name.compare(ci->
getName()) == 0) {
2202 for (
unsigned int i = 0; i < nbpt - 1; i++)
2222 bool changed =
false;
2229 #ifdef VISP_HAVE_OGRE
2238 newvisibleline =
true;
2240 newvisibleline =
false;
2264 for (
unsigned int i = 0; i < nbpt - 1; i++)
2288 for (
unsigned int i = 0; i < nbpt - 1; i++)
2294 unsigned int &nberrors_circles)
2296 unsigned int nbrow = 0;
2298 nberrors_cylinders = 0;
2299 nberrors_circles = 0;
2313 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2324 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2350 const std::string &name)
2352 addCircle(p1, p2, p3, radius, (
int)idFace, name);
2366 const std::string &name)
2383 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2385 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2392 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2400 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2416 #ifdef VISP_HAVE_OGRE
2457 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2459 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2466 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2474 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2517 unsigned int nbGoodPoints = 0;
2519 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2522 for (
size_t a = 0; a < l->
meline.size(); a++) {
2524 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2525 itme != l->
meline[a]->getMeList().end(); ++itme) {
2534 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2538 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2539 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2543 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2544 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2552 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2555 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2556 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2563 return nbGoodPoints;
2589 unsigned int nbActivatedLevels = 0;
2590 for (
unsigned int i = 0; i < scale.size(); i++) {
2592 nbActivatedLevels++;
2596 if (scale.empty() || (nbActivatedLevels == 0)) {
2597 vpERROR_TRACE(
" !! WARNING : must use at least one level for the "
2598 "tracking. Use the global one");
2600 this->
scales.push_back(
true);
2613 lines.resize(scale.size());
2617 for (
unsigned int i = 0; i <
lines.size(); i++) {
2633 std::cerr <<
"Far clipping value cannot be inferior than near clipping "
2634 "value. Far clipping won't be considered."
2637 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping "
2638 "won't be considered."
2644 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2646 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2663 std::cerr <<
"Near clipping value cannot be superior than far clipping "
2664 "value. Near clipping won't be considered."
2667 std::cerr <<
"Near clipping value cannot be inferior than 0. Near "
2668 "clipping won't be considered."
2674 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2676 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2698 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2700 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2726 _pyramid.resize(
scales.size());
2734 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2736 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2738 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2739 IplImage *vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (
int)_I.
getHeight()), IPL_DEPTH_8U, 1);
2740 vpI0->imageData = (
char *)(_I.
bitmap);
2742 cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
2743 cvResize(vpI0, vpI, CV_INTER_NN);
2745 cvReleaseImage(&vpI);
2746 vpI0->imageData = NULL;
2747 cvReleaseImageHeader(&vpI0);
2749 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2750 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2751 (*I)[k][l] = _I[ii][jj];
2770 if (_pyramid.size() > 0) {
2772 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2773 if (_pyramid[i] != NULL) {
2795 std::ostringstream oss;
2797 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2801 linesList =
lines[level];
2817 std::ostringstream oss;
2819 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2839 std::ostringstream oss;
2841 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2856 const double ratio = pow(2., (
int)_scale);
2877 const double ratio = pow(2., (
int)_scale);
2905 if ((*it)->isTracked()) {
2912 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2914 if ((*it)->isTracked()) {
2921 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2923 if ((*it)->isTracked()) {
2943 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2945 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2956 (*it)->setTracked(name, useEdgeTracking);
2959 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2961 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2962 (*it)->setTracked(useEdgeTracking);
2966 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2967 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2968 (*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 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)
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)
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)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
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.
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
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)
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.
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.
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.
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 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'....
@ TOO_NEAR
Point removed because too near image borders.
@ THRESHOLD
Point removed due to a threshold problem.
@ CONTRAST
Point removed due to a contrast 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)