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);
249 double rawTotalProjectionError = 0.0;
250 unsigned int nbTotalFeaturesUsed = 0;
252 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
254 TrackerWrapper *tracker = it->second;
256 double curProjError = tracker->getProjectionError();
257 unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
259 if (nbFeaturesUsed > 0) {
260 nbTotalFeaturesUsed += nbFeaturesUsed;
261 rawTotalProjectionError += (
vpMath::rad(curProjError) * nbFeaturesUsed);
265 if (nbTotalFeaturesUsed > 0) {
284 double normRes_1 = -1;
285 unsigned int iter = 0;
294 bool isoJoIdentity_ =
true;
301 std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
306 mapOfVelocityTwist[it->first] = cVo;
310 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
319 bool reStartFromLastIncrement =
false;
321 if (reStartFromLastIncrement) {
322 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
324 TrackerWrapper *tracker = it->second;
328 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
331 tracker->ctTc0 = c_curr_tTc_curr0;
336 if (!reStartFromLastIncrement) {
341 if (!isoJoIdentity_) {
344 LVJ_true = (
m_L * (cVo *
oJo));
350 isoJoIdentity_ =
true;
357 if (isoJoIdentity_) {
361 unsigned int rank = (
m_L * cVo).kernel(K);
371 isoJoIdentity_ =
false;
380 unsigned int start_index = 0;
381 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
383 TrackerWrapper *tracker = it->second;
386 for (
unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
387 double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
388 W_true[start_index + i] = wi;
394 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
395 m_L[start_index + i][j] *= wi;
399 start_index += tracker->m_error_edge.
getRows();
402 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
404 for (
unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
405 double wi = tracker->m_w_klt[i] * factorKlt;
406 W_true[start_index + i] = wi;
412 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
413 m_L[start_index + i][j] *= wi;
417 start_index += tracker->m_error_klt.
getRows();
422 for (
unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
423 double wi = tracker->m_w_depthNormal[i] * factorDepth;
424 W_true[start_index + i] = wi;
430 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
431 m_L[start_index + i][j] *= wi;
435 start_index += tracker->m_error_depthNormal.
getRows();
439 for (
unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
440 double wi = tracker->m_w_depthDense[i] * factorDepthDense;
441 W_true[start_index + i] = wi;
447 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
448 m_L[start_index + i][j] *= wi;
452 start_index += tracker->m_error_depthDense.
getRows();
457 normRes = sqrt(num / den);
465 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
466 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
468 TrackerWrapper *tracker = it->second;
472 tracker->ctTc0 = c_curr_tTc_curr0;
477 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
479 TrackerWrapper *tracker = it->second;
489 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
491 TrackerWrapper *tracker = it->second;
494 tracker->updateMovingEdgeWeights();
506 unsigned int nbFeatures = 0;
508 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
510 TrackerWrapper *tracker = it->second;
511 tracker->computeVVSInit(mapOfImages[it->first]);
513 nbFeatures += tracker->m_error.getRows();
527 "computeVVSInteractionMatrixAndR"
528 "esidu() should not be called");
533 std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
535 unsigned int start_index = 0;
537 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
539 TrackerWrapper *tracker = it->second;
542 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
544 tracker->ctTc0 = c_curr_tTc_curr0;
547 tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
549 m_L.
insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
552 start_index += tracker->m_error.getRows();
558 unsigned int start_index = 0;
560 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
562 TrackerWrapper *tracker = it->second;
563 tracker->computeVVSWeights();
566 start_index += tracker->m_w.getRows();
585 bool displayFullModel)
589 TrackerWrapper *tracker = it->second;
590 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
611 bool displayFullModel)
615 TrackerWrapper *tracker = it->second;
616 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
641 unsigned int thickness,
bool displayFullModel)
644 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
645 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
648 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
650 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
674 bool displayFullModel)
677 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
678 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
681 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
683 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
700 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
701 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
702 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
706 it_img != mapOfImages.end(); ++it_img) {
707 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
708 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
709 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
711 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
712 it_cam != mapOfCameraParameters.end()) {
713 TrackerWrapper *tracker = it_tracker->second;
714 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
716 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
733 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
734 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
735 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
738 for (std::map<std::string,
const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
739 it_img != mapOfImages.end(); ++it_img) {
740 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
741 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
742 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
744 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
745 it_cam != mapOfCameraParameters.end()) {
746 TrackerWrapper *tracker = it_tracker->second;
747 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
749 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
761 std::vector<std::string> cameraNames;
763 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
765 cameraNames.push_back(it_tracker->first);
787 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
788 it->second->getCameraParameters(cam1);
791 it->second->getCameraParameters(cam2);
793 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
806 mapOfCameraParameters.clear();
808 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
811 it->second->getCameraParameters(cam_);
812 mapOfCameraParameters[it->first] = cam_;
824 std::map<std::string, int> trackingTypes;
826 TrackerWrapper *traker;
827 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
829 traker = it_tracker->second;
830 trackingTypes[it_tracker->first] = traker->getTrackerType();
833 return trackingTypes;
847 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
848 clippingFlag1 = it->second->getClipping();
851 clippingFlag2 = it->second->getClipping();
853 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
865 mapOfClippingFlags.clear();
867 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
869 TrackerWrapper *tracker = it->second;
870 mapOfClippingFlags[it->first] = tracker->getClipping();
883 return it->second->getFaces();
897 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
899 return it->second->getFaces();
902 std::cerr <<
"The camera: " << cameraName <<
" cannot be found!" << std::endl;
906 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
914 TrackerWrapper *tracker = it->second;
915 return tracker->getFeaturesCircle();
929 TrackerWrapper *tracker = it->second;
930 return tracker->getFeaturesKltCylinder();
944 TrackerWrapper *tracker = it->second;
945 return tracker->getFeaturesKlt();
983 return it->second->getFeaturesForDisplay();
988 return std::vector<std::vector<double> >();
1017 mapOfFeatures.clear();
1019 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1021 mapOfFeatures[it->first] = it->second->getFeaturesForDisplay();
1036 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
1049 TrackerWrapper *tracker = it->second;
1050 return tracker->getKltImagePoints();
1055 return std::vector<vpImagePoint>();
1070 TrackerWrapper *tracker = it->second;
1071 return tracker->getKltImagePointsWithId();
1076 return std::map<int, vpImagePoint>();
1088 TrackerWrapper *tracker = it->second;
1089 return tracker->getKltMaskBorder();
1106 TrackerWrapper *tracker = it->second;
1107 return tracker->getKltNbPoints();
1125 TrackerWrapper *tracker;
1126 tracker = it_tracker->second;
1127 return tracker->getKltOpencv();
1146 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1147 klt1 = it->second->getKltOpencv();
1150 klt2 = it->second->getKltOpencv();
1152 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1166 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1168 TrackerWrapper *tracker = it->second;
1169 mapOfKlts[it->first] = tracker->getKltOpencv();
1173 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
1183 TrackerWrapper *tracker = it->second;
1184 return tracker->getKltPoints();
1189 return std::vector<cv::Point2f>();
1215 unsigned int level)
const
1219 it->second->getLcircle(circlesList, level);
1239 unsigned int level)
const
1241 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1243 it->second->getLcircle(circlesList, level);
1245 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1262 unsigned int level)
const
1266 it->second->getLcylinder(cylindersList, level);
1286 unsigned int level)
const
1288 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1290 it->second->getLcylinder(cylindersList, level);
1292 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1309 unsigned int level)
const
1314 it->second->getLline(linesList, level);
1334 unsigned int level)
const
1336 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1338 it->second->getLline(linesList, level);
1340 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1372 bool displayFullModel)
1377 return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1382 return std::vector<std::vector<double> >();
1412 const std::map<std::string, unsigned int> &mapOfwidths,
1413 const std::map<std::string, unsigned int> &mapOfheights,
1414 const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
1415 const std::map<std::string, vpCameraParameters> &mapOfCams,
1416 bool displayFullModel)
1419 mapOfModels.clear();
1421 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1423 std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
1424 std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
1425 std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
1426 std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
1428 if (findWidth != mapOfwidths.end() &&
1429 findHeight != mapOfheights.end() &&
1430 findcMo != mapOfcMos.end() &&
1431 findCam != mapOfCams.end()) {
1432 mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second,
1433 findcMo->second, findCam->second, displayFullModel);
1448 return it->second->getMovingEdge();
1467 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1468 it->second->getMovingEdge(me1);
1471 it->second->getMovingEdge(me2);
1473 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1485 mapOfMovingEdges.clear();
1487 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1489 TrackerWrapper *tracker = it->second;
1490 mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1513 unsigned int nbGoodPoints = 0;
1516 nbGoodPoints = it->second->getNbPoints(level);
1521 return nbGoodPoints;
1540 mapOfNbPoints.clear();
1542 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1544 TrackerWrapper *tracker = it->second;
1545 mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1558 return it->second->getNbPolygon();
1572 mapOfNbPolygons.clear();
1574 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1576 TrackerWrapper *tracker = it->second;
1577 mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1595 return it->second->getPolygon(index);
1615 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1617 return it->second->getPolygon(index);
1620 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1639 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1642 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1646 TrackerWrapper *tracker = it->second;
1647 polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1652 return polygonFaces;
1673 std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1674 bool orderPolygons,
bool useVisibility,
bool clipPolygon)
1676 mapOfPolygons.clear();
1677 mapOfPoints.clear();
1679 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1681 TrackerWrapper *tracker = it->second;
1682 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1683 tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1685 mapOfPolygons[it->first] = polygonFaces.first;
1686 mapOfPoints[it->first] = polygonFaces.second;
1706 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1707 it->second->getPose(c1Mo);
1710 it->second->getPose(c2Mo);
1712 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1725 mapOfCameraPoses.clear();
1727 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1729 TrackerWrapper *tracker = it->second;
1730 tracker->getPose(mapOfCameraPoses[it->first]);
1749 TrackerWrapper *tracker = it->second;
1750 return tracker->getTrackerType();
1759 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1761 TrackerWrapper *tracker = it->second;
1768 const double ,
const int ,
const std::string & )
1773 #ifdef VISP_HAVE_MODULE_GUI
1818 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1822 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1823 TrackerWrapper *tracker = it->second;
1824 tracker->initClick(I1, initFile1, displayHelp, T1);
1828 tracker = it->second;
1829 tracker->initClick(I2, initFile2, displayHelp, T2);
1833 tracker = it->second;
1836 tracker->getPose(
m_cMo);
1840 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1887 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1891 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1892 TrackerWrapper *tracker = it->second;
1893 tracker->initClick(I_color1, initFile1, displayHelp, T1);
1897 tracker = it->second;
1898 tracker->initClick(I_color2, initFile2, displayHelp, T2);
1902 tracker = it->second;
1905 tracker->getPose(
m_cMo);
1909 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1956 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
1957 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
1960 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1962 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
1964 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1965 TrackerWrapper *tracker = it_tracker->second;
1966 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
1967 if (it_T != mapOfT.end())
1968 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
1970 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1971 tracker->getPose(
m_cMo);
1977 std::vector<std::string> vectorOfMissingCameraPoses;
1982 it_img = mapOfImages.find(it_tracker->first);
1983 it_initFile = mapOfInitFiles.find(it_tracker->first);
1985 if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1987 TrackerWrapper *tracker = it_tracker->second;
1988 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1990 vectorOfMissingCameraPoses.push_back(it_tracker->first);
1996 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
1997 it != vectorOfMissingCameraPoses.end(); ++it) {
1998 it_img = mapOfImages.find(*it);
1999 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2008 "Missing image or missing camera transformation "
2009 "matrix! Cannot set the pose for camera: %s!",
2058 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2059 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2062 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2064 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2066 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2067 TrackerWrapper *tracker = it_tracker->second;
2068 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2069 if (it_T != mapOfT.end())
2070 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2072 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2073 tracker->getPose(
m_cMo);
2079 std::vector<std::string> vectorOfMissingCameraPoses;
2084 it_img = mapOfColorImages.find(it_tracker->first);
2085 it_initFile = mapOfInitFiles.find(it_tracker->first);
2087 if (it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2089 TrackerWrapper *tracker = it_tracker->second;
2090 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2092 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2098 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2099 it != vectorOfMissingCameraPoses.end(); ++it) {
2100 it_img = mapOfColorImages.find(*it);
2101 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2111 "Missing image or missing camera transformation "
2112 "matrix! Cannot set the pose for camera: %s!",
2120 const int ,
const std::string & )
2165 const std::string &initFile1,
const std::string &initFile2)
2168 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2169 TrackerWrapper *tracker = it->second;
2170 tracker->initFromPoints(I1, initFile1);
2174 tracker = it->second;
2175 tracker->initFromPoints(I2, initFile2);
2179 tracker = it->second;
2182 tracker->getPose(
m_cMo);
2185 tracker->getCameraParameters(
m_cam);
2189 "Cannot initFromPoints()! Require two cameras but "
2190 "there are %d cameras!",
2225 const std::string &initFile1,
const std::string &initFile2)
2228 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2229 TrackerWrapper *tracker = it->second;
2230 tracker->initFromPoints(I_color1, initFile1);
2234 tracker = it->second;
2235 tracker->initFromPoints(I_color2, initFile2);
2239 tracker = it->second;
2242 tracker->getPose(
m_cMo);
2245 tracker->getCameraParameters(
m_cam);
2249 "Cannot initFromPoints()! Require two cameras but "
2250 "there are %d cameras!",
2256 const std::map<std::string, std::string> &mapOfInitPoints)
2260 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2262 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2264 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2265 TrackerWrapper *tracker = it_tracker->second;
2266 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2267 tracker->getPose(
m_cMo);
2273 std::vector<std::string> vectorOfMissingCameraPoints;
2277 it_img = mapOfImages.find(it_tracker->first);
2278 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2280 if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2282 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2284 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2288 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2289 it != vectorOfMissingCameraPoints.end(); ++it) {
2290 it_img = mapOfImages.find(*it);
2291 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2299 "Missing image or missing camera transformation "
2300 "matrix! Cannot init the pose for camera: %s!",
2307 const std::map<std::string, std::string> &mapOfInitPoints)
2311 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2313 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2315 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2316 TrackerWrapper *tracker = it_tracker->second;
2317 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2318 tracker->getPose(
m_cMo);
2324 std::vector<std::string> vectorOfMissingCameraPoints;
2328 it_img = mapOfColorImages.find(it_tracker->first);
2329 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2331 if (it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2333 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2335 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2339 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2340 it != vectorOfMissingCameraPoints.end(); ++it) {
2341 it_img = mapOfColorImages.find(*it);
2342 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2350 "Missing image or missing camera transformation "
2351 "matrix! Cannot init the pose for camera: %s!",
2374 const std::string &initFile1,
const std::string &initFile2)
2377 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2378 TrackerWrapper *tracker = it->second;
2379 tracker->initFromPose(I1, initFile1);
2383 tracker = it->second;
2384 tracker->initFromPose(I2, initFile2);
2388 tracker = it->second;
2391 tracker->getPose(
m_cMo);
2394 tracker->getCameraParameters(
m_cam);
2398 "Cannot initFromPose()! Require two cameras but there "
2416 const std::string &initFile1,
const std::string &initFile2)
2419 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2420 TrackerWrapper *tracker = it->second;
2421 tracker->initFromPose(I_color1, initFile1);
2425 tracker = it->second;
2426 tracker->initFromPose(I_color2, initFile2);
2430 tracker = it->second;
2433 tracker->getPose(
m_cMo);
2436 tracker->getCameraParameters(
m_cam);
2440 "Cannot initFromPose()! Require two cameras but there "
2461 const std::map<std::string, std::string> &mapOfInitPoses)
2465 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2467 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2469 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2470 TrackerWrapper *tracker = it_tracker->second;
2471 tracker->initFromPose(*it_img->second, it_initPose->second);
2472 tracker->getPose(
m_cMo);
2478 std::vector<std::string> vectorOfMissingCameraPoses;
2482 it_img = mapOfImages.find(it_tracker->first);
2483 it_initPose = mapOfInitPoses.find(it_tracker->first);
2485 if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2487 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2489 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2493 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2494 it != vectorOfMissingCameraPoses.end(); ++it) {
2495 it_img = mapOfImages.find(*it);
2496 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2504 "Missing image or missing camera transformation "
2505 "matrix! Cannot init the pose for camera: %s!",
2526 const std::map<std::string, std::string> &mapOfInitPoses)
2530 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2532 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2534 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2535 TrackerWrapper *tracker = it_tracker->second;
2536 tracker->initFromPose(*it_img->second, it_initPose->second);
2537 tracker->getPose(
m_cMo);
2543 std::vector<std::string> vectorOfMissingCameraPoses;
2547 it_img = mapOfColorImages.find(it_tracker->first);
2548 it_initPose = mapOfInitPoses.find(it_tracker->first);
2550 if (it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2552 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2554 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2558 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2559 it != vectorOfMissingCameraPoses.end(); ++it) {
2560 it_img = mapOfColorImages.find(*it);
2561 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2569 "Missing image or missing camera transformation "
2570 "matrix! Cannot init the pose for camera: %s!",
2590 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2591 it->second->initFromPose(I1, c1Mo);
2595 it->second->initFromPose(I2, c2Mo);
2600 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2618 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2619 it->second->initFromPose(I_color1, c1Mo);
2623 it->second->initFromPose(I_color2, c2Mo);
2628 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2646 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2650 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2652 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2654 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2655 TrackerWrapper *tracker = it_tracker->second;
2656 tracker->initFromPose(*it_img->second, it_camPose->second);
2657 tracker->getPose(
m_cMo);
2663 std::vector<std::string> vectorOfMissingCameraPoses;
2667 it_img = mapOfImages.find(it_tracker->first);
2668 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2670 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2672 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2674 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2678 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2679 it != vectorOfMissingCameraPoses.end(); ++it) {
2680 it_img = mapOfImages.find(*it);
2681 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2689 "Missing image or missing camera transformation "
2690 "matrix! Cannot set the pose for camera: %s!",
2710 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2714 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2716 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2718 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2719 TrackerWrapper *tracker = it_tracker->second;
2720 tracker->initFromPose(*it_img->second, it_camPose->second);
2721 tracker->getPose(
m_cMo);
2727 std::vector<std::string> vectorOfMissingCameraPoses;
2731 it_img = mapOfColorImages.find(it_tracker->first);
2732 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2734 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2736 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2738 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2742 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2743 it != vectorOfMissingCameraPoses.end(); ++it) {
2744 it_img = mapOfColorImages.find(*it);
2745 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2753 "Missing image or missing camera transformation "
2754 "matrix! Cannot set the pose for camera: %s!",
2772 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2774 TrackerWrapper *tracker = it->second;
2775 tracker->loadConfigFile(configFile);
2807 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2808 TrackerWrapper *tracker = it_tracker->second;
2809 tracker->loadConfigFile(configFile1);
2812 tracker = it_tracker->second;
2813 tracker->loadConfigFile(configFile2);
2839 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2841 TrackerWrapper *tracker = it_tracker->second;
2843 std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
2844 if (it_config != mapOfConfigFiles.end()) {
2845 tracker->loadConfigFile(it_config->second);
2848 it_tracker->first.c_str());
2855 TrackerWrapper *tracker = it->second;
2856 tracker->getCameraParameters(
m_cam);
2900 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2902 TrackerWrapper *tracker = it->second;
2903 tracker->loadModel(modelFile, verbose, T);
2948 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2949 TrackerWrapper *tracker = it_tracker->second;
2950 tracker->loadModel(modelFile1, verbose, T1);
2953 tracker = it_tracker->second;
2954 tracker->loadModel(modelFile2, verbose, T2);
2989 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2991 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2993 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
2995 if (it_model != mapOfModelFiles.end()) {
2996 TrackerWrapper *tracker = it_tracker->second;
2997 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2999 if (it_T != mapOfT.end())
3000 tracker->loadModel(it_model->second, verbose, it_T->second);
3002 tracker->loadModel(it_model->second, verbose);
3005 it_tracker->first.c_str());
3010 #ifdef VISP_HAVE_PCL
3012 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
3014 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3016 TrackerWrapper *tracker = it->second;
3017 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
3023 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
3024 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
3025 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
3027 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3029 TrackerWrapper *tracker = it->second;
3030 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
3031 mapOfPointCloudHeights[it->first]);
3057 TrackerWrapper *tracker = it_tracker->second;
3058 tracker->reInitModel(I, cad_name, cMo, verbose, T);
3061 tracker->getPose(
m_cMo);
3091 TrackerWrapper *tracker = it_tracker->second;
3092 tracker->reInitModel(I_color, cad_name, cMo, verbose, T);
3095 tracker->getPose(
m_cMo);
3124 const std::string &cad_name1,
const std::string &cad_name2,
3130 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3132 it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
3136 it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
3141 it_tracker->second->getPose(
m_cMo);
3171 const std::string &cad_name1,
const std::string &cad_name2,
3177 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3179 it_tracker->second->reInitModel(I_color1, cad_name1, c1Mo, verbose, T1);
3183 it_tracker->second->reInitModel(I_color2, cad_name2, c2Mo, verbose, T2);
3188 it_tracker->second->getPose(
m_cMo);
3212 const std::map<std::string, std::string> &mapOfModelFiles,
3213 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
3215 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3218 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3220 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3221 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3223 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3224 it_camPose != mapOfCameraPoses.end()) {
3225 TrackerWrapper *tracker = it_tracker->second;
3226 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3227 if (it_T != mapOfT.end())
3228 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3230 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3233 tracker->getPose(
m_cMo);
3238 std::vector<std::string> vectorOfMissingCameras;
3241 it_img = mapOfImages.find(it_tracker->first);
3242 it_model = mapOfModelFiles.find(it_tracker->first);
3243 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3245 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3246 TrackerWrapper *tracker = it_tracker->second;
3247 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3249 vectorOfMissingCameras.push_back(it_tracker->first);
3254 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3256 it_img = mapOfImages.find(*it);
3257 it_model = mapOfModelFiles.find(*it);
3258 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3261 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3264 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3286 const std::map<std::string, std::string> &mapOfModelFiles,
3287 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
3289 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3292 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
3294 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3295 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3297 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3298 it_camPose != mapOfCameraPoses.end()) {
3299 TrackerWrapper *tracker = it_tracker->second;
3300 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3301 if (it_T != mapOfT.end())
3302 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3304 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3307 tracker->getPose(
m_cMo);
3312 std::vector<std::string> vectorOfMissingCameras;
3315 it_img = mapOfColorImages.find(it_tracker->first);
3316 it_model = mapOfModelFiles.find(it_tracker->first);
3317 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3319 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3320 TrackerWrapper *tracker = it_tracker->second;
3321 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3323 vectorOfMissingCameras.push_back(it_tracker->first);
3328 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3330 it_img = mapOfColorImages.find(*it);
3331 it_model = mapOfModelFiles.find(*it);
3332 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3335 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3338 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3355 #ifdef VISP_HAVE_OGRE
3382 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3389 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3391 TrackerWrapper *tracker = it->second;
3392 tracker->resetTracker();
3409 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3411 TrackerWrapper *tracker = it->second;
3412 tracker->setAngleAppear(a);
3431 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3432 it->second->setAngleAppear(a1);
3435 it->second->setAngleAppear(a2);
3460 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3461 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3464 TrackerWrapper *tracker = it_tracker->second;
3465 tracker->setAngleAppear(it->second);
3487 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3489 TrackerWrapper *tracker = it->second;
3490 tracker->setAngleDisappear(a);
3509 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3510 it->second->setAngleDisappear(a1);
3513 it->second->setAngleDisappear(a2);
3538 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3539 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3542 TrackerWrapper *tracker = it_tracker->second;
3543 tracker->setAngleDisappear(it->second);
3561 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3563 TrackerWrapper *tracker = it->second;
3564 tracker->setCameraParameters(camera);
3579 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3580 it->second->setCameraParameters(camera1);
3583 it->second->setCameraParameters(camera2);
3587 it->second->getCameraParameters(
m_cam);
3607 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
3608 it != mapOfCameraParameters.end(); ++it) {
3609 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3612 TrackerWrapper *tracker = it_tracker->second;
3613 tracker->setCameraParameters(it->second);
3636 it->second = cameraTransformationMatrix;
3650 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
3652 for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
3653 it != mapOfTransformationMatrix.end(); ++it) {
3654 std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
3658 it_camTrans->second = it->second;
3676 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3678 TrackerWrapper *tracker = it->second;
3679 tracker->setClipping(flags);
3696 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3697 it->second->setClipping(flags1);
3700 it->second->setClipping(flags2);
3709 std::stringstream ss;
3710 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
3724 for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
3725 it != mapOfClippingFlags.end(); ++it) {
3726 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3729 TrackerWrapper *tracker = it_tracker->second;
3730 tracker->setClipping(it->second);
3750 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3752 TrackerWrapper *tracker = it->second;
3753 tracker->setDepthDenseFilteringMaxDistance(maxDistance);
3767 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3769 TrackerWrapper *tracker = it->second;
3770 tracker->setDepthDenseFilteringMethod(method);
3785 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3787 TrackerWrapper *tracker = it->second;
3788 tracker->setDepthDenseFilteringMinDistance(minDistance);
3803 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3805 TrackerWrapper *tracker = it->second;
3806 tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
3820 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3822 TrackerWrapper *tracker = it->second;
3823 tracker->setDepthDenseSamplingStep(stepX, stepY);
3836 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3838 TrackerWrapper *tracker = it->second;
3839 tracker->setDepthNormalFaceCentroidMethod(method);
3853 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3855 TrackerWrapper *tracker = it->second;
3856 tracker->setDepthNormalFeatureEstimationMethod(method);
3869 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3871 TrackerWrapper *tracker = it->second;
3872 tracker->setDepthNormalPclPlaneEstimationMethod(method);
3885 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3887 TrackerWrapper *tracker = it->second;
3888 tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
3901 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3903 TrackerWrapper *tracker = it->second;
3904 tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
3918 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3920 TrackerWrapper *tracker = it->second;
3921 tracker->setDepthNormalSamplingStep(stepX, stepY);
3947 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3949 TrackerWrapper *tracker = it->second;
3950 tracker->setDisplayFeatures(displayF);
3965 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3967 TrackerWrapper *tracker = it->second;
3968 tracker->setFarClippingDistance(dist);
3983 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3984 it->second->setFarClippingDistance(dist1);
3987 it->second->setFarClippingDistance(dist2);
3991 distFarClip = it->second->getFarClippingDistance();
4008 for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
4010 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4013 TrackerWrapper *tracker = it_tracker->second;
4014 tracker->setFarClippingDistance(it->second);
4033 std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
4034 if (it_factor != mapOfFeatureFactors.end()) {
4035 it->second = it_factor->second;
4059 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4061 TrackerWrapper *tracker = it->second;
4062 tracker->setGoodMovingEdgesRatioThreshold(threshold);
4066 #ifdef VISP_HAVE_OGRE
4080 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4082 TrackerWrapper *tracker = it->second;
4083 tracker->setGoodNbRayCastingAttemptsRatio(ratio);
4100 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4102 TrackerWrapper *tracker = it->second;
4103 tracker->setNbRayCastingAttemptsForVisibility(attempts);
4108 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4118 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4120 TrackerWrapper *tracker = it->second;
4121 tracker->setKltOpencv(t);
4136 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4137 it->second->setKltOpencv(t1);
4140 it->second->setKltOpencv(t2);
4154 for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
4155 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4158 TrackerWrapper *tracker = it_tracker->second;
4159 tracker->setKltOpencv(it->second);
4175 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4177 TrackerWrapper *tracker = it->second;
4178 tracker->setKltThresholdAcceptation(th);
4197 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4199 TrackerWrapper *tracker = it->second;
4200 tracker->setLod(useLod, name);
4204 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4214 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4216 TrackerWrapper *tracker = it->second;
4217 tracker->setKltMaskBorder(e);
4232 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4233 it->second->setKltMaskBorder(e1);
4237 it->second->setKltMaskBorder(e2);
4251 for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
4253 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4256 TrackerWrapper *tracker = it_tracker->second;
4257 tracker->setKltMaskBorder(it->second);
4272 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4274 TrackerWrapper *tracker = it->second;
4275 tracker->setMask(mask);
4293 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4295 TrackerWrapper *tracker = it->second;
4296 tracker->setMinLineLengthThresh(minLineLengthThresh, name);
4312 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4314 TrackerWrapper *tracker = it->second;
4315 tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
4328 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4330 TrackerWrapper *tracker = it->second;
4331 tracker->setMovingEdge(me);
4347 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4348 it->second->setMovingEdge(me1);
4352 it->second->setMovingEdge(me2);
4366 for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
4367 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4370 TrackerWrapper *tracker = it_tracker->second;
4371 tracker->setMovingEdge(it->second);
4387 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4389 TrackerWrapper *tracker = it->second;
4390 tracker->setNearClippingDistance(dist);
4405 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4406 it->second->setNearClippingDistance(dist1);
4410 it->second->setNearClippingDistance(dist2);
4431 for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
4432 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4435 TrackerWrapper *tracker = it_tracker->second;
4436 tracker->setNearClippingDistance(it->second);
4461 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4463 TrackerWrapper *tracker = it->second;
4464 tracker->setOgreShowConfigDialog(showConfigDialog);
4482 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4484 TrackerWrapper *tracker = it->second;
4485 tracker->setOgreVisibilityTest(v);
4488 #ifdef VISP_HAVE_OGRE
4489 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4491 TrackerWrapper *tracker = it->second;
4492 tracker->faces.getOgreContext()->setWindowName(
"Multi Generic MBT (" + it->first +
")");
4508 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4510 TrackerWrapper *tracker = it->second;
4511 tracker->setOptimizationMethod(opt);
4531 "to be configured with only one camera!");
4538 TrackerWrapper *tracker = it->second;
4539 tracker->setPose(I, cdMo);
4562 "to be configured with only one camera!");
4569 TrackerWrapper *tracker = it->second;
4571 tracker->setPose(
m_I, cdMo);
4593 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4594 it->second->setPose(I1, c1Mo);
4598 it->second->setPose(I2, c2Mo);
4603 it->second->getPose(
m_cMo);
4629 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4630 it->second->setPose(I_color1, c1Mo);
4634 it->second->setPose(I_color2, c2Mo);
4639 it->second->getPose(
m_cMo);
4666 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4670 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
4672 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4674 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4675 TrackerWrapper *tracker = it_tracker->second;
4676 tracker->setPose(*it_img->second, it_camPose->second);
4677 tracker->getPose(
m_cMo);
4683 std::vector<std::string> vectorOfMissingCameraPoses;
4688 it_img = mapOfImages.find(it_tracker->first);
4689 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4691 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4693 TrackerWrapper *tracker = it_tracker->second;
4694 tracker->setPose(*it_img->second, it_camPose->second);
4696 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4701 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4702 it != vectorOfMissingCameraPoses.end(); ++it) {
4703 it_img = mapOfImages.find(*it);
4704 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4712 "Missing image or missing camera transformation "
4713 "matrix! Cannot set pose for camera: %s!",
4735 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4739 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
4741 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4743 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4744 TrackerWrapper *tracker = it_tracker->second;
4745 tracker->setPose(*it_img->second, it_camPose->second);
4746 tracker->getPose(
m_cMo);
4752 std::vector<std::string> vectorOfMissingCameraPoses;
4757 it_img = mapOfColorImages.find(it_tracker->first);
4758 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4760 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4762 TrackerWrapper *tracker = it_tracker->second;
4763 tracker->setPose(*it_img->second, it_camPose->second);
4765 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4770 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4771 it != vectorOfMissingCameraPoses.end(); ++it) {
4772 it_img = mapOfColorImages.find(*it);
4773 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4781 "Missing image or missing camera transformation "
4782 "matrix! Cannot set pose for camera: %s!",
4806 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4808 TrackerWrapper *tracker = it->second;
4809 tracker->setProjectionErrorComputation(flag);
4820 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4822 TrackerWrapper *tracker = it->second;
4823 tracker->setProjectionErrorDisplay(
display);
4834 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4836 TrackerWrapper *tracker = it->second;
4837 tracker->setProjectionErrorDisplayArrowLength(length);
4845 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4847 TrackerWrapper *tracker = it->second;
4848 tracker->setProjectionErrorDisplayArrowThickness(thickness);
4859 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(referenceCameraName);
4863 std::cerr <<
"The reference camera: " << referenceCameraName <<
" does not exist!";
4871 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4873 TrackerWrapper *tracker = it->second;
4874 tracker->setScanLineVisibilityTest(v);
4891 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4893 TrackerWrapper *tracker = it->second;
4894 tracker->setTrackerType(type);
4909 for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
4910 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4912 TrackerWrapper *tracker = it_tracker->second;
4913 tracker->setTrackerType(it->second);
4929 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4931 TrackerWrapper *tracker = it->second;
4932 tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
4947 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4949 TrackerWrapper *tracker = it->second;
4950 tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
4965 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4967 TrackerWrapper *tracker = it->second;
4968 tracker->setUseEdgeTracking(name, useEdgeTracking);
4972 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4984 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4986 TrackerWrapper *tracker = it->second;
4987 tracker->setUseKltTracking(name, useKltTracking);
4995 bool isOneTestTrackingOk =
false;
4996 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4998 TrackerWrapper *tracker = it->second;
5000 tracker->testTracking();
5001 isOneTestTrackingOk =
true;
5006 if (!isOneTestTrackingOk) {
5007 std::ostringstream oss;
5008 oss <<
"Not enough moving edges to track the object. Try to reduce the "
5026 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5029 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5030 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5032 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5046 std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
5049 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5050 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5052 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5068 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5069 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5070 mapOfImages[it->first] = &I1;
5073 mapOfImages[it->first] = &I2;
5075 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5076 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5078 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5080 std::stringstream ss;
5081 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5099 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5100 std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
5101 mapOfImages[it->first] = &I_color1;
5104 mapOfImages[it->first] = &_colorI2;
5106 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5107 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5109 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5111 std::stringstream ss;
5112 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5126 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5127 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5129 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5141 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5142 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5144 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5147 #ifdef VISP_HAVE_PCL
5157 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5159 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5161 TrackerWrapper *tracker = it->second;
5164 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5172 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5176 mapOfImages[it->first] == NULL) {
5181 ! mapOfPointClouds[it->first]) {
5197 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5199 TrackerWrapper *tracker = it->second;
5202 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5205 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5208 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5210 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5215 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5232 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5234 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5235 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5237 TrackerWrapper *tracker = it->second;
5240 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5248 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5251 ) && mapOfImages[it->first] == NULL) {
5254 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5257 ) && mapOfImages[it->first] != NULL) {
5259 mapOfImages[it->first] = &tracker->m_I;
5263 ! mapOfPointClouds[it->first]) {
5279 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5281 TrackerWrapper *tracker = it->second;
5284 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5287 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5290 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5292 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5297 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5317 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5318 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5319 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5321 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5323 TrackerWrapper *tracker = it->second;
5326 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5334 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5338 mapOfImages[it->first] == NULL) {
5343 (mapOfPointClouds[it->first] == NULL)) {
5348 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5359 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5361 TrackerWrapper *tracker = it->second;
5364 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5367 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5370 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5372 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5377 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5396 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5397 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5398 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5400 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5401 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5403 TrackerWrapper *tracker = it->second;
5406 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5414 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5417 ) && mapOfColorImages[it->first] == NULL) {
5420 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5423 ) && mapOfColorImages[it->first] != NULL) {
5425 mapOfImages[it->first] = &tracker->m_I;
5429 (mapOfPointClouds[it->first] == NULL)) {
5434 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5445 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5447 TrackerWrapper *tracker = it->second;
5450 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5453 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5456 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5458 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5463 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5472 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
5473 : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
5478 #ifdef VISP_HAVE_OGRE
5485 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
int trackerType)
5486 : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
5489 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5499 #ifdef VISP_HAVE_OGRE
5506 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
5511 computeVVSInit(ptr_I);
5513 if (m_error.getRows() < 4) {
5518 double normRes_1 = -1;
5519 unsigned int iter = 0;
5521 double factorEdge = 1.0;
5522 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5523 double factorKlt = 1.0;
5525 double factorDepth = 1.0;
5526 double factorDepthDense = 1.0;
5532 double mu = m_initialMu;
5534 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5537 bool isoJoIdentity_ =
true;
5543 unsigned int nb_edge_features = m_error_edge.
getRows();
5544 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5545 unsigned int nb_klt_features = m_error_klt.getRows();
5547 unsigned int nb_depth_features = m_error_depthNormal.getRows();
5548 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5550 while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
5551 computeVVSInteractionMatrixAndResidu(ptr_I);
5553 bool reStartFromLastIncrement =
false;
5554 computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
5556 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5557 if (reStartFromLastIncrement) {
5558 if (m_trackerType & KLT_TRACKER) {
5564 if (!reStartFromLastIncrement) {
5565 computeVVSWeights();
5567 if (computeCovariance) {
5569 if (!isoJoIdentity_) {
5572 LVJ_true = (m_L * cVo * oJo);
5578 isoJoIdentity_ =
true;
5585 if (isoJoIdentity_) {
5589 unsigned int rank = (m_L * cVo).kernel(K);
5599 isoJoIdentity_ =
false;
5608 unsigned int start_index = 0;
5609 if (m_trackerType & EDGE_TRACKER) {
5610 for (
unsigned int i = 0; i < nb_edge_features; i++) {
5611 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5613 m_weightedError[i] = wi * m_error[i];
5618 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5623 start_index += nb_edge_features;
5626 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5627 if (m_trackerType & KLT_TRACKER) {
5628 for (
unsigned int i = 0; i < nb_klt_features; i++) {
5629 double wi = m_w_klt[i] * factorKlt;
5630 W_true[start_index + i] = wi;
5631 m_weightedError[start_index + i] = wi * m_error_klt[i];
5633 num += wi *
vpMath::sqr(m_error[start_index + i]);
5636 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5637 m_L[start_index + i][j] *= wi;
5641 start_index += nb_klt_features;
5645 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5646 for (
unsigned int i = 0; i < nb_depth_features; i++) {
5647 double wi = m_w_depthNormal[i] * factorDepth;
5648 m_w[start_index + i] = m_w_depthNormal[i];
5649 m_weightedError[start_index + i] = wi * m_error[start_index + i];
5651 num += wi *
vpMath::sqr(m_error[start_index + i]);
5654 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5655 m_L[start_index + i][j] *= wi;
5659 start_index += nb_depth_features;
5662 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5663 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
5664 double wi = m_w_depthDense[i] * factorDepthDense;
5665 m_w[start_index + i] = m_w_depthDense[i];
5666 m_weightedError[start_index + i] = wi * m_error[start_index + i];
5668 num += wi *
vpMath::sqr(m_error[start_index + i]);
5671 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5672 m_L[start_index + i][j] *= wi;
5679 computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
5682 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5683 if (m_trackerType & KLT_TRACKER) {
5690 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5691 if (m_trackerType & KLT_TRACKER) {
5695 normRes_1 = normRes;
5697 normRes = sqrt(num / den);
5703 computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
5705 if (m_trackerType & EDGE_TRACKER) {
5710 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
5713 "TrackerWrapper::computeVVSInit("
5714 ") should not be called!");
5719 initMbtTracking(ptr_I);
5721 unsigned int nbFeatures = 0;
5723 if (m_trackerType & EDGE_TRACKER) {
5724 nbFeatures += m_error_edge.getRows();
5726 m_error_edge.clear();
5727 m_weightedError_edge.clear();
5732 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5733 if (m_trackerType & KLT_TRACKER) {
5735 nbFeatures += m_error_klt.getRows();
5737 m_error_klt.clear();
5738 m_weightedError_klt.clear();
5744 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5746 nbFeatures += m_error_depthNormal.getRows();
5748 m_error_depthNormal.clear();
5749 m_weightedError_depthNormal.clear();
5750 m_L_depthNormal.clear();
5751 m_w_depthNormal.clear();
5754 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5756 nbFeatures += m_error_depthDense.getRows();
5758 m_error_depthDense.clear();
5759 m_weightedError_depthDense.clear();
5760 m_L_depthDense.clear();
5761 m_w_depthDense.clear();
5764 m_L.resize(nbFeatures, 6,
false,
false);
5765 m_error.resize(nbFeatures,
false);
5767 m_weightedError.resize(nbFeatures,
false);
5768 m_w.resize(nbFeatures,
false);
5776 "computeVVSInteractionMatrixAndR"
5777 "esidu() should not be called!");
5782 if (m_trackerType & EDGE_TRACKER) {
5786 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5787 if (m_trackerType & KLT_TRACKER) {
5792 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5796 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5800 unsigned int start_index = 0;
5801 if (m_trackerType & EDGE_TRACKER) {
5802 m_L.insert(m_L_edge, start_index, 0);
5803 m_error.insert(start_index, m_error_edge);
5805 start_index += m_error_edge.getRows();
5808 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5809 if (m_trackerType & KLT_TRACKER) {
5810 m_L.insert(m_L_klt, start_index, 0);
5811 m_error.insert(start_index, m_error_klt);
5813 start_index += m_error_klt.getRows();
5817 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5818 m_L.insert(m_L_depthNormal, start_index, 0);
5819 m_error.insert(start_index, m_error_depthNormal);
5821 start_index += m_error_depthNormal.getRows();
5824 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5825 m_L.insert(m_L_depthDense, start_index, 0);
5826 m_error.insert(start_index, m_error_depthDense);
5834 unsigned int start_index = 0;
5836 if (m_trackerType & EDGE_TRACKER) {
5838 m_w.insert(start_index, m_w_edge);
5840 start_index += m_w_edge.getRows();
5843 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5844 if (m_trackerType & KLT_TRACKER) {
5846 m_w.insert(start_index, m_w_klt);
5848 start_index += m_w_klt.getRows();
5852 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5853 if (m_depthNormalUseRobust) {
5855 m_w.insert(start_index, m_w_depthNormal);
5858 start_index += m_w_depthNormal.getRows();
5861 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5863 m_w.insert(start_index, m_w_depthDense);
5871 unsigned int thickness,
bool displayFullModel)
5873 if (displayFeatures) {
5874 std::vector<std::vector<double> > features = getFeaturesForDisplay();
5875 for (
size_t i = 0; i < features.size(); i++) {
5878 int state =
static_cast<int>(features[i][3]);
5909 double id = features[i][5];
5910 std::stringstream ss;
5914 vpImagePoint im_centroid(features[i][1], features[i][2]);
5915 vpImagePoint im_extremity(features[i][3], features[i][4]);
5922 std::vector<std::vector<double> > models = getModelForDisplay(I.
getWidth(), I.
getHeight(), cMo, cam, displayFullModel);
5923 for (
size_t i = 0; i < models.size(); i++) {
5930 double mu20 = models[i][3];
5931 double mu11 = models[i][4];
5932 double mu02 = models[i][5];
5937 #ifdef VISP_HAVE_OGRE
5938 if ((m_trackerType & EDGE_TRACKER)
5939 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5940 || (m_trackerType & KLT_TRACKER)
5944 faces.displayOgre(cMo);
5951 unsigned int thickness,
bool displayFullModel)
5953 if (displayFeatures) {
5954 std::vector<std::vector<double> > features = getFeaturesForDisplay();
5955 for (
size_t i = 0; i < features.size(); i++) {
5958 int state =
static_cast<int>(features[i][3]);
5989 double id = features[i][5];
5990 std::stringstream ss;
5994 vpImagePoint im_centroid(features[i][1], features[i][2]);
5995 vpImagePoint im_extremity(features[i][3], features[i][4]);
6002 std::vector<std::vector<double> > models = getModelForDisplay(I.
getWidth(), I.
getHeight(), cMo, cam, displayFullModel);
6003 for (
size_t i = 0; i < models.size(); i++) {
6010 double mu20 = models[i][3];
6011 double mu11 = models[i][4];
6012 double mu02 = models[i][5];
6017 #ifdef VISP_HAVE_OGRE
6018 if ((m_trackerType & EDGE_TRACKER)
6019 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6020 || (m_trackerType & KLT_TRACKER)
6024 faces.displayOgre(cMo);
6029 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6031 std::vector<std::vector<double> > features;
6033 if (m_trackerType & EDGE_TRACKER) {
6035 features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6038 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6039 if (m_trackerType & KLT_TRACKER) {
6041 features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6045 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6047 features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(), m_featuresToBeDisplayedDepthNormal.end());
6053 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(
unsigned int width,
unsigned int height,
6056 bool displayFullModel)
6058 std::vector<std::vector<double> > models;
6061 if (m_trackerType == EDGE_TRACKER) {
6064 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6065 else if (m_trackerType == KLT_TRACKER) {
6069 else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
6071 }
else if (m_trackerType == DEPTH_DENSE_TRACKER) {
6075 if (m_trackerType & EDGE_TRACKER) {
6077 models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6081 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6083 models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6092 if (!modelInitialised) {
6096 if (useScanLine || clippingFlag > 3)
6099 bool reInitialisation =
false;
6101 faces.setVisible(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6103 #ifdef VISP_HAVE_OGRE
6104 if (!faces.isOgreInitialised()) {
6107 faces.setOgreShowConfigDialog(ogreShowConfigDialog);
6108 faces.initOgre(m_cam);
6112 ogreShowConfigDialog =
false;
6115 faces.setVisibleOgre(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6117 faces.setVisible(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6122 faces.computeClippedPolygons(m_cMo, m_cam);
6126 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6127 if (m_trackerType & KLT_TRACKER)
6131 if (m_trackerType & EDGE_TRACKER) {
6137 initMovingEdge(I, m_cMo);
6140 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6143 if (m_trackerType & DEPTH_DENSE_TRACKER)
6147 void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
6148 double radius,
int idFace,
const std::string &name)
6150 if (m_trackerType & EDGE_TRACKER)
6153 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6154 if (m_trackerType & KLT_TRACKER)
6159 void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
6160 int idFace,
const std::string &name)
6162 if (m_trackerType & EDGE_TRACKER)
6165 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6166 if (m_trackerType & KLT_TRACKER)
6171 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
6173 if (m_trackerType & EDGE_TRACKER)
6176 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6177 if (m_trackerType & KLT_TRACKER)
6181 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6184 if (m_trackerType & DEPTH_DENSE_TRACKER)
6188 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
6190 if (m_trackerType & EDGE_TRACKER)
6193 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6194 if (m_trackerType & KLT_TRACKER)
6198 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6201 if (m_trackerType & DEPTH_DENSE_TRACKER)
6207 if (m_trackerType & EDGE_TRACKER) {
6213 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile)
6218 #ifdef VISP_HAVE_PUGIXML
6221 xmlp.setCameraParameters(m_cam);
6223 xmlp.setAngleDisappear(
vpMath::deg(angleDisappears));
6229 xmlp.setKltMaxFeatures(10000);
6230 xmlp.setKltWindowSize(5);
6231 xmlp.setKltQuality(0.01);
6232 xmlp.setKltMinDistance(5);
6233 xmlp.setKltHarrisParam(0.01);
6234 xmlp.setKltBlockSize(3);
6235 xmlp.setKltPyramidLevels(3);
6236 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6237 xmlp.setKltMaskBorder(maskBorder);
6241 xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
6242 xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
6243 xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
6244 xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
6245 xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
6246 xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
6249 xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
6250 xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
6253 std::cout <<
" *********** Parsing XML for";
6255 std::vector<std::string> tracker_names;
6256 if (m_trackerType & EDGE_TRACKER)
6257 tracker_names.push_back(
"Edge");
6258 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6259 if (m_trackerType & KLT_TRACKER)
6260 tracker_names.push_back(
"Klt");
6262 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6263 tracker_names.push_back(
"Depth Normal");
6264 if (m_trackerType & DEPTH_DENSE_TRACKER)
6265 tracker_names.push_back(
"Depth Dense");
6267 for (
size_t i = 0; i < tracker_names.size(); i++) {
6268 std::cout <<
" " << tracker_names[i];
6269 if (i == tracker_names.size() - 1) {
6274 std::cout <<
"Model-Based Tracker ************ " << std::endl;
6275 xmlp.parse(configFile);
6281 xmlp.getCameraParameters(camera);
6282 setCameraParameters(camera);
6284 angleAppears =
vpMath::rad(xmlp.getAngleAppear());
6285 angleDisappears =
vpMath::rad(xmlp.getAngleDisappear());
6287 if (xmlp.hasNearClippingDistance())
6288 setNearClippingDistance(xmlp.getNearClippingDistance());
6290 if (xmlp.hasFarClippingDistance())
6291 setFarClippingDistance(xmlp.getFarClippingDistance());
6293 if (xmlp.getFovClipping()) {
6297 useLodGeneral = xmlp.getLodState();
6298 minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
6299 minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
6301 applyLodSettingInConfig =
false;
6302 if (this->getNbPolygon() > 0) {
6303 applyLodSettingInConfig =
true;
6304 setLod(useLodGeneral);
6305 setMinLineLengthThresh(minLineLengthThresholdGeneral);
6306 setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
6311 xmlp.getEdgeMe(meParser);
6315 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6316 tracker.setMaxFeatures((
int)xmlp.getKltMaxFeatures());
6317 tracker.setWindowSize((
int)xmlp.getKltWindowSize());
6318 tracker.setQuality(xmlp.getKltQuality());
6319 tracker.setMinDistance(xmlp.getKltMinDistance());
6320 tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
6321 tracker.setBlockSize((
int)xmlp.getKltBlockSize());
6322 tracker.setPyramidLevels((
int)xmlp.getKltPyramidLevels());
6323 maskBorder = xmlp.getKltMaskBorder();
6326 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
6330 setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod());
6331 setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod());
6332 setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter());
6333 setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold());
6334 setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY());
6337 setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
6339 std::cerr <<
"pugixml third-party is not properly built to read config file: " << configFile << std::endl;
6343 #ifdef VISP_HAVE_PCL
6345 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6347 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6349 if (m_trackerType & KLT_TRACKER) {
6357 if (m_trackerType & EDGE_TRACKER) {
6358 bool newvisibleface =
false;
6362 faces.computeClippedPolygons(m_cMo, m_cam);
6368 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6372 if (m_trackerType & DEPTH_DENSE_TRACKER)
6376 if (m_trackerType & EDGE_TRACKER) {
6383 if (computeProjError) {
6390 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6392 if (m_trackerType & EDGE_TRACKER) {
6396 std::cerr <<
"Error in moving edge tracking" << std::endl;
6401 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6402 if (m_trackerType & KLT_TRACKER) {
6406 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6412 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6416 std::cerr <<
"Error in Depth normal tracking" << std::endl;
6421 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6425 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6433 const unsigned int pointcloud_width,
6434 const unsigned int pointcloud_height)
6436 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6438 if (m_trackerType & KLT_TRACKER) {
6446 if (m_trackerType & EDGE_TRACKER) {
6447 bool newvisibleface =
false;
6451 faces.computeClippedPolygons(m_cMo, m_cam);
6457 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6461 if (m_trackerType & DEPTH_DENSE_TRACKER)
6465 if (m_trackerType & EDGE_TRACKER) {
6472 if (computeProjError) {
6479 const std::vector<vpColVector> *
const point_cloud,
6480 const unsigned int pointcloud_width,
6481 const unsigned int pointcloud_height)
6483 if (m_trackerType & EDGE_TRACKER) {
6487 std::cerr <<
"Error in moving edge tracking" << std::endl;
6492 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6493 if (m_trackerType & KLT_TRACKER) {
6497 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6503 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6507 std::cerr <<
"Error in Depth tracking" << std::endl;
6512 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6516 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6533 for (
unsigned int i = 0; i < scales.size(); i++) {
6535 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
6542 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
6550 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
6558 cylinders[i].clear();
6566 nbvisiblepolygone = 0;
6569 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6570 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
6572 cvReleaseImage(&cur);
6578 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
6580 if (kltpoly != NULL) {
6585 kltPolygons.clear();
6587 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
6590 if (kltPolyCylinder != NULL) {
6591 delete kltPolyCylinder;
6593 kltPolyCylinder = NULL;
6595 kltCylinders.clear();
6598 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
6605 circles_disp.clear();
6607 firstInitialisation =
true;
6612 for (
size_t i = 0; i < m_depthNormalFaces.size(); i++) {
6613 delete m_depthNormalFaces[i];
6614 m_depthNormalFaces[i] = NULL;
6616 m_depthNormalFaces.clear();
6619 for (
size_t i = 0; i < m_depthDenseFaces.size(); i++) {
6620 delete m_depthDenseFaces[i];
6621 m_depthDenseFaces[i] = NULL;
6623 m_depthDenseFaces.clear();
6627 loadModel(cad_name, verbose, T);
6629 initFromPose(*I, cMo);
6631 initFromPose(*I_color, cMo);
6639 reInitModel(&I, NULL, cad_name, cMo, verbose, T);
6646 reInitModel(NULL, &I_color, cad_name, cMo, verbose, T);
6649 void vpMbGenericTracker::TrackerWrapper::resetTracker()
6652 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6659 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(
const vpCameraParameters &cam)
6664 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6671 void vpMbGenericTracker::TrackerWrapper::setClipping(
const unsigned int &flags)
6676 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(
const double &dist)
6681 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(
const double &dist)
6686 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(
const bool &v)
6689 #ifdef VISP_HAVE_OGRE
6690 faces.getOgreContext()->setWindowName(
"TrackerWrapper");
6697 bool performKltSetPose =
false;
6702 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6703 if (m_trackerType & KLT_TRACKER) {
6704 performKltSetPose =
true;
6706 if (useScanLine || clippingFlag > 3) {
6707 m_cam.computeFov(I ? I->
getWidth() : m_I.getWidth(), I ? I->
getHeight() : m_I.getHeight());
6714 if (!performKltSetPose) {
6720 if (m_trackerType & EDGE_TRACKER)
6724 faces.computeClippedPolygons(m_cMo, m_cam);
6725 faces.computeScanLineRender(m_cam, I ? I->
getWidth() : m_I.getWidth(), I ? I->
getHeight() : m_I.getHeight());
6729 if (m_trackerType & EDGE_TRACKER) {
6730 initPyramid(I, Ipyramid);
6732 unsigned int i = (
unsigned int) scales.size();
6737 initMovingEdge(*Ipyramid[i], cMo);
6742 cleanPyramid(Ipyramid);
6745 if (m_trackerType & EDGE_TRACKER)
6746 initMovingEdge(I ? *I : m_I, m_cMo);
6758 setPose(&I, NULL, cdMo);
6763 setPose(NULL, &I_color, cdMo);
6766 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(
const bool &flag)
6771 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(
const bool &v)
6774 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6781 void vpMbGenericTracker::TrackerWrapper::setTrackerType(
int type)
6783 if ((type & (EDGE_TRACKER |
6784 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6787 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
6791 m_trackerType = type;
6794 void vpMbGenericTracker::TrackerWrapper::testTracking()
6796 if (m_trackerType & EDGE_TRACKER) {
6802 #
if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6807 if ((m_trackerType & (EDGE_TRACKER
6808 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6812 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6816 #if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6826 #ifdef VISP_HAVE_PCL
6828 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6830 if ((m_trackerType & (EDGE_TRACKER |
6831 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6834 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
6835 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6839 if (m_trackerType & (EDGE_TRACKER
6840 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6848 if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && ! point_cloud) {
6855 preTracking(ptr_I, point_cloud);
6860 covarianceMatrix = -1;
6864 if (m_trackerType == EDGE_TRACKER)
6867 postTracking(ptr_I, point_cloud);
6870 std::cerr <<
"Exception: " << e.
what() << std::endl;