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(),
46 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
56 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 71 }
else if (nbCameras == 1) {
77 for (
unsigned int i = 1; i <= nbCameras; i++) {
93 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 106 if (trackerTypes.empty()) {
110 if (trackerTypes.size() == 1) {
116 for (
size_t i = 1; i <= trackerTypes.size(); i++) {
117 std::stringstream ss;
132 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 141 const std::vector<int> &trackerTypes)
146 if (cameraNames.size() != trackerTypes.size() || cameraNames.empty()) {
148 "cameraNames.size() != trackerTypes.size() || cameraNames.empty()");
151 for (
size_t i = 0; i < cameraNames.size(); i++) {
164 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 201 double rawTotalProjectionError = 0.0;
202 unsigned int nbTotalFeaturesUsed = 0;
204 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
206 TrackerWrapper *tracker = it->second;
208 unsigned int nbFeaturesUsed = 0;
209 double curProjError = tracker->computeProjectionErrorImpl(I, cMo, cam, nbFeaturesUsed);
211 if (nbFeaturesUsed > 0) {
212 nbTotalFeaturesUsed += nbFeaturesUsed;
213 rawTotalProjectionError += curProjError;
217 if (nbTotalFeaturesUsed > 0) {
218 return vpMath::deg(rawTotalProjectionError / (
double)nbTotalFeaturesUsed);
254 double rawTotalProjectionError = 0.0;
255 unsigned int nbTotalFeaturesUsed = 0;
257 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
259 TrackerWrapper *tracker = it->second;
261 double curProjError = tracker->getProjectionError();
262 unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
264 if (nbFeaturesUsed > 0) {
265 nbTotalFeaturesUsed += nbFeaturesUsed;
266 rawTotalProjectionError += (
vpMath::rad(curProjError) * nbFeaturesUsed);
270 if (nbTotalFeaturesUsed > 0) {
289 double normRes_1 = -1;
290 unsigned int iter = 0;
299 bool isoJoIdentity_ =
true;
306 std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
311 mapOfVelocityTwist[it->first] = cVo;
315 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 329 bool reStartFromLastIncrement =
false;
331 if (reStartFromLastIncrement) {
332 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
334 TrackerWrapper *tracker = it->second;
338 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 341 tracker->ctTc0 = c_curr_tTc_curr0;
346 if (!reStartFromLastIncrement) {
351 if (!isoJoIdentity_) {
354 LVJ_true = (
m_L * (cVo *
oJo));
360 isoJoIdentity_ =
true;
367 if (isoJoIdentity_) {
371 unsigned int rank = (
m_L * cVo).kernel(K);
381 isoJoIdentity_ =
false;
390 unsigned int start_index = 0;
391 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
393 TrackerWrapper *tracker = it->second;
396 for (
unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
397 double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
398 W_true[start_index + i] = wi;
404 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
405 m_L[start_index + i][j] *= wi;
409 start_index += tracker->m_error_edge.
getRows();
412 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 414 for (
unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
415 double wi = tracker->m_w_klt[i] * factorKlt;
416 W_true[start_index + i] = wi;
422 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
423 m_L[start_index + i][j] *= wi;
427 start_index += tracker->m_error_klt.
getRows();
432 for (
unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
433 double wi = tracker->m_w_depthNormal[i] * factorDepth;
434 W_true[start_index + i] = wi;
440 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
441 m_L[start_index + i][j] *= wi;
445 start_index += tracker->m_error_depthNormal.
getRows();
449 for (
unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
450 double wi = tracker->m_w_depthDense[i] * factorDepthDense;
451 W_true[start_index + i] = wi;
457 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
458 m_L[start_index + i][j] *= wi;
462 start_index += tracker->m_error_depthDense.
getRows();
467 normRes = sqrt(num / den);
475 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 476 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
478 TrackerWrapper *tracker = it->second;
482 tracker->ctTc0 = c_curr_tTc_curr0;
487 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
489 TrackerWrapper *tracker = it->second;
498 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
500 TrackerWrapper *tracker = it->second;
504 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 519 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
521 TrackerWrapper *tracker = it->second;
524 tracker->updateMovingEdgeWeights();
536 unsigned int nbFeatures = 0;
538 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
540 TrackerWrapper *tracker = it->second;
541 tracker->computeVVSInit(mapOfImages[it->first]);
543 nbFeatures += tracker->m_error.getRows();
557 "computeVVSInteractionMatrixAndR" 558 "esidu() should not be called");
563 std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
565 unsigned int start_index = 0;
567 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
569 TrackerWrapper *tracker = it->second;
572 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 575 tracker->ctTc0 = c_curr_tTc_curr0;
578 tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
580 m_L.
insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
583 start_index += tracker->m_error.getRows();
589 unsigned int start_index = 0;
591 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
593 TrackerWrapper *tracker = it->second;
594 tracker->computeVVSWeights();
597 start_index += tracker->m_w.getRows();
616 bool displayFullModel)
620 TrackerWrapper *tracker = it->second;
621 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
642 bool displayFullModel)
646 TrackerWrapper *tracker = it->second;
647 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
672 unsigned int thickness,
bool displayFullModel)
675 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
676 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
679 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
681 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 705 bool displayFullModel)
708 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
709 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
712 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
714 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 731 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
732 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
733 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
737 it_img != mapOfImages.end(); ++it_img) {
738 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
739 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
740 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
742 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
743 it_cam != mapOfCameraParameters.end()) {
744 TrackerWrapper *tracker = it_tracker->second;
745 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
747 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
764 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
765 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
766 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
769 for (std::map<std::string,
const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
770 it_img != mapOfImages.end(); ++it_img) {
771 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
772 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
773 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
775 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
776 it_cam != mapOfCameraParameters.end()) {
777 TrackerWrapper *tracker = it_tracker->second;
778 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
780 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
792 std::vector<std::string> cameraNames;
794 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
796 cameraNames.push_back(it_tracker->first);
818 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
819 it->second->getCameraParameters(cam1);
822 it->second->getCameraParameters(cam2);
824 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 837 mapOfCameraParameters.clear();
839 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
842 it->second->getCameraParameters(cam_);
843 mapOfCameraParameters[it->first] = cam_;
855 std::map<std::string, int> trackingTypes;
857 TrackerWrapper *traker;
858 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
860 traker = it_tracker->second;
861 trackingTypes[it_tracker->first] = traker->getTrackerType();
864 return trackingTypes;
878 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
879 clippingFlag1 = it->second->getClipping();
882 clippingFlag2 = it->second->getClipping();
884 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 896 mapOfClippingFlags.clear();
898 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
900 TrackerWrapper *tracker = it->second;
901 mapOfClippingFlags[it->first] = tracker->getClipping();
914 return it->second->getFaces();
928 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
930 return it->second->getFaces();
933 std::cerr <<
"The camera: " << cameraName <<
" cannot be found!" << std::endl;
937 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 945 TrackerWrapper *tracker = it->second;
946 return tracker->getFeaturesCircle();
960 TrackerWrapper *tracker = it->second;
961 return tracker->getFeaturesKltCylinder();
975 TrackerWrapper *tracker = it->second;
976 return tracker->getFeaturesKlt();
1015 return it->second->getFeaturesForDisplay();
1020 return std::vector<std::vector<double> >();
1051 mapOfFeatures.clear();
1053 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1055 mapOfFeatures[it->first] = it->second->getFeaturesForDisplay();
1070 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 1083 TrackerWrapper *tracker = it->second;
1084 return tracker->getKltImagePoints();
1089 return std::vector<vpImagePoint>();
1104 TrackerWrapper *tracker = it->second;
1105 return tracker->getKltImagePointsWithId();
1110 return std::map<int, vpImagePoint>();
1122 TrackerWrapper *tracker = it->second;
1123 return tracker->getKltMaskBorder();
1140 TrackerWrapper *tracker = it->second;
1141 return tracker->getKltNbPoints();
1159 TrackerWrapper *tracker;
1160 tracker = it_tracker->second;
1161 return tracker->getKltOpencv();
1180 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1181 klt1 = it->second->getKltOpencv();
1184 klt2 = it->second->getKltOpencv();
1186 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 1200 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1202 TrackerWrapper *tracker = it->second;
1203 mapOfKlts[it->first] = tracker->getKltOpencv();
1207 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 1217 TrackerWrapper *tracker = it->second;
1218 return tracker->getKltPoints();
1223 return std::vector<cv::Point2f>();
1252 it->second->getLcircle(circlesList, level);
1272 unsigned int level)
const 1274 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1276 it->second->getLcircle(circlesList, level);
1278 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1298 it->second->getLcylinder(cylindersList, level);
1318 unsigned int level)
const 1320 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1322 it->second->getLcylinder(cylindersList, level);
1324 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1345 it->second->getLline(linesList, level);
1365 unsigned int level)
const 1367 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1369 it->second->getLline(linesList, level);
1371 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1407 bool displayFullModel)
1412 return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1417 return std::vector<std::vector<double> >();
1446 const std::map<std::string, unsigned int> &mapOfwidths,
1447 const std::map<std::string, unsigned int> &mapOfheights,
1448 const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
1449 const std::map<std::string, vpCameraParameters> &mapOfCams,
1450 bool displayFullModel)
1453 mapOfModels.clear();
1455 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1457 std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
1458 std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
1459 std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
1460 std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
1462 if (findWidth != mapOfwidths.end() && findHeight != mapOfheights.end() && findcMo != mapOfcMos.end() &&
1463 findCam != mapOfCams.end()) {
1464 mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second, findcMo->second,
1465 findCam->second, displayFullModel);
1480 return it->second->getMovingEdge();
1499 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1500 it->second->getMovingEdge(me1);
1503 it->second->getMovingEdge(me2);
1505 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 1517 mapOfMovingEdges.clear();
1519 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1521 TrackerWrapper *tracker = it->second;
1522 mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1547 unsigned int nbGoodPoints = 0;
1550 nbGoodPoints = it->second->getNbPoints(level);
1555 return nbGoodPoints;
1574 mapOfNbPoints.clear();
1576 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1578 TrackerWrapper *tracker = it->second;
1579 mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1592 return it->second->getNbPolygon();
1606 mapOfNbPolygons.clear();
1608 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1610 TrackerWrapper *tracker = it->second;
1611 mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1629 return it->second->getPolygon(index);
1649 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1651 return it->second->getPolygon(index);
1654 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1673 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1676 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1680 TrackerWrapper *tracker = it->second;
1681 polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1686 return polygonFaces;
1707 std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1708 bool orderPolygons,
bool useVisibility,
bool clipPolygon)
1710 mapOfPolygons.clear();
1711 mapOfPoints.clear();
1713 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1715 TrackerWrapper *tracker = it->second;
1716 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1717 tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1719 mapOfPolygons[it->first] = polygonFaces.first;
1720 mapOfPoints[it->first] = polygonFaces.second;
1737 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1738 it->second->getPose(c1Mo);
1741 it->second->getPose(c2Mo);
1743 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 1756 mapOfCameraPoses.clear();
1758 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1760 TrackerWrapper *tracker = it->second;
1761 tracker->getPose(mapOfCameraPoses[it->first]);
1777 TrackerWrapper *tracker = it->second;
1778 return tracker->getTrackerType();
1787 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1789 TrackerWrapper *tracker = it->second;
1796 double ,
int ,
const std::string & )
1801 #ifdef VISP_HAVE_MODULE_GUI 1846 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1850 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1851 TrackerWrapper *tracker = it->second;
1852 tracker->initClick(I1, initFile1, displayHelp, T1);
1856 tracker = it->second;
1857 tracker->initClick(I2, initFile2, displayHelp, T2);
1861 tracker = it->second;
1864 tracker->getPose(
m_cMo);
1868 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1915 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1919 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1920 TrackerWrapper *tracker = it->second;
1921 tracker->initClick(I_color1, initFile1, displayHelp, T1);
1925 tracker = it->second;
1926 tracker->initClick(I_color2, initFile2, displayHelp, T2);
1930 tracker = it->second;
1933 tracker->getPose(
m_cMo);
1937 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1984 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
1985 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
1988 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1990 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
1992 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1993 TrackerWrapper *tracker = it_tracker->second;
1994 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
1995 if (it_T != mapOfT.end())
1996 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
1998 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1999 tracker->getPose(
m_cMo);
2005 std::vector<std::string> vectorOfMissingCameraPoses;
2010 it_img = mapOfImages.find(it_tracker->first);
2011 it_initFile = mapOfInitFiles.find(it_tracker->first);
2013 if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
2015 TrackerWrapper *tracker = it_tracker->second;
2016 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2018 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2024 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2025 it != vectorOfMissingCameraPoses.end(); ++it) {
2026 it_img = mapOfImages.find(*it);
2027 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2036 "Missing image or missing camera transformation " 2037 "matrix! Cannot set the pose for camera: %s!",
2086 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2087 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2090 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2091 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2093 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2094 TrackerWrapper *tracker = it_tracker->second;
2095 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2096 if (it_T != mapOfT.end())
2097 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2099 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2100 tracker->getPose(
m_cMo);
2106 std::vector<std::string> vectorOfMissingCameraPoses;
2111 it_img = mapOfColorImages.find(it_tracker->first);
2112 it_initFile = mapOfInitFiles.find(it_tracker->first);
2114 if (it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2116 TrackerWrapper *tracker = it_tracker->second;
2117 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2119 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2125 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2126 it != vectorOfMissingCameraPoses.end(); ++it) {
2127 it_img = mapOfColorImages.find(*it);
2128 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2138 "Missing image or missing camera transformation " 2139 "matrix! Cannot set the pose for camera: %s!",
2147 const int ,
const std::string & )
2192 const std::string &initFile1,
const std::string &initFile2)
2195 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2196 TrackerWrapper *tracker = it->second;
2197 tracker->initFromPoints(I1, initFile1);
2201 tracker = it->second;
2202 tracker->initFromPoints(I2, initFile2);
2206 tracker = it->second;
2209 tracker->getPose(
m_cMo);
2212 tracker->getCameraParameters(
m_cam);
2216 "Cannot initFromPoints()! Require two cameras but " 2217 "there are %d cameras!",
2252 const std::string &initFile1,
const std::string &initFile2)
2255 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2256 TrackerWrapper *tracker = it->second;
2257 tracker->initFromPoints(I_color1, initFile1);
2261 tracker = it->second;
2262 tracker->initFromPoints(I_color2, initFile2);
2266 tracker = it->second;
2269 tracker->getPose(
m_cMo);
2272 tracker->getCameraParameters(
m_cam);
2276 "Cannot initFromPoints()! Require two cameras but " 2277 "there are %d cameras!",
2283 const std::map<std::string, std::string> &mapOfInitPoints)
2287 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2289 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2291 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2292 TrackerWrapper *tracker = it_tracker->second;
2293 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2294 tracker->getPose(
m_cMo);
2300 std::vector<std::string> vectorOfMissingCameraPoints;
2304 it_img = mapOfImages.find(it_tracker->first);
2305 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2307 if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2309 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2311 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2315 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2316 it != vectorOfMissingCameraPoints.end(); ++it) {
2317 it_img = mapOfImages.find(*it);
2318 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2326 "Missing image or missing camera transformation " 2327 "matrix! Cannot init the pose for camera: %s!",
2334 const std::map<std::string, std::string> &mapOfInitPoints)
2338 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2339 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2341 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() &&
2342 it_initPoints != mapOfInitPoints.end()) {
2343 TrackerWrapper *tracker = it_tracker->second;
2344 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2345 tracker->getPose(
m_cMo);
2351 std::vector<std::string> vectorOfMissingCameraPoints;
2355 it_img = mapOfColorImages.find(it_tracker->first);
2356 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2358 if (it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2360 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2362 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2366 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2367 it != vectorOfMissingCameraPoints.end(); ++it) {
2368 it_img = mapOfColorImages.find(*it);
2369 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2377 "Missing image or missing camera transformation " 2378 "matrix! Cannot init the pose for camera: %s!",
2401 const std::string &initFile1,
const std::string &initFile2)
2404 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2405 TrackerWrapper *tracker = it->second;
2406 tracker->initFromPose(I1, initFile1);
2410 tracker = it->second;
2411 tracker->initFromPose(I2, initFile2);
2415 tracker = it->second;
2418 tracker->getPose(
m_cMo);
2421 tracker->getCameraParameters(
m_cam);
2425 "Cannot initFromPose()! Require two cameras but there " 2443 const std::string &initFile1,
const std::string &initFile2)
2446 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2447 TrackerWrapper *tracker = it->second;
2448 tracker->initFromPose(I_color1, initFile1);
2452 tracker = it->second;
2453 tracker->initFromPose(I_color2, initFile2);
2457 tracker = it->second;
2460 tracker->getPose(
m_cMo);
2463 tracker->getCameraParameters(
m_cam);
2467 "Cannot initFromPose()! Require two cameras but there " 2488 const std::map<std::string, std::string> &mapOfInitPoses)
2492 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2494 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2496 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2497 TrackerWrapper *tracker = it_tracker->second;
2498 tracker->initFromPose(*it_img->second, it_initPose->second);
2499 tracker->getPose(
m_cMo);
2505 std::vector<std::string> vectorOfMissingCameraPoses;
2509 it_img = mapOfImages.find(it_tracker->first);
2510 it_initPose = mapOfInitPoses.find(it_tracker->first);
2512 if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2514 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2516 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2520 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2521 it != vectorOfMissingCameraPoses.end(); ++it) {
2522 it_img = mapOfImages.find(*it);
2523 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2531 "Missing image or missing camera transformation " 2532 "matrix! Cannot init the pose for camera: %s!",
2553 const std::map<std::string, std::string> &mapOfInitPoses)
2557 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2558 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2560 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2561 TrackerWrapper *tracker = it_tracker->second;
2562 tracker->initFromPose(*it_img->second, it_initPose->second);
2563 tracker->getPose(
m_cMo);
2569 std::vector<std::string> vectorOfMissingCameraPoses;
2573 it_img = mapOfColorImages.find(it_tracker->first);
2574 it_initPose = mapOfInitPoses.find(it_tracker->first);
2576 if (it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2578 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2580 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2584 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2585 it != vectorOfMissingCameraPoses.end(); ++it) {
2586 it_img = mapOfColorImages.find(*it);
2587 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2595 "Missing image or missing camera transformation " 2596 "matrix! Cannot init the pose for camera: %s!",
2616 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2617 it->second->initFromPose(I1, c1Mo);
2621 it->second->initFromPose(I2, c2Mo);
2626 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2644 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2645 it->second->initFromPose(I_color1, c1Mo);
2649 it->second->initFromPose(I_color2, c2Mo);
2654 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2672 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2676 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2678 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2680 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2681 TrackerWrapper *tracker = it_tracker->second;
2682 tracker->initFromPose(*it_img->second, it_camPose->second);
2683 tracker->getPose(
m_cMo);
2689 std::vector<std::string> vectorOfMissingCameraPoses;
2693 it_img = mapOfImages.find(it_tracker->first);
2694 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2696 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2698 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2700 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2704 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2705 it != vectorOfMissingCameraPoses.end(); ++it) {
2706 it_img = mapOfImages.find(*it);
2707 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2715 "Missing image or missing camera transformation " 2716 "matrix! Cannot set the pose for camera: %s!",
2736 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2740 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2741 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2743 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2744 TrackerWrapper *tracker = it_tracker->second;
2745 tracker->initFromPose(*it_img->second, it_camPose->second);
2746 tracker->getPose(
m_cMo);
2752 std::vector<std::string> vectorOfMissingCameraPoses;
2756 it_img = mapOfColorImages.find(it_tracker->first);
2757 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2759 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2761 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2763 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2767 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2768 it != vectorOfMissingCameraPoses.end(); ++it) {
2769 it_img = mapOfColorImages.find(*it);
2770 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2778 "Missing image or missing camera transformation " 2779 "matrix! Cannot set the pose for camera: %s!",
2798 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2800 TrackerWrapper *tracker = it->second;
2801 tracker->loadConfigFile(configFile, verbose);
2834 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2835 TrackerWrapper *tracker = it_tracker->second;
2836 tracker->loadConfigFile(configFile1, verbose);
2839 tracker = it_tracker->second;
2840 tracker->loadConfigFile(configFile2, verbose);
2867 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2869 TrackerWrapper *tracker = it_tracker->second;
2871 std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
2872 if (it_config != mapOfConfigFiles.end()) {
2873 tracker->loadConfigFile(it_config->second, verbose);
2876 it_tracker->first.c_str());
2883 TrackerWrapper *tracker = it->second;
2884 tracker->getCameraParameters(
m_cam);
2916 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2918 TrackerWrapper *tracker = it->second;
2919 tracker->loadModel(modelFile, verbose, T);
2952 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2953 TrackerWrapper *tracker = it_tracker->second;
2954 tracker->loadModel(modelFile1, verbose, T1);
2957 tracker = it_tracker->second;
2958 tracker->loadModel(modelFile2, verbose, T2);
2981 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2983 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2985 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
2987 if (it_model != mapOfModelFiles.end()) {
2988 TrackerWrapper *tracker = it_tracker->second;
2989 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2991 if (it_T != mapOfT.end())
2992 tracker->loadModel(it_model->second, verbose, it_T->second);
2994 tracker->loadModel(it_model->second, verbose);
2997 it_tracker->first.c_str());
3002 #ifdef VISP_HAVE_PCL 3004 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
3006 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3008 TrackerWrapper *tracker = it->second;
3009 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
3015 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
3016 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
3017 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
3019 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3021 TrackerWrapper *tracker = it->second;
3022 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
3023 mapOfPointCloudHeights[it->first]);
3048 TrackerWrapper *tracker = it_tracker->second;
3049 tracker->reInitModel(I, cad_name, cMo, verbose, T);
3052 tracker->getPose(
m_cMo);
3081 TrackerWrapper *tracker = it_tracker->second;
3082 tracker->reInitModel(I_color, cad_name, cMo, verbose, T);
3085 tracker->getPose(
m_cMo);
3114 const std::string &cad_name1,
const std::string &cad_name2,
3119 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3121 it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
3125 it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
3130 it_tracker->second->getPose(
m_cMo);
3160 const std::string &cad_name1,
const std::string &cad_name2,
3165 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3167 it_tracker->second->reInitModel(I_color1, cad_name1, c1Mo, verbose, T1);
3171 it_tracker->second->reInitModel(I_color2, cad_name2, c2Mo, verbose, T2);
3176 it_tracker->second->getPose(
m_cMo);
3200 const std::map<std::string, std::string> &mapOfModelFiles,
3201 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
bool verbose,
3202 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3205 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3207 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3208 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3210 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3211 it_camPose != mapOfCameraPoses.end()) {
3212 TrackerWrapper *tracker = it_tracker->second;
3213 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3214 if (it_T != mapOfT.end())
3215 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3217 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3220 tracker->getPose(
m_cMo);
3225 std::vector<std::string> vectorOfMissingCameras;
3228 it_img = mapOfImages.find(it_tracker->first);
3229 it_model = mapOfModelFiles.find(it_tracker->first);
3230 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3232 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3233 TrackerWrapper *tracker = it_tracker->second;
3234 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3236 vectorOfMissingCameras.push_back(it_tracker->first);
3241 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3243 it_img = mapOfImages.find(*it);
3244 it_model = mapOfModelFiles.find(*it);
3245 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3248 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3251 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3273 const std::map<std::string, std::string> &mapOfModelFiles,
3274 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
bool verbose,
3275 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3278 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
3279 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3280 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3282 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3283 it_camPose != mapOfCameraPoses.end()) {
3284 TrackerWrapper *tracker = it_tracker->second;
3285 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3286 if (it_T != mapOfT.end())
3287 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3289 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3292 tracker->getPose(
m_cMo);
3297 std::vector<std::string> vectorOfMissingCameras;
3300 it_img = mapOfColorImages.find(it_tracker->first);
3301 it_model = mapOfModelFiles.find(it_tracker->first);
3302 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3304 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3305 it_camPose != mapOfCameraPoses.end()) {
3306 TrackerWrapper *tracker = it_tracker->second;
3307 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3309 vectorOfMissingCameras.push_back(it_tracker->first);
3314 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3316 it_img = mapOfColorImages.find(*it);
3317 it_model = mapOfModelFiles.find(*it);
3318 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3321 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3324 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3341 #ifdef VISP_HAVE_OGRE 3368 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 3375 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3377 TrackerWrapper *tracker = it->second;
3378 tracker->resetTracker();
3395 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3397 TrackerWrapper *tracker = it->second;
3398 tracker->setAngleAppear(a);
3417 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3418 it->second->setAngleAppear(a1);
3421 it->second->setAngleAppear(a2);
3446 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3447 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3450 TrackerWrapper *tracker = it_tracker->second;
3451 tracker->setAngleAppear(it->second);
3473 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3475 TrackerWrapper *tracker = it->second;
3476 tracker->setAngleDisappear(a);
3495 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3496 it->second->setAngleDisappear(a1);
3499 it->second->setAngleDisappear(a2);
3524 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3525 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3528 TrackerWrapper *tracker = it_tracker->second;
3529 tracker->setAngleDisappear(it->second);
3547 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3549 TrackerWrapper *tracker = it->second;
3550 tracker->setCameraParameters(camera);
3565 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3566 it->second->setCameraParameters(camera1);
3569 it->second->setCameraParameters(camera2);
3573 it->second->getCameraParameters(
m_cam);
3593 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
3594 it != mapOfCameraParameters.end(); ++it) {
3595 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3598 TrackerWrapper *tracker = it_tracker->second;
3599 tracker->setCameraParameters(it->second);
3622 it->second = cameraTransformationMatrix;
3636 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
3638 for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
3639 it != mapOfTransformationMatrix.end(); ++it) {
3640 std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
3644 it_camTrans->second = it->second;
3662 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3664 TrackerWrapper *tracker = it->second;
3665 tracker->setClipping(flags);
3682 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3683 it->second->setClipping(flags1);
3686 it->second->setClipping(flags2);
3695 std::stringstream ss;
3696 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
3710 for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
3711 it != mapOfClippingFlags.end(); ++it) {
3712 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3715 TrackerWrapper *tracker = it_tracker->second;
3716 tracker->setClipping(it->second);
3736 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3738 TrackerWrapper *tracker = it->second;
3739 tracker->setDepthDenseFilteringMaxDistance(maxDistance);
3753 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3755 TrackerWrapper *tracker = it->second;
3756 tracker->setDepthDenseFilteringMethod(method);
3771 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3773 TrackerWrapper *tracker = it->second;
3774 tracker->setDepthDenseFilteringMinDistance(minDistance);
3789 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3791 TrackerWrapper *tracker = it->second;
3792 tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
3806 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3808 TrackerWrapper *tracker = it->second;
3809 tracker->setDepthDenseSamplingStep(stepX, stepY);
3822 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3824 TrackerWrapper *tracker = it->second;
3825 tracker->setDepthNormalFaceCentroidMethod(method);
3839 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3841 TrackerWrapper *tracker = it->second;
3842 tracker->setDepthNormalFeatureEstimationMethod(method);
3855 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3857 TrackerWrapper *tracker = it->second;
3858 tracker->setDepthNormalPclPlaneEstimationMethod(method);
3871 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3873 TrackerWrapper *tracker = it->second;
3874 tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
3887 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3889 TrackerWrapper *tracker = it->second;
3890 tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
3904 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3906 TrackerWrapper *tracker = it->second;
3907 tracker->setDepthNormalSamplingStep(stepX, stepY);
3933 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3935 TrackerWrapper *tracker = it->second;
3936 tracker->setDisplayFeatures(displayF);
3951 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3953 TrackerWrapper *tracker = it->second;
3954 tracker->setFarClippingDistance(dist);
3969 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3970 it->second->setFarClippingDistance(dist1);
3973 it->second->setFarClippingDistance(dist2);
3977 distFarClip = it->second->getFarClippingDistance();
3994 for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
3996 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3999 TrackerWrapper *tracker = it_tracker->second;
4000 tracker->setFarClippingDistance(it->second);
4019 std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
4020 if (it_factor != mapOfFeatureFactors.end()) {
4021 it->second = it_factor->second;
4045 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4047 TrackerWrapper *tracker = it->second;
4048 tracker->setGoodMovingEdgesRatioThreshold(threshold);
4052 #ifdef VISP_HAVE_OGRE 4066 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4068 TrackerWrapper *tracker = it->second;
4069 tracker->setGoodNbRayCastingAttemptsRatio(ratio);
4086 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4088 TrackerWrapper *tracker = it->second;
4089 tracker->setNbRayCastingAttemptsForVisibility(attempts);
4094 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 4104 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4106 TrackerWrapper *tracker = it->second;
4107 tracker->setKltOpencv(t);
4122 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4123 it->second->setKltOpencv(t1);
4126 it->second->setKltOpencv(t2);
4140 for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
4141 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4144 TrackerWrapper *tracker = it_tracker->second;
4145 tracker->setKltOpencv(it->second);
4161 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4163 TrackerWrapper *tracker = it->second;
4164 tracker->setKltThresholdAcceptation(th);
4183 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4185 TrackerWrapper *tracker = it->second;
4186 tracker->setLod(useLod, name);
4190 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 4200 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4202 TrackerWrapper *tracker = it->second;
4203 tracker->setKltMaskBorder(e);
4218 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4219 it->second->setKltMaskBorder(e1);
4223 it->second->setKltMaskBorder(e2);
4237 for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
4239 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4242 TrackerWrapper *tracker = it_tracker->second;
4243 tracker->setKltMaskBorder(it->second);
4258 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4260 TrackerWrapper *tracker = it->second;
4261 tracker->setMask(mask);
4278 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4280 TrackerWrapper *tracker = it->second;
4281 tracker->setMinLineLengthThresh(minLineLengthThresh, name);
4297 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4299 TrackerWrapper *tracker = it->second;
4300 tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
4313 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4315 TrackerWrapper *tracker = it->second;
4316 tracker->setMovingEdge(me);
4332 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4333 it->second->setMovingEdge(me1);
4337 it->second->setMovingEdge(me2);
4351 for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
4352 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4355 TrackerWrapper *tracker = it_tracker->second;
4356 tracker->setMovingEdge(it->second);
4372 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4374 TrackerWrapper *tracker = it->second;
4375 tracker->setNearClippingDistance(dist);
4390 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4391 it->second->setNearClippingDistance(dist1);
4395 it->second->setNearClippingDistance(dist2);
4416 for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
4417 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4420 TrackerWrapper *tracker = it_tracker->second;
4421 tracker->setNearClippingDistance(it->second);
4446 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4448 TrackerWrapper *tracker = it->second;
4449 tracker->setOgreShowConfigDialog(showConfigDialog);
4467 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4469 TrackerWrapper *tracker = it->second;
4470 tracker->setOgreVisibilityTest(v);
4473 #ifdef VISP_HAVE_OGRE 4474 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4476 TrackerWrapper *tracker = it->second;
4477 tracker->faces.getOgreContext()->setWindowName(
"Multi Generic MBT (" + it->first +
")");
4493 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4495 TrackerWrapper *tracker = it->second;
4496 tracker->setOptimizationMethod(opt);
4516 "to be configured with only one camera!");
4523 TrackerWrapper *tracker = it->second;
4524 tracker->setPose(I, cdMo);
4547 "to be configured with only one camera!");
4554 TrackerWrapper *tracker = it->second;
4556 tracker->setPose(
m_I, cdMo);
4578 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4579 it->second->setPose(I1, c1Mo);
4583 it->second->setPose(I2, c2Mo);
4588 it->second->getPose(
m_cMo);
4614 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4615 it->second->setPose(I_color1, c1Mo);
4619 it->second->setPose(I_color2, c2Mo);
4624 it->second->getPose(
m_cMo);
4651 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4655 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
4657 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4659 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4660 TrackerWrapper *tracker = it_tracker->second;
4661 tracker->setPose(*it_img->second, it_camPose->second);
4662 tracker->getPose(
m_cMo);
4668 std::vector<std::string> vectorOfMissingCameraPoses;
4673 it_img = mapOfImages.find(it_tracker->first);
4674 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4676 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4678 TrackerWrapper *tracker = it_tracker->second;
4679 tracker->setPose(*it_img->second, it_camPose->second);
4681 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4686 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4687 it != vectorOfMissingCameraPoses.end(); ++it) {
4688 it_img = mapOfImages.find(*it);
4689 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4697 "Missing image or missing camera transformation " 4698 "matrix! Cannot set pose for camera: %s!",
4720 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4724 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
4725 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4727 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4728 TrackerWrapper *tracker = it_tracker->second;
4729 tracker->setPose(*it_img->second, it_camPose->second);
4730 tracker->getPose(
m_cMo);
4736 std::vector<std::string> vectorOfMissingCameraPoses;
4741 it_img = mapOfColorImages.find(it_tracker->first);
4742 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4744 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4746 TrackerWrapper *tracker = it_tracker->second;
4747 tracker->setPose(*it_img->second, it_camPose->second);
4749 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4754 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4755 it != vectorOfMissingCameraPoses.end(); ++it) {
4756 it_img = mapOfColorImages.find(*it);
4757 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4765 "Missing image or missing camera transformation " 4766 "matrix! Cannot set pose for camera: %s!",
4790 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4792 TrackerWrapper *tracker = it->second;
4793 tracker->setProjectionErrorComputation(flag);
4804 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4806 TrackerWrapper *tracker = it->second;
4807 tracker->setProjectionErrorDisplay(display);
4818 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4820 TrackerWrapper *tracker = it->second;
4821 tracker->setProjectionErrorDisplayArrowLength(length);
4829 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4831 TrackerWrapper *tracker = it->second;
4832 tracker->setProjectionErrorDisplayArrowThickness(thickness);
4843 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(referenceCameraName);
4847 std::cerr <<
"The reference camera: " << referenceCameraName <<
" does not exist!";
4855 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4857 TrackerWrapper *tracker = it->second;
4858 tracker->setScanLineVisibilityTest(v);
4875 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4877 TrackerWrapper *tracker = it->second;
4878 tracker->setTrackerType(type);
4893 for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
4894 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4896 TrackerWrapper *tracker = it_tracker->second;
4897 tracker->setTrackerType(it->second);
4913 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4915 TrackerWrapper *tracker = it->second;
4916 tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
4931 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4933 TrackerWrapper *tracker = it->second;
4934 tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
4949 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4951 TrackerWrapper *tracker = it->second;
4952 tracker->setUseEdgeTracking(name, useEdgeTracking);
4956 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 4968 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4970 TrackerWrapper *tracker = it->second;
4971 tracker->setUseKltTracking(name, useKltTracking);
4979 bool isOneTestTrackingOk =
false;
4980 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4982 TrackerWrapper *tracker = it->second;
4984 tracker->testTracking();
4985 isOneTestTrackingOk =
true;
4990 if (!isOneTestTrackingOk) {
4991 std::ostringstream oss;
4992 oss <<
"Not enough moving edges to track the object. Try to reduce the " 5010 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5013 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5014 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5016 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5030 std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
5033 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5034 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5036 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5052 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5053 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5054 mapOfImages[it->first] = &I1;
5057 mapOfImages[it->first] = &I2;
5059 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5060 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5062 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5064 std::stringstream ss;
5065 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5083 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5084 std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
5085 mapOfImages[it->first] = &I_color1;
5088 mapOfImages[it->first] = &_colorI2;
5090 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5091 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5093 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5095 std::stringstream ss;
5096 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5110 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5111 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5113 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5125 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5126 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5128 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5131 #ifdef VISP_HAVE_PCL 5141 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5143 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5145 TrackerWrapper *tracker = it->second;
5148 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5156 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5160 mapOfImages[it->first] == NULL) {
5165 !mapOfPointClouds[it->first]) {
5181 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5183 TrackerWrapper *tracker = it->second;
5186 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5189 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5192 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5194 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5199 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5216 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5218 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5219 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5221 TrackerWrapper *tracker = it->second;
5224 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5232 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5236 mapOfImages[it->first] == NULL) {
5239 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5243 mapOfImages[it->first] != NULL) {
5245 mapOfImages[it->first] = &tracker->m_I;
5249 !mapOfPointClouds[it->first]) {
5265 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5267 TrackerWrapper *tracker = it->second;
5270 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5273 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5276 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5278 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5283 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5303 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5304 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5305 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5307 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5309 TrackerWrapper *tracker = it->second;
5312 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5320 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5324 mapOfImages[it->first] == NULL) {
5329 (mapOfPointClouds[it->first] == NULL)) {
5334 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5345 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5347 TrackerWrapper *tracker = it->second;
5350 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5353 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5356 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5358 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5363 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5382 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5383 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5384 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5386 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5387 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5389 TrackerWrapper *tracker = it->second;
5392 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5400 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5404 mapOfColorImages[it->first] == NULL) {
5407 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5411 mapOfColorImages[it->first] != NULL) {
5413 mapOfImages[it->first] = &tracker->m_I;
5417 (mapOfPointClouds[it->first] == NULL)) {
5422 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5433 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5435 TrackerWrapper *tracker = it->second;
5438 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5441 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5444 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5446 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5451 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5460 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
5466 #ifdef VISP_HAVE_OGRE 5473 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
int trackerType)
5477 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5487 #ifdef VISP_HAVE_OGRE 5494 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() {}
5506 double normRes_1 = -1;
5507 unsigned int iter = 0;
5509 double factorEdge = 1.0;
5510 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5511 double factorKlt = 1.0;
5513 double factorDepth = 1.0;
5514 double factorDepthDense = 1.0;
5522 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5525 bool isoJoIdentity_ =
true;
5531 unsigned int nb_edge_features = m_error_edge.
getRows();
5532 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5533 unsigned int nb_klt_features = m_error_klt.getRows();
5535 unsigned int nb_depth_features = m_error_depthNormal.getRows();
5536 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5541 bool reStartFromLastIncrement =
false;
5544 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5545 if (reStartFromLastIncrement) {
5552 if (!reStartFromLastIncrement) {
5557 if (!isoJoIdentity_) {
5560 LVJ_true = (
m_L * cVo *
oJo);
5566 isoJoIdentity_ =
true;
5573 if (isoJoIdentity_) {
5577 unsigned int rank = (
m_L * cVo).kernel(K);
5587 isoJoIdentity_ =
false;
5596 unsigned int start_index = 0;
5598 for (
unsigned int i = 0; i < nb_edge_features; i++) {
5599 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5606 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5611 start_index += nb_edge_features;
5614 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5616 for (
unsigned int i = 0; i < nb_klt_features; i++) {
5617 double wi = m_w_klt[i] * factorKlt;
5618 W_true[start_index + i] = wi;
5624 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5625 m_L[start_index + i][j] *= wi;
5629 start_index += nb_klt_features;
5634 for (
unsigned int i = 0; i < nb_depth_features; i++) {
5635 double wi = m_w_depthNormal[i] * factorDepth;
5636 m_w[start_index + i] = m_w_depthNormal[i];
5639 num += wi *
vpMath::sqr(m_error[start_index + i]);
5642 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5643 m_L[start_index + i][j] *= wi;
5647 start_index += nb_depth_features;
5651 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
5652 double wi = m_w_depthDense[i] * factorDepthDense;
5653 m_w[start_index + i] = m_w_depthDense[i];
5656 num += wi *
vpMath::sqr(m_error[start_index + i]);
5659 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5660 m_L[start_index + i][j] *= wi;
5670 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5671 if (m_trackerType & KLT_TRACKER) {
5678 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5679 if (m_trackerType & KLT_TRACKER) {
5683 normRes_1 = normRes;
5685 normRes = sqrt(num / den);
5698 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
5701 "TrackerWrapper::computeVVSInit(" 5702 ") should not be called!");
5707 initMbtTracking(ptr_I);
5709 unsigned int nbFeatures = 0;
5712 nbFeatures += m_error_edge.getRows();
5714 m_error_edge.clear();
5715 m_weightedError_edge.clear();
5720 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5723 nbFeatures += m_error_klt.getRows();
5725 m_error_klt.clear();
5726 m_weightedError_klt.clear();
5734 nbFeatures += m_error_depthNormal.getRows();
5736 m_error_depthNormal.clear();
5737 m_weightedError_depthNormal.clear();
5738 m_L_depthNormal.clear();
5739 m_w_depthNormal.clear();
5744 nbFeatures += m_error_depthDense.getRows();
5746 m_error_depthDense.clear();
5747 m_weightedError_depthDense.clear();
5748 m_L_depthDense.clear();
5749 m_w_depthDense.clear();
5752 m_L.
resize(nbFeatures, 6,
false,
false);
5760 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu()
5764 "computeVVSInteractionMatrixAndR" 5765 "esidu() should not be called!");
5768 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(
const vpImage<unsigned char> *
const ptr_I)
5774 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5788 unsigned int start_index = 0;
5789 if (m_trackerType & EDGE_TRACKER) {
5793 start_index += m_error_edge.getRows();
5796 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5797 if (m_trackerType & KLT_TRACKER) {
5801 start_index += m_error_klt.getRows();
5805 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5806 m_L.
insert(m_L_depthNormal, start_index, 0);
5809 start_index += m_error_depthNormal.getRows();
5812 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5813 m_L.
insert(m_L_depthDense, start_index, 0);
5822 unsigned int start_index = 0;
5828 start_index += m_w_edge.getRows();
5831 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5836 start_index += m_w_klt.getRows();
5841 if (m_depthNormalUseRobust) {
5843 m_w.
insert(start_index, m_w_depthNormal);
5846 start_index += m_w_depthNormal.getRows();
5851 m_w.
insert(start_index, m_w_depthDense);
5859 unsigned int thickness,
bool displayFullModel)
5863 for (
size_t i = 0; i < features.size(); i++) {
5866 int state =
static_cast<int>(features[i][3]);
5897 double id = features[i][5];
5898 std::stringstream ss;
5902 vpImagePoint im_centroid(features[i][1], features[i][2]);
5903 vpImagePoint im_extremity(features[i][3], features[i][4]);
5910 std::vector<std::vector<double> > models =
5912 for (
size_t i = 0; i < models.size(); i++) {
5919 double n20 = models[i][3];
5920 double n11 = models[i][4];
5921 double n02 = models[i][5];
5926 #ifdef VISP_HAVE_OGRE 5928 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5940 unsigned int thickness,
bool displayFullModel)
5944 for (
size_t i = 0; i < features.size(); i++) {
5947 int state =
static_cast<int>(features[i][3]);
5978 double id = features[i][5];
5979 std::stringstream ss;
5983 vpImagePoint im_centroid(features[i][1], features[i][2]);
5984 vpImagePoint im_extremity(features[i][3], features[i][4]);
5991 std::vector<std::vector<double> > models =
5993 for (
size_t i = 0; i < models.size(); i++) {
6000 double n20 = models[i][3];
6001 double n11 = models[i][4];
6002 double n02 = models[i][5];
6007 #ifdef VISP_HAVE_OGRE 6009 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6019 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6021 std::vector<std::vector<double> > features;
6025 features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6028 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6031 features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6037 features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(),
6038 m_featuresToBeDisplayedDepthNormal.end());
6044 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(
unsigned int width,
6045 unsigned int height,
6048 bool displayFullModel)
6050 std::vector<std::vector<double> > models;
6056 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6068 std::vector<std::vector<double> > edgeModels =
6070 models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6075 std::vector<std::vector<double> > depthDenseModels =
6077 models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6093 bool reInitialisation =
false;
6097 #ifdef VISP_HAVE_OGRE 6120 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6141 void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
6142 double radius,
int idFace,
const std::string &name)
6147 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6153 void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
int idFace,
6154 const std::string &name)
6159 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6165 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
6170 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6182 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
6187 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6207 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile,
bool verbose)
6229 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6247 std::cout <<
" *********** Parsing XML for";
6250 std::vector<std::string> tracker_names;
6252 tracker_names.push_back(
"Edge");
6253 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6255 tracker_names.push_back(
"Klt");
6258 tracker_names.push_back(
"Depth Normal");
6260 tracker_names.push_back(
"Depth Dense");
6263 for (
size_t i = 0; i < tracker_names.size(); i++) {
6264 std::cout <<
" " << tracker_names[i];
6265 if (i == tracker_names.size() - 1) {
6270 std::cout <<
"Model-Based Tracker ************ " << std::endl;
6273 xmlp.
parse(configFile);
6313 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6338 #ifdef VISP_HAVE_PCL 6340 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6342 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6353 bool newvisibleface =
false;
6371 if (m_trackerType & EDGE_TRACKER) {
6385 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6391 std::cerr <<
"Error in moving edge tracking" << std::endl;
6396 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6401 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6411 std::cerr <<
"Error in Depth normal tracking" << std::endl;
6420 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6428 const unsigned int pointcloud_width,
6429 const unsigned int pointcloud_height)
6431 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6442 bool newvisibleface =
false;
6460 if (m_trackerType & EDGE_TRACKER) {
6474 const std::vector<vpColVector> *
const point_cloud,
6475 const unsigned int pointcloud_width,
6476 const unsigned int pointcloud_height)
6482 std::cerr <<
"Error in moving edge tracking" << std::endl;
6487 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6492 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6502 std::cerr <<
"Error in Depth tracking" << std::endl;
6511 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6529 for (
unsigned int i = 0; i < scales.size(); i++) {
6531 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
6538 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
6546 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
6554 cylinders[i].clear();
6562 nbvisiblepolygone = 0;
6565 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6566 #if (VISP_HAVE_OPENCV_VERSION < 0x020408) 6568 cvReleaseImage(&cur);
6574 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
6576 if (kltpoly != NULL) {
6581 kltPolygons.clear();
6583 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
6586 if (kltPolyCylinder != NULL) {
6587 delete kltPolyCylinder;
6589 kltPolyCylinder = NULL;
6591 kltCylinders.clear();
6594 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
6601 circles_disp.clear();
6603 firstInitialisation =
true;
6608 for (
size_t i = 0; i < m_depthNormalFaces.size(); i++) {
6609 delete m_depthNormalFaces[i];
6610 m_depthNormalFaces[i] = NULL;
6612 m_depthNormalFaces.clear();
6615 for (
size_t i = 0; i < m_depthDenseFaces.size(); i++) {
6616 delete m_depthDenseFaces[i];
6617 m_depthDenseFaces[i] = NULL;
6619 m_depthDenseFaces.clear();
6631 void vpMbGenericTracker::TrackerWrapper::reInitModel(
const vpImage<unsigned char> &I,
const std::string &cad_name,
6638 void vpMbGenericTracker::TrackerWrapper::reInitModel(
const vpImage<vpRGBa> &I_color,
const std::string &cad_name,
6642 reInitModel(NULL, &I_color, cad_name, cMo, verbose, T);
6645 void vpMbGenericTracker::TrackerWrapper::resetTracker()
6648 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6655 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(
const vpCameraParameters &cam)
6660 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6669 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(
const double &dist)
6674 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(
const double &dist)
6679 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(
const bool &v)
6682 #ifdef VISP_HAVE_OGRE 6690 bool performKltSetPose =
false;
6695 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6697 performKltSetPose =
true;
6707 if (!performKltSetPose) {
6723 if (m_trackerType & EDGE_TRACKER) {
6724 initPyramid(I, Ipyramid);
6726 unsigned int i = (
unsigned int) scales.size();
6736 cleanPyramid(Ipyramid);
6739 if (m_trackerType & EDGE_TRACKER) {
6758 setPose(NULL, &I_color, cdMo);
6761 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(
const bool &flag)
6766 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(
const bool &v)
6769 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6776 void vpMbGenericTracker::TrackerWrapper::setTrackerType(
int type)
6779 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6786 m_trackerType = type;
6789 void vpMbGenericTracker::TrackerWrapper::testTracking()
6797 #
if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6803 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6807 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6811 #if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 6816 void vpMbGenericTracker::TrackerWrapper::track(
const vpImage<vpRGBa> &)
6821 #ifdef VISP_HAVE_PCL 6823 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6826 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6830 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6835 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6862 postTracking(ptr_I, point_cloud);
6865 std::cerr <<
"Exception: " << e.
what() << std::endl;
virtual unsigned int getClipping() const
bool m_computeInteraction
void setWindowName(const Ogre::String &n)
unsigned int getDepthNormalSamplingStepY() const
virtual void setFeatureFactors(const std::map< vpTrackerType, double > &mapOfFeatureFactors)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Used to indicate that a value is not in the allowed range.
Implementation of a matrix and operations on matrices.
vpMatrix covarianceMatrix
Covariance matrix.
void setMovingEdge(const vpMe &me)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setScanLineVisibilityTest(const bool &v)
vpCameraParameters m_cam
The camera parameters.
virtual void computeVVSWeights()
Point removed during virtual visual-servoing because considered as an outlier.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void track(const vpImage< unsigned char > &I)
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual vpMbtPolygon * getPolygon(unsigned int index)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int m_nb_feat_edge
Number of moving-edges features.
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double threshold)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpColVector m_w
Robust weights.
void setVerbose(bool verbose)
bool hasFarClippingDistance() const
int getDepthNormalPclPlaneEstimationMethod() const
void setKltQuality(const double &q)
virtual void setAngleDisappear(const double &a)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual std::list< vpMbtDistanceKltPoints * > & getFeaturesKlt()
void getCameraParameters(vpCameraParameters &cam) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual std::map< std::string, int > getCameraTrackerTypes() const
void setKltPyramidLevels(const unsigned int &pL)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void setTrackerType(int type)
virtual int getTrackerType() const
virtual void setDepthDenseFilteringMethod(int method)
Point removed because too near image borders.
virtual void resetTracker()
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void setDepthNormalSamplingStepY(unsigned int stepY)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
Class to define RGB colors available for display functionnalities.
static bool equal(double x, double y, double s=0.001)
Point removed due to a threshold problem.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
double getFarClippingDistance() const
virtual unsigned int getNbPoints(unsigned int level=0) const
virtual void setClipping(const unsigned int &flags)
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
virtual void setDisplayFeatures(bool displayF)
double getDepthNormalPclPlaneEstimationRansacThreshold() const
virtual void resetTracker()
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void initClick(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2, bool displayHelp=false, const vpHomogeneousMatrix &T1=vpHomogeneousMatrix(), const vpHomogeneousMatrix &T2=vpHomogeneousMatrix())
virtual void setAngleDisappear(const double &a)
virtual void computeVVSInit()
virtual void setGoodMovingEdgesRatioThreshold(double threshold)
error that can be emited by ViSP classes.
Manage a cylinder used in the model-based tracker.
vpMbScanLine & getMbScanLineRenderer()
unsigned int getRows() const
vpHomogeneousMatrix inverse() const
virtual void setMask(const vpImage< bool > &mask)
Manage the line of a polygon used in the model-based tracker.
virtual double getGoodMovingEdgesRatioThreshold() const
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void computeVVSInteractionMatrixAndResidu()
bool useOgre
Use Ogre3d for visibility tests.
unsigned int getKltBlockSize() const
void setKltMaskBorder(const unsigned int &mb)
double getNearClippingDistance() const
virtual void setMovingEdge(const vpMe &me)
virtual void setLod(bool useLod, const std::string &name="")
virtual int getKltNbPoints() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void setEdgeMe(const vpMe &ecm)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
static const vpColor green
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void updateMovingEdge(const vpImage< unsigned char > &I)
std::map< vpTrackerType, double > m_mapOfFeatureFactors
Ponderation between each feature type in the VVS stage.
std::map< std::string, TrackerWrapper * > m_mapOfTrackers
virtual void computeVVS(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages)
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
unsigned int getDepthDenseSamplingStepY() const
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void computeVVSInit()
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
double getAngleAppear() const
virtual void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
virtual std::string getReferenceCameraName() const
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpMatrix m_L
Interaction matrix.
unsigned int getCols() const
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
double distFarClip
Distance for near clipping.
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
virtual vpHomogeneousMatrix getPose() const
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void getLcylinder(std::list< vpMbtDistanceCylinder *> &cylindersList, unsigned int level=0) const
Manage a circle used in the model-based tracker.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void setKltMaskBorder(const unsigned int &e)
void computeVisibility(unsigned int width, unsigned int height)
vpColVector m_weightedError
Weighted error.
virtual void setCameraParameters(const vpCameraParameters &cam)
Error that can be emited by the vpTracker class and its derivates.
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
Point used by the tracker.
static const vpColor cyan
vp_deprecated void init()
virtual void setScanLineVisibilityTest(const bool &v)
Implementation of a polygon of the model used by the model-based tracker.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setAngleDisappear(const double &adisappear)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void setProjectionErrorComputation(const bool &flag)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual std::vector< cv::Point2f > getKltPoints() const
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
virtual std::list< vpMbtDistanceCircle * > & getFeaturesCircle()
bool useScanLine
Use Scanline for visibility tests.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setKltMinDistance(const double &mD)
static double sqr(double x)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Point removed due to a contrast problem.
virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual unsigned int getNbPolygon() const
void setDepthNormalSamplingStepX(unsigned int stepX)
virtual void testTracking()
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
double getAngleDisappear() const
bool getFovClipping() const
Generic class defining intrinsic camera parameters.
virtual unsigned int getKltMaskBorder() const
unsigned int getKltWindowSize() const
void setOgreShowConfigDialog(bool showConfigDialog)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
unsigned int getKltMaxFeatures() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void setKltThresholdAcceptation(double th)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void setAngleAppear(const double &a)
double getKltMinDistance() const
virtual double getKltThresholdAcceptation() const
virtual void computeVVSInteractionMatrixAndResidu()
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
double m_lambda
Gain of the virtual visual servoing stage.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void setFarClippingDistance(const double &dist)
virtual void setAngleAppear(const double &a)
virtual void getLline(std::list< vpMbtDistanceLine *> &linesList, unsigned int level=0) const
void setDepthDenseSamplingStepY(unsigned int stepY)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
unsigned int getDepthNormalSamplingStepX() const
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
virtual std::vector< std::vector< double > > getFeaturesForDisplay()
void computeVisibility(unsigned int width, unsigned int height)
double angleAppears
Angle used to detect a face appearance.
void setKltWindowSize(const unsigned int &w)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
virtual void setScanLineVisibilityTest(const bool &v)
virtual void computeVVSWeights()
void setAngleAppear(const double &aappear)
const char * what() const
static double rad(double deg)
virtual void testTracking()
virtual void computeVVSInteractionMatrixAndResidu()
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
unsigned int m_nb_feat_depthDense
Number of depth dense features.
virtual void setProjectionErrorDisplay(bool display)
void setDepthDenseSamplingStepX(unsigned int stepX)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
void computeProjectionError(const vpImage< unsigned char > &_I)
void insert(unsigned int i, const vpColVector &v)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setNearClippingDistance(const double &dist)
virtual void computeVVSInit()
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void resetTracker()
void resize(unsigned int i, bool flagNullify=true)
virtual void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
virtual void setScanLineVisibilityTest(const bool &v)
void getEdgeMe(vpMe &ecm) const
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix <L, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.