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)
1295 std::vector<std::vector<double> > models =
1298 for (
size_t i = 0; i < models.size(); i++) {
1305 double n20 = models[i][3];
1306 double n11 = models[i][4];
1307 double n02 = models[i][5];
1312 #ifdef VISP_HAVE_OGRE
1330 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
1337 std::vector<std::vector<double> > models =
1340 for (
size_t i = 0; i < models.size(); i++) {
1347 double n20 = models[i][3];
1348 double n11 = models[i][4];
1349 double n02 = models[i][5];
1354 #ifdef VISP_HAVE_OGRE
1362 std::vector<std::vector<double> > features;
1364 const unsigned int lvl = 0;
1365 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1369 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1373 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1378 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1382 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1386 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1411 bool displayFullModel)
1413 std::vector<std::vector<double> > models;
1415 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1419 std::vector<std::vector<double> > currentModel =
1420 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1421 models.insert(models.end(), currentModel.begin(), currentModel.end());
1424 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1426 std::vector<std::vector<double> > currentModel =
1427 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1428 models.insert(models.end(), currentModel.begin(), currentModel.end());
1431 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1433 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1434 if (!paramsCircle.empty()) {
1435 models.push_back(paramsCircle);
1525 const bool doNotTrack =
false;
1530 bool isvisible =
false;
1534 int index = *itindex;
1554 for (
size_t a = 0; a < l->
meline.size(); a++) {
1555 if (l->
meline[a] != NULL)
1557 if (a < l->nbFeature.size())
1566 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1570 bool isvisible =
false;
1602 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1605 bool isvisible =
false;
1638 const bool doNotTrack =
false;
1651 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1662 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1684 if ((*it)->isTracked()) {
1694 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1696 if ((*it)->isTracked()) {
1706 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1708 if ((*it)->isTracked()) {
1725 if ((*it)->isTracked()) {
1727 unsigned int indexLine = 0;
1729 for (
size_t a = 0; a < l->
meline.size(); a++) {
1731 std::list<vpMeSite>::iterator itListLine;
1732 itListLine = l->
meline[a]->getMeList().begin();
1734 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
1737 if (
m_w_edge[n + indexLine] < 0.5) {
1764 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1766 if ((*it)->isTracked()) {
1769 std::list<vpMeSite>::iterator itListCyl1;
1770 std::list<vpMeSite>::iterator itListCyl2;
1773 itListCyl1 = cy->
meline1->getMeList().begin();
1774 itListCyl2 = cy->
meline2->getMeList().begin();
1776 for (
unsigned int i = 0; i < cy->
nbFeaturel1; i++) {
1801 for (
unsigned int i = cy->
nbFeaturel1; i < cy->nbFeature; i++) {
1830 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1832 if ((*it)->isTracked()) {
1835 std::list<vpMeSite>::iterator itListCir;
1838 itListCir = ci->
meEllipse->getMeList().begin();
1842 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
1884 if ((*it)->isTracked()) {
1892 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1894 if ((*it)->isTracked()) {
1902 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1904 if ((*it)->isTracked()) {
1917 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1919 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1920 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
1921 if ((*it)->meline[a] != NULL) {
1922 delete (*it)->meline[a];
1923 (*it)->meline[a] = NULL;
1927 (*it)->meline.clear();
1928 (*it)->nbFeature.clear();
1929 (*it)->nbFeatureTotal = 0;
1932 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1934 if ((*it)->meline1 != NULL) {
1935 delete (*it)->meline1;
1936 (*it)->meline1 = NULL;
1938 if ((*it)->meline2 != NULL) {
1939 delete (*it)->meline2;
1940 (*it)->meline2 = NULL;
1943 (*it)->nbFeature = 0;
1944 (*it)->nbFeaturel1 = 0;
1945 (*it)->nbFeaturel2 = 0;
1948 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1949 if ((*it)->meEllipse != NULL) {
1950 delete (*it)->meEllipse;
1951 (*it)->meEllipse = NULL;
1953 (*it)->nbFeature = 0;
1975 bool already_here =
false;
1978 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1981 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1985 already_here =
true;
1991 if (!already_here) {
2014 lines[i].push_back(l);
2031 for (
unsigned int i = 0; i <
scales.size(); i++) {
2033 for (std::list<vpMbtDistanceLine *>::iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2035 if (name.compare(l->
getName()) == 0) {
2055 const std::string &name)
2058 bool already_here =
false;
2061 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2064 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2073 if (!already_here) {
2116 bool already_here =
false;
2119 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2122 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2132 if (!already_here) {
2160 for (
unsigned int i = 0; i <
scales.size(); i++) {
2162 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2164 if (name.compare(cy->
getName()) == 0) {
2182 for (
unsigned int i = 0; i <
scales.size(); i++) {
2184 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2186 if (name.compare(ci->
getName()) == 0) {
2204 for (
unsigned int i = 0; i < nbpt - 1; i++)
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 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
2459 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2461 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2468 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2476 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2519 unsigned int nbGoodPoints = 0;
2521 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2524 for (
size_t a = 0; a < l->
meline.size(); a++) {
2526 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2527 itme != l->
meline[a]->getMeList().end(); ++itme) {
2536 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2540 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2541 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2545 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2546 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2554 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2557 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2558 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2565 return nbGoodPoints;
2591 unsigned int nbActivatedLevels = 0;
2592 for (
unsigned int i = 0; i < scale.size(); i++) {
2594 nbActivatedLevels++;
2598 if (scale.empty() || (nbActivatedLevels == 0)) {
2599 vpERROR_TRACE(
" !! WARNING : must use at least one level for the "
2600 "tracking. Use the global one");
2602 this->
scales.push_back(
true);
2615 lines.resize(scale.size());
2619 for (
unsigned int i = 0; i <
lines.size(); i++) {
2635 std::cerr <<
"Far clipping value cannot be inferior than near clipping "
2636 "value. Far clipping won't be considered."
2639 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping "
2640 "won't be considered."
2646 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2648 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2665 std::cerr <<
"Near clipping value cannot be superior than far clipping "
2666 "value. Near clipping won't be considered."
2669 std::cerr <<
"Near clipping value cannot be inferior than 0. Near "
2670 "clipping won't be considered."
2676 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2678 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2700 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2702 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2728 _pyramid.resize(
scales.size());
2736 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2738 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2740 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2741 IplImage *vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (
int)_I.
getHeight()), IPL_DEPTH_8U, 1);
2742 vpI0->imageData = (
char *)(_I.
bitmap);
2744 cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
2745 cvResize(vpI0, vpI, CV_INTER_NN);
2747 cvReleaseImage(&vpI);
2748 vpI0->imageData = NULL;
2749 cvReleaseImageHeader(&vpI0);
2751 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2752 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2753 (*I)[k][l] = _I[ii][jj];
2772 if (_pyramid.size() > 0) {
2774 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2775 if (_pyramid[i] != NULL) {
2797 std::ostringstream oss;
2799 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2803 linesList =
lines[level];
2819 std::ostringstream oss;
2821 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2841 std::ostringstream oss;
2843 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2858 const double ratio = pow(2., (
int)_scale);
2879 const double ratio = pow(2., (
int)_scale);
2907 if ((*it)->isTracked()) {
2914 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2916 if ((*it)->isTracked()) {
2923 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2925 if ((*it)->isTracked()) {
2945 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2947 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2958 (*it)->setTracked(name, useEdgeTracking);
2961 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2963 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2964 (*it)->setTracked(useEdgeTracking);
2968 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2969 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2970 (*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)