 |
Visual Servoing Platform
version 3.3.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/vpMbtXmlGenericParser.h>
58 #include <visp3/vision/vpPose.h>
70 : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
71 percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
72 m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
73 m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
74 m_robust_edge(), m_featuresToBeDisplayedEdge()
92 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
94 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
102 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
111 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
137 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
139 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
144 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
150 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
169 double residu_1 = 1e3;
172 unsigned int iter = 0;
185 while (reloop ==
true && iter < 10) {
190 count = count / (double)nbrow;
222 while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter <
m_maxIter)) {
225 bool reStartFromLastIncrement =
false;
229 if (!reStartFromLastIncrement) {
237 if (!isoJoIdentity_) {
243 double wi = 0.0, eri = 0.0;
244 double num = 0.0, den = 0.0;
246 for (
unsigned int i = 0; i < nbrow; i++) {
255 for (
unsigned int j = 0; j < 6; j++) {
260 for (
unsigned int i = 0; i < nbrow; i++) {
303 double e_prev = 0, e_cur, e_next;
305 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
306 if ((*it)->isTracked()) {
314 int index = *itindex;
326 std::list<vpMeSite>::const_iterator itListLine;
328 unsigned int indexFeature = 0;
330 for (
size_t a = 0; a < l->
meline.size(); a++) {
331 if (iter == 0 && l->
meline[a] != NULL)
332 itListLine = l->
meline[a]->getMeList().begin();
334 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
335 for (
unsigned int j = 0; j < 6; j++) {
336 m_L_edge[n + i][j] = l->
L[indexFeature][j];
354 if (indexFeature == 0) {
357 e_next = l->
error[1];
368 e_cur = l->
error[indexFeature];
375 e_cur = l->
error[indexFeature];
376 e_next = l->
error[indexFeature + 1];
377 if (fabs(e_cur - e_prev) < limite) {
380 if (fabs(e_cur - e_next) < limite) {
392 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
394 if ((*it)->isTracked()) {
399 std::list<vpMeSite>::const_iterator itCyl1;
400 std::list<vpMeSite>::const_iterator itCyl2;
402 itCyl1 = cy->
meline1->getMeList().begin();
403 itCyl2 = cy->
meline2->getMeList().begin();
406 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
407 for (
unsigned int j = 0; j < 6; j++) {
420 if (i < cy->nbFeaturel1) {
433 e_cur = cy->
error[0];
435 e_next = cy->
error[1];
444 e_cur = cy->
error[i];
446 e_next = cy->
error[i + 1];
457 e_cur = cy->
error[i];
464 e_cur = cy->
error[i];
471 e_cur = cy->
error[i];
472 e_next = cy->
error[i + 1];
473 if (fabs(e_cur - e_prev) < limite) {
476 if (fabs(e_cur - e_next) < limite) {
487 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
488 if ((*it)->isTracked()) {
493 std::list<vpMeSite>::const_iterator itCir;
494 if (iter == 0 && (ci->
meEllipse != NULL)) {
495 itCir = ci->
meEllipse->getMeList().begin();
498 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
499 for (
unsigned int j = 0; j < 6; j++) {
519 e_cur = ci->
error[0];
521 e_next = ci->
error[1];
532 e_cur = ci->
error[i];
539 e_cur = ci->
error[i];
540 e_next = ci->
error[i + 1];
541 if (fabs(e_cur - e_prev) < limite) {
544 if (fabs(e_cur - e_next) < limite) {
563 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
564 if ((*it)->isTracked()) {
571 int index = *itindex;
582 unsigned int indexFeature = 0;
583 for (
size_t a = 0; a < l->
meline.size(); a++) {
584 std::list<vpMeSite>::const_iterator itListLine;
585 if (l->
meline[a] != NULL) {
586 itListLine = l->
meline[a]->getMeList().begin();
588 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
602 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
604 if ((*it)->isTracked()) {
608 std::list<vpMeSite>::const_iterator itCyl1;
609 std::list<vpMeSite>::const_iterator itCyl2;
611 itCyl1 = cy->
meline1->getMeList().begin();
612 itCyl2 = cy->
meline2->getMeList().begin();
615 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
618 if (i < cy->nbFeaturel1) {
633 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
634 if ((*it)->isTracked()) {
638 std::list<vpMeSite>::const_iterator itCir;
640 itCir = ci->
meEllipse->getMeList().begin();
643 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
662 for (
unsigned int i = 0; i < nerror; i++) {
668 for (
unsigned int j = 0; j < 6; j++) {
673 for (
unsigned int i = 0; i < nerror; i++) {
687 if (isoJoIdentity_) {
691 unsigned int rank = (
m_L_edge * cVo).kernel(K);
700 isoJoIdentity_ =
false;
708 if (isoJoIdentity_) {
718 v = -0.7 * LVJTLVJ.
pseudoInverse(LVJTLVJ.
getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
728 unsigned int nbrow = 0;
729 unsigned int nberrors_lines = 0;
730 unsigned int nberrors_cylinders = 0;
731 unsigned int nberrors_circles = 0;
733 nbrow =
initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
737 "No data found to compute the interaction matrix...");
771 "computeVVSInteractionMatrixAndR"
772 "esidu() should not be called!");
782 unsigned int nlines = 0;
783 unsigned int ncylinders = 0;
784 unsigned int ncircles = 0;
788 if ((*it)->isTracked()) {
792 for (
unsigned int j = 0; j < 6; j++) {
803 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
805 if ((*it)->isTracked()) {
808 for (
unsigned int i = 0; i < cy->
nbFeature; i++) {
809 for (
unsigned int j = 0; j < 6; j++) {
821 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
823 if ((*it)->isTracked()) {
826 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
827 for (
unsigned int j = 0; j < 6; j++) {
845 if (nberrors_lines > 0)
847 if (nberrors_cylinders > 0)
849 if (nberrors_circles > 0)
868 unsigned int nbFeatures = 0;
873 for (
size_t a = 0; a < l->
meline.size(); a++) {
874 if (l->
meline[a] != NULL) {
875 double lineNormGradient;
876 unsigned int lineNbFeatures;
881 nbFeatures += lineNbFeatures;
887 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
892 double cylinderNormGradient = 0;
893 unsigned int cylinderNbFeatures = 0;
898 nbFeatures += cylinderNbFeatures;
902 double cylinderNormGradient = 0;
903 unsigned int cylinderNbFeatures = 0;
908 nbFeatures += cylinderNbFeatures;
913 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
917 double circleNormGradient = 0;
918 unsigned int circleNbFeatures = 0;
923 nbFeatures += circleNbFeatures;
927 if (nbFeatures > 0) {
944 int nbExpectedPoint = 0;
952 for (
size_t a = 0; a < l->
meline.size(); a++) {
953 if (l->
meline[a] != NULL) {
954 nbExpectedPoint += (int)l->
meline[a]->expecteddensity;
955 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
956 itme != l->
meline[a]->getMeList().end(); ++itme) {
968 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
972 nbExpectedPoint += (int)cy->
meline1->expecteddensity;
973 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
974 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
981 nbExpectedPoint += (int)cy->
meline2->expecteddensity;
982 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
983 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
993 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
997 nbExpectedPoint += ci->
meEllipse->getExpectedDensity();
998 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
999 itme != ci->
meEllipse->getMeList().end(); ++itme) {
1013 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1014 std::ostringstream oss;
1015 oss <<
"Not enough moving edges (" << nbGoodPoint <<
") to track the object: expected " << nb_min
1017 <<
" using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1033 unsigned int lvl = (
unsigned int)
scales.size();
1047 vpTRACE(
"Error in moving edge tracking");
1089 bool newvisibleface =
false;
1143 #ifdef VISP_HAVE_OGRE
1172 unsigned int i = (
unsigned int)
scales.size();
1229 #ifdef VISP_HAVE_PUGIXML
1238 std::cout <<
" *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1239 xmlp.
parse(configFile);
1276 std::cerr <<
"pugixml third-party is not properly built to read config file: " << configFile << std::endl;
1293 bool displayFullModel)
1302 for (
size_t i = 0; i < models.size(); i++) {
1309 double mu20 = models[i][3];
1310 double mu11 = models[i][4];
1311 double mu02 = models[i][5];
1316 #ifdef VISP_HAVE_OGRE
1335 bool displayFullModel)
1344 for (
size_t i = 0; i < models.size(); i++) {
1351 double mu20 = models[i][3];
1352 double mu11 = models[i][4];
1353 double mu02 = models[i][5];
1358 #ifdef VISP_HAVE_OGRE
1366 std::vector<std::vector<double> > features;
1368 const unsigned int lvl = 0;
1369 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[lvl].begin(); it !=
lines[lvl].end(); ++it) {
1373 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1377 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[lvl].begin(); it !=
cylinders[lvl].end();
1382 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1386 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[lvl].begin(); it !=
circles[lvl].end(); ++it) {
1390 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1413 bool displayFullModel)
1415 std::vector<std::vector<double> > models;
1417 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1421 std::vector<std::vector<double> > currentModel =
1422 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1423 models.insert(models.end(), currentModel.begin(), currentModel.end());
1426 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1428 std::vector<std::vector<double> > currentModel =
1429 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1430 models.insert(models.end(), currentModel.begin(), currentModel.end());
1433 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1435 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1436 models.push_back(paramsCircle);
1525 const bool doNotTrack =
false;
1530 bool isvisible =
false;
1534 int index = *itindex;
1554 for (
size_t a = 0; a < l->
meline.size(); a++) {
1555 if (l->
meline[a] != NULL)
1557 if (a < l->nbFeature.size())
1566 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1570 bool isvisible =
false;
1602 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1605 bool isvisible =
false;
1638 const bool doNotTrack =
false;
1651 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1662 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1684 if ((*it)->isTracked()) {
1694 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1696 if ((*it)->isTracked()) {
1706 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1708 if ((*it)->isTracked()) {
1725 if ((*it)->isTracked()) {
1727 unsigned int indexLine = 0;
1729 for (
size_t a = 0; a < l->
meline.size(); a++) {
1731 std::list<vpMeSite>::iterator itListLine;
1732 itListLine = l->
meline[a]->getMeList().begin();
1734 for (
unsigned int i = 0; i < l->
nbFeature[a]; i++) {
1737 if (
m_w_edge[n + indexLine] < 0.5) {
1764 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1766 if ((*it)->isTracked()) {
1769 std::list<vpMeSite>::iterator itListCyl1;
1770 std::list<vpMeSite>::iterator itListCyl2;
1773 itListCyl1 = cy->
meline1->getMeList().begin();
1774 itListCyl2 = cy->
meline2->getMeList().begin();
1776 for (
unsigned int i = 0; i < cy->
nbFeaturel1; i++) {
1801 for (
unsigned int i = cy->
nbFeaturel1; i < cy->nbFeature; i++) {
1830 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1832 if ((*it)->isTracked()) {
1835 std::list<vpMeSite>::iterator itListCir;
1838 itListCir = ci->
meEllipse->getMeList().begin();
1842 for (
unsigned int i = 0; i < ci->
nbFeature; i++) {
1884 if ((*it)->isTracked()) {
1892 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
1894 if ((*it)->isTracked()) {
1902 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
1904 if ((*it)->isTracked()) {
1914 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1916 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1917 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
1918 if ((*it)->meline[a] != NULL) {
1919 delete (*it)->meline[a];
1920 (*it)->meline[a] = NULL;
1924 (*it)->meline.clear();
1925 (*it)->nbFeature.clear();
1926 (*it)->nbFeatureTotal = 0;
1929 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
1931 if ((*it)->meline1 != NULL) {
1932 delete (*it)->meline1;
1933 (*it)->meline1 = NULL;
1935 if ((*it)->meline2 != NULL) {
1936 delete (*it)->meline2;
1937 (*it)->meline2 = NULL;
1940 (*it)->nbFeature = 0;
1941 (*it)->nbFeaturel1 = 0;
1942 (*it)->nbFeaturel2 = 0;
1945 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
1946 if ((*it)->meEllipse != NULL) {
1947 delete (*it)->meEllipse;
1948 (*it)->meEllipse = NULL;
1950 (*it)->nbFeature = 0;
1972 bool already_here =
false;
1975 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
1978 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
1982 already_here =
true;
1988 if (!already_here) {
2011 lines[i].push_back(l);
2028 for (
unsigned int i = 0; i <
scales.size(); i++) {
2030 for (std::list<vpMbtDistanceLine *>::iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2032 if (name.compare(l->
getName()) == 0) {
2052 const std::string &name)
2055 bool already_here =
false;
2058 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2061 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2070 if (!already_here) {
2111 const std::string &name)
2114 bool already_here =
false;
2117 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2120 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2130 if (!already_here) {
2158 for (
unsigned int i = 0; i <
scales.size(); i++) {
2160 for (std::list<vpMbtDistanceCylinder *>::iterator it =
cylinders[i].begin(); it !=
cylinders[i].end(); ++it) {
2162 if (name.compare(cy->
getName()) == 0) {
2180 for (
unsigned int i = 0; i <
scales.size(); i++) {
2182 for (std::list<vpMbtDistanceCircle *>::iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2184 if (name.compare(ci->
getName()) == 0) {
2202 for (
unsigned int i = 0; i < nbpt - 1; i++)
2220 bool &newvisibleline)
2223 bool changed =
false;
2230 #ifdef VISP_HAVE_OGRE
2239 newvisibleline =
true;
2241 newvisibleline =
false;
2265 for (
unsigned int i = 0; i < nbpt - 1; i++)
2289 for (
unsigned int i = 0; i < nbpt - 1; i++)
2295 unsigned int &nberrors_circles)
2297 unsigned int nbrow = 0;
2299 nberrors_cylinders = 0;
2300 nberrors_circles = 0;
2314 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2325 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2351 int idFace,
const std::string &name)
2353 addCircle(p1, p2, p3, radius, (
int)idFace, name);
2367 const std::string &name)
2384 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2386 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2393 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2401 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2417 #ifdef VISP_HAVE_OGRE
2459 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2461 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2468 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2476 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2519 unsigned int nbGoodPoints = 0;
2521 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[level].begin(); it !=
lines[level].end(); ++it) {
2524 for (
size_t a = 0; a < l->
meline.size(); a++) {
2526 for (std::list<vpMeSite>::const_iterator itme = l->
meline[a]->getMeList().begin();
2527 itme != l->
meline[a]->getMeList().end(); ++itme) {
2536 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[level].begin(); it !=
cylinders[level].end();
2540 for (std::list<vpMeSite>::const_iterator itme1 = cy->
meline1->getMeList().begin();
2541 itme1 != cy->
meline1->getMeList().end(); ++itme1) {
2545 for (std::list<vpMeSite>::const_iterator itme2 = cy->
meline2->getMeList().begin();
2546 itme2 != cy->
meline2->getMeList().end(); ++itme2) {
2554 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[level].begin(); it !=
circles[level].end(); ++it) {
2557 for (std::list<vpMeSite>::const_iterator itme = ci->
meEllipse->getMeList().begin();
2558 itme != ci->
meEllipse->getMeList().end(); ++itme) {
2565 return nbGoodPoints;
2591 unsigned int nbActivatedLevels = 0;
2592 for (
unsigned int i = 0; i < scale.size(); i++) {
2594 nbActivatedLevels++;
2598 if (scale.empty() || (nbActivatedLevels == 0)) {
2599 vpERROR_TRACE(
" !! WARNING : must use at least one level for the "
2600 "tracking. Use the global one");
2602 this->
scales.push_back(
true);
2615 lines.resize(scale.size());
2619 for (
unsigned int i = 0; i <
lines.size(); i++) {
2635 std::cerr <<
"Far clipping value cannot be inferior than near clipping "
2636 "value. Far clipping won't be considered."
2639 std::cerr <<
"Far clipping value cannot be inferior than 0. Far clipping "
2640 "won't be considered."
2646 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2648 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2665 std::cerr <<
"Near clipping value cannot be superior than far clipping "
2666 "value. Near clipping won't be considered."
2669 std::cerr <<
"Near clipping value cannot be inferior than 0. Near "
2670 "clipping won't be considered."
2676 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2678 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2700 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2702 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2728 _pyramid.resize(
scales.size());
2736 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2738 unsigned int cScale =
static_cast<unsigned int>(pow(2., (
int)i));
2740 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2741 IplImage *vpI0 = cvCreateImageHeader(cvSize((
int)_I.
getWidth(), (
int)_I.
getHeight()), IPL_DEPTH_8U, 1);
2742 vpI0->imageData = (
char *)(_I.
bitmap);
2744 cvCreateImage(cvSize((
int)(_I.
getWidth() / cScale), (
int)(_I.
getHeight() / cScale)), IPL_DEPTH_8U, 1);
2745 cvResize(vpI0, vpI, CV_INTER_NN);
2747 cvReleaseImage(&vpI);
2748 vpI0->imageData = NULL;
2749 cvReleaseImageHeader(&vpI0);
2751 for (
unsigned int k = 0, ii = 0; k < I->
getHeight(); k += 1, ii += cScale) {
2752 for (
unsigned int l = 0, jj = 0; l < I->
getWidth(); l += 1, jj += cScale) {
2753 (*I)[k][l] = _I[ii][jj];
2772 if (_pyramid.size() > 0) {
2774 for (
unsigned int i = 1; i < _pyramid.size(); i += 1) {
2775 if (_pyramid[i] != NULL) {
2797 std::ostringstream oss;
2799 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2803 linesList =
lines[level];
2819 std::ostringstream oss;
2821 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2841 std::ostringstream oss;
2843 std::string errorMsg =
"level " + oss.str() +
" is not used, cannot get its distance lines.";
2858 const double ratio = pow(2., (
int)_scale);
2879 const double ratio = pow(2., (
int)_scale);
2907 if ((*it)->isTracked()) {
2914 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[
scaleLevel].begin();
2916 if ((*it)->isTracked()) {
2923 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[
scaleLevel].begin();
2925 if ((*it)->isTracked()) {
2945 for (
unsigned int i = 0; i <
scales.size(); i += 1) {
2947 for (std::list<vpMbtDistanceLine *>::const_iterator it =
lines[i].begin(); it !=
lines[i].end(); ++it) {
2958 (*it)->setTracked(name, useEdgeTracking);
2961 for (std::list<vpMbtDistanceCylinder *>::const_iterator it =
cylinders[i].begin(); it !=
cylinders[i].end();
2963 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2964 (*it)->setTracked(useEdgeTracking);
2968 for (std::list<vpMbtDistanceCircle *>::const_iterator it =
circles[i].begin(); it !=
circles[i].end(); ++it) {
2969 if (
faces[(
unsigned)(*it)->index_polygon]->getName() == name) {
2970 (*it)->setTracked(useEdgeTracking);
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)
void setAngleAppear(const double &aappear)
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.
vpColVector m_weightedError_edge
Weighted error.
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void addPolygon(vpMbtPolygon &p)
void downScale(const unsigned int _scale)
bool m_computeInteraction
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_)
vpColVector m_factor
Edge VVS variables.
void setScales(const std::vector< bool > &_scales)
virtual void init(const vpImage< unsigned char > &I)
void setMeanWeight(double _wmean)
std::string getName() const
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
virtual 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.
vpHomogeneousMatrix m_cMo
The current pose.
Manage a cylinder used in the model-based tracker.
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
void trackMovingEdge(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
virtual ~vpMbEdgeTracker()
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
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)
void setMeanWeight2(double wmean)
vpCameraParameters m_cam
The camera parameters.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
virtual void computeVVSWeights()
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
void setAngleDisappear(const double &adisappear)
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 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)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
vpColVector m_errorCircles
static double rad(double deg)
vpPoint * p1
The first extremity.
std::string getName() const
void setMeanWeight1(double wmean)
Generic class defining intrinsic camera parameters.
void computeVVS(const vpImage< unsigned char > &_I, unsigned int lvl)
void setMovingEdge(const vpMe &me)
vpMbtPolygon & getPolygon()
double getAngleDisappear() const
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual unsigned int getNbPoints(unsigned int level=0) const
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void loadConfigFile(const std::string &configFile)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
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.
static double deg(double rad)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setMovingEdge(vpMe *Me)
std::vector< std::vector< double > > getFeaturesForDisplay()
double getLodMinPolygonAreaThreshold() const
double getLodMinLineLengthThreshold() 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)
vpColVector error
The error vector.
void displayFeaturesOnImage(const vpImage< unsigned char > &I)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
@ TOO_NEAR
Point removed because too near image borders.
virtual void setNearClippingDistance(const double &dist)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
bool Reinit
Indicates if the line has to be reinitialized.
bool isVisible(unsigned int i)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
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)
void computeVVSFirstPhase(const vpImage< unsigned char > &I, unsigned int iter, double &count, unsigned int lvl=0)
static const vpColor blue
vpPoint * p2
The second extremity on the axe.
Manage a circle used in the model-based tracker.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
@ NO_SUPPRESSION
Point used by the tracker.
std::string getName() const
void reInitLevel(const unsigned int _lvl)
std::string getName() const
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
bool getFovClipping() const
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.
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, unsigned int level=0) const
vpColVector error
The error vector.
virtual void setLod(bool useLod, const std::string &name="")
void updateMovingEdge(const vpImage< unsigned char > &I)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
vpPoint * p
corners in the object frame
unsigned int nbFeature
The number of moving edges.
vpColVector m_w_edge
Robust weights.
@ dimensionError
Bad dimension.
static const vpColor yellow
void removeLine(const std::string &name)
static const vpColor purple
vpRobust m_robustCylinders
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.
Parse an Xml file to extract configuration parameters of a mbtConfig object.
vpMe me
The moving edges parameters.
@ THRESHOLD
Point removed due to a threshold problem.
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
double getNearClippingDistance() const
virtual void setFarClippingDistance(const double &dist)
unsigned int nbFeaturel1
The number of moving edges on line 1.
void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
Implementation of a matrix and operations on matrices.
void setThreshold(double noise_threshold)
void setEdgeMe(const vpMe &ecm)
Implementation of a polygon of the model used by the model-based tracker.
void setMeanWeight(double w_mean)
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
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 getEdgeMe(vpMe &ecm) const
void setMovingEdge(vpMe *Me)
vpMbtMeEllipse * meEllipse
The moving edge containers.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
double distFarClip
Distance for near clipping.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
void setCameraParameters(const vpCameraParameters &camera)
void parse(const std::string &filename)
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 initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
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
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
bool applyLodSettingInConfig
bool isAppearing(unsigned int i)
void setIndex(unsigned int i)
unsigned int getHeight() const
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
double getAngleAppear() const
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.
std::vector< std::vector< double > > getFeaturesForDisplay()
int index_polygon
Index of the face which contains the cylinder.
virtual void computeVVSInit()
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
static bool equal(double x, double y, double s=0.001)
vpMatrix L
The interaction matrix.
static Type minimum(const Type &a, const Type &b)
double angleAppears
Angle used to detect a face appearance.
void getCameraParameters(vpCameraParameters &cam) const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
std::vector< const vpImage< unsigned char > * > Ipyramid
void setName(const std::string &cyl_name)
void setCameraParameters(const vpCameraParameters &cam)
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
double getFarClippingDistance() const
bool hasFarClippingDistance() const
Manage the line of a polygon used in the model-based tracker.
void setOgreShowConfigDialog(bool showConfigDialog)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
void resize(unsigned int i, bool flagNullify=true)
bool hasNearClippingDistance() const
std::vector< std::vector< double > > getFeaturesForDisplay()
void initInteractionMatrixError()
void resize(unsigned int n_data)
Resize containers for sort methods.
virtual void setCameraParameters(const vpCameraParameters &cam)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
virtual void testTracking()
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.
unsigned int nbFeaturel2
The number of moving edges on line 2.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
unsigned int clippingFlag
Flags specifying which clipping to used.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
static const vpColor green
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)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, unsigned int level=0) const
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)
Class that defines what is a point.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void addPolygon(const int &index)
void initInteractionMatrixError()
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
Class to define colors available for display functionnalities.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
@ CONSTRAST
Point removed due to a contrast problem.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
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 setIndex(unsigned int i)
void setVisible(bool _isvisible)
void setIndex(unsigned int i)
error that can be emited by ViSP classes.
void computeFov(const unsigned int &w, const unsigned int &h)
void initInteractionMatrixError()
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
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)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setCameraParameters(const vpCameraParameters &camera)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void initFromCalibrationMatrix(const vpMatrix &_K)
double angleDisappears
Angle used to detect a face disappearance.
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
static const vpColor cyan
std::vector< std::list< vpMbtDistanceLine * > > lines
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
bool useLodGeneral
True if LOD mode is enabled.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
unsigned int getWidth() const
vpPoint * p2
A point on the plane containing the circle.
unsigned int getRows() const