 |
Visual Servoing Platform
version 3.2.0
|
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);
void setOgreShowConfigDialog(const bool showConfigDialog)
Error that can be emited by the vpTracker class and its derivates.
virtual void setClipping(const unsigned int &flags)
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
bool isVisible(const unsigned int i)
vpColVector m_error_edge
(s - s*)
void removeCircle(const std::string &name)
void upScale(const unsigned int _scale)
virtual void loadConfigFile(const std::string &configFile)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
std::vector< vpMbtMeLine * > meline
The moving edge container.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
vpColVector m_weightedError_edge
Weighted error.
virtual void setCameraParameters(const vpCameraParameters &camera)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void resize(const unsigned int i, const bool flagNullify=true)
void addPolygon(vpMbtPolygon &p)
virtual void setLod(const bool useLod, const std::string &name="")
void parse(const std::string &filename)
void downScale(const unsigned int _scale)
bool m_computeInteraction
vpColVector m_factor
Edge VVS variables.
void setScales(const std::vector< bool > &_scales)
void init(const vpImage< unsigned char > &I)
std::string getName() const
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
void track(const vpImage< unsigned char > &I)
vpMatrix covarianceMatrix
Covariance matrix.
double distNearClip
Distance for near clipping.
static double sqr(double x)
bool Reinit
Indicates if the line has to be reinitialized.
vpMatrix L
The interaction matrix.
std::vector< unsigned int > nbFeature
The number of moving edges.
Manage a cylinder used in the model-based tracker.
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual ~vpMbEdgeTracker()
bool isAppearing(const unsigned int i)
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)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
unsigned int nbFeature
The number of moving edges.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void displayOgre(const vpHomogeneousMatrix &cMo)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2)
void setThreshold(const double noise_threshold)
virtual void computeVVSWeights()
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
bool useScanLine
Use scanline rendering.
double radius
The radius of the cylinder.
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
unsigned int nbFeatureTotal
The number of moving edges.
bool useOgre
Use Ogre3d for visibility tests.
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
vpColVector m_errorCircles
static double rad(double deg)
vpPoint * p1
The first extremity.
vpHomogeneousMatrix cMo
The current pose.
double getMinPolygonAreaThreshold() const
std::string getName() const
Generic class defining intrinsic camera parameters.
void setMovingEdge(const vpMe &me)
void displayMovingEdges(const vpImage< unsigned char > &I)
vpMbtPolygon & getPolygon()
void trackMovingEdge(const vpImage< unsigned char > &I)
void loadConfigFile(const std::string &configFile)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
vpMatrix L
The interaction matrix.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setFarClippingDistance(const double &dist)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
double getFarClippingDistance() const
static double deg(double rad)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setMovingEdge(vpMe *Me)
void getCameraParameters(vpCameraParameters &_cam) const
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setVisible(bool _isvisible)
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0) const
vpColVector error
The error vector.
void computeVVSFirstPhase(const vpImage< unsigned char > &I, const unsigned int iter, double &count, const unsigned int lvl=0)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
bool hasFarClippingDistance() const
virtual void setNearClippingDistance(const double &dist)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
bool Reinit
Indicates if the line has to be reinitialized.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
vpPoint * p2
The second extremity on the axe.
Manage a circle used in the model-based tracker.
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setAngleAppear(const double &aappear)
void displayMovingEdges(const vpImage< unsigned char > &I)
bool getFovClipping() const
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
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, const double r)
std::string getName() const
void reInitLevel(const unsigned int _lvl)
std::string getName() const
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void removeCylinder(const std::string &name)
static int() sign(double x)
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
vpColVector error
The error vector.
void updateMovingEdge(const vpImage< unsigned char > &I)
vpPoint * p
corners in the object frame
unsigned int nbFeature
The number of moving edges.
vpColVector m_w_edge
Robust weights.
void removeLine(const std::string &name)
void setIndex(const unsigned int i)
void setIndex(const unsigned int i)
vpRobust m_robustCylinders
vpCameraParameters cam
The camera parameters.
double getAngleAppear() const
double m_lambda
Gain of the virtual visual servoing stage.
Implementation of column vector and the associated operations.
vpPoint * p1
The center of the circle.
bool useScanLine
Use Scanline for visibility tests.
vpMe me
The moving edges parameters.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
void setWindowName(const Ogre::String &n)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Type * bitmap
points toward the bitmap
void computeVVS(const vpImage< unsigned char > &_I, const unsigned int lvl)
virtual void setFarClippingDistance(const double &dist)
unsigned int nbFeaturel1
The number of moving edges on line 1.
void setCameraParameters(const vpCameraParameters &_cam)
Implementation of a matrix and operations on matrices.
void setMovingEdge(const vpMe &_ecm)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
Implementation of a polygon of the model used by the model-based tracker.
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0) const
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void insert(unsigned int i, const vpColVector &v)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
void setMovingEdge(vpMe *Me)
vpMbtMeEllipse * meEllipse
The moving edge containers.
double distFarClip
Distance for near clipping.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
void setCameraParameters(const vpCameraParameters &camera)
double radius
The radius of the circle.
bool displayFeatures
If true, the features are displayed.
vpPoint * p1
The first extremity on the axe.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
virtual unsigned int getNbPolygon() const
void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
void setMeanWeight1(const double wmean)
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
bool applyLodSettingInConfig
unsigned int getHeight() const
bool hasNearClippingDistance() const
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
void setVisible(bool _isvisible)
void computeProjectionError(const vpImage< unsigned char > &_I)
int index_polygon
Index of the faces which contain the line.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
vpPoint * p2
The second extremity.
void updateMovingEdgeWeights()
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
int index_polygon
Index of the face which contains the cylinder.
virtual void computeVVSInit()
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
vpMatrix L
The interaction matrix.
void displayFeaturesOnImage(const vpImage< unsigned char > &I, const unsigned int lvl)
static Type minimum(const Type &a, const Type &b)
double angleAppears
Angle used to detect a face appearance.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
std::vector< const vpImage< unsigned char > * > Ipyramid
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)
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setName(const std::string &cyl_name)
vpMatrix m_SobelY
Sobel kernel in Y.
vpMatrix m_SobelX
Sobel kernel in X.
static vpHomogeneousMatrix direct(const vpColVector &v)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getNbPoint() const
Manage the line of a polygon used in the model-based tracker.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
double getAngleDisappear() const
void displayMovingEdges(const vpImage< unsigned char > &I)
void initInteractionMatrixError()
void resize(unsigned int n_data)
Resize containers for sort methods.
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.
vpColVector m_errorCylinders
void setName(const std::string &circle_name)
virtual void setClipping(const unsigned int &flags)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
bool ogreShowConfigDialog
void setCameraParameters(const vpCameraParameters &camera)
vpMatrix m_L_edge
Interaction matrix.
void setMeanWeight2(const double wmean)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0) const
unsigned int clippingFlag
Flags specifying which clipping to used.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void setState(const vpMeSiteState &flag)
void setClipping(const unsigned int &flags)
vpMeSiteState getState() const
vpAROgre * getOgreContext()
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpHomogeneousMatrix inverse() const
void setName(const std::string &line_name)
double getNearClippingDistance() const
Class that defines what is a point.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void addPolygon(const int &index)
void initInteractionMatrixError()
Class to define colors available for display functionnalities.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
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)
void setIndex(const unsigned int i)
void setNearClippingDistance(const double &dist)
vpColVector error
The error vector.
static Type maximum(const Type &a, const Type &b)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setMovingEdge(vpMe *Me)
void getMe(vpMe &_ecm) const
virtual unsigned int getNbPoints(const unsigned int level=0) const
void setVisible(bool _isvisible)
error that can be emited by ViSP classes.
void computeFov(const unsigned int &w, const unsigned int &h)
void setMeanWeight(const double _wmean)
void initInteractionMatrixError()
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
bool Reinit
Indicates if the circle has to be reinitialized.
virtual void computeVVSInteractionMatrixAndResidu()
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void setCameraParameters(const vpCameraParameters &camera)
void initFromCalibrationMatrix(const vpMatrix &_K)
double angleDisappears
Angle used to detect a face disappearance.
std::vector< std::list< vpMbtDistanceLine * > > lines
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
double getMinLineLengthThreshold() const
bool useLodGeneral
True if LOD mode is enabled.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
unsigned int getWidth() const
void setMeanWeight(const double w_mean)
vpPoint * p2
A point on the plane containing the circle.
void setAngleDisappear(const double &adisappear)
unsigned int getRows() const