 |
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.
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
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 computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
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 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 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.
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
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.
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.
unsigned int getHeight() const
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)
void displayOgre(const vpHomogeneousMatrix &cMo)
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)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
double getNearClippingDistance() const
virtual void setFarClippingDistance(const double &dist)
unsigned int nbFeaturel1
The number of moving edges on line 1.
Type * bitmap
points toward the bitmap
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)
unsigned int getWidth() const
Implementation of a polygon of the model used by the model-based tracker.
void setMeanWeight(double w_mean)
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.
virtual unsigned int getNbPolygon() const
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
bool applyLodSettingInConfig
void setIndex(unsigned int i)
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)
vpAROgre * getOgreContext()
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 setOgreShowConfigDialog(bool showConfigDialog)
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
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
double getFarClippingDistance() const
bool hasFarClippingDistance() const
bool isVisible(unsigned int i)
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 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
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)
bool isAppearing(unsigned int i)
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()
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
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
vpPoint * p2
A point on the plane containing the circle.
unsigned int getRows() const