36 #include <visp3/mbt/vpMbGenericTracker.h>
38 #include <visp3/core/vpDisplay.h>
39 #include <visp3/core/vpExponentialMap.h>
40 #include <visp3/core/vpTrackingException.h>
41 #include <visp3/mbt/vpMbtXmlGenericParser.h>
44 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
45 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
55 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
64 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
65 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
69 }
else if (nbCameras == 1) {
75 for (
unsigned int i = 1; i <= nbCameras; i++) {
91 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
100 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
101 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
103 if (trackerTypes.empty()) {
107 if (trackerTypes.size() == 1) {
113 for (
size_t i = 1; i <= trackerTypes.size(); i++) {
114 std::stringstream ss;
129 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
138 const std::vector<int> &trackerTypes)
139 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
140 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
142 if (cameraNames.size() != trackerTypes.size() || cameraNames.empty()) {
144 "cameraNames.size() != trackerTypes.size() || cameraNames.empty()");
147 for (
size_t i = 0; i < cameraNames.size(); i++) {
160 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
197 double rawTotalProjectionError = 0.0;
198 unsigned int nbTotalFeaturesUsed = 0;
200 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
202 TrackerWrapper *tracker = it->second;
204 unsigned int nbFeaturesUsed = 0;
205 double curProjError = tracker->computeProjectionErrorImpl(I, _cMo, _cam, nbFeaturesUsed);
207 if (nbFeaturesUsed > 0) {
208 nbTotalFeaturesUsed += nbFeaturesUsed;
209 rawTotalProjectionError += curProjError;
213 if (nbTotalFeaturesUsed > 0) {
214 return vpMath::deg(rawTotalProjectionError / (
double)nbTotalFeaturesUsed);
223 double rawTotalProjectionError = 0.0;
224 unsigned int nbTotalFeaturesUsed = 0;
226 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
228 TrackerWrapper *tracker = it->second;
230 double curProjError = tracker->getProjectionError();
231 unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
233 if (nbFeaturesUsed > 0) {
234 nbTotalFeaturesUsed += nbFeaturesUsed;
235 rawTotalProjectionError += (
vpMath::rad(curProjError) * nbFeaturesUsed);
239 if (nbTotalFeaturesUsed > 0) {
258 double normRes_1 = -1;
259 unsigned int iter = 0;
268 bool isoJoIdentity_ =
true;
275 std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
280 mapOfVelocityTwist[it->first] = cVo;
284 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
293 bool reStartFromLastIncrement =
false;
295 if (reStartFromLastIncrement) {
296 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
298 TrackerWrapper *tracker = it->second;
302 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
305 tracker->ctTc0 = c_curr_tTc_curr0;
310 if (!reStartFromLastIncrement) {
315 if (!isoJoIdentity_) {
318 LVJ_true = (
m_L * (cVo *
oJo));
324 isoJoIdentity_ =
true;
331 if (isoJoIdentity_) {
335 unsigned int rank = (
m_L * cVo).kernel(K);
345 isoJoIdentity_ =
false;
354 unsigned int start_index = 0;
355 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
357 TrackerWrapper *tracker = it->second;
360 for (
unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
361 double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
362 W_true[start_index + i] = wi;
368 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
369 m_L[start_index + i][j] *= wi;
373 start_index += tracker->m_error_edge.
getRows();
376 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
378 for (
unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
379 double wi = tracker->m_w_klt[i] * factorKlt;
380 W_true[start_index + i] = wi;
386 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
387 m_L[start_index + i][j] *= wi;
391 start_index += tracker->m_error_klt.
getRows();
396 for (
unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
397 double wi = tracker->m_w_depthNormal[i] * factorDepth;
398 W_true[start_index + i] = wi;
404 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
405 m_L[start_index + i][j] *= wi;
409 start_index += tracker->m_error_depthNormal.
getRows();
413 for (
unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
414 double wi = tracker->m_w_depthDense[i] * factorDepthDense;
415 W_true[start_index + i] = wi;
421 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
422 m_L[start_index + i][j] *= wi;
426 start_index += tracker->m_error_depthDense.
getRows();
431 normRes = sqrt(num / den);
439 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
440 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
442 TrackerWrapper *tracker = it->second;
446 tracker->ctTc0 = c_curr_tTc_curr0;
451 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
453 TrackerWrapper *tracker = it->second;
463 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
465 TrackerWrapper *tracker = it->second;
468 tracker->updateMovingEdgeWeights();
480 unsigned int nbFeatures = 0;
482 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
484 TrackerWrapper *tracker = it->second;
485 tracker->computeVVSInit(mapOfImages[it->first]);
487 nbFeatures += tracker->m_error.getRows();
501 "computeVVSInteractionMatrixAndR"
502 "esidu() should not be called");
507 std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
509 unsigned int start_index = 0;
511 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
513 TrackerWrapper *tracker = it->second;
516 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
518 tracker->ctTc0 = c_curr_tTc_curr0;
521 tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
523 m_L.
insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
526 start_index += tracker->m_error.getRows();
532 unsigned int start_index = 0;
534 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
536 TrackerWrapper *tracker = it->second;
537 tracker->computeVVSWeights();
540 start_index += tracker->m_w.getRows();
559 const bool displayFullModel)
563 TrackerWrapper *tracker = it->second;
564 tracker->display(I, cMo_, cam_, col, thickness, displayFullModel);
585 const bool displayFullModel)
589 TrackerWrapper *tracker = it->second;
590 tracker->display(I, cMo_, cam_, col, thickness, displayFullModel);
615 const unsigned int thickness,
const bool displayFullModel)
618 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
619 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
622 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
624 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
648 const bool displayFullModel)
651 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
652 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
655 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
657 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
674 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
675 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
676 const vpColor &col,
const unsigned int thickness,
const bool displayFullModel)
680 it_img != mapOfImages.end(); ++it_img) {
681 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
682 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
683 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
685 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
686 it_cam != mapOfCameraParameters.end()) {
687 TrackerWrapper *tracker = it_tracker->second;
688 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
690 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
707 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
708 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
709 const vpColor &col,
const unsigned int thickness,
const bool displayFullModel)
712 for (std::map<std::string,
const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
713 it_img != mapOfImages.end(); ++it_img) {
714 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
715 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
716 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
718 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
719 it_cam != mapOfCameraParameters.end()) {
720 TrackerWrapper *tracker = it_tracker->second;
721 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
723 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
735 std::vector<std::string> cameraNames;
737 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
739 cameraNames.push_back(it_tracker->first);
756 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
757 it->second->getCameraParameters(cam1);
760 it->second->getCameraParameters(cam2);
762 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
775 mapOfCameraParameters.clear();
777 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
780 it->second->getCameraParameters(cam_);
781 mapOfCameraParameters[it->first] = cam_;
793 std::map<std::string, int> trackingTypes;
795 TrackerWrapper *traker;
796 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
798 traker = it_tracker->second;
799 trackingTypes[it_tracker->first] = traker->getTrackerType();
802 return trackingTypes;
816 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
817 clippingFlag1 = it->second->getClipping();
820 clippingFlag2 = it->second->getClipping();
822 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
834 mapOfClippingFlags.clear();
836 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
838 TrackerWrapper *tracker = it->second;
839 mapOfClippingFlags[it->first] = tracker->getClipping();
852 return it->second->getFaces();
866 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
868 return it->second->getFaces();
871 std::cerr <<
"The camera: " << cameraName <<
" cannot be found!" << std::endl;
875 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
883 TrackerWrapper *tracker = it->second;
884 return tracker->getFeaturesCircle();
898 TrackerWrapper *tracker = it->second;
899 return tracker->getFeaturesKltCylinder();
913 TrackerWrapper *tracker = it->second;
914 return tracker->getFeaturesKlt();
933 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
946 TrackerWrapper *tracker = it->second;
947 return tracker->getKltImagePoints();
952 return std::vector<vpImagePoint>();
967 TrackerWrapper *tracker = it->second;
968 return tracker->getKltImagePointsWithId();
973 return std::map<int, vpImagePoint>();
985 TrackerWrapper *tracker = it->second;
986 return tracker->getKltMaskBorder();
1003 TrackerWrapper *tracker = it->second;
1004 return tracker->getKltNbPoints();
1022 TrackerWrapper *tracker;
1023 tracker = it_tracker->second;
1024 return tracker->getKltOpencv();
1043 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1044 klt1 = it->second->getKltOpencv();
1047 klt2 = it->second->getKltOpencv();
1049 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1063 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1065 TrackerWrapper *tracker = it->second;
1066 mapOfKlts[it->first] = tracker->getKltOpencv();
1070 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
1080 TrackerWrapper *tracker = it->second;
1081 return tracker->getKltPoints();
1086 return std::vector<cv::Point2f>();
1112 const unsigned int level)
const
1116 it->second->getLcircle(circlesList, level);
1136 const unsigned int level)
const
1138 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1140 it->second->getLcircle(circlesList, level);
1142 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1159 const unsigned int level)
const
1163 it->second->getLcylinder(cylindersList, level);
1183 const unsigned int level)
const
1185 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1187 it->second->getLcylinder(cylindersList, level);
1189 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1206 const unsigned int level)
const
1211 it->second->getLline(linesList, level);
1231 const unsigned int level)
const
1233 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1235 it->second->getLline(linesList, level);
1237 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1251 return it->second->getMovingEdge();
1270 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1271 it->second->getMovingEdge(me1);
1274 it->second->getMovingEdge(me2);
1276 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1288 mapOfMovingEdges.clear();
1290 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1292 TrackerWrapper *tracker = it->second;
1293 mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1316 unsigned int nbGoodPoints = 0;
1319 nbGoodPoints = it->second->getNbPoints(level);
1324 return nbGoodPoints;
1343 mapOfNbPoints.clear();
1345 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1347 TrackerWrapper *tracker = it->second;
1348 mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1361 return it->second->getNbPolygon();
1375 mapOfNbPolygons.clear();
1377 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1379 TrackerWrapper *tracker = it->second;
1380 mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1398 return it->second->getPolygon(index);
1418 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1420 return it->second->getPolygon(index);
1423 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1442 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1445 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1449 TrackerWrapper *tracker = it->second;
1450 polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1455 return polygonFaces;
1476 std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1477 const bool orderPolygons,
const bool useVisibility,
const bool clipPolygon)
1479 mapOfPolygons.clear();
1480 mapOfPoints.clear();
1482 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1484 TrackerWrapper *tracker = it->second;
1485 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1486 tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1488 mapOfPolygons[it->first] = polygonFaces.first;
1489 mapOfPoints[it->first] = polygonFaces.second;
1504 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1505 it->second->getPose(c1Mo);
1508 it->second->getPose(c2Mo);
1510 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1523 mapOfCameraPoses.clear();
1525 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1527 TrackerWrapper *tracker = it->second;
1528 tracker->getPose(mapOfCameraPoses[it->first]);
1539 TrackerWrapper *tracker = it->second;
1540 return tracker->getTrackerType();
1549 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1551 TrackerWrapper *tracker = it->second;
1558 const double ,
const int ,
const std::string & )
1563 #ifdef VISP_HAVE_MODULE_GUI
1608 const std::string &initFile1,
const std::string &initFile2,
const bool displayHelp,
1612 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1613 TrackerWrapper *tracker = it->second;
1614 tracker->initClick(I1, initFile1, displayHelp, T1);
1618 tracker = it->second;
1619 tracker->initClick(I2, initFile2, displayHelp, T2);
1623 tracker = it->second;
1626 tracker->getPose(
cMo);
1630 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1676 const std::map<std::string, std::string> &mapOfInitFiles,
const bool displayHelp,
1677 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
1680 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1682 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
1684 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1685 TrackerWrapper *tracker = it_tracker->second;
1686 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
1687 if (it_T != mapOfT.end())
1688 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
1690 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1691 tracker->getPose(
cMo);
1697 std::vector<std::string> vectorOfMissingCameraPoses;
1702 it_img = mapOfImages.find(it_tracker->first);
1703 it_initFile = mapOfInitFiles.find(it_tracker->first);
1705 if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1707 TrackerWrapper *tracker = it_tracker->second;
1708 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1710 vectorOfMissingCameraPoses.push_back(it_tracker->first);
1716 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
1717 it != vectorOfMissingCameraPoses.end(); ++it) {
1718 it_img = mapOfImages.find(*it);
1719 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1728 "Missing image or missing camera transformation "
1729 "matrix! Cannot set the pose for camera: %s!",
1737 const int ,
const std::string & )
1782 const std::string &initFile1,
const std::string &initFile2)
1785 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1786 TrackerWrapper *tracker = it->second;
1787 tracker->initFromPoints(I1, initFile1);
1791 tracker = it->second;
1792 tracker->initFromPoints(I2, initFile2);
1796 tracker = it->second;
1799 tracker->getPose(
cMo);
1802 tracker->getCameraParameters(
cam);
1806 "Cannot initFromPoints()! Require two cameras but "
1807 "there are %d cameras!",
1813 const std::map<std::string, std::string> &mapOfInitPoints)
1817 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1819 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
1821 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
1822 TrackerWrapper *tracker = it_tracker->second;
1823 tracker->initFromPoints(*it_img->second, it_initPoints->second);
1824 tracker->getPose(
cMo);
1830 std::vector<std::string> vectorOfMissingCameraPoints;
1834 it_img = mapOfImages.find(it_tracker->first);
1835 it_initPoints = mapOfInitPoints.find(it_tracker->first);
1837 if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
1839 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
1841 vectorOfMissingCameraPoints.push_back(it_tracker->first);
1845 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
1846 it != vectorOfMissingCameraPoints.end(); ++it) {
1847 it_img = mapOfImages.find(*it);
1848 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1856 "Missing image or missing camera transformation "
1857 "matrix! Cannot init the pose for camera: %s!",
1875 const std::string &initFile1,
const std::string &initFile2)
1878 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1879 TrackerWrapper *tracker = it->second;
1880 tracker->initFromPose(I1, initFile1);
1884 tracker = it->second;
1885 tracker->initFromPose(I2, initFile2);
1889 tracker = it->second;
1892 tracker->getPose(
cMo);
1895 tracker->getCameraParameters(
cam);
1899 "Cannot initFromPose()! Require two cameras but there "
1920 const std::map<std::string, std::string> &mapOfInitPoses)
1924 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1926 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
1928 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
1929 TrackerWrapper *tracker = it_tracker->second;
1930 tracker->initFromPose(*it_img->second, it_initPose->second);
1931 tracker->getPose(
cMo);
1937 std::vector<std::string> vectorOfMissingCameraPoses;
1941 it_img = mapOfImages.find(it_tracker->first);
1942 it_initPose = mapOfInitPoses.find(it_tracker->first);
1944 if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
1946 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
1948 vectorOfMissingCameraPoses.push_back(it_tracker->first);
1952 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
1953 it != vectorOfMissingCameraPoses.end(); ++it) {
1954 it_img = mapOfImages.find(*it);
1955 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1963 "Missing image or missing camera transformation "
1964 "matrix! Cannot init the pose for camera: %s!",
1984 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1985 it->second->initFromPose(I1, c1Mo);
1989 it->second->initFromPose(I2, c2Mo);
1994 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2012 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2016 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2018 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2020 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2021 TrackerWrapper *tracker = it_tracker->second;
2022 tracker->initFromPose(*it_img->second, it_camPose->second);
2023 tracker->getPose(
cMo);
2029 std::vector<std::string> vectorOfMissingCameraPoses;
2033 it_img = mapOfImages.find(it_tracker->first);
2034 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2036 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2038 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2040 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2044 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2045 it != vectorOfMissingCameraPoses.end(); ++it) {
2046 it_img = mapOfImages.find(*it);
2047 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2055 "Missing image or missing camera transformation "
2056 "matrix! Cannot set the pose for camera: %s!",
2079 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2081 TrackerWrapper *tracker = it->second;
2082 tracker->loadConfigFile(configFile);
2116 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2117 TrackerWrapper *tracker = it_tracker->second;
2118 tracker->loadConfigFile(configFile1);
2121 tracker = it_tracker->second;
2122 tracker->loadConfigFile(configFile2);
2150 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2152 TrackerWrapper *tracker = it_tracker->second;
2154 std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
2155 if (it_config != mapOfConfigFiles.end()) {
2156 tracker->loadConfigFile(it_config->second);
2159 it_tracker->first.c_str());
2166 TrackerWrapper *tracker = it->second;
2167 tracker->getCameraParameters(
cam);
2211 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2213 TrackerWrapper *tracker = it->second;
2214 tracker->loadModel(modelFile, verbose, T);
2259 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2260 TrackerWrapper *tracker = it_tracker->second;
2261 tracker->loadModel(modelFile1, verbose, T1);
2264 tracker = it_tracker->second;
2265 tracker->loadModel(modelFile2, verbose, T2);
2300 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2302 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2304 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
2306 if (it_model != mapOfModelFiles.end()) {
2307 TrackerWrapper *tracker = it_tracker->second;
2308 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2310 if (it_T != mapOfT.end())
2311 tracker->loadModel(it_model->second, verbose, it_T->second);
2313 tracker->loadModel(it_model->second, verbose);
2316 it_tracker->first.c_str());
2321 #ifdef VISP_HAVE_PCL
2323 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
2325 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2327 TrackerWrapper *tracker = it->second;
2328 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
2334 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
2335 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
2336 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
2338 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2340 TrackerWrapper *tracker = it->second;
2341 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
2342 mapOfPointCloudHeights[it->first]);
2368 TrackerWrapper *tracker = it_tracker->second;
2369 tracker->reInitModel(I, cad_name, cMo_, verbose, T);
2372 tracker->getPose(
cMo);
2401 const std::string &cad_name1,
const std::string &cad_name2,
2407 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2409 it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
2413 it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
2418 it_tracker->second->getPose(
cMo);
2442 const std::map<std::string, std::string> &mapOfModelFiles,
2443 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
2445 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2448 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2450 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
2451 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2453 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
2454 it_camPose != mapOfCameraPoses.end()) {
2455 TrackerWrapper *tracker = it_tracker->second;
2456 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2457 if (it_T != mapOfT.end())
2458 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
2460 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
2463 tracker->getPose(
cMo);
2468 std::vector<std::string> vectorOfMissingCameras;
2471 it_img = mapOfImages.find(it_tracker->first);
2472 it_model = mapOfModelFiles.find(it_tracker->first);
2473 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2475 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
2476 TrackerWrapper *tracker = it_tracker->second;
2477 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
2479 vectorOfMissingCameras.push_back(it_tracker->first);
2484 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
2486 it_img = mapOfImages.find(*it);
2487 it_model = mapOfModelFiles.find(*it);
2488 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2491 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
2494 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
2511 #ifdef VISP_HAVE_OGRE
2538 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
2545 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2547 TrackerWrapper *tracker = it->second;
2548 tracker->resetTracker();
2565 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2567 TrackerWrapper *tracker = it->second;
2568 tracker->setAngleAppear(a);
2587 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2588 it->second->setAngleAppear(a1);
2591 it->second->setAngleAppear(a2);
2616 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
2617 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
2620 TrackerWrapper *tracker = it_tracker->second;
2621 tracker->setAngleAppear(it->second);
2643 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2645 TrackerWrapper *tracker = it->second;
2646 tracker->setAngleDisappear(a);
2665 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2666 it->second->setAngleDisappear(a1);
2669 it->second->setAngleDisappear(a2);
2694 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
2695 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
2698 TrackerWrapper *tracker = it_tracker->second;
2699 tracker->setAngleDisappear(it->second);
2717 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2719 TrackerWrapper *tracker = it->second;
2720 tracker->setCameraParameters(camera);
2735 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2736 it->second->setCameraParameters(camera1);
2739 it->second->setCameraParameters(camera2);
2743 it->second->getCameraParameters(
cam);
2763 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
2764 it != mapOfCameraParameters.end(); ++it) {
2765 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
2768 TrackerWrapper *tracker = it_tracker->second;
2769 tracker->setCameraParameters(it->second);
2792 it->second = cameraTransformationMatrix;
2806 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
2808 for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
2809 it != mapOfTransformationMatrix.end(); ++it) {
2810 std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
2814 it_camTrans->second = it->second;
2832 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2834 TrackerWrapper *tracker = it->second;
2835 tracker->setClipping(flags);
2852 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2853 it->second->setClipping(flags1);
2856 it->second->setClipping(flags2);
2865 std::stringstream ss;
2866 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
2880 for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
2881 it != mapOfClippingFlags.end(); ++it) {
2882 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
2885 TrackerWrapper *tracker = it_tracker->second;
2886 tracker->setClipping(it->second);
2906 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2908 TrackerWrapper *tracker = it->second;
2909 tracker->setDepthDenseFilteringMaxDistance(maxDistance);
2923 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2925 TrackerWrapper *tracker = it->second;
2926 tracker->setDepthDenseFilteringMethod(method);
2941 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2943 TrackerWrapper *tracker = it->second;
2944 tracker->setDepthDenseFilteringMinDistance(minDistance);
2959 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2961 TrackerWrapper *tracker = it->second;
2962 tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
2976 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2978 TrackerWrapper *tracker = it->second;
2979 tracker->setDepthDenseSamplingStep(stepX, stepY);
2992 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2994 TrackerWrapper *tracker = it->second;
2995 tracker->setDepthNormalFaceCentroidMethod(method);
3009 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3011 TrackerWrapper *tracker = it->second;
3012 tracker->setDepthNormalFeatureEstimationMethod(method);
3025 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3027 TrackerWrapper *tracker = it->second;
3028 tracker->setDepthNormalPclPlaneEstimationMethod(method);
3041 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3043 TrackerWrapper *tracker = it->second;
3044 tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
3057 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3059 TrackerWrapper *tracker = it->second;
3060 tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
3074 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3076 TrackerWrapper *tracker = it->second;
3077 tracker->setDepthNormalSamplingStep(stepX, stepY);
3103 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3105 TrackerWrapper *tracker = it->second;
3106 tracker->setDisplayFeatures(displayF);
3121 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3123 TrackerWrapper *tracker = it->second;
3124 tracker->setFarClippingDistance(dist);
3139 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3140 it->second->setFarClippingDistance(dist1);
3143 it->second->setFarClippingDistance(dist2);
3147 distFarClip = it->second->getFarClippingDistance();
3164 for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
3166 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3169 TrackerWrapper *tracker = it_tracker->second;
3170 tracker->setFarClippingDistance(it->second);
3189 std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
3190 if (it_factor != mapOfFeatureFactors.end()) {
3191 it->second = it_factor->second;
3215 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3217 TrackerWrapper *tracker = it->second;
3218 tracker->setGoodMovingEdgesRatioThreshold(threshold);
3222 #ifdef VISP_HAVE_OGRE
3236 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3238 TrackerWrapper *tracker = it->second;
3239 tracker->setGoodNbRayCastingAttemptsRatio(ratio);
3256 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3258 TrackerWrapper *tracker = it->second;
3259 tracker->setNbRayCastingAttemptsForVisibility(attempts);
3264 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3274 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3276 TrackerWrapper *tracker = it->second;
3277 tracker->setKltOpencv(t);
3292 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3293 it->second->setKltOpencv(t1);
3296 it->second->setKltOpencv(t2);
3310 for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
3311 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3314 TrackerWrapper *tracker = it_tracker->second;
3315 tracker->setKltOpencv(it->second);
3331 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3333 TrackerWrapper *tracker = it->second;
3334 tracker->setKltThresholdAcceptation(th);
3353 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3355 TrackerWrapper *tracker = it->second;
3356 tracker->setLod(useLod, name);
3360 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3370 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3372 TrackerWrapper *tracker = it->second;
3373 tracker->setKltMaskBorder(e);
3388 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3389 it->second->setKltMaskBorder(e1);
3393 it->second->setKltMaskBorder(e2);
3407 for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
3409 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3412 TrackerWrapper *tracker = it_tracker->second;
3413 tracker->setKltMaskBorder(it->second);
3428 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3430 TrackerWrapper *tracker = it->second;
3431 tracker->setMask(mask);
3449 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3451 TrackerWrapper *tracker = it->second;
3452 tracker->setMinLineLengthThresh(minLineLengthThresh, name);
3468 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3470 TrackerWrapper *tracker = it->second;
3471 tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
3484 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3486 TrackerWrapper *tracker = it->second;
3487 tracker->setMovingEdge(me);
3503 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3504 it->second->setMovingEdge(me1);
3508 it->second->setMovingEdge(me2);
3522 for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
3523 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3526 TrackerWrapper *tracker = it_tracker->second;
3527 tracker->setMovingEdge(it->second);
3543 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3545 TrackerWrapper *tracker = it->second;
3546 tracker->setNearClippingDistance(dist);
3561 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3562 it->second->setNearClippingDistance(dist1);
3566 it->second->setNearClippingDistance(dist2);
3587 for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
3588 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3591 TrackerWrapper *tracker = it_tracker->second;
3592 tracker->setNearClippingDistance(it->second);
3617 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3619 TrackerWrapper *tracker = it->second;
3620 tracker->setOgreShowConfigDialog(showConfigDialog);
3638 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3640 TrackerWrapper *tracker = it->second;
3641 tracker->setOgreVisibilityTest(v);
3644 #ifdef VISP_HAVE_OGRE
3645 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3647 TrackerWrapper *tracker = it->second;
3648 tracker->faces.getOgreContext()->setWindowName(
"Multi Generic MBT (" + it->first +
")");
3664 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3666 TrackerWrapper *tracker = it->second;
3667 tracker->setOptimizationMethod(opt);
3687 "to be configured with only one camera!");
3694 TrackerWrapper *tracker = it->second;
3695 tracker->setPose(I, cdMo);
3717 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3718 it->second->setPose(I1, c1Mo);
3722 it->second->setPose(I2, c2Mo);
3727 it->second->getPose(
cMo);
3754 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
3758 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3760 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3762 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
3763 TrackerWrapper *tracker = it_tracker->second;
3764 tracker->setPose(*it_img->second, it_camPose->second);
3765 tracker->getPose(
cMo);
3771 std::vector<std::string> vectorOfMissingCameraPoses;
3776 it_img = mapOfImages.find(it_tracker->first);
3777 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3779 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
3781 TrackerWrapper *tracker = it_tracker->second;
3782 tracker->setPose(*it_img->second, it_camPose->second);
3784 vectorOfMissingCameraPoses.push_back(it_tracker->first);
3789 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
3790 it != vectorOfMissingCameraPoses.end(); ++it) {
3791 it_img = mapOfImages.find(*it);
3792 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3800 "Missing image or missing camera transformation "
3801 "matrix! Cannot set pose for camera: %s!",
3825 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3827 TrackerWrapper *tracker = it->second;
3828 tracker->setProjectionErrorComputation(flag);
3839 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3841 TrackerWrapper *tracker = it->second;
3842 tracker->setProjectionErrorDisplay(
display);
3853 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3855 TrackerWrapper *tracker = it->second;
3856 tracker->setProjectionErrorDisplayArrowLength(length);
3864 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3866 TrackerWrapper *tracker = it->second;
3867 tracker->setProjectionErrorDisplayArrowThickness(thickness);
3878 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(referenceCameraName);
3882 std::cerr <<
"The reference camera: " << referenceCameraName <<
" does not exist!";
3890 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3892 TrackerWrapper *tracker = it->second;
3893 tracker->setScanLineVisibilityTest(v);
3910 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3912 TrackerWrapper *tracker = it->second;
3913 tracker->setTrackerType(type);
3928 for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
3929 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3931 TrackerWrapper *tracker = it_tracker->second;
3932 tracker->setTrackerType(it->second);
3948 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3950 TrackerWrapper *tracker = it->second;
3951 tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
3966 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3968 TrackerWrapper *tracker = it->second;
3969 tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
3984 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3986 TrackerWrapper *tracker = it->second;
3987 tracker->setUseEdgeTracking(name, useEdgeTracking);
3991 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4003 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4005 TrackerWrapper *tracker = it->second;
4006 tracker->setUseKltTracking(name, useKltTracking);
4014 bool isOneTestTrackingOk =
false;
4015 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4017 TrackerWrapper *tracker = it->second;
4019 tracker->testTracking();
4020 isOneTestTrackingOk =
true;
4025 if (!isOneTestTrackingOk) {
4026 std::ostringstream oss;
4027 oss <<
"Not enough moving edges to track the object. Try to reduce the "
4045 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
4048 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
4049 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
4051 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
4067 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4068 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
4069 mapOfImages[it->first] = &I1;
4072 mapOfImages[it->first] = &I2;
4074 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
4075 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
4077 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
4079 std::stringstream ss;
4080 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
4094 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
4095 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
4097 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
4100 #ifdef VISP_HAVE_PCL
4110 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
4112 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4114 TrackerWrapper *tracker = it->second;
4117 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4125 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4129 mapOfImages[it->first] == NULL) {
4134 mapOfPointClouds[it->first] ==
nullptr) {
4150 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4152 TrackerWrapper *tracker = it->second;
4154 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
4172 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
4173 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
4174 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
4176 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4178 TrackerWrapper *tracker = it->second;
4181 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4189 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4193 mapOfImages[it->first] == NULL) {
4198 (mapOfPointClouds[it->first] == NULL)) {
4203 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
4214 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4216 TrackerWrapper *tracker = it->second;
4218 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
4225 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
4226 : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
4231 #ifdef VISP_HAVE_OGRE
4238 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
const int trackerType)
4239 : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
4242 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4252 #ifdef VISP_HAVE_OGRE
4259 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
4264 computeVVSInit(ptr_I);
4266 if (m_error.getRows() < 4) {
4271 double normRes_1 = -1;
4272 unsigned int iter = 0;
4274 double factorEdge = 1.0;
4275 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4276 double factorKlt = 1.0;
4278 double factorDepth = 1.0;
4279 double factorDepthDense = 1.0;
4285 double mu = m_initialMu;
4287 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4290 bool isoJoIdentity_ =
true;
4296 unsigned int nb_edge_features = m_error_edge.
getRows();
4297 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4298 unsigned int nb_klt_features = m_error_klt.getRows();
4300 unsigned int nb_depth_features = m_error_depthNormal.getRows();
4301 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
4303 while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
4304 computeVVSInteractionMatrixAndResidu(ptr_I);
4306 bool reStartFromLastIncrement =
false;
4307 computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
4309 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4310 if (reStartFromLastIncrement) {
4311 if (m_trackerType & KLT_TRACKER) {
4317 if (!reStartFromLastIncrement) {
4318 computeVVSWeights();
4320 if (computeCovariance) {
4322 if (!isoJoIdentity_) {
4325 LVJ_true = (m_L * cVo * oJo);
4331 isoJoIdentity_ =
true;
4338 if (isoJoIdentity_) {
4342 unsigned int rank = (m_L * cVo).kernel(K);
4352 isoJoIdentity_ =
false;
4361 unsigned int start_index = 0;
4362 if (m_trackerType & EDGE_TRACKER) {
4363 for (
unsigned int i = 0; i < nb_edge_features; i++) {
4364 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
4366 m_weightedError[i] = wi * m_error[i];
4371 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
4376 start_index += nb_edge_features;
4379 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4380 if (m_trackerType & KLT_TRACKER) {
4381 for (
unsigned int i = 0; i < nb_klt_features; i++) {
4382 double wi = m_w_klt[i] * factorKlt;
4383 W_true[start_index + i] = wi;
4384 m_weightedError[start_index + i] = wi * m_error_klt[i];
4386 num += wi *
vpMath::sqr(m_error[start_index + i]);
4389 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
4390 m_L[start_index + i][j] *= wi;
4394 start_index += nb_klt_features;
4398 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4399 for (
unsigned int i = 0; i < nb_depth_features; i++) {
4400 double wi = m_w_depthNormal[i] * factorDepth;
4401 m_w[start_index + i] = m_w_depthNormal[i];
4402 m_weightedError[start_index + i] = wi * m_error[start_index + i];
4404 num += wi *
vpMath::sqr(m_error[start_index + i]);
4407 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
4408 m_L[start_index + i][j] *= wi;
4412 start_index += nb_depth_features;
4415 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4416 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
4417 double wi = m_w_depthDense[i] * factorDepthDense;
4418 m_w[start_index + i] = m_w_depthDense[i];
4419 m_weightedError[start_index + i] = wi * m_error[start_index + i];
4421 num += wi *
vpMath::sqr(m_error[start_index + i]);
4424 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
4425 m_L[start_index + i][j] *= wi;
4432 computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
4435 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4436 if (m_trackerType & KLT_TRACKER) {
4443 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4444 if (m_trackerType & KLT_TRACKER) {
4448 normRes_1 = normRes;
4450 normRes = sqrt(num / den);
4456 computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
4458 if (m_trackerType & EDGE_TRACKER) {
4463 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
4466 "TrackerWrapper::computeVVSInit("
4467 ") should not be called!");
4472 initMbtTracking(ptr_I);
4474 unsigned int nbFeatures = 0;
4476 if (m_trackerType & EDGE_TRACKER) {
4477 nbFeatures += m_error_edge.getRows();
4479 m_error_edge.clear();
4480 m_weightedError_edge.clear();
4485 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4486 if (m_trackerType & KLT_TRACKER) {
4488 nbFeatures += m_error_klt.getRows();
4490 m_error_klt.clear();
4491 m_weightedError_klt.clear();
4497 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4499 nbFeatures += m_error_depthNormal.getRows();
4501 m_error_depthNormal.clear();
4502 m_weightedError_depthNormal.clear();
4503 m_L_depthNormal.clear();
4504 m_w_depthNormal.clear();
4507 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4509 nbFeatures += m_error_depthDense.getRows();
4511 m_error_depthDense.clear();
4512 m_weightedError_depthDense.clear();
4513 m_L_depthDense.clear();
4514 m_w_depthDense.clear();
4517 m_L.resize(nbFeatures, 6,
false,
false);
4518 m_error.resize(nbFeatures,
false);
4520 m_weightedError.resize(nbFeatures,
false);
4521 m_w.resize(nbFeatures,
false);
4529 "computeVVSInteractionMatrixAndR"
4530 "esidu() should not be called!");
4535 if (m_trackerType & EDGE_TRACKER) {
4539 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4540 if (m_trackerType & KLT_TRACKER) {
4545 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4549 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4553 unsigned int start_index = 0;
4554 if (m_trackerType & EDGE_TRACKER) {
4555 m_L.insert(m_L_edge, start_index, 0);
4556 m_error.insert(start_index, m_error_edge);
4558 start_index += m_error_edge.getRows();
4561 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4562 if (m_trackerType & KLT_TRACKER) {
4563 m_L.insert(m_L_klt, start_index, 0);
4564 m_error.insert(start_index, m_error_klt);
4566 start_index += m_error_klt.getRows();
4570 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4571 m_L.insert(m_L_depthNormal, start_index, 0);
4572 m_error.insert(start_index, m_error_depthNormal);
4574 start_index += m_error_depthNormal.getRows();
4577 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4578 m_L.insert(m_L_depthDense, start_index, 0);
4579 m_error.insert(start_index, m_error_depthDense);
4587 unsigned int start_index = 0;
4589 if (m_trackerType & EDGE_TRACKER) {
4591 m_w.insert(start_index, m_w_edge);
4593 start_index += m_w_edge.getRows();
4596 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4597 if (m_trackerType & KLT_TRACKER) {
4599 m_w.insert(start_index, m_w_klt);
4601 start_index += m_w_klt.getRows();
4605 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4606 if (m_depthNormalUseRobust) {
4608 m_w.insert(start_index, m_w_depthNormal);
4611 start_index += m_w_depthNormal.getRows();
4614 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4616 m_w.insert(start_index, m_w_depthDense);
4624 const unsigned int thickness,
const bool displayFullModel)
4626 if (m_trackerType == EDGE_TRACKER) {
4628 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4629 }
else if (m_trackerType == KLT_TRACKER) {
4632 }
else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
4634 }
else if (m_trackerType == DEPTH_DENSE_TRACKER) {
4637 if (m_trackerType & EDGE_TRACKER) {
4638 for (
unsigned int i = 0; i < scales.size(); i += 1) {
4640 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
4641 it != lines[scaleLevel].end(); ++it) {
4642 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4645 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
4646 it != cylinders[scaleLevel].end(); ++it) {
4647 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4650 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
4651 it != circles[scaleLevel].end(); ++it) {
4652 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4660 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4661 if (m_trackerType & KLT_TRACKER) {
4662 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
4670 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
4679 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4683 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4687 #ifdef VISP_HAVE_OGRE
4689 faces.displayOgre(cMo_);
4696 const unsigned int thickness,
const bool displayFullModel)
4698 if (m_trackerType == EDGE_TRACKER) {
4700 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4701 }
else if (m_trackerType == KLT_TRACKER) {
4704 }
else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
4706 }
else if (m_trackerType == DEPTH_DENSE_TRACKER) {
4709 if (m_trackerType & EDGE_TRACKER) {
4710 for (
unsigned int i = 0; i < scales.size(); i += 1) {
4712 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
4713 it != lines[scaleLevel].end(); ++it) {
4714 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4717 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
4718 it != cylinders[scaleLevel].end(); ++it) {
4719 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4722 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
4723 it != circles[scaleLevel].end(); ++it) {
4724 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4732 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4733 if (m_trackerType & KLT_TRACKER) {
4734 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
4742 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
4751 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4755 if (m_trackerType & DEPTH_DENSE_TRACKER) {
4759 #ifdef VISP_HAVE_OGRE
4761 faces.displayOgre(cMo_);
4768 if (!modelInitialised) {
4772 if (useScanLine || clippingFlag > 3)
4775 bool reInitialisation =
false;
4777 faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
4779 #ifdef VISP_HAVE_OGRE
4780 if (!faces.isOgreInitialised()) {
4783 faces.setOgreShowConfigDialog(ogreShowConfigDialog);
4784 faces.initOgre(cam);
4788 ogreShowConfigDialog =
false;
4791 faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
4793 faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
4798 faces.computeClippedPolygons(cMo, cam);
4802 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4803 if (m_trackerType & KLT_TRACKER)
4807 if (m_trackerType & EDGE_TRACKER) {
4813 initMovingEdge(I, cMo);
4816 if (m_trackerType & DEPTH_NORMAL_TRACKER)
4819 if (m_trackerType & DEPTH_DENSE_TRACKER)
4823 void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
4824 const double radius,
const int idFace,
const std::string &name)
4826 if (m_trackerType & EDGE_TRACKER)
4829 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4830 if (m_trackerType & KLT_TRACKER)
4835 void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
const double radius,
4836 const int idFace,
const std::string &name)
4838 if (m_trackerType & EDGE_TRACKER)
4841 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4842 if (m_trackerType & KLT_TRACKER)
4847 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
4849 if (m_trackerType & EDGE_TRACKER)
4852 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4853 if (m_trackerType & KLT_TRACKER)
4857 if (m_trackerType & DEPTH_NORMAL_TRACKER)
4860 if (m_trackerType & DEPTH_DENSE_TRACKER)
4864 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
4866 if (m_trackerType & EDGE_TRACKER)
4869 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4870 if (m_trackerType & KLT_TRACKER)
4874 if (m_trackerType & DEPTH_NORMAL_TRACKER)
4877 if (m_trackerType & DEPTH_DENSE_TRACKER)
4883 if (m_trackerType & EDGE_TRACKER) {
4889 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile)
4894 #ifdef VISP_HAVE_XML2
4897 xmlp.setCameraParameters(cam);
4899 xmlp.setAngleDisappear(
vpMath::deg(angleDisappears));
4905 xmlp.setKltMaxFeatures(10000);
4906 xmlp.setKltWindowSize(5);
4907 xmlp.setKltQuality(0.01);
4908 xmlp.setKltMinDistance(5);
4909 xmlp.setKltHarrisParam(0.01);
4910 xmlp.setKltBlockSize(3);
4911 xmlp.setKltPyramidLevels(3);
4912 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4913 xmlp.setKltMaskBorder(maskBorder);
4917 xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
4918 xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
4919 xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
4920 xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
4921 xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
4922 xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
4925 xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
4926 xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
4930 std::cout <<
" *********** Parsing XML for";
4932 std::vector<std::string> tracker_names;
4933 if (m_trackerType & EDGE_TRACKER)
4934 tracker_names.push_back(
"Edge");
4935 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4936 if (m_trackerType & KLT_TRACKER)
4937 tracker_names.push_back(
"Klt");
4939 if (m_trackerType & DEPTH_NORMAL_TRACKER)
4940 tracker_names.push_back(
"Depth Normal");
4941 if (m_trackerType & DEPTH_DENSE_TRACKER)
4942 tracker_names.push_back(
"Depth Dense");
4944 for (
size_t i = 0; i < tracker_names.size(); i++) {
4945 std::cout <<
" " << tracker_names[i];
4946 if (i == tracker_names.size() - 1) {
4951 std::cout <<
"Model-Based Tracker ************ " << std::endl;
4952 xmlp.parse(configFile);
4958 xmlp.getCameraParameters(camera);
4959 setCameraParameters(camera);
4961 angleAppears =
vpMath::rad(xmlp.getAngleAppear());
4962 angleDisappears =
vpMath::rad(xmlp.getAngleDisappear());
4964 if (xmlp.hasNearClippingDistance())
4965 setNearClippingDistance(xmlp.getNearClippingDistance());
4967 if (xmlp.hasFarClippingDistance())
4968 setFarClippingDistance(xmlp.getFarClippingDistance());
4970 if (xmlp.getFovClipping()) {
4974 useLodGeneral = xmlp.getLodState();
4975 minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
4976 minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
4978 applyLodSettingInConfig =
false;
4979 if (this->getNbPolygon() > 0) {
4980 applyLodSettingInConfig =
true;
4981 setLod(useLodGeneral);
4982 setMinLineLengthThresh(minLineLengthThresholdGeneral);
4983 setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
4988 xmlp.getEdgeMe(meParser);
4992 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4993 tracker.setMaxFeatures((
int)xmlp.getKltMaxFeatures());
4994 tracker.setWindowSize((
int)xmlp.getKltWindowSize());
4995 tracker.setQuality(xmlp.getKltQuality());
4996 tracker.setMinDistance(xmlp.getKltMinDistance());
4997 tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
4998 tracker.setBlockSize((
int)xmlp.getKltBlockSize());
4999 tracker.setPyramidLevels((
int)xmlp.getKltPyramidLevels());
5000 maskBorder = xmlp.getKltMaskBorder();
5003 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
5007 setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod());
5008 setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod());
5009 setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter());
5010 setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold());
5011 setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY());
5014 setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
5016 std::cerr <<
"You need the libXML2 to read the config file: " << configFile << std::endl;
5020 #ifdef VISP_HAVE_PCL
5022 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
5024 if (displayFeatures) {
5025 if (m_trackerType & EDGE_TRACKER) {
5030 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5032 if (m_trackerType & KLT_TRACKER) {
5040 if (m_trackerType & EDGE_TRACKER) {
5041 bool newvisibleface =
false;
5045 faces.computeClippedPolygons(cMo, cam);
5051 if (m_trackerType & DEPTH_NORMAL_TRACKER)
5055 if (m_trackerType & DEPTH_DENSE_TRACKER)
5059 if (m_trackerType & EDGE_TRACKER) {
5066 if (computeProjError) {
5073 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
5075 if (m_trackerType & EDGE_TRACKER) {
5079 std::cerr <<
"Error in moving edge tracking" << std::endl;
5084 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5085 if (m_trackerType & KLT_TRACKER) {
5089 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
5095 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5099 std::cerr <<
"Error in Depth normal tracking" << std::endl;
5104 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5108 std::cerr <<
"Error in Depth dense tracking" << std::endl;
5116 const unsigned int pointcloud_width,
5117 const unsigned int pointcloud_height)
5119 if (displayFeatures) {
5120 if (m_trackerType & EDGE_TRACKER) {
5125 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5127 if (m_trackerType & KLT_TRACKER) {
5135 if (m_trackerType & EDGE_TRACKER) {
5136 bool newvisibleface =
false;
5140 faces.computeClippedPolygons(cMo, cam);
5146 if (m_trackerType & DEPTH_NORMAL_TRACKER)
5150 if (m_trackerType & DEPTH_DENSE_TRACKER)
5154 if (m_trackerType & EDGE_TRACKER) {
5161 if (computeProjError) {
5168 const std::vector<vpColVector> *
const point_cloud,
5169 const unsigned int pointcloud_width,
5170 const unsigned int pointcloud_height)
5172 if (m_trackerType & EDGE_TRACKER) {
5176 std::cerr <<
"Error in moving edge tracking" << std::endl;
5181 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5182 if (m_trackerType & KLT_TRACKER) {
5186 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
5192 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5196 std::cerr <<
"Error in Depth tracking" << std::endl;
5201 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5205 std::cerr <<
"Error in Depth dense tracking" << std::endl;
5222 for (
unsigned int i = 0; i < scales.size(); i++) {
5224 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
5231 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
5239 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
5247 cylinders[i].clear();
5255 nbvisiblepolygone = 0;
5258 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5259 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
5261 cvReleaseImage(&cur);
5267 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
5269 if (kltpoly != NULL) {
5274 kltPolygons.clear();
5276 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
5279 if (kltPolyCylinder != NULL) {
5280 delete kltPolyCylinder;
5282 kltPolyCylinder = NULL;
5284 kltCylinders.clear();
5287 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
5294 circles_disp.clear();
5296 firstInitialisation =
true;
5301 for (
size_t i = 0; i < m_depthNormalFaces.size(); i++) {
5302 delete m_depthNormalFaces[i];
5303 m_depthNormalFaces[i] = NULL;
5305 m_depthNormalFaces.clear();
5308 for (
size_t i = 0; i < m_depthDenseFaces.size(); i++) {
5309 delete m_depthDenseFaces[i];
5310 m_depthDenseFaces[i] = NULL;
5312 m_depthDenseFaces.clear();
5316 loadModel(cad_name, verbose, T);
5317 initFromPose(I, cMo_);
5320 void vpMbGenericTracker::TrackerWrapper::resetTracker()
5323 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5330 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(
const vpCameraParameters &camera)
5335 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5342 void vpMbGenericTracker::TrackerWrapper::setClipping(
const unsigned int &flags)
5347 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(
const double &dist)
5352 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(
const double &dist)
5357 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(
const bool &v)
5360 #ifdef VISP_HAVE_OGRE
5361 faces.getOgreContext()->setWindowName(
"TrackerWrapper");
5367 bool performKltSetPose =
false;
5369 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5370 if (m_trackerType & KLT_TRACKER) {
5371 performKltSetPose =
true;
5373 if (useScanLine || clippingFlag > 3)
5380 if (!performKltSetPose) {
5386 if (m_trackerType & EDGE_TRACKER)
5390 faces.computeClippedPolygons(cMo, cam);
5395 if (m_trackerType & EDGE_TRACKER) {
5396 initPyramid(I, Ipyramid);
5398 unsigned int i = (
unsigned int) scales.size();
5403 initMovingEdge(*Ipyramid[i], cMo);
5408 cleanPyramid(Ipyramid);
5411 if (m_trackerType & EDGE_TRACKER)
5412 initMovingEdge(I, cMo);
5422 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(
const bool &flag)
5427 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(
const bool &v)
5430 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5437 void vpMbGenericTracker::TrackerWrapper::setTrackerType(
const int type)
5439 if ((type & (EDGE_TRACKER |
5440 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5443 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
5447 m_trackerType = type;
5450 void vpMbGenericTracker::TrackerWrapper::testTracking()
5452 if (m_trackerType & EDGE_TRACKER) {
5458 #ifdef VISP_HAVE_PCL
5463 if ((m_trackerType & (EDGE_TRACKER
5464 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5468 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
5472 #ifdef VISP_HAVE_PCL
5477 #ifdef VISP_HAVE_PCL
5479 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
5481 if ((m_trackerType & (EDGE_TRACKER |
5482 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5485 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
5486 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
5490 if (m_trackerType & (EDGE_TRACKER
5491 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5499 if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && point_cloud ==
nullptr) {
5506 preTracking(ptr_I, point_cloud);
5511 covarianceMatrix = -1;
5515 if (m_trackerType == EDGE_TRACKER)
5518 postTracking(ptr_I, point_cloud);
5521 std::cerr <<
"Exception: " << e.
what() << std::endl;