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);
253 double rawTotalProjectionError = 0.0;
254 unsigned int nbTotalFeaturesUsed = 0;
256 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
258 TrackerWrapper *tracker = it->second;
260 double curProjError = tracker->getProjectionError();
261 unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
263 if (nbFeaturesUsed > 0) {
264 nbTotalFeaturesUsed += nbFeaturesUsed;
265 rawTotalProjectionError += (
vpMath::rad(curProjError) * nbFeaturesUsed);
269 if (nbTotalFeaturesUsed > 0) {
288 double normRes_1 = -1;
289 unsigned int iter = 0;
298 bool isoJoIdentity_ =
true;
305 std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
310 mapOfVelocityTwist[it->first] = cVo;
314 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 328 bool reStartFromLastIncrement =
false;
330 if (reStartFromLastIncrement) {
331 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
333 TrackerWrapper *tracker = it->second;
337 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 340 tracker->ctTc0 = c_curr_tTc_curr0;
345 if (!reStartFromLastIncrement) {
350 if (!isoJoIdentity_) {
353 LVJ_true = (
m_L * (cVo *
oJo));
359 isoJoIdentity_ =
true;
366 if (isoJoIdentity_) {
370 unsigned int rank = (
m_L * cVo).kernel(K);
380 isoJoIdentity_ =
false;
389 unsigned int start_index = 0;
390 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
392 TrackerWrapper *tracker = it->second;
395 for (
unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
396 double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
397 W_true[start_index + i] = wi;
403 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
404 m_L[start_index + i][j] *= wi;
408 start_index += tracker->m_error_edge.
getRows();
411 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 413 for (
unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
414 double wi = tracker->m_w_klt[i] * factorKlt;
415 W_true[start_index + i] = wi;
421 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
422 m_L[start_index + i][j] *= wi;
426 start_index += tracker->m_error_klt.
getRows();
431 for (
unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
432 double wi = tracker->m_w_depthNormal[i] * factorDepth;
433 W_true[start_index + i] = wi;
439 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
440 m_L[start_index + i][j] *= wi;
444 start_index += tracker->m_error_depthNormal.
getRows();
448 for (
unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
449 double wi = tracker->m_w_depthDense[i] * factorDepthDense;
450 W_true[start_index + i] = wi;
456 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
457 m_L[start_index + i][j] *= wi;
461 start_index += tracker->m_error_depthDense.
getRows();
466 normRes = sqrt(num / den);
474 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 475 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
477 TrackerWrapper *tracker = it->second;
481 tracker->ctTc0 = c_curr_tTc_curr0;
486 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
488 TrackerWrapper *tracker = it->second;
497 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
499 TrackerWrapper *tracker = it->second;
503 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 518 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
520 TrackerWrapper *tracker = it->second;
523 tracker->updateMovingEdgeWeights();
535 unsigned int nbFeatures = 0;
537 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
539 TrackerWrapper *tracker = it->second;
540 tracker->computeVVSInit(mapOfImages[it->first]);
542 nbFeatures += tracker->m_error.getRows();
556 "computeVVSInteractionMatrixAndR" 557 "esidu() should not be called");
562 std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
564 unsigned int start_index = 0;
566 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
568 TrackerWrapper *tracker = it->second;
571 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 573 tracker->ctTc0 = c_curr_tTc_curr0;
576 tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
578 m_L.
insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
581 start_index += tracker->m_error.getRows();
587 unsigned int start_index = 0;
589 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
591 TrackerWrapper *tracker = it->second;
592 tracker->computeVVSWeights();
595 start_index += tracker->m_w.getRows();
614 bool displayFullModel)
618 TrackerWrapper *tracker = it->second;
619 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
640 bool displayFullModel)
644 TrackerWrapper *tracker = it->second;
645 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
670 unsigned int thickness,
bool displayFullModel)
673 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
674 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
677 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
679 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 703 bool displayFullModel)
706 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
707 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
710 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
712 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 729 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
730 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
731 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
735 it_img != mapOfImages.end(); ++it_img) {
736 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
737 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
738 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
740 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
741 it_cam != mapOfCameraParameters.end()) {
742 TrackerWrapper *tracker = it_tracker->second;
743 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
745 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
762 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
763 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
764 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
767 for (std::map<std::string,
const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
768 it_img != mapOfImages.end(); ++it_img) {
769 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
770 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
771 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
773 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
774 it_cam != mapOfCameraParameters.end()) {
775 TrackerWrapper *tracker = it_tracker->second;
776 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
778 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
790 std::vector<std::string> cameraNames;
792 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
794 cameraNames.push_back(it_tracker->first);
816 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
817 it->second->getCameraParameters(cam1);
820 it->second->getCameraParameters(cam2);
822 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 835 mapOfCameraParameters.clear();
837 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
840 it->second->getCameraParameters(cam_);
841 mapOfCameraParameters[it->first] = cam_;
853 std::map<std::string, int> trackingTypes;
855 TrackerWrapper *traker;
856 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
858 traker = it_tracker->second;
859 trackingTypes[it_tracker->first] = traker->getTrackerType();
862 return trackingTypes;
876 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
877 clippingFlag1 = it->second->getClipping();
880 clippingFlag2 = it->second->getClipping();
882 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 894 mapOfClippingFlags.clear();
896 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
898 TrackerWrapper *tracker = it->second;
899 mapOfClippingFlags[it->first] = tracker->getClipping();
912 return it->second->getFaces();
926 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
928 return it->second->getFaces();
931 std::cerr <<
"The camera: " << cameraName <<
" cannot be found!" << std::endl;
935 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 943 TrackerWrapper *tracker = it->second;
944 return tracker->getFeaturesCircle();
958 TrackerWrapper *tracker = it->second;
959 return tracker->getFeaturesKltCylinder();
973 TrackerWrapper *tracker = it->second;
974 return tracker->getFeaturesKlt();
1012 return it->second->getFeaturesForDisplay();
1017 return std::vector<std::vector<double> >();
1046 mapOfFeatures.clear();
1048 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1050 mapOfFeatures[it->first] = it->second->getFeaturesForDisplay();
1065 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 1078 TrackerWrapper *tracker = it->second;
1079 return tracker->getKltImagePoints();
1084 return std::vector<vpImagePoint>();
1099 TrackerWrapper *tracker = it->second;
1100 return tracker->getKltImagePointsWithId();
1105 return std::map<int, vpImagePoint>();
1117 TrackerWrapper *tracker = it->second;
1118 return tracker->getKltMaskBorder();
1135 TrackerWrapper *tracker = it->second;
1136 return tracker->getKltNbPoints();
1154 TrackerWrapper *tracker;
1155 tracker = it_tracker->second;
1156 return tracker->getKltOpencv();
1175 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1176 klt1 = it->second->getKltOpencv();
1179 klt2 = it->second->getKltOpencv();
1181 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 1195 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1197 TrackerWrapper *tracker = it->second;
1198 mapOfKlts[it->first] = tracker->getKltOpencv();
1202 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 1212 TrackerWrapper *tracker = it->second;
1213 return tracker->getKltPoints();
1218 return std::vector<cv::Point2f>();
1244 unsigned int level)
const 1248 it->second->getLcircle(circlesList, level);
1268 unsigned int level)
const 1270 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1272 it->second->getLcircle(circlesList, level);
1274 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1291 unsigned int level)
const 1295 it->second->getLcylinder(cylindersList, level);
1315 unsigned int level)
const 1317 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1319 it->second->getLcylinder(cylindersList, level);
1321 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1338 unsigned int level)
const 1343 it->second->getLline(linesList, level);
1363 unsigned int level)
const 1365 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1367 it->second->getLline(linesList, level);
1369 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1402 bool displayFullModel)
1407 return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1412 return std::vector<std::vector<double> >();
1443 const std::map<std::string, unsigned int> &mapOfwidths,
1444 const std::map<std::string, unsigned int> &mapOfheights,
1445 const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
1446 const std::map<std::string, vpCameraParameters> &mapOfCams,
1447 bool displayFullModel)
1450 mapOfModels.clear();
1452 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1454 std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
1455 std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
1456 std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
1457 std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
1459 if (findWidth != mapOfwidths.end() &&
1460 findHeight != mapOfheights.end() &&
1461 findcMo != mapOfcMos.end() &&
1462 findCam != mapOfCams.end()) {
1463 mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second,
1464 findcMo->second, findCam->second, displayFullModel);
1479 return it->second->getMovingEdge();
1498 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1499 it->second->getMovingEdge(me1);
1502 it->second->getMovingEdge(me2);
1504 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 1516 mapOfMovingEdges.clear();
1518 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1520 TrackerWrapper *tracker = it->second;
1521 mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1546 unsigned int nbGoodPoints = 0;
1549 nbGoodPoints = it->second->getNbPoints(level);
1554 return nbGoodPoints;
1573 mapOfNbPoints.clear();
1575 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1577 TrackerWrapper *tracker = it->second;
1578 mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1591 return it->second->getNbPolygon();
1605 mapOfNbPolygons.clear();
1607 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1609 TrackerWrapper *tracker = it->second;
1610 mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1628 return it->second->getPolygon(index);
1648 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1650 return it->second->getPolygon(index);
1653 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1672 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1675 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1679 TrackerWrapper *tracker = it->second;
1680 polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1685 return polygonFaces;
1706 std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1707 bool orderPolygons,
bool useVisibility,
bool clipPolygon)
1709 mapOfPolygons.clear();
1710 mapOfPoints.clear();
1712 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1714 TrackerWrapper *tracker = it->second;
1715 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1716 tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1718 mapOfPolygons[it->first] = polygonFaces.first;
1719 mapOfPoints[it->first] = polygonFaces.second;
1739 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1740 it->second->getPose(c1Mo);
1743 it->second->getPose(c2Mo);
1745 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!" 1758 mapOfCameraPoses.clear();
1760 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1762 TrackerWrapper *tracker = it->second;
1763 tracker->getPose(mapOfCameraPoses[it->first]);
1782 TrackerWrapper *tracker = it->second;
1783 return tracker->getTrackerType();
1792 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1794 TrackerWrapper *tracker = it->second;
1801 double ,
int ,
const std::string & )
1806 #ifdef VISP_HAVE_MODULE_GUI 1851 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1855 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1856 TrackerWrapper *tracker = it->second;
1857 tracker->initClick(I1, initFile1, displayHelp, T1);
1861 tracker = it->second;
1862 tracker->initClick(I2, initFile2, displayHelp, T2);
1866 tracker = it->second;
1869 tracker->getPose(
m_cMo);
1873 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1920 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1924 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1925 TrackerWrapper *tracker = it->second;
1926 tracker->initClick(I_color1, initFile1, displayHelp, T1);
1930 tracker = it->second;
1931 tracker->initClick(I_color2, initFile2, displayHelp, T2);
1935 tracker = it->second;
1938 tracker->getPose(
m_cMo);
1942 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1989 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
1990 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
1993 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1995 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
1997 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1998 TrackerWrapper *tracker = it_tracker->second;
1999 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2000 if (it_T != mapOfT.end())
2001 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2003 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2004 tracker->getPose(
m_cMo);
2010 std::vector<std::string> vectorOfMissingCameraPoses;
2015 it_img = mapOfImages.find(it_tracker->first);
2016 it_initFile = mapOfInitFiles.find(it_tracker->first);
2018 if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
2020 TrackerWrapper *tracker = it_tracker->second;
2021 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2023 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2029 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2030 it != vectorOfMissingCameraPoses.end(); ++it) {
2031 it_img = mapOfImages.find(*it);
2032 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2041 "Missing image or missing camera transformation " 2042 "matrix! Cannot set the pose for camera: %s!",
2091 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2092 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2095 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2097 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2099 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2100 TrackerWrapper *tracker = it_tracker->second;
2101 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2102 if (it_T != mapOfT.end())
2103 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2105 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2106 tracker->getPose(
m_cMo);
2112 std::vector<std::string> vectorOfMissingCameraPoses;
2117 it_img = mapOfColorImages.find(it_tracker->first);
2118 it_initFile = mapOfInitFiles.find(it_tracker->first);
2120 if (it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2122 TrackerWrapper *tracker = it_tracker->second;
2123 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2125 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2131 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2132 it != vectorOfMissingCameraPoses.end(); ++it) {
2133 it_img = mapOfColorImages.find(*it);
2134 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2144 "Missing image or missing camera transformation " 2145 "matrix! Cannot set the pose for camera: %s!",
2153 const int ,
const std::string & )
2198 const std::string &initFile1,
const std::string &initFile2)
2201 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2202 TrackerWrapper *tracker = it->second;
2203 tracker->initFromPoints(I1, initFile1);
2207 tracker = it->second;
2208 tracker->initFromPoints(I2, initFile2);
2212 tracker = it->second;
2215 tracker->getPose(
m_cMo);
2218 tracker->getCameraParameters(
m_cam);
2222 "Cannot initFromPoints()! Require two cameras but " 2223 "there are %d cameras!",
2258 const std::string &initFile1,
const std::string &initFile2)
2261 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2262 TrackerWrapper *tracker = it->second;
2263 tracker->initFromPoints(I_color1, initFile1);
2267 tracker = it->second;
2268 tracker->initFromPoints(I_color2, initFile2);
2272 tracker = it->second;
2275 tracker->getPose(
m_cMo);
2278 tracker->getCameraParameters(
m_cam);
2282 "Cannot initFromPoints()! Require two cameras but " 2283 "there are %d cameras!",
2289 const std::map<std::string, std::string> &mapOfInitPoints)
2293 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2295 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2297 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2298 TrackerWrapper *tracker = it_tracker->second;
2299 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2300 tracker->getPose(
m_cMo);
2306 std::vector<std::string> vectorOfMissingCameraPoints;
2310 it_img = mapOfImages.find(it_tracker->first);
2311 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2313 if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2315 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2317 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2321 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2322 it != vectorOfMissingCameraPoints.end(); ++it) {
2323 it_img = mapOfImages.find(*it);
2324 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2332 "Missing image or missing camera transformation " 2333 "matrix! Cannot init the pose for camera: %s!",
2340 const std::map<std::string, std::string> &mapOfInitPoints)
2344 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2346 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2348 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2349 TrackerWrapper *tracker = it_tracker->second;
2350 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2351 tracker->getPose(
m_cMo);
2357 std::vector<std::string> vectorOfMissingCameraPoints;
2361 it_img = mapOfColorImages.find(it_tracker->first);
2362 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2364 if (it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2366 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2368 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2372 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2373 it != vectorOfMissingCameraPoints.end(); ++it) {
2374 it_img = mapOfColorImages.find(*it);
2375 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2383 "Missing image or missing camera transformation " 2384 "matrix! Cannot init the pose for camera: %s!",
2407 const std::string &initFile1,
const std::string &initFile2)
2410 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2411 TrackerWrapper *tracker = it->second;
2412 tracker->initFromPose(I1, initFile1);
2416 tracker = it->second;
2417 tracker->initFromPose(I2, initFile2);
2421 tracker = it->second;
2424 tracker->getPose(
m_cMo);
2427 tracker->getCameraParameters(
m_cam);
2431 "Cannot initFromPose()! Require two cameras but there " 2449 const std::string &initFile1,
const std::string &initFile2)
2452 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2453 TrackerWrapper *tracker = it->second;
2454 tracker->initFromPose(I_color1, initFile1);
2458 tracker = it->second;
2459 tracker->initFromPose(I_color2, initFile2);
2463 tracker = it->second;
2466 tracker->getPose(
m_cMo);
2469 tracker->getCameraParameters(
m_cam);
2473 "Cannot initFromPose()! Require two cameras but there " 2494 const std::map<std::string, std::string> &mapOfInitPoses)
2498 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2500 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2502 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2503 TrackerWrapper *tracker = it_tracker->second;
2504 tracker->initFromPose(*it_img->second, it_initPose->second);
2505 tracker->getPose(
m_cMo);
2511 std::vector<std::string> vectorOfMissingCameraPoses;
2515 it_img = mapOfImages.find(it_tracker->first);
2516 it_initPose = mapOfInitPoses.find(it_tracker->first);
2518 if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2520 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2522 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2526 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2527 it != vectorOfMissingCameraPoses.end(); ++it) {
2528 it_img = mapOfImages.find(*it);
2529 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2537 "Missing image or missing camera transformation " 2538 "matrix! Cannot init the pose for camera: %s!",
2559 const std::map<std::string, std::string> &mapOfInitPoses)
2563 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2565 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2567 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2568 TrackerWrapper *tracker = it_tracker->second;
2569 tracker->initFromPose(*it_img->second, it_initPose->second);
2570 tracker->getPose(
m_cMo);
2576 std::vector<std::string> vectorOfMissingCameraPoses;
2580 it_img = mapOfColorImages.find(it_tracker->first);
2581 it_initPose = mapOfInitPoses.find(it_tracker->first);
2583 if (it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2585 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2587 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2591 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2592 it != vectorOfMissingCameraPoses.end(); ++it) {
2593 it_img = mapOfColorImages.find(*it);
2594 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2602 "Missing image or missing camera transformation " 2603 "matrix! Cannot init the pose for camera: %s!",
2623 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2624 it->second->initFromPose(I1, c1Mo);
2628 it->second->initFromPose(I2, c2Mo);
2633 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2651 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2652 it->second->initFromPose(I_color1, c1Mo);
2656 it->second->initFromPose(I_color2, c2Mo);
2661 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2679 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2683 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2685 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2687 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2688 TrackerWrapper *tracker = it_tracker->second;
2689 tracker->initFromPose(*it_img->second, it_camPose->second);
2690 tracker->getPose(
m_cMo);
2696 std::vector<std::string> vectorOfMissingCameraPoses;
2700 it_img = mapOfImages.find(it_tracker->first);
2701 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2703 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2705 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2707 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2711 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2712 it != vectorOfMissingCameraPoses.end(); ++it) {
2713 it_img = mapOfImages.find(*it);
2714 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2722 "Missing image or missing camera transformation " 2723 "matrix! Cannot set the pose for camera: %s!",
2743 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2747 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2749 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2751 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2752 TrackerWrapper *tracker = it_tracker->second;
2753 tracker->initFromPose(*it_img->second, it_camPose->second);
2754 tracker->getPose(
m_cMo);
2760 std::vector<std::string> vectorOfMissingCameraPoses;
2764 it_img = mapOfColorImages.find(it_tracker->first);
2765 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2767 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2769 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2771 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2775 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2776 it != vectorOfMissingCameraPoses.end(); ++it) {
2777 it_img = mapOfColorImages.find(*it);
2778 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2786 "Missing image or missing camera transformation " 2787 "matrix! Cannot set the pose for camera: %s!",
2806 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2808 TrackerWrapper *tracker = it->second;
2809 tracker->loadConfigFile(configFile, verbose);
2842 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2843 TrackerWrapper *tracker = it_tracker->second;
2844 tracker->loadConfigFile(configFile1, verbose);
2847 tracker = it_tracker->second;
2848 tracker->loadConfigFile(configFile2, verbose);
2875 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2877 TrackerWrapper *tracker = it_tracker->second;
2879 std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
2880 if (it_config != mapOfConfigFiles.end()) {
2881 tracker->loadConfigFile(it_config->second, verbose);
2884 it_tracker->first.c_str());
2891 TrackerWrapper *tracker = it->second;
2892 tracker->getCameraParameters(
m_cam);
2924 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2926 TrackerWrapper *tracker = it->second;
2927 tracker->loadModel(modelFile, verbose, T);
2960 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2961 TrackerWrapper *tracker = it_tracker->second;
2962 tracker->loadModel(modelFile1, verbose, T1);
2965 tracker = it_tracker->second;
2966 tracker->loadModel(modelFile2, verbose, T2);
2989 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2991 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
2993 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
2995 if (it_model != mapOfModelFiles.end()) {
2996 TrackerWrapper *tracker = it_tracker->second;
2997 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2999 if (it_T != mapOfT.end())
3000 tracker->loadModel(it_model->second, verbose, it_T->second);
3002 tracker->loadModel(it_model->second, verbose);
3005 it_tracker->first.c_str());
3010 #ifdef VISP_HAVE_PCL 3012 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
3014 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3016 TrackerWrapper *tracker = it->second;
3017 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
3023 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
3024 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
3025 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
3027 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3029 TrackerWrapper *tracker = it->second;
3030 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
3031 mapOfPointCloudHeights[it->first]);
3057 TrackerWrapper *tracker = it_tracker->second;
3058 tracker->reInitModel(I, cad_name, cMo, verbose, T);
3061 tracker->getPose(
m_cMo);
3091 TrackerWrapper *tracker = it_tracker->second;
3092 tracker->reInitModel(I_color, cad_name, cMo, verbose, T);
3095 tracker->getPose(
m_cMo);
3124 const std::string &cad_name1,
const std::string &cad_name2,
3130 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3132 it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
3136 it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
3141 it_tracker->second->getPose(
m_cMo);
3171 const std::string &cad_name1,
const std::string &cad_name2,
3177 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3179 it_tracker->second->reInitModel(I_color1, cad_name1, c1Mo, verbose, T1);
3183 it_tracker->second->reInitModel(I_color2, cad_name2, c2Mo, verbose, T2);
3188 it_tracker->second->getPose(
m_cMo);
3212 const std::map<std::string, std::string> &mapOfModelFiles,
3213 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
3215 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3218 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3220 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3221 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3223 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3224 it_camPose != mapOfCameraPoses.end()) {
3225 TrackerWrapper *tracker = it_tracker->second;
3226 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3227 if (it_T != mapOfT.end())
3228 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3230 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3233 tracker->getPose(
m_cMo);
3238 std::vector<std::string> vectorOfMissingCameras;
3241 it_img = mapOfImages.find(it_tracker->first);
3242 it_model = mapOfModelFiles.find(it_tracker->first);
3243 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3245 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3246 TrackerWrapper *tracker = it_tracker->second;
3247 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3249 vectorOfMissingCameras.push_back(it_tracker->first);
3254 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3256 it_img = mapOfImages.find(*it);
3257 it_model = mapOfModelFiles.find(*it);
3258 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3261 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3264 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3286 const std::map<std::string, std::string> &mapOfModelFiles,
3287 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
3289 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3292 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
3294 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3295 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3297 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3298 it_camPose != mapOfCameraPoses.end()) {
3299 TrackerWrapper *tracker = it_tracker->second;
3300 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3301 if (it_T != mapOfT.end())
3302 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3304 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3307 tracker->getPose(
m_cMo);
3312 std::vector<std::string> vectorOfMissingCameras;
3315 it_img = mapOfColorImages.find(it_tracker->first);
3316 it_model = mapOfModelFiles.find(it_tracker->first);
3317 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3319 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3320 TrackerWrapper *tracker = it_tracker->second;
3321 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3323 vectorOfMissingCameras.push_back(it_tracker->first);
3328 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3330 it_img = mapOfColorImages.find(*it);
3331 it_model = mapOfModelFiles.find(*it);
3332 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3335 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3338 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3355 #ifdef VISP_HAVE_OGRE 3382 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 3389 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3391 TrackerWrapper *tracker = it->second;
3392 tracker->resetTracker();
3409 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3411 TrackerWrapper *tracker = it->second;
3412 tracker->setAngleAppear(a);
3431 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3432 it->second->setAngleAppear(a1);
3435 it->second->setAngleAppear(a2);
3460 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3461 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3464 TrackerWrapper *tracker = it_tracker->second;
3465 tracker->setAngleAppear(it->second);
3487 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3489 TrackerWrapper *tracker = it->second;
3490 tracker->setAngleDisappear(a);
3509 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3510 it->second->setAngleDisappear(a1);
3513 it->second->setAngleDisappear(a2);
3538 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3539 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3542 TrackerWrapper *tracker = it_tracker->second;
3543 tracker->setAngleDisappear(it->second);
3561 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3563 TrackerWrapper *tracker = it->second;
3564 tracker->setCameraParameters(camera);
3579 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3580 it->second->setCameraParameters(camera1);
3583 it->second->setCameraParameters(camera2);
3587 it->second->getCameraParameters(
m_cam);
3607 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
3608 it != mapOfCameraParameters.end(); ++it) {
3609 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3612 TrackerWrapper *tracker = it_tracker->second;
3613 tracker->setCameraParameters(it->second);
3636 it->second = cameraTransformationMatrix;
3650 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
3652 for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
3653 it != mapOfTransformationMatrix.end(); ++it) {
3654 std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
3658 it_camTrans->second = it->second;
3676 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3678 TrackerWrapper *tracker = it->second;
3679 tracker->setClipping(flags);
3696 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3697 it->second->setClipping(flags1);
3700 it->second->setClipping(flags2);
3709 std::stringstream ss;
3710 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
3724 for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
3725 it != mapOfClippingFlags.end(); ++it) {
3726 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3729 TrackerWrapper *tracker = it_tracker->second;
3730 tracker->setClipping(it->second);
3750 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3752 TrackerWrapper *tracker = it->second;
3753 tracker->setDepthDenseFilteringMaxDistance(maxDistance);
3767 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3769 TrackerWrapper *tracker = it->second;
3770 tracker->setDepthDenseFilteringMethod(method);
3785 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3787 TrackerWrapper *tracker = it->second;
3788 tracker->setDepthDenseFilteringMinDistance(minDistance);
3803 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3805 TrackerWrapper *tracker = it->second;
3806 tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
3820 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3822 TrackerWrapper *tracker = it->second;
3823 tracker->setDepthDenseSamplingStep(stepX, stepY);
3836 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3838 TrackerWrapper *tracker = it->second;
3839 tracker->setDepthNormalFaceCentroidMethod(method);
3853 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3855 TrackerWrapper *tracker = it->second;
3856 tracker->setDepthNormalFeatureEstimationMethod(method);
3869 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3871 TrackerWrapper *tracker = it->second;
3872 tracker->setDepthNormalPclPlaneEstimationMethod(method);
3885 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3887 TrackerWrapper *tracker = it->second;
3888 tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
3901 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3903 TrackerWrapper *tracker = it->second;
3904 tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
3918 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3920 TrackerWrapper *tracker = it->second;
3921 tracker->setDepthNormalSamplingStep(stepX, stepY);
3947 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3949 TrackerWrapper *tracker = it->second;
3950 tracker->setDisplayFeatures(displayF);
3965 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3967 TrackerWrapper *tracker = it->second;
3968 tracker->setFarClippingDistance(dist);
3983 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3984 it->second->setFarClippingDistance(dist1);
3987 it->second->setFarClippingDistance(dist2);
3991 distFarClip = it->second->getFarClippingDistance();
4008 for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
4010 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4013 TrackerWrapper *tracker = it_tracker->second;
4014 tracker->setFarClippingDistance(it->second);
4033 std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
4034 if (it_factor != mapOfFeatureFactors.end()) {
4035 it->second = it_factor->second;
4059 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4061 TrackerWrapper *tracker = it->second;
4062 tracker->setGoodMovingEdgesRatioThreshold(threshold);
4066 #ifdef VISP_HAVE_OGRE 4080 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4082 TrackerWrapper *tracker = it->second;
4083 tracker->setGoodNbRayCastingAttemptsRatio(ratio);
4100 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4102 TrackerWrapper *tracker = it->second;
4103 tracker->setNbRayCastingAttemptsForVisibility(attempts);
4108 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 4118 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4120 TrackerWrapper *tracker = it->second;
4121 tracker->setKltOpencv(t);
4136 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4137 it->second->setKltOpencv(t1);
4140 it->second->setKltOpencv(t2);
4154 for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
4155 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4158 TrackerWrapper *tracker = it_tracker->second;
4159 tracker->setKltOpencv(it->second);
4175 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4177 TrackerWrapper *tracker = it->second;
4178 tracker->setKltThresholdAcceptation(th);
4197 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4199 TrackerWrapper *tracker = it->second;
4200 tracker->setLod(useLod, name);
4204 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 4214 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4216 TrackerWrapper *tracker = it->second;
4217 tracker->setKltMaskBorder(e);
4232 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4233 it->second->setKltMaskBorder(e1);
4237 it->second->setKltMaskBorder(e2);
4251 for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
4253 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4256 TrackerWrapper *tracker = it_tracker->second;
4257 tracker->setKltMaskBorder(it->second);
4272 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4274 TrackerWrapper *tracker = it->second;
4275 tracker->setMask(mask);
4293 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4295 TrackerWrapper *tracker = it->second;
4296 tracker->setMinLineLengthThresh(minLineLengthThresh, name);
4312 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4314 TrackerWrapper *tracker = it->second;
4315 tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
4328 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4330 TrackerWrapper *tracker = it->second;
4331 tracker->setMovingEdge(me);
4347 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4348 it->second->setMovingEdge(me1);
4352 it->second->setMovingEdge(me2);
4366 for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
4367 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4370 TrackerWrapper *tracker = it_tracker->second;
4371 tracker->setMovingEdge(it->second);
4387 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4389 TrackerWrapper *tracker = it->second;
4390 tracker->setNearClippingDistance(dist);
4405 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4406 it->second->setNearClippingDistance(dist1);
4410 it->second->setNearClippingDistance(dist2);
4431 for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
4432 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4435 TrackerWrapper *tracker = it_tracker->second;
4436 tracker->setNearClippingDistance(it->second);
4461 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4463 TrackerWrapper *tracker = it->second;
4464 tracker->setOgreShowConfigDialog(showConfigDialog);
4482 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4484 TrackerWrapper *tracker = it->second;
4485 tracker->setOgreVisibilityTest(v);
4488 #ifdef VISP_HAVE_OGRE 4489 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4491 TrackerWrapper *tracker = it->second;
4492 tracker->faces.getOgreContext()->setWindowName(
"Multi Generic MBT (" + it->first +
")");
4508 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4510 TrackerWrapper *tracker = it->second;
4511 tracker->setOptimizationMethod(opt);
4531 "to be configured with only one camera!");
4538 TrackerWrapper *tracker = it->second;
4539 tracker->setPose(I, cdMo);
4562 "to be configured with only one camera!");
4569 TrackerWrapper *tracker = it->second;
4571 tracker->setPose(
m_I, cdMo);
4593 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4594 it->second->setPose(I1, c1Mo);
4598 it->second->setPose(I2, c2Mo);
4603 it->second->getPose(
m_cMo);
4629 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4630 it->second->setPose(I_color1, c1Mo);
4634 it->second->setPose(I_color2, c2Mo);
4639 it->second->getPose(
m_cMo);
4666 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4670 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
4672 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4674 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4675 TrackerWrapper *tracker = it_tracker->second;
4676 tracker->setPose(*it_img->second, it_camPose->second);
4677 tracker->getPose(
m_cMo);
4683 std::vector<std::string> vectorOfMissingCameraPoses;
4688 it_img = mapOfImages.find(it_tracker->first);
4689 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4691 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4693 TrackerWrapper *tracker = it_tracker->second;
4694 tracker->setPose(*it_img->second, it_camPose->second);
4696 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4701 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4702 it != vectorOfMissingCameraPoses.end(); ++it) {
4703 it_img = mapOfImages.find(*it);
4704 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4712 "Missing image or missing camera transformation " 4713 "matrix! Cannot set pose for camera: %s!",
4735 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4739 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
4741 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4743 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4744 TrackerWrapper *tracker = it_tracker->second;
4745 tracker->setPose(*it_img->second, it_camPose->second);
4746 tracker->getPose(
m_cMo);
4752 std::vector<std::string> vectorOfMissingCameraPoses;
4757 it_img = mapOfColorImages.find(it_tracker->first);
4758 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4760 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4762 TrackerWrapper *tracker = it_tracker->second;
4763 tracker->setPose(*it_img->second, it_camPose->second);
4765 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4770 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4771 it != vectorOfMissingCameraPoses.end(); ++it) {
4772 it_img = mapOfColorImages.find(*it);
4773 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4781 "Missing image or missing camera transformation " 4782 "matrix! Cannot set pose for camera: %s!",
4806 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4808 TrackerWrapper *tracker = it->second;
4809 tracker->setProjectionErrorComputation(flag);
4820 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4822 TrackerWrapper *tracker = it->second;
4823 tracker->setProjectionErrorDisplay(display);
4834 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4836 TrackerWrapper *tracker = it->second;
4837 tracker->setProjectionErrorDisplayArrowLength(length);
4845 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4847 TrackerWrapper *tracker = it->second;
4848 tracker->setProjectionErrorDisplayArrowThickness(thickness);
4859 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(referenceCameraName);
4863 std::cerr <<
"The reference camera: " << referenceCameraName <<
" does not exist!";
4871 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4873 TrackerWrapper *tracker = it->second;
4874 tracker->setScanLineVisibilityTest(v);
4891 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4893 TrackerWrapper *tracker = it->second;
4894 tracker->setTrackerType(type);
4909 for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
4910 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4912 TrackerWrapper *tracker = it_tracker->second;
4913 tracker->setTrackerType(it->second);
4929 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4931 TrackerWrapper *tracker = it->second;
4932 tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
4947 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4949 TrackerWrapper *tracker = it->second;
4950 tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
4965 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4967 TrackerWrapper *tracker = it->second;
4968 tracker->setUseEdgeTracking(name, useEdgeTracking);
4972 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 4984 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4986 TrackerWrapper *tracker = it->second;
4987 tracker->setUseKltTracking(name, useKltTracking);
4995 bool isOneTestTrackingOk =
false;
4996 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4998 TrackerWrapper *tracker = it->second;
5000 tracker->testTracking();
5001 isOneTestTrackingOk =
true;
5006 if (!isOneTestTrackingOk) {
5007 std::ostringstream oss;
5008 oss <<
"Not enough moving edges to track the object. Try to reduce the " 5026 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5029 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5030 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5032 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5046 std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
5049 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5050 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5052 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5068 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5069 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5070 mapOfImages[it->first] = &I1;
5073 mapOfImages[it->first] = &I2;
5075 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5076 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5078 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5080 std::stringstream ss;
5081 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5099 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5100 std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
5101 mapOfImages[it->first] = &I_color1;
5104 mapOfImages[it->first] = &_colorI2;
5106 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5107 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5109 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5111 std::stringstream ss;
5112 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5126 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5127 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5129 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5141 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5142 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5144 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5147 #ifdef VISP_HAVE_PCL 5157 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5159 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5161 TrackerWrapper *tracker = it->second;
5164 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5172 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5176 mapOfImages[it->first] == NULL) {
5181 ! mapOfPointClouds[it->first]) {
5197 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5199 TrackerWrapper *tracker = it->second;
5202 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5205 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5208 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5210 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5215 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5232 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5234 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5235 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5237 TrackerWrapper *tracker = it->second;
5240 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5248 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5251 ) && mapOfImages[it->first] == NULL) {
5254 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5257 ) && mapOfImages[it->first] != NULL) {
5259 mapOfImages[it->first] = &tracker->m_I;
5263 ! mapOfPointClouds[it->first]) {
5279 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5281 TrackerWrapper *tracker = it->second;
5284 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5287 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5290 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5292 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5297 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5317 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5318 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5319 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5321 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5323 TrackerWrapper *tracker = it->second;
5326 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5334 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5338 mapOfImages[it->first] == NULL) {
5343 (mapOfPointClouds[it->first] == NULL)) {
5348 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5359 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5361 TrackerWrapper *tracker = it->second;
5364 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5367 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5370 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5372 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5377 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5396 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5397 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5398 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5400 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5401 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5403 TrackerWrapper *tracker = it->second;
5406 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5414 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5417 ) && mapOfColorImages[it->first] == NULL) {
5420 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5423 ) && mapOfColorImages[it->first] != NULL) {
5425 mapOfImages[it->first] = &tracker->m_I;
5429 (mapOfPointClouds[it->first] == NULL)) {
5434 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5445 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5447 TrackerWrapper *tracker = it->second;
5450 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5453 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5456 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5458 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5463 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5472 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
5478 #ifdef VISP_HAVE_OGRE 5485 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
int trackerType)
5489 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5499 #ifdef VISP_HAVE_OGRE 5506 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
5518 double normRes_1 = -1;
5519 unsigned int iter = 0;
5521 double factorEdge = 1.0;
5522 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5523 double factorKlt = 1.0;
5525 double factorDepth = 1.0;
5526 double factorDepthDense = 1.0;
5534 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5537 bool isoJoIdentity_ =
true;
5543 unsigned int nb_edge_features = m_error_edge.
getRows();
5544 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5545 unsigned int nb_klt_features = m_error_klt.getRows();
5547 unsigned int nb_depth_features = m_error_depthNormal.getRows();
5548 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5553 bool reStartFromLastIncrement =
false;
5556 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5557 if (reStartFromLastIncrement) {
5564 if (!reStartFromLastIncrement) {
5569 if (!isoJoIdentity_) {
5572 LVJ_true = (
m_L * cVo *
oJo);
5578 isoJoIdentity_ =
true;
5585 if (isoJoIdentity_) {
5589 unsigned int rank = (
m_L * cVo).kernel(K);
5599 isoJoIdentity_ =
false;
5608 unsigned int start_index = 0;
5610 for (
unsigned int i = 0; i < nb_edge_features; i++) {
5611 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5618 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5623 start_index += nb_edge_features;
5626 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5628 for (
unsigned int i = 0; i < nb_klt_features; i++) {
5629 double wi = m_w_klt[i] * factorKlt;
5630 W_true[start_index + i] = wi;
5636 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5637 m_L[start_index + i][j] *= wi;
5641 start_index += nb_klt_features;
5646 for (
unsigned int i = 0; i < nb_depth_features; i++) {
5647 double wi = m_w_depthNormal[i] * factorDepth;
5648 m_w[start_index + i] = m_w_depthNormal[i];
5651 num += wi *
vpMath::sqr(m_error[start_index + i]);
5654 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5655 m_L[start_index + i][j] *= wi;
5659 start_index += nb_depth_features;
5663 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
5664 double wi = m_w_depthDense[i] * factorDepthDense;
5665 m_w[start_index + i] = m_w_depthDense[i];
5668 num += wi *
vpMath::sqr(m_error[start_index + i]);
5671 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5672 m_L[start_index + i][j] *= wi;
5682 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5683 if (m_trackerType & KLT_TRACKER) {
5690 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5691 if (m_trackerType & KLT_TRACKER) {
5695 normRes_1 = normRes;
5697 normRes = sqrt(num / den);
5710 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
5713 "TrackerWrapper::computeVVSInit(" 5714 ") should not be called!");
5719 initMbtTracking(ptr_I);
5721 unsigned int nbFeatures = 0;
5724 nbFeatures += m_error_edge.getRows();
5726 m_error_edge.clear();
5727 m_weightedError_edge.clear();
5732 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5735 nbFeatures += m_error_klt.getRows();
5737 m_error_klt.clear();
5738 m_weightedError_klt.clear();
5746 nbFeatures += m_error_depthNormal.getRows();
5748 m_error_depthNormal.clear();
5749 m_weightedError_depthNormal.clear();
5750 m_L_depthNormal.clear();
5751 m_w_depthNormal.clear();
5756 nbFeatures += m_error_depthDense.getRows();
5758 m_error_depthDense.clear();
5759 m_weightedError_depthDense.clear();
5760 m_L_depthDense.clear();
5761 m_w_depthDense.clear();
5764 m_L.
resize(nbFeatures, 6,
false,
false);
5772 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu()
5776 "computeVVSInteractionMatrixAndR" 5777 "esidu() should not be called!");
5780 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(
const vpImage<unsigned char> *
const ptr_I)
5786 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5800 unsigned int start_index = 0;
5801 if (m_trackerType & EDGE_TRACKER) {
5805 start_index += m_error_edge.getRows();
5808 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5809 if (m_trackerType & KLT_TRACKER) {
5813 start_index += m_error_klt.getRows();
5817 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5818 m_L.
insert(m_L_depthNormal, start_index, 0);
5821 start_index += m_error_depthNormal.getRows();
5824 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5825 m_L.
insert(m_L_depthDense, start_index, 0);
5834 unsigned int start_index = 0;
5840 start_index += m_w_edge.getRows();
5843 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 5848 start_index += m_w_klt.getRows();
5853 if (m_depthNormalUseRobust) {
5855 m_w.
insert(start_index, m_w_depthNormal);
5858 start_index += m_w_depthNormal.getRows();
5863 m_w.
insert(start_index, m_w_depthDense);
5871 unsigned int thickness,
bool displayFullModel)
5875 for (
size_t i = 0; i < features.size(); i++) {
5878 int state =
static_cast<int>(features[i][3]);
5909 double id = features[i][5];
5910 std::stringstream ss;
5914 vpImagePoint im_centroid(features[i][1], features[i][2]);
5915 vpImagePoint im_extremity(features[i][3], features[i][4]);
5923 for (
size_t i = 0; i < models.size(); i++) {
5930 double n20 = models[i][3];
5931 double n11 = models[i][4];
5932 double n02 = models[i][5];
5937 #ifdef VISP_HAVE_OGRE 5939 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5951 unsigned int thickness,
bool displayFullModel)
5955 for (
size_t i = 0; i < features.size(); i++) {
5958 int state =
static_cast<int>(features[i][3]);
5989 double id = features[i][5];
5990 std::stringstream ss;
5994 vpImagePoint im_centroid(features[i][1], features[i][2]);
5995 vpImagePoint im_extremity(features[i][3], features[i][4]);
6003 for (
size_t i = 0; i < models.size(); i++) {
6010 double n20 = models[i][3];
6011 double n11 = models[i][4];
6012 double n02 = models[i][5];
6017 #ifdef VISP_HAVE_OGRE 6019 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6029 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6031 std::vector<std::vector<double> > features;
6035 features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6038 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6041 features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6047 features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(), m_featuresToBeDisplayedDepthNormal.end());
6053 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(
unsigned int width,
unsigned int height,
6056 bool displayFullModel)
6058 std::vector<std::vector<double> > models;
6064 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6077 models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6083 models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6099 bool reInitialisation =
false;
6103 #ifdef VISP_HAVE_OGRE 6126 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6147 void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
6148 double radius,
int idFace,
const std::string &name)
6153 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6159 void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
6160 int idFace,
const std::string &name)
6165 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6171 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
6176 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6188 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
6193 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6213 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile,
bool verbose)
6235 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6253 std::cout <<
" *********** Parsing XML for";
6256 std::vector<std::string> tracker_names;
6258 tracker_names.push_back(
"Edge");
6259 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6261 tracker_names.push_back(
"Klt");
6264 tracker_names.push_back(
"Depth Normal");
6266 tracker_names.push_back(
"Depth Dense");
6269 for (
size_t i = 0; i < tracker_names.size(); i++) {
6270 std::cout <<
" " << tracker_names[i];
6271 if (i == tracker_names.size() - 1) {
6276 std::cout <<
"Model-Based Tracker ************ " << std::endl;
6279 xmlp.
parse(configFile);
6319 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6344 #ifdef VISP_HAVE_PCL 6346 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6348 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6359 bool newvisibleface =
false;
6377 if (m_trackerType & EDGE_TRACKER) {
6391 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6397 std::cerr <<
"Error in moving edge tracking" << std::endl;
6402 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6407 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6417 std::cerr <<
"Error in Depth normal tracking" << std::endl;
6426 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6434 const unsigned int pointcloud_width,
6435 const unsigned int pointcloud_height)
6437 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6448 bool newvisibleface =
false;
6466 if (m_trackerType & EDGE_TRACKER) {
6480 const std::vector<vpColVector> *
const point_cloud,
6481 const unsigned int pointcloud_width,
6482 const unsigned int pointcloud_height)
6488 std::cerr <<
"Error in moving edge tracking" << std::endl;
6493 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6498 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6508 std::cerr <<
"Error in Depth tracking" << std::endl;
6517 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6534 for (
unsigned int i = 0; i < scales.size(); i++) {
6536 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
6543 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
6551 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
6559 cylinders[i].clear();
6567 nbvisiblepolygone = 0;
6570 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6571 #if (VISP_HAVE_OPENCV_VERSION < 0x020408) 6573 cvReleaseImage(&cur);
6579 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
6581 if (kltpoly != NULL) {
6586 kltPolygons.clear();
6588 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
6591 if (kltPolyCylinder != NULL) {
6592 delete kltPolyCylinder;
6594 kltPolyCylinder = NULL;
6596 kltCylinders.clear();
6599 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
6606 circles_disp.clear();
6608 firstInitialisation =
true;
6613 for (
size_t i = 0; i < m_depthNormalFaces.size(); i++) {
6614 delete m_depthNormalFaces[i];
6615 m_depthNormalFaces[i] = NULL;
6617 m_depthNormalFaces.clear();
6620 for (
size_t i = 0; i < m_depthDenseFaces.size(); i++) {
6621 delete m_depthDenseFaces[i];
6622 m_depthDenseFaces[i] = NULL;
6624 m_depthDenseFaces.clear();
6636 void vpMbGenericTracker::TrackerWrapper::reInitModel(
const vpImage<unsigned char> &I,
const std::string &cad_name,
6643 void vpMbGenericTracker::TrackerWrapper::reInitModel(
const vpImage<vpRGBa> &I_color,
const std::string &cad_name,
6647 reInitModel(NULL, &I_color, cad_name, cMo, verbose, T);
6650 void vpMbGenericTracker::TrackerWrapper::resetTracker()
6653 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6660 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(
const vpCameraParameters &cam)
6665 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6672 void vpMbGenericTracker::TrackerWrapper::setClipping(
const unsigned int &flags)
6677 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(
const double &dist)
6682 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(
const double &dist)
6687 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(
const bool &v)
6690 #ifdef VISP_HAVE_OGRE 6698 bool performKltSetPose =
false;
6703 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6705 performKltSetPose =
true;
6715 if (!performKltSetPose) {
6731 if (m_trackerType & EDGE_TRACKER) {
6732 initPyramid(I, Ipyramid);
6734 unsigned int i = (
unsigned int) scales.size();
6744 cleanPyramid(Ipyramid);
6747 if (m_trackerType & EDGE_TRACKER) {
6766 setPose(NULL, &I_color, cdMo);
6769 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(
const bool &flag)
6774 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(
const bool &v)
6777 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 6784 void vpMbGenericTracker::TrackerWrapper::setTrackerType(
int type)
6787 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6794 m_trackerType = type;
6797 void vpMbGenericTracker::TrackerWrapper::testTracking()
6805 #
if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6811 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6815 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6819 #if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 6824 void vpMbGenericTracker::TrackerWrapper::track(
const vpImage<vpRGBa> &)
6829 #ifdef VISP_HAVE_PCL 6831 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6834 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6838 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6843 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6870 postTracking(ptr_I, point_cloud);
6873 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)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
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)
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)