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/vpMbtXmlParser.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(),
95 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
97 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
105 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
114 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
140 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
142 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
147 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
153 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
172 double residu_1 = 1e3;
175 unsigned int iter = 0;
188 while (reloop ==
true && iter < 10) {
193 count = count / (double)nbrow;
225 while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter <
m_maxIter)) {
228 bool reStartFromLastIncrement =
false;
232 if (!reStartFromLastIncrement) {
240 if (!isoJoIdentity_) {
246 double wi = 0.0, eri = 0.0;
247 double num = 0.0, den = 0.0;
249 for (
unsigned int i = 0; i < nbrow; i++) {
258 for (
unsigned int j = 0; j < 6; j++) {
263 for (
unsigned int i = 0; i < nbrow; i++) {
294 const unsigned int lvl)
306 double e_prev = 0, e_cur, e_next;
308 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
309 if ((*it)->isTracked()) {
317 int index = *itindex;
329 std::list<vpMeSite>::const_iterator itListLine;
331 unsigned int indexFeature = 0;
333 for (
size_t a = 0; a < l->
meline.size(); a++) {
334 if (iter == 0 && l->
meline[a] != NULL)
335 itListLine = l->
meline[a]->getMeList().begin();
337 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
338 for (
unsigned int j = 0; j < 6; j++) {
339 m_L_edge[n + i][j] = l->
L[indexFeature][j];
357 if (indexFeature == 0) {
360 e_next = l->
error[1];
371 e_cur = l->
error[indexFeature];
378 e_cur = l->
error[indexFeature];
379 e_next = l->
error[indexFeature + 1];
380 if (fabs(e_cur - e_prev) < limite) {
383 if (fabs(e_cur - e_next) < limite) {
395 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
397 if ((*it)->isTracked()) {
402 std::list<vpMeSite>::const_iterator itCyl1;
403 std::list<vpMeSite>::const_iterator itCyl2;
405 itCyl1 = cy->
meline1->getMeList().begin();
406 itCyl2 = cy->
meline2->getMeList().begin();
409 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
410 for (
unsigned int j = 0; j < 6; j++) {
423 if (i < cy->nbFeaturel1) {
436 e_cur = cy->
error[0];
438 e_next = cy->
error[1];
447 e_cur = cy->
error[i];
449 e_next = cy->
error[i + 1];
460 e_cur = cy->
error[i];
467 e_cur = cy->
error[i];
474 e_cur = cy->
error[i];
475 e_next = cy->
error[i + 1];
476 if (fabs(e_cur - e_prev) < limite) {
479 if (fabs(e_cur - e_next) < limite) {
490 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
491 if ((*it)->isTracked()) {
496 std::list<vpMeSite>::const_iterator itCir;
497 if (iter == 0 && (ci->
meEllipse != NULL)) {
498 itCir = ci->
meEllipse->getMeList().begin();
501 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
502 for (
unsigned int j = 0; j < 6; j++) {
522 e_cur = ci->
error[0];
524 e_next = ci->
error[1];
535 e_cur = ci->
error[i];
542 e_cur = ci->
error[i];
543 e_next = ci->
error[i + 1];
544 if (fabs(e_cur - e_prev) < limite) {
547 if (fabs(e_cur - e_next) < limite) {
566 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
567 if ((*it)->isTracked()) {
574 int index = *itindex;
585 unsigned int indexFeature = 0;
586 for (
size_t a = 0; a < l->
meline.size(); a++) {
587 std::list<vpMeSite>::const_iterator itListLine;
588 if (l->
meline[a] != NULL) {
589 itListLine = l->
meline[a]->getMeList().begin();
591 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
605 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
607 if ((*it)->isTracked()) {
611 std::list<vpMeSite>::const_iterator itCyl1;
612 std::list<vpMeSite>::const_iterator itCyl2;
614 itCyl1 = cy->
meline1->getMeList().begin();
615 itCyl2 = cy->
meline2->getMeList().begin();
618 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
621 if (i < cy->nbFeaturel1) {
636 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
637 if ((*it)->isTracked()) {
641 std::list<vpMeSite>::const_iterator itCir;
643 itCir = ci->
meEllipse->getMeList().begin();
646 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
665 for (
unsigned int i = 0; i < nerror; i++) {
671 for (
unsigned int j = 0; j < 6; j++) {
676 for (
unsigned int i = 0; i < nerror; i++) {
690 if (isoJoIdentity_) {
694 unsigned int rank = (
m_L_edge * cVo).kernel(K);
703 isoJoIdentity_ =
false;
711 if (isoJoIdentity_) {
721 v = -0.7 * LVJTLVJ.
pseudoInverse(LVJTLVJ.
getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
731 unsigned int nbrow = 0;
732 unsigned int nberrors_lines = 0;
733 unsigned int nberrors_cylinders = 0;
734 unsigned int nberrors_circles = 0;
736 nbrow =
initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
740 "No data found to compute the interaction matrix...");
774 "computeVVSInteractionMatrixAndR" 775 "esidu() should not be called!");
785 unsigned int nlines = 0;
786 unsigned int ncylinders = 0;
787 unsigned int ncircles = 0;
791 if ((*it)->isTracked()) {
795 for (
unsigned int j = 0; j < 6; j++) {
806 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
808 if ((*it)->isTracked()) {
811 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
812 for (
unsigned int j = 0; j < 6; j++) {
824 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
826 if ((*it)->isTracked()) {
829 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
830 for (
unsigned int j = 0; j < 6; j++) {
848 if (nberrors_lines > 0)
850 if (nberrors_cylinders > 0)
852 if (nberrors_circles > 0)
871 unsigned int nbFeatures = 0;
876 for (
size_t a = 0; a < l->
meline.size(); a++) {
877 if (l->
meline[a] != NULL) {
878 double lineNormGradient;
879 unsigned int lineNbFeatures;
884 nbFeatures += lineNbFeatures;
890 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
895 double cylinderNormGradient = 0;
896 unsigned int cylinderNbFeatures = 0;
901 nbFeatures += cylinderNbFeatures;
905 double cylinderNormGradient = 0;
906 unsigned int cylinderNbFeatures = 0;
911 nbFeatures += cylinderNbFeatures;
916 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
920 double circleNormGradient = 0;
921 unsigned int circleNbFeatures = 0;
926 nbFeatures += circleNbFeatures;
930 if (nbFeatures > 0) {
947 int nbExpectedPoint = 0;
955 for (
size_t a = 0; a < l->
meline.size(); a++) {
956 if (l->
meline[a] != NULL) {
957 nbExpectedPoint += (int)l->
meline[a]->expecteddensity;
958 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
959 itme != l->
meline[a]->getMeList().end(); ++itme) {
971 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
975 nbExpectedPoint += (int)cy->
meline1->expecteddensity;
976 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
977 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
984 nbExpectedPoint += (int)cy->
meline2->expecteddensity;
985 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
986 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
996 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1000 nbExpectedPoint += ci->
meEllipse->getExpectedDensity();
1001 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
1002 itme != ci->
meEllipse->getMeList().end(); ++itme) {
1016 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1017 std::ostringstream oss;
1018 oss <<
"Not enough moving edges (" << nbGoodPoint <<
") to track the object: expected " << nb_min
1020 <<
" using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1037 unsigned int lvl = (
unsigned int)
scales.size();
1051 vpTRACE(
"Error in moving edge tracking");
1093 bool newvisibleface =
false;
1141 #ifdef VISP_HAVE_OGRE 1170 unsigned int i = (
unsigned int)
scales.size();
1215 #ifdef VISP_HAVE_XML2 1224 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1225 xmlp.
parse(configFile);
1233 xmlp.
getMe(meParser);
1262 vpTRACE(
"You need the libXML2 to read the config file %s", configFile.c_str());
1279 const bool displayFullModel)
1281 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1285 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1288 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1290 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1293 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1295 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1302 #ifdef VISP_HAVE_OGRE 1321 const bool displayFullModel)
1323 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1327 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1330 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1332 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1335 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1337 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1343 #ifdef VISP_HAVE_OGRE 1352 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1359 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1367 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1386 const bool doNotTrack =
false;
1391 bool isvisible =
false;
1395 int index = *itindex;
1415 for (
size_t a = 0; a < l->
meline.size(); a++) {
1416 if (l->
meline[a] != NULL)
1418 if (a < l->nbFeature.size())
1427 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1431 bool isvisible =
false;
1463 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1466 bool isvisible =
false;
1499 const bool doNotTrack =
false;
1512 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1523 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1545 if ((*it)->isTracked()) {
1555 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1557 if ((*it)->isTracked()) {
1567 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1569 if ((*it)->isTracked()) {
1586 if ((*it)->isTracked()) {
1588 unsigned int indexLine = 0;
1590 for (
size_t a = 0; a < l->
meline.size(); a++) {
1592 std::list<vpMeSite>::iterator itListLine;
1593 itListLine = l->
meline[a]->getMeList().begin();
1595 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
1598 if (
m_w_edge[n + indexLine] < 0.5) {
1625 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1627 if ((*it)->isTracked()) {
1630 std::list<vpMeSite>::iterator itListCyl1;
1631 std::list<vpMeSite>::iterator itListCyl2;
1634 itListCyl1 = cy->
meline1->getMeList().begin();
1635 itListCyl2 = cy->
meline2->getMeList().begin();
1637 for (
unsigned int i = 0; i < cy->
nbFeaturel1; i++) {
1662 for (
unsigned int i = cy->
nbFeaturel1; i < cy->nbFeature; i++) {
1691 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1693 if ((*it)->isTracked()) {
1696 std::list<vpMeSite>::iterator itListCir;
1699 itListCir = ci->
meEllipse->getMeList().begin();
1703 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
1745 if ((*it)->isTracked()) {
1753 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1755 if ((*it)->isTracked()) {
1763 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1765 if ((*it)->isTracked()) {
1775 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1777 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1778 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
1779 if ((*it)->meline[a] != NULL) {
1780 delete (*it)->meline[a];
1781 (*it)->meline[a] = NULL;
1785 (*it)->meline.clear();
1786 (*it)->nbFeature.clear();
1787 (*it)->nbFeatureTotal = 0;
1790 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1792 if ((*it)->meline1 != NULL) {
1793 delete (*it)->meline1;
1794 (*it)->meline1 = NULL;
1796 if ((*it)->meline2 != NULL) {
1797 delete (*it)->meline2;
1798 (*it)->meline2 = NULL;
1801 (*it)->nbFeature = 0;
1802 (*it)->nbFeaturel1 = 0;
1803 (*it)->nbFeaturel2 = 0;
1806 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1807 if ((*it)->meEllipse != NULL) {
1808 delete (*it)->meEllipse;
1809 (*it)->meEllipse = NULL;
1811 (*it)->nbFeature = 0;
1833 bool already_here =
false;
1836 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1839 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1843 already_here =
true;
1849 if (!already_here) {
1872 lines[i].push_back(l);
1889 for (
unsigned int i = 0; i <
scales.size(); i++) {
1891 for (std::list<vpMbtDistanceLine *>::iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1893 if (name.compare(l->
getName()) == 0) {
1913 const std::string &name)
1916 bool already_here =
false;
1919 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1922 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1931 if (!already_here) {
1972 const std::string &name)
1975 bool already_here =
false;
1978 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1981 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1991 if (!already_here) {
2019 for (
unsigned int i = 0; i <
scales.size(); i++) {
2021 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2023 if (name.compare(cy->
getName()) == 0) {
2041 for (
unsigned int i = 0; i <
scales.size(); i++) {
2043 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2045 if (name.compare(ci->
getName()) == 0) {
2063 for (
unsigned int i = 0; i < nbpt - 1; i++)
2081 bool &newvisibleline)
2084 bool changed =
false;
2091 #ifdef VISP_HAVE_OGRE 2100 newvisibleline =
true;
2102 newvisibleline =
false;
2126 for (
unsigned int i = 0; i < nbpt - 1; i++)
2150 for (
unsigned int i = 0; i < nbpt - 1; i++)
2156 unsigned int &nberrors_circles)
2158 unsigned int nbrow = 0;
2160 nberrors_cylinders = 0;
2161 nberrors_circles = 0;
2175 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2186 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2212 const int idFace,
const std::string &name)
2214 addCircle(p1, p2, p3, radius, (
int)idFace, name);
2228 const std::string &name)
2245 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2247 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2254 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2262 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2278 #ifdef VISP_HAVE_OGRE 2320 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2322 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2329 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2337 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2380 unsigned int nbGoodPoints = 0;
2382 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2385 for (
size_t a = 0; a < l->
meline.size(); a++) {
2387 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2388 itme != l->
meline[a]->getMeList().end(); ++itme) {
2397 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2401 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2402 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2406 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2407 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2415 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2418 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2419 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2426 return nbGoodPoints;
2452 unsigned int nbActivatedLevels = 0;
2453 for (
unsigned int i = 0; i < scale.size(); i++) {
2455 nbActivatedLevels++;
2459 if (scale.empty() || (nbActivatedLevels == 0)) {
2460 vpERROR_TRACE(
" !! WARNING : must use at least one level for the " 2461 "tracking. Use the global one");
2463 this->
scales.push_back(
true);
2476 lines.resize(scale.size());
2480 for (
unsigned int i = 0; i <
lines.size(); i++) {
2496 std::cerr <<
"Far clipping value cannot be inferior than near clipping " 2497 "value. Far clipping won't be considered." 2500 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping " 2501 "won't be considered." 2507 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2509 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2526 std::cerr <<
"Near clipping value cannot be superior than far clipping " 2527 "value. Near clipping won't be considered." 2530 std::cerr <<
"Near clipping value cannot be inferior than 0. Near " 2531 "clipping won't be considered." 2537 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2539 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2561 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2563 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2589 _pyramid.resize(
scales.size());
2597 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2599 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2601 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)) 2602 IplImage *vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (int)_I.
getHeight()), IPL_DEPTH_8U, 1);
2603 vpI0->imageData = (
char *)(_I.
bitmap);
2605 cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
2606 cvResize(vpI0, vpI, CV_INTER_NN);
2608 cvReleaseImage(&vpI);
2609 vpI0->imageData = NULL;
2610 cvReleaseImageHeader(&vpI0);
2612 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2613 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2614 (*I)[k][l] = _I[ii][jj];
2633 if (_pyramid.size() > 0) {
2635 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2636 if (_pyramid[i] != NULL) {
2658 std::ostringstream oss;
2660 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2664 linesList =
lines[level];
2680 std::ostringstream oss;
2682 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2702 std::ostringstream oss;
2704 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2719 const double ratio = pow(2., (
int)_scale);
2740 const double ratio = pow(2., (
int)_scale);
2768 if ((*it)->isTracked()) {
2775 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2777 if ((*it)->isTracked()) {
2784 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2786 if ((*it)->isTracked()) {
2806 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2808 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2819 (*it)->setTracked(name, useEdgeTracking);
2822 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2824 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2825 (*it)->setTracked(useEdgeTracking);
2829 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2830 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2831 (*it)->setTracked(useEdgeTracking);
bool m_computeInteraction
void setWindowName(const Ogre::String &n)
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0) const
void setMovingEdge(vpMe *Me)
Implementation of a matrix and operations on matrices.
void displayMovingEdges(const vpImage< unsigned char > &I)
vpMatrix covarianceMatrix
Covariance matrix.
void initFromCalibrationMatrix(const vpMatrix &_K)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setMovingEdge(const vpMe &me)
void trackMovingEdge(const vpImage< unsigned char > &I)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setCameraParameters(const vpCameraParameters &camera)
vpColVector m_error_edge
(s - s*)
void upScale(const unsigned int _scale)
std::string getName() const
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpColVector m_weightedError_edge
Weighted error.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void getCameraParameters(vpCameraParameters &_cam) const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(const double wmean)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void init(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
Type * bitmap
points toward the bitmap
vpMatrix L
The interaction matrix.
void setFarClippingDistance(const double &dist)
void setCameraParameters(const vpCameraParameters &camera)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
virtual void loadConfigFile(const std::string &configFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0) const
Class to define colors available for display functionnalities.
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
vpColVector m_factor
Edge VVS variables.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
vpPoint * p1
The first extremity.
void displayMovingEdges(const vpImage< unsigned char > &I)
double getNearClippingDistance() const
void setOgreShowConfigDialog(const bool showConfigDialog)
virtual ~vpMbEdgeTracker()
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector error
The error vector.
void getMe(vpMe &_ecm) const
vpColVector m_w_edge
Robust weights.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
error that can be emited by ViSP classes.
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void reInitLevel(const unsigned int _lvl)
vpPoint * p
corners in the object frame
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Manage the line of a polygon used in the model-based tracker.
unsigned int nbFeature
The number of moving edges.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void removeCircle(const std::string &name)
bool useOgre
Use Ogre3d for visibility tests.
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
void setIndex(const unsigned int i)
virtual void setCameraParameters(const vpCameraParameters &camera)
void displayFeaturesOnImage(const vpImage< unsigned char > &I, const unsigned int lvl)
void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
void computeVVS(const vpImage< unsigned char > &_I, const unsigned int lvl)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void removeCylinder(const std::string &name)
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
void loadConfigFile(const std::string &configFile)
vpMe me
The moving edges parameters.
void updateMovingEdge(const vpImage< unsigned char > &I)
virtual unsigned int getNbPoints(const unsigned int level=0) const
vpPoint * p1
The first extremity on the axe.
void initInteractionMatrixError()
void downScale(const unsigned int _scale)
bool hasNearClippingDistance() const
Class that defines what is a point.
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
vpCameraParameters cam
The camera parameters.
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static Type maximum(const Type &a, const Type &b)
virtual void setNearClippingDistance(const double &dist)
double getFarClippingDistance() const
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
bool hasFarClippingDistance() const
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0) const
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool Reinit
Indicates if the circle has to be reinitialized.
Error that can be emited by the vpTracker class and its derivates.
double getAngleDisappear() const
Implementation of a polygon of the model used by the model-based tracker.
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setName(const std::string &circle_name)
std::string getName() const
vpPoint * p2
The second extremity.
bool useScanLine
Use Scanline for visibility tests.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
void setAngleDisappear(const double &adisappear)
double getMinPolygonAreaThreshold() const
vpColVector error
The error vector.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
Generic class defining intrinsic camera parameters.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void setIndex(const unsigned int i)
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
vpMatrix m_SobelX
Sobel kernel in X.
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
virtual void computeVVSInteractionMatrixAndResidu()
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
virtual void setFarClippingDistance(const double &dist)
static Type minimum(const Type &a, const Type &b)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
unsigned int getRows() const
void setCameraParameters(const vpCameraParameters &_cam)
double angleAppears
Angle used to detect a face appearance.
vpColVector m_errorCylinders
void setState(const vpMeSiteState &flag)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpColVector m_errorCircles
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
std::string getName() const
static double rad(double deg)
void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void setMeanWeight1(const double wmean)
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void setClipping(const unsigned int &flags)
void setMovingEdge(vpMe *Me)
void computeProjectionError(const vpImage< unsigned char > &_I)
void setVisible(bool _isvisible)
void insert(unsigned int i, const vpColVector &v)
vpRobust m_robustCylinders
virtual void initFaceFromLines(vpMbtPolygon &polygon)
double getMinLineLengthThreshold() const
void setName(const std::string &line_name)
void setScales(const std::vector< bool > &_scales)
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void setCameraParameters(const vpCameraParameters &camera)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void setIndex(const unsigned int i)
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
static double deg(double rad)
void initInteractionMatrixError()
bool displayFeatures
If true, the features are displayed.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
bool ogreShowConfigDialog
bool applyLodSettingInConfig
Implementation of column vector and the associated operations.
void setAngleAppear(const double &aappear)
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const 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_SobelY
Sobel kernel in Y.
vpHomogeneousMatrix inverse() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
void setMeanWeight(const double _wmean)
bool isVisible(const unsigned int i)
void updateMovingEdgeWeights()
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
double angleDisappears
Angle used to detect a face disappearance.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual unsigned int getNbPolygon() const
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void computeVVSInit()
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
unsigned int getHeight() const
unsigned int getNbPoint() const
void setMeanWeight(const double w_mean)
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMatrix pseudoInverse(double svThreshold=1e-6) const
virtual void setClipping(const unsigned int &flags)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
bool Reinit
Indicates if the line has to be reinitialized.
double radius
The radius of the cylinder.
std::vector< vpMbtMeLine * > meline
The moving edge container.
unsigned int clippingFlag
Flags specifying which clipping to used.
vpMatrix m_L_edge
Interaction matrix.
void addPolygon(vpMbtPolygon &p)
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
virtual void setClipping(const unsigned int &flags)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
void setThreshold(const double noise_threshold)
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void computeVVSFirstPhase(const vpImage< unsigned char > &I, const unsigned int iter, double &count, const unsigned int lvl=0)
void setName(const std::string &cyl_name)
static int() sign(double x)
void resize(unsigned int n_data)
Resize containers for sort methods.
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void removeLine(const std::string &name)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
bool getFovClipping() const
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
double distNearClip
Distance for near clipping.
bool useLodGeneral
True if LOD mode is enabled.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpMatrix L
The interaction matrix.
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void initInteractionMatrixError()
virtual void computeVVSCheckLevenbergMarquardt(const 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)
void setMovingEdge(const vpMe &_ecm)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
std::string getName() const
vpPoint * p1
The center of the circle.
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
void resize(const unsigned int i, const bool flagNullify=true)
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)