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))
65 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
66 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
67 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
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))
102 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
103 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
104 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
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)
142 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
143 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
144 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
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()
5473 : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
5478 #ifdef VISP_HAVE_OGRE
5485 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
int trackerType)
5486 : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
5489 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5499 #ifdef VISP_HAVE_OGRE
5506 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
5511 computeVVSInit(ptr_I);
5513 if (m_error.getRows() < 4) {
5518 double normRes_1 = -1;
5519 unsigned int iter = 0;
5521 double factorEdge = 1.0;
5522 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5523 double factorKlt = 1.0;
5525 double factorDepth = 1.0;
5526 double factorDepthDense = 1.0;
5532 double mu = m_initialMu;
5534 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5537 bool isoJoIdentity_ =
true;
5543 unsigned int nb_edge_features = m_error_edge.
getRows();
5544 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5545 unsigned int nb_klt_features = m_error_klt.getRows();
5547 unsigned int nb_depth_features = m_error_depthNormal.getRows();
5548 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5550 while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
5551 computeVVSInteractionMatrixAndResidu(ptr_I);
5553 bool reStartFromLastIncrement =
false;
5554 computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
5556 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5557 if (reStartFromLastIncrement) {
5558 if (m_trackerType & KLT_TRACKER) {
5564 if (!reStartFromLastIncrement) {
5565 computeVVSWeights();
5567 if (computeCovariance) {
5569 if (!isoJoIdentity_) {
5572 LVJ_true = (m_L * cVo * oJo);
5578 isoJoIdentity_ =
true;
5585 if (isoJoIdentity_) {
5589 unsigned int rank = (m_L * cVo).kernel(K);
5599 isoJoIdentity_ =
false;
5608 unsigned int start_index = 0;
5609 if (m_trackerType & EDGE_TRACKER) {
5610 for (
unsigned int i = 0; i < nb_edge_features; i++) {
5611 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5613 m_weightedError[i] = wi * m_error[i];
5618 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5623 start_index += nb_edge_features;
5626 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5627 if (m_trackerType & KLT_TRACKER) {
5628 for (
unsigned int i = 0; i < nb_klt_features; i++) {
5629 double wi = m_w_klt[i] * factorKlt;
5630 W_true[start_index + i] = wi;
5631 m_weightedError[start_index + i] = wi * m_error_klt[i];
5633 num += wi *
vpMath::sqr(m_error[start_index + i]);
5636 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5637 m_L[start_index + i][j] *= wi;
5641 start_index += nb_klt_features;
5645 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5646 for (
unsigned int i = 0; i < nb_depth_features; i++) {
5647 double wi = m_w_depthNormal[i] * factorDepth;
5648 m_w[start_index + i] = m_w_depthNormal[i];
5649 m_weightedError[start_index + i] = wi * m_error[start_index + i];
5651 num += wi *
vpMath::sqr(m_error[start_index + i]);
5654 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5655 m_L[start_index + i][j] *= wi;
5659 start_index += nb_depth_features;
5662 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5663 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
5664 double wi = m_w_depthDense[i] * factorDepthDense;
5665 m_w[start_index + i] = m_w_depthDense[i];
5666 m_weightedError[start_index + i] = wi * m_error[start_index + i];
5668 num += wi *
vpMath::sqr(m_error[start_index + i]);
5671 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5672 m_L[start_index + i][j] *= wi;
5679 computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
5682 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5683 if (m_trackerType & KLT_TRACKER) {
5690 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5691 if (m_trackerType & KLT_TRACKER) {
5695 normRes_1 = normRes;
5697 normRes = sqrt(num / den);
5703 computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
5705 if (m_trackerType & EDGE_TRACKER) {
5710 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
5713 "TrackerWrapper::computeVVSInit("
5714 ") should not be called!");
5719 initMbtTracking(ptr_I);
5721 unsigned int nbFeatures = 0;
5723 if (m_trackerType & EDGE_TRACKER) {
5724 nbFeatures += m_error_edge.getRows();
5726 m_error_edge.clear();
5727 m_weightedError_edge.clear();
5732 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5733 if (m_trackerType & KLT_TRACKER) {
5735 nbFeatures += m_error_klt.getRows();
5737 m_error_klt.clear();
5738 m_weightedError_klt.clear();
5744 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5746 nbFeatures += m_error_depthNormal.getRows();
5748 m_error_depthNormal.clear();
5749 m_weightedError_depthNormal.clear();
5750 m_L_depthNormal.clear();
5751 m_w_depthNormal.clear();
5754 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5756 nbFeatures += m_error_depthDense.getRows();
5758 m_error_depthDense.clear();
5759 m_weightedError_depthDense.clear();
5760 m_L_depthDense.clear();
5761 m_w_depthDense.clear();
5764 m_L.resize(nbFeatures, 6,
false,
false);
5765 m_error.resize(nbFeatures,
false);
5767 m_weightedError.resize(nbFeatures,
false);
5768 m_w.resize(nbFeatures,
false);
5776 "computeVVSInteractionMatrixAndR"
5777 "esidu() should not be called!");
5782 if (m_trackerType & EDGE_TRACKER) {
5786 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5787 if (m_trackerType & KLT_TRACKER) {
5792 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5796 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5800 unsigned int start_index = 0;
5801 if (m_trackerType & EDGE_TRACKER) {
5802 m_L.insert(m_L_edge, start_index, 0);
5803 m_error.insert(start_index, m_error_edge);
5805 start_index += m_error_edge.getRows();
5808 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5809 if (m_trackerType & KLT_TRACKER) {
5810 m_L.insert(m_L_klt, start_index, 0);
5811 m_error.insert(start_index, m_error_klt);
5813 start_index += m_error_klt.getRows();
5817 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5818 m_L.insert(m_L_depthNormal, start_index, 0);
5819 m_error.insert(start_index, m_error_depthNormal);
5821 start_index += m_error_depthNormal.getRows();
5824 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5825 m_L.insert(m_L_depthDense, start_index, 0);
5826 m_error.insert(start_index, m_error_depthDense);
5834 unsigned int start_index = 0;
5836 if (m_trackerType & EDGE_TRACKER) {
5838 m_w.insert(start_index, m_w_edge);
5840 start_index += m_w_edge.getRows();
5843 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5844 if (m_trackerType & KLT_TRACKER) {
5846 m_w.insert(start_index, m_w_klt);
5848 start_index += m_w_klt.getRows();
5852 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5853 if (m_depthNormalUseRobust) {
5855 m_w.insert(start_index, m_w_depthNormal);
5858 start_index += m_w_depthNormal.getRows();
5861 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5863 m_w.insert(start_index, m_w_depthDense);
5871 unsigned int thickness,
bool displayFullModel)
5873 if (displayFeatures) {
5874 std::vector<std::vector<double> > features = getFeaturesForDisplay();
5875 for (
size_t i = 0; i < features.size(); i++) {
5878 int state =
static_cast<int>(features[i][3]);
5909 double id = features[i][5];
5910 std::stringstream ss;
5914 vpImagePoint im_centroid(features[i][1], features[i][2]);
5915 vpImagePoint im_extremity(features[i][3], features[i][4]);
5922 std::vector<std::vector<double> > models = getModelForDisplay(I.
getWidth(), I.
getHeight(), cMo, cam, displayFullModel);
5923 for (
size_t i = 0; i < models.size(); i++) {
5930 double n20 = models[i][3];
5931 double n11 = models[i][4];
5932 double n02 = models[i][5];
5937 #ifdef VISP_HAVE_OGRE
5938 if ((m_trackerType & EDGE_TRACKER)
5939 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5940 || (m_trackerType & KLT_TRACKER)
5944 faces.displayOgre(cMo);
5951 unsigned int thickness,
bool displayFullModel)
5953 if (displayFeatures) {
5954 std::vector<std::vector<double> > features = getFeaturesForDisplay();
5955 for (
size_t i = 0; i < features.size(); i++) {
5958 int state =
static_cast<int>(features[i][3]);
5989 double id = features[i][5];
5990 std::stringstream ss;
5994 vpImagePoint im_centroid(features[i][1], features[i][2]);
5995 vpImagePoint im_extremity(features[i][3], features[i][4]);
6002 std::vector<std::vector<double> > models = getModelForDisplay(I.
getWidth(), I.
getHeight(), cMo, cam, displayFullModel);
6003 for (
size_t i = 0; i < models.size(); i++) {
6010 double n20 = models[i][3];
6011 double n11 = models[i][4];
6012 double n02 = models[i][5];
6017 #ifdef VISP_HAVE_OGRE
6018 if ((m_trackerType & EDGE_TRACKER)
6019 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6020 || (m_trackerType & KLT_TRACKER)
6024 faces.displayOgre(cMo);
6029 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6031 std::vector<std::vector<double> > features;
6033 if (m_trackerType & EDGE_TRACKER) {
6035 features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6038 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6039 if (m_trackerType & KLT_TRACKER) {
6041 features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6045 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6047 features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(), m_featuresToBeDisplayedDepthNormal.end());
6053 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(
unsigned int width,
unsigned int height,
6056 bool displayFullModel)
6058 std::vector<std::vector<double> > models;
6061 if (m_trackerType == EDGE_TRACKER) {
6064 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6065 else if (m_trackerType == KLT_TRACKER) {
6069 else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
6071 }
else if (m_trackerType == DEPTH_DENSE_TRACKER) {
6075 if (m_trackerType & EDGE_TRACKER) {
6077 models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6081 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6083 models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6092 if (!modelInitialised) {
6096 if (useScanLine || clippingFlag > 3)
6099 bool reInitialisation =
false;
6101 faces.setVisible(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6103 #ifdef VISP_HAVE_OGRE
6104 if (!faces.isOgreInitialised()) {
6107 faces.setOgreShowConfigDialog(ogreShowConfigDialog);
6108 faces.initOgre(m_cam);
6112 ogreShowConfigDialog =
false;
6115 faces.setVisibleOgre(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6117 faces.setVisible(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6122 faces.computeClippedPolygons(m_cMo, m_cam);
6126 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6127 if (m_trackerType & KLT_TRACKER)
6131 if (m_trackerType & EDGE_TRACKER) {
6140 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6143 if (m_trackerType & DEPTH_DENSE_TRACKER)
6147 void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
6148 double radius,
int idFace,
const std::string &name)
6150 if (m_trackerType & EDGE_TRACKER)
6153 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6154 if (m_trackerType & KLT_TRACKER)
6159 void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
6160 int idFace,
const std::string &name)
6162 if (m_trackerType & EDGE_TRACKER)
6165 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6166 if (m_trackerType & KLT_TRACKER)
6171 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
6173 if (m_trackerType & EDGE_TRACKER)
6176 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6177 if (m_trackerType & KLT_TRACKER)
6181 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6184 if (m_trackerType & DEPTH_DENSE_TRACKER)
6188 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
6190 if (m_trackerType & EDGE_TRACKER)
6193 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6194 if (m_trackerType & KLT_TRACKER)
6198 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6201 if (m_trackerType & DEPTH_DENSE_TRACKER)
6207 if (m_trackerType & EDGE_TRACKER) {
6213 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile,
bool verbose)
6219 xmlp.setVerbose(verbose);
6220 xmlp.setCameraParameters(m_cam);
6222 xmlp.setAngleDisappear(
vpMath::deg(angleDisappears));
6228 xmlp.setKltMaxFeatures(10000);
6229 xmlp.setKltWindowSize(5);
6230 xmlp.setKltQuality(0.01);
6231 xmlp.setKltMinDistance(5);
6232 xmlp.setKltHarrisParam(0.01);
6233 xmlp.setKltBlockSize(3);
6234 xmlp.setKltPyramidLevels(3);
6235 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6236 xmlp.setKltMaskBorder(maskBorder);
6240 xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
6241 xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
6242 xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
6243 xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
6244 xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
6245 xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
6248 xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
6249 xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
6253 std::cout <<
" *********** Parsing XML for";
6256 std::vector<std::string> tracker_names;
6257 if (m_trackerType & EDGE_TRACKER)
6258 tracker_names.push_back(
"Edge");
6259 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6260 if (m_trackerType & KLT_TRACKER)
6261 tracker_names.push_back(
"Klt");
6263 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6264 tracker_names.push_back(
"Depth Normal");
6265 if (m_trackerType & DEPTH_DENSE_TRACKER)
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);
6285 xmlp.getCameraParameters(camera);
6286 setCameraParameters(camera);
6288 angleAppears =
vpMath::rad(xmlp.getAngleAppear());
6289 angleDisappears =
vpMath::rad(xmlp.getAngleDisappear());
6291 if (xmlp.hasNearClippingDistance())
6292 setNearClippingDistance(xmlp.getNearClippingDistance());
6294 if (xmlp.hasFarClippingDistance())
6295 setFarClippingDistance(xmlp.getFarClippingDistance());
6297 if (xmlp.getFovClipping()) {
6301 useLodGeneral = xmlp.getLodState();
6302 minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
6303 minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
6305 applyLodSettingInConfig =
false;
6306 if (this->getNbPolygon() > 0) {
6307 applyLodSettingInConfig =
true;
6308 setLod(useLodGeneral);
6309 setMinLineLengthThresh(minLineLengthThresholdGeneral);
6310 setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
6315 xmlp.getEdgeMe(meParser);
6319 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6320 tracker.setMaxFeatures((
int)xmlp.getKltMaxFeatures());
6321 tracker.setWindowSize((
int)xmlp.getKltWindowSize());
6322 tracker.setQuality(xmlp.getKltQuality());
6323 tracker.setMinDistance(xmlp.getKltMinDistance());
6324 tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
6325 tracker.setBlockSize((
int)xmlp.getKltBlockSize());
6326 tracker.setPyramidLevels((
int)xmlp.getKltPyramidLevels());
6327 maskBorder = xmlp.getKltMaskBorder();
6330 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
6334 setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod());
6335 setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod());
6336 setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter());
6337 setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold());
6338 setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY());
6341 setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
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))
6350 if (m_trackerType & KLT_TRACKER) {
6358 if (m_trackerType & EDGE_TRACKER) {
6359 bool newvisibleface =
false;
6363 faces.computeClippedPolygons(m_cMo, m_cam);
6369 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6373 if (m_trackerType & DEPTH_DENSE_TRACKER)
6377 if (m_trackerType & EDGE_TRACKER) {
6384 if (computeProjError) {
6391 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6393 if (m_trackerType & EDGE_TRACKER) {
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))
6403 if (m_trackerType & KLT_TRACKER) {
6407 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6413 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6417 std::cerr <<
"Error in Depth normal tracking" << std::endl;
6422 if (m_trackerType & DEPTH_DENSE_TRACKER) {
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))
6439 if (m_trackerType & KLT_TRACKER) {
6447 if (m_trackerType & EDGE_TRACKER) {
6448 bool newvisibleface =
false;
6452 faces.computeClippedPolygons(m_cMo, m_cam);
6458 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6462 if (m_trackerType & DEPTH_DENSE_TRACKER)
6466 if (m_trackerType & EDGE_TRACKER) {
6473 if (computeProjError) {
6480 const std::vector<vpColVector> *
const point_cloud,
6481 const unsigned int pointcloud_width,
6482 const unsigned int pointcloud_height)
6484 if (m_trackerType & EDGE_TRACKER) {
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))
6494 if (m_trackerType & KLT_TRACKER) {
6498 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6504 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6508 std::cerr <<
"Error in Depth tracking" << std::endl;
6513 if (m_trackerType & DEPTH_DENSE_TRACKER) {
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();
6628 loadModel(cad_name, verbose, T);
6630 initFromPose(*I, cMo);
6632 initFromPose(*I_color, cMo);
6640 reInitModel(&I, NULL, cad_name, cMo, verbose, T);
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
6691 faces.getOgreContext()->setWindowName(
"TrackerWrapper");
6698 bool performKltSetPose =
false;
6703 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6704 if (m_trackerType & KLT_TRACKER) {
6705 performKltSetPose =
true;
6707 if (useScanLine || clippingFlag > 3) {
6708 m_cam.computeFov(I ? I->
getWidth() : m_I.getWidth(), I ? I->
getHeight() : m_I.getHeight());
6715 if (!performKltSetPose) {
6721 if (m_trackerType & EDGE_TRACKER) {
6726 faces.computeClippedPolygons(m_cMo, m_cam);
6727 faces.computeScanLineRender(m_cam, I ? I->
getWidth() : m_I.getWidth(), I ? I->
getHeight() : m_I.getHeight());
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) {
6761 setPose(&I, NULL, cdMo);
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)
6786 if ((type & (EDGE_TRACKER |
6787 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6790 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
6794 m_trackerType = type;
6797 void vpMbGenericTracker::TrackerWrapper::testTracking()
6799 if (m_trackerType & EDGE_TRACKER) {
6805 #
if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6810 if ((m_trackerType & (EDGE_TRACKER
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)
6829 #ifdef VISP_HAVE_PCL
6831 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6833 if ((m_trackerType & (EDGE_TRACKER |
6834 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6837 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
6838 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
6842 if (m_trackerType & (EDGE_TRACKER
6843 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6851 if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && ! point_cloud) {
6858 preTracking(ptr_I, point_cloud);
6863 covarianceMatrix = -1;
6867 if (m_trackerType == EDGE_TRACKER)
6870 postTracking(ptr_I, point_cloud);
6873 std::cerr <<
"Exception: " << e.
what() << std::endl;
void setWindowName(const Ogre::String &n)
unsigned int getCols() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionnalities.
static const vpColor cyan
static const vpColor blue
static const vpColor purple
static const vpColor yellow
static const vpColor green
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
const char * what() const
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
vp_deprecated void init()
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
void init(unsigned int h, unsigned int w, Type value)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
static double rad(double deg)
static double sqr(double x)
static bool equal(double x, double y, double s=0.001)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void computeVisibility(unsigned int width, unsigned int height)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void computeVVSInit()
virtual void resetTracker()
virtual void computeVVSWeights()
virtual void setScanLineVisibilityTest(const bool &v)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setCameraParameters(const vpCameraParameters &camera)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void resetTracker()
virtual void computeVVSInit()
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void track(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setPose(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void computeVisibility(unsigned int width, unsigned int height)
void computeVVS(const vpImage< unsigned char > &_I, unsigned int lvl)
void updateMovingEdgeWeights()
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setNearClippingDistance(const double &dist)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
virtual void setFarClippingDistance(const double &dist)
void computeProjectionError(const vpImage< unsigned char > &_I)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void computeVVSWeights()
virtual void setClipping(const unsigned int &flags)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
void updateMovingEdge(const vpImage< unsigned char > &I)
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setMovingEdge(const vpMe &me)
virtual void computeVVSInteractionMatrixAndResidu(const vpImage< unsigned char > &I)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void testTracking()
virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual std::vector< std::vector< double > > getFeaturesForDisplay()
virtual void setLod(bool useLod, const std::string &name="")
virtual void setDisplayFeatures(bool displayF)
virtual double getGoodMovingEdgesRatioThreshold() const
virtual int getTrackerType() const
std::map< std::string, TrackerWrapper * > m_mapOfTrackers
virtual void setKltMaskBorder(const unsigned int &e)
virtual void setProjectionErrorComputation(const bool &flag)
unsigned int m_nb_feat_edge
Number of moving-edges features.
virtual void setKltThresholdAcceptation(double th)
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
virtual double getKltThresholdAcceptation() const
virtual void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, unsigned int level=0) const
virtual void getCameraParameters(vpCameraParameters &cam) const
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void resetTracker()
virtual std::vector< cv::Point2f > getKltPoints() const
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setGoodMovingEdgesRatioThreshold(double threshold)
virtual void setAngleAppear(const double &a)
virtual void setDepthDenseFilteringMinDistance(double minDistance)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
vpColVector m_w
Robust weights.
virtual ~vpMbGenericTracker()
virtual std::string getReferenceCameraName() const
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
virtual std::map< std::string, int > getCameraTrackerTypes() const
virtual void setNearClippingDistance(const double &dist)
unsigned int m_nb_feat_depthDense
Number of depth dense features.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setAngleDisappear(const double &a)
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
virtual void computeProjectionError()
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 setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
virtual void setMovingEdge(const vpMe &me)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double threshold)
virtual void setFeatureFactors(const std::map< vpTrackerType, double > &mapOfFeatureFactors)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void testTracking()
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, unsigned int level=0) const
virtual std::vector< vpImagePoint > getKltImagePoints() const
virtual void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
virtual std::vector< std::string > getCameraNames() const
virtual void setDepthDenseFilteringMethod(int method)
vpColVector m_weightedError
Weighted error.
virtual std::list< vpMbtDistanceKltPoints * > & getFeaturesKlt()
vpMatrix m_L
Interaction matrix.
virtual void init(const vpImage< unsigned char > &I)
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
virtual void computeVVSWeights()
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void computeVVSInit()
virtual void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
virtual unsigned int getKltMaskBorder() const
virtual unsigned int getNbPolygon() const
vpColVector m_error
(s - s*)
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual void setProjectionErrorDisplay(bool display)
std::map< vpTrackerType, double > m_mapOfFeatureFactors
Ponderation between each feature type in the VVS stage.
double m_thresholdOutlier
virtual void computeVVS(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
virtual std::list< vpMbtDistanceCircle * > & getFeaturesCircle()
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
virtual void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
virtual void setTrackerType(int type)
unsigned int m_nb_feat_klt
Number of klt features.
virtual unsigned int getNbPoints(unsigned int level=0) const
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
virtual vpKltOpencv getKltOpencv() const
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual vpMbtPolygon * getPolygon(unsigned int index)
virtual int getKltNbPoints() const
unsigned int m_nb_feat_depthNormal
Number of depth normal features.
virtual vpMe getMovingEdge() const
virtual void setFarClippingDistance(const double &dist)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
virtual void preTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages, std::map< std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr > &mapOfPointClouds)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
virtual void setClipping(const unsigned int &flags)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
virtual void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setOgreVisibilityTest(const bool &v)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
std::string m_referenceCameraName
Name of the reference camera.
virtual void setMask(const vpImage< bool > &mask)
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void track(const vpImage< unsigned char > &I)
virtual unsigned int getClipping() const
virtual std::map< int, vpImagePoint > getKltImagePointsWithId() const
vpAROgre * getOgreContext()
virtual void setScanLineVisibilityTest(const bool &v)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
void preTracking(const vpImage< unsigned char > &I)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void reinit(const vpImage< unsigned char > &I)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setCameraParameters(const vpCameraParameters &cam)
virtual void computeVVSInit()
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
double m_lambda
Gain of the virtual visual servoing stage.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual void setMask(const vpImage< bool > &mask)
virtual void getCameraParameters(vpCameraParameters &cam) const
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
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 void setDisplayFeatures(bool displayF)
virtual vpHomogeneousMatrix getPose() const
bool m_computeInteraction
vpMatrix oJo
The Degrees of Freedom to estimate.
vpMatrix covarianceMatrix
Covariance matrix.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
vpHomogeneousMatrix m_cMo
The current pose.
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)
vpCameraParameters m_cam
The camera parameters.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setAngleDisappear(const double &a)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setOgreVisibilityTest(const bool &v)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
bool displayFeatures
If true, the features are displayed.
virtual void setProjectionErrorDisplay(bool display)
double angleDisappears
Angle used to detect a face disappearance.
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool useScanLine
Use Scanline for visibility tests.
virtual void setProjectionErrorComputation(const bool &flag)
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void setAngleAppear(const double &a)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
double distNearClip
Distance for near clipping.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
unsigned int clippingFlag
Flags specifying which clipping to used.
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
Manage a cylinder used in the model-based tracker.
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
Manage the line of a polygon used in the model-based tracker.
Implementation of a polygon of the model used by the model-based tracker.
Parse an Xml file to extract configuration parameters of a mbtConfig object.
@ CONSTRAST
Point removed due to a contrast problem.
@ TOO_NEAR
Point removed because too near image borders.
@ THRESHOLD
Point removed due to a threshold problem.
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
@ NO_SUPPRESSION
Point used by the tracker.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)