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++) {
255 for (
unsigned int i = 0; i < nbrow; i++) {
298 double e_prev = 0, e_cur, e_next;
300 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
301 if ((*it)->isTracked()) {
309 int index = *itindex;
321 std::list<vpMeSite>::const_iterator itListLine;
323 unsigned int indexFeature = 0;
325 for (
size_t a = 0; a < l->
meline.size(); a++) {
326 if (iter == 0 && l->
meline[a] != NULL)
327 itListLine = l->
meline[a]->getMeList().begin();
329 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
330 for (
unsigned int j = 0; j < 6; j++) {
331 m_L_edge[n + i][j] = l->
L[indexFeature][j];
349 if (indexFeature == 0) {
352 e_next = l->
error[1];
363 e_cur = l->
error[indexFeature];
370 e_cur = l->
error[indexFeature];
371 e_next = l->
error[indexFeature + 1];
372 if (fabs(e_cur - e_prev) < limite) {
375 if (fabs(e_cur - e_next) < limite) {
387 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
389 if ((*it)->isTracked()) {
394 std::list<vpMeSite>::const_iterator itCyl1;
395 std::list<vpMeSite>::const_iterator itCyl2;
397 itCyl1 = cy->
meline1->getMeList().begin();
398 itCyl2 = cy->
meline2->getMeList().begin();
401 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
402 for (
unsigned int j = 0; j < 6; j++) {
415 if (i < cy->nbFeaturel1) {
428 e_cur = cy->
error[0];
430 e_next = cy->
error[1];
439 e_cur = cy->
error[i];
441 e_next = cy->
error[i + 1];
452 e_cur = cy->
error[i];
459 e_cur = cy->
error[i];
466 e_cur = cy->
error[i];
467 e_next = cy->
error[i + 1];
468 if (fabs(e_cur - e_prev) < limite) {
471 if (fabs(e_cur - e_next) < limite) {
482 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
483 if ((*it)->isTracked()) {
488 std::list<vpMeSite>::const_iterator itCir;
489 if (iter == 0 && (ci->
meEllipse != NULL)) {
490 itCir = ci->
meEllipse->getMeList().begin();
493 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
494 for (
unsigned int j = 0; j < 6; j++) {
514 e_cur = ci->
error[0];
516 e_next = ci->
error[1];
527 e_cur = ci->
error[i];
534 e_cur = ci->
error[i];
535 e_next = ci->
error[i + 1];
536 if (fabs(e_cur - e_prev) < limite) {
539 if (fabs(e_cur - e_next) < limite) {
558 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
559 if ((*it)->isTracked()) {
566 int index = *itindex;
577 for (
size_t a = 0; a < l->
meline.size(); a++) {
578 std::list<vpMeSite>::const_iterator itListLine;
579 if (l->
meline[a] != NULL) {
580 itListLine = l->
meline[a]->getMeList().begin();
582 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
595 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
597 if ((*it)->isTracked()) {
601 std::list<vpMeSite>::const_iterator itCyl1;
602 std::list<vpMeSite>::const_iterator itCyl2;
604 itCyl1 = cy->
meline1->getMeList().begin();
605 itCyl2 = cy->
meline2->getMeList().begin();
608 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
611 if (i < cy->nbFeaturel1) {
626 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
627 if ((*it)->isTracked()) {
631 std::list<vpMeSite>::const_iterator itCir;
633 itCir = ci->
meEllipse->getMeList().begin();
636 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
655 for (
unsigned int i = 0; i < nerror; i++) {
661 for (
unsigned int j = 0; j < 6; j++) {
666 for (
unsigned int i = 0; i < nerror; i++) {
684 unsigned int rank = (
m_L_edge * cVo).kernel(K);
693 isoJoIdentity =
false;
711 v = -0.7 * LVJTLVJ.
pseudoInverse(LVJTLVJ.
getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
721 unsigned int nbrow = 0;
722 unsigned int nberrors_lines = 0;
723 unsigned int nberrors_cylinders = 0;
724 unsigned int nberrors_circles = 0;
726 nbrow =
initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
730 "No data found to compute the interaction matrix...");
761 "computeVVSInteractionMatrixAndR"
762 "esidu() should not be called!");
772 unsigned int nlines = 0;
773 unsigned int ncylinders = 0;
774 unsigned int ncircles = 0;
778 if ((*it)->isTracked()) {
782 for (
unsigned int j = 0; j < 6; j++) {
793 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
795 if ((*it)->isTracked()) {
798 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
799 for (
unsigned int j = 0; j < 6; j++) {
811 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
813 if ((*it)->isTracked()) {
816 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
817 for (
unsigned int j = 0; j < 6; j++) {
835 if (nberrors_lines > 0)
837 if (nberrors_cylinders > 0)
839 if (nberrors_circles > 0)
858 unsigned int nbFeatures = 0;
863 for (
size_t a = 0; a < l->
meline.size(); a++) {
864 if (l->
meline[a] != NULL) {
865 double lineNormGradient;
866 unsigned int lineNbFeatures;
871 nbFeatures += lineNbFeatures;
877 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
882 double cylinderNormGradient = 0;
883 unsigned int cylinderNbFeatures = 0;
888 nbFeatures += cylinderNbFeatures;
892 double cylinderNormGradient = 0;
893 unsigned int cylinderNbFeatures = 0;
898 nbFeatures += cylinderNbFeatures;
903 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
907 double circleNormGradient = 0;
908 unsigned int circleNbFeatures = 0;
913 nbFeatures += circleNbFeatures;
917 if (nbFeatures > 0) {
934 int nbExpectedPoint = 0;
942 for (
size_t a = 0; a < l->
meline.size(); a++) {
943 if (l->
meline[a] != NULL) {
944 nbExpectedPoint += (int)l->
meline[a]->expecteddensity;
945 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
946 itme != l->
meline[a]->getMeList().end(); ++itme) {
958 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
962 nbExpectedPoint += (int)cy->
meline1->expecteddensity;
963 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
964 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
971 nbExpectedPoint += (int)cy->
meline2->expecteddensity;
972 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
973 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
983 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
987 nbExpectedPoint += ci->
meEllipse->getExpectedDensity();
988 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
989 itme != ci->
meEllipse->getMeList().end(); ++itme) {
1003 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1004 std::ostringstream oss;
1005 oss <<
"Not enough moving edges (" << nbGoodPoint <<
") to track the object: expected " << nb_min
1007 <<
" using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1023 unsigned int lvl = (
unsigned int)
scales.size();
1037 vpTRACE(
"Error in moving edge tracking");
1079 bool newvisibleface =
false;
1133#ifdef VISP_HAVE_OGRE
1162 unsigned int i = (
unsigned int)
scales.size();
1229 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1231 xmlp.
parse(configFile);
1281 bool displayFullModel)
1288 std::vector<std::vector<double> > models =
1291 for (
size_t i = 0; i < models.size(); i++) {
1298 double n20 = models[i][3];
1299 double n11 = models[i][4];
1300 double n02 = models[i][5];
1305#ifdef VISP_HAVE_OGRE
1323 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
1330 std::vector<std::vector<double> > models =
1333 for (
size_t i = 0; i < models.size(); i++) {
1340 double n20 = models[i][3];
1341 double n11 = models[i][4];
1342 double n02 = models[i][5];
1347#ifdef VISP_HAVE_OGRE
1355 std::vector<std::vector<double> > features;
1357 const unsigned int lvl = 0;
1358 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1362 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1366 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1371 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1375 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1379 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1404 bool displayFullModel)
1406 std::vector<std::vector<double> > models;
1408 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1412 std::vector<std::vector<double> > currentModel =
1413 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1414 models.insert(models.end(), currentModel.begin(), currentModel.end());
1417 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
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<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1426 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1427 if (!paramsCircle.empty()) {
1428 models.push_back(paramsCircle);
1518 const bool doNotTrack =
false;
1523 bool isvisible =
false;
1527 int index = *itindex;
1547 for (
size_t a = 0; a < l->
meline.size(); a++) {
1548 if (l->
meline[a] != NULL)
1550 if (a < l->nbFeature.size())
1559 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1563 bool isvisible =
false;
1595 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1598 bool isvisible =
false;
1631 const bool doNotTrack =
false;
1644 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1655 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1677 if ((*it)->isTracked()) {
1687 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1689 if ((*it)->isTracked()) {
1699 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1701 if ((*it)->isTracked()) {
1718 if ((*it)->isTracked()) {
1720 unsigned int indexLine = 0;
1722 for (
size_t a = 0; a < l->
meline.size(); a++) {
1724 std::list<vpMeSite>::iterator itListLine;
1725 itListLine = l->
meline[a]->getMeList().begin();
1727 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
1730 if (
m_w_edge[n + indexLine] < 0.5) {
1757 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1759 if ((*it)->isTracked()) {
1762 std::list<vpMeSite>::iterator itListCyl1;
1763 std::list<vpMeSite>::iterator itListCyl2;
1766 itListCyl1 = cy->
meline1->getMeList().begin();
1767 itListCyl2 = cy->
meline2->getMeList().begin();
1769 for (
unsigned int i = 0; i < cy->
nbFeaturel1; i++) {
1823 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1825 if ((*it)->isTracked()) {
1828 std::list<vpMeSite>::iterator itListCir;
1831 itListCir = ci->
meEllipse->getMeList().begin();
1835 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
1877 if ((*it)->isTracked()) {
1885 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1887 if ((*it)->isTracked()) {
1895 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1897 if ((*it)->isTracked()) {
1910 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1912 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1913 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
1914 if ((*it)->meline[a] != NULL) {
1915 delete (*it)->meline[a];
1916 (*it)->meline[a] = NULL;
1920 (*it)->meline.clear();
1921 (*it)->nbFeature.clear();
1922 (*it)->nbFeatureTotal = 0;
1925 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1927 if ((*it)->meline1 != NULL) {
1928 delete (*it)->meline1;
1929 (*it)->meline1 = NULL;
1931 if ((*it)->meline2 != NULL) {
1932 delete (*it)->meline2;
1933 (*it)->meline2 = NULL;
1936 (*it)->nbFeature = 0;
1937 (*it)->nbFeaturel1 = 0;
1938 (*it)->nbFeaturel2 = 0;
1941 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1942 if ((*it)->meEllipse != NULL) {
1943 delete (*it)->meEllipse;
1944 (*it)->meEllipse = NULL;
1946 (*it)->nbFeature = 0;
1968 bool already_here =
false;
1971 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1974 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1978 already_here =
true;
1984 if (!already_here) {
2007 lines[i].push_back(l);
2024 for (
unsigned int i = 0; i <
scales.size(); i++) {
2026 for (std::list<vpMbtDistanceLine *>::iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2028 if (name.compare(l->
getName()) == 0) {
2048 const std::string &name)
2051 bool already_here =
false;
2054 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2057 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2066 if (!already_here) {
2109 bool already_here =
false;
2112 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2115 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2125 if (!already_here) {
2153 for (
unsigned int i = 0; i <
scales.size(); i++) {
2155 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2157 if (name.compare(cy->
getName()) == 0) {
2175 for (
unsigned int i = 0; i <
scales.size(); i++) {
2177 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2179 if (name.compare(ci->
getName()) == 0) {
2197 for (
unsigned int i = 0; i < nbpt - 1; i++)
2217 bool changed =
false;
2224#ifdef VISP_HAVE_OGRE
2233 newvisibleline =
true;
2235 newvisibleline =
false;
2259 for (
unsigned int i = 0; i < nbpt - 1; i++)
2283 for (
unsigned int i = 0; i < nbpt - 1; i++)
2289 unsigned int &nberrors_circles)
2291 unsigned int nbrow = 0;
2293 nberrors_cylinders = 0;
2294 nberrors_circles = 0;
2308 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2319 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2345 const std::string &name)
2347 addCircle(p1, p2, p3, radius, (
int)idFace, name);
2361 const std::string &name)
2378 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2380 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2387 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2395 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2411#ifdef VISP_HAVE_OGRE
2452 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2454 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2461 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2469 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2512 unsigned int nbGoodPoints = 0;
2514 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2517 for (
size_t a = 0; a < l->
meline.size(); a++) {
2519 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2520 itme != l->
meline[a]->getMeList().end(); ++itme) {
2529 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2533 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2534 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2538 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2539 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2547 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2550 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2551 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2558 return nbGoodPoints;
2584 unsigned int nbActivatedLevels = 0;
2585 for (
unsigned int i = 0; i < scale.size(); i++) {
2587 nbActivatedLevels++;
2591 if (scale.empty() || (nbActivatedLevels == 0)) {
2592 vpERROR_TRACE(
" !! WARNING : must use at least one level for the "
2593 "tracking. Use the global one");
2595 this->
scales.push_back(
true);
2608 lines.resize(scale.size());
2612 for (
unsigned int i = 0; i <
lines.size(); i++) {
2628 std::cerr <<
"Far clipping value cannot be inferior than near clipping "
2629 "value. Far clipping won't be considered."
2632 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping "
2633 "won't be considered."
2639 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2641 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2658 std::cerr <<
"Near clipping value cannot be superior than far clipping "
2659 "value. Near clipping won't be considered."
2662 std::cerr <<
"Near clipping value cannot be inferior than 0. Near "
2663 "clipping won't be considered."
2669 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2671 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2693 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2695 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2721 _pyramid.resize(
scales.size());
2729 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2731 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2733 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2734 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2735 (*I)[k][l] = _I[ii][jj];
2753 if (_pyramid.size() > 0) {
2755 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2756 if (_pyramid[i] != NULL) {
2778 std::ostringstream oss;
2780 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2784 linesList =
lines[level];
2800 std::ostringstream oss;
2802 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2822 std::ostringstream oss;
2824 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2839 const double ratio = pow(2., (
int)_scale);
2860 const double ratio = pow(2., (
int)_scale);
2888 if ((*it)->isTracked()) {
2895 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2897 if ((*it)->isTracked()) {
2904 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2906 if ((*it)->isTracked()) {
2926 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2928 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2939 (*it)->setTracked(name, useEdgeTracking);
2942 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2944 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2945 (*it)->setTracked(useEdgeTracking);
2949 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2950 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2951 (*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 ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
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
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)
vpAROgre * getOgreContext()
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)
vpMbtPolygon & getPolygon()
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)
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 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)