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/core/vpIoTools.h>
42 #include <visp3/mbt/vpMbtXmlGenericParser.h>
44 #ifdef VISP_HAVE_NLOHMANN_JSON
45 #include <nlohmann/json.hpp>
46 using json = nlohmann::json;
50 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
51 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
52 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
62 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
71 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
72 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
73 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
78 else if (nbCameras == 1) {
85 for (
unsigned int i = 1; i <= nbCameras; i++) {
101 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
110 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
111 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
112 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
114 if (trackerTypes.empty()) {
118 if (trackerTypes.size() == 1) {
125 for (
size_t i = 1; i <= trackerTypes.size(); i++) {
126 std::stringstream ss;
141 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
150 const std::vector<int> &trackerTypes)
151 : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
152 m_percentageGdPt(0.4), m_referenceCameraName(
"Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError(),
153 m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
155 if (cameraNames.size() != trackerTypes.size() || cameraNames.empty()) {
157 "cameraNames.size() != trackerTypes.size() || cameraNames.empty()");
160 for (
size_t i = 0; i < cameraNames.size(); i++) {
173 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
210 double rawTotalProjectionError = 0.0;
211 unsigned int nbTotalFeaturesUsed = 0;
213 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
215 TrackerWrapper *tracker = it->second;
217 unsigned int nbFeaturesUsed = 0;
218 double curProjError = tracker->computeProjectionErrorImpl(I, cMo, cam, nbFeaturesUsed);
220 if (nbFeaturesUsed > 0) {
221 nbTotalFeaturesUsed += nbFeaturesUsed;
222 rawTotalProjectionError += curProjError;
226 if (nbTotalFeaturesUsed > 0) {
227 return vpMath::deg(rawTotalProjectionError / (
double)nbTotalFeaturesUsed);
263 double rawTotalProjectionError = 0.0;
264 unsigned int nbTotalFeaturesUsed = 0;
266 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
268 TrackerWrapper *tracker = it->second;
270 double curProjError = tracker->getProjectionError();
271 unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
273 if (nbFeaturesUsed > 0) {
274 nbTotalFeaturesUsed += nbFeaturesUsed;
275 rawTotalProjectionError += (
vpMath::rad(curProjError) * nbFeaturesUsed);
279 if (nbTotalFeaturesUsed > 0) {
300 double normRes_1 = -1;
301 unsigned int iter = 0;
319 std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
324 mapOfVelocityTwist[it->first] = cVo;
328 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
342 bool reStartFromLastIncrement =
false;
344 if (reStartFromLastIncrement) {
345 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
347 TrackerWrapper *tracker = it->second;
351 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
354 tracker->ctTc0 = c_curr_tTc_curr0;
359 if (!reStartFromLastIncrement) {
364 if (!isoJoIdentity) {
367 LVJ_true = (
m_L * (cVo *
oJo));
381 unsigned int rank = (
m_L * cVo).kernel(K);
390 isoJoIdentity =
false;
399 unsigned int start_index = 0;
400 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
402 TrackerWrapper *tracker = it->second;
405 for (
unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
406 double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
407 W_true[start_index + i] = wi;
413 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
414 m_L[start_index + i][j] *= wi;
418 start_index += tracker->m_error_edge.
getRows();
421 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
423 for (
unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
424 double wi = tracker->m_w_klt[i] * factorKlt;
425 W_true[start_index + i] = wi;
431 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
432 m_L[start_index + i][j] *= wi;
436 start_index += tracker->m_error_klt.
getRows();
441 for (
unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
442 double wi = tracker->m_w_depthNormal[i] * factorDepth;
443 W_true[start_index + i] = wi;
449 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
450 m_L[start_index + i][j] *= wi;
454 start_index += tracker->m_error_depthNormal.
getRows();
458 for (
unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
459 double wi = tracker->m_w_depthDense[i] * factorDepthDense;
460 W_true[start_index + i] = wi;
466 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
467 m_L[start_index + i][j] *= wi;
471 start_index += tracker->m_error_depthDense.
getRows();
476 normRes = sqrt(num / den);
484 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
485 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
487 TrackerWrapper *tracker = it->second;
491 tracker->ctTc0 = c_curr_tTc_curr0;
496 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
498 TrackerWrapper *tracker = it->second;
507 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
509 TrackerWrapper *tracker = it->second;
513 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
528 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
530 TrackerWrapper *tracker = it->second;
533 tracker->updateMovingEdgeWeights();
545 unsigned int nbFeatures = 0;
547 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
549 TrackerWrapper *tracker = it->second;
550 tracker->computeVVSInit(mapOfImages[it->first]);
552 nbFeatures += tracker->m_error.getRows();
566 "computeVVSInteractionMatrixAndR"
567 "esidu() should not be called");
572 std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
574 unsigned int start_index = 0;
576 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
578 TrackerWrapper *tracker = it->second;
581 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
584 tracker->ctTc0 = c_curr_tTc_curr0;
587 tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
589 m_L.
insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
592 start_index += tracker->m_error.getRows();
598 unsigned int start_index = 0;
600 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
602 TrackerWrapper *tracker = it->second;
603 tracker->computeVVSWeights();
606 start_index += tracker->m_w.getRows();
625 bool displayFullModel)
629 TrackerWrapper *tracker = it->second;
630 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
652 bool displayFullModel)
656 TrackerWrapper *tracker = it->second;
657 tracker->display(I, cMo, cam, col, thickness, displayFullModel);
683 unsigned int thickness,
bool displayFullModel)
686 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
687 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
690 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
693 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
717 bool displayFullModel)
720 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
721 it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
724 it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
727 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
744 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
745 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
746 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
750 it_img != mapOfImages.end(); ++it_img) {
751 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
752 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
753 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
755 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
756 it_cam != mapOfCameraParameters.end()) {
757 TrackerWrapper *tracker = it_tracker->second;
758 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
761 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
778 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
779 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
780 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
783 for (std::map<std::string,
const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
784 it_img != mapOfImages.end(); ++it_img) {
785 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it_img->first);
786 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
787 std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
789 if (it_tracker !=
m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
790 it_cam != mapOfCameraParameters.end()) {
791 TrackerWrapper *tracker = it_tracker->second;
792 tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
795 std::cerr <<
"Missing elements for image:" << it_img->first <<
"!" << std::endl;
807 std::vector<std::string> cameraNames;
809 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
811 cameraNames.push_back(it_tracker->first);
833 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
834 it->second->getCameraParameters(cam1);
837 it->second->getCameraParameters(cam2);
840 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
853 mapOfCameraParameters.clear();
855 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
858 it->second->getCameraParameters(cam_);
859 mapOfCameraParameters[it->first] = cam_;
871 std::map<std::string, int> trackingTypes;
873 TrackerWrapper *traker;
874 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
876 traker = it_tracker->second;
877 trackingTypes[it_tracker->first] = traker->getTrackerType();
880 return trackingTypes;
894 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
895 clippingFlag1 = it->second->getClipping();
898 clippingFlag2 = it->second->getClipping();
901 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
913 mapOfClippingFlags.clear();
915 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
917 TrackerWrapper *tracker = it->second;
918 mapOfClippingFlags[it->first] = tracker->getClipping();
931 return it->second->getFaces();
945 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
947 return it->second->getFaces();
950 std::cerr <<
"The camera: " << cameraName <<
" cannot be found!" << std::endl;
954 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
962 TrackerWrapper *tracker = it->second;
963 return tracker->getFeaturesCircle();
978 TrackerWrapper *tracker = it->second;
979 return tracker->getFeaturesKltCylinder();
994 TrackerWrapper *tracker = it->second;
995 return tracker->getFeaturesKlt();
1035 return it->second->getFeaturesForDisplay();
1041 return std::vector<std::vector<double> >();
1072 mapOfFeatures.clear();
1074 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1076 mapOfFeatures[it->first] = it->second->getFeaturesForDisplay();
1091 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
1104 TrackerWrapper *tracker = it->second;
1105 return tracker->getKltImagePoints();
1111 return std::vector<vpImagePoint>();
1126 TrackerWrapper *tracker = it->second;
1127 return tracker->getKltImagePointsWithId();
1133 return std::map<int, vpImagePoint>();
1145 TrackerWrapper *tracker = it->second;
1146 return tracker->getKltMaskBorder();
1164 TrackerWrapper *tracker = it->second;
1165 return tracker->getKltNbPoints();
1184 TrackerWrapper *tracker;
1185 tracker = it_tracker->second;
1186 return tracker->getKltOpencv();
1206 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1207 klt1 = it->second->getKltOpencv();
1210 klt2 = it->second->getKltOpencv();
1213 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1227 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1229 TrackerWrapper *tracker = it->second;
1230 mapOfKlts[it->first] = tracker->getKltOpencv();
1234 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
1244 TrackerWrapper *tracker = it->second;
1245 return tracker->getKltPoints();
1251 return std::vector<cv::Point2f>();
1280 it->second->getLcircle(circlesList, level);
1301 unsigned int level)
const
1303 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1305 it->second->getLcircle(circlesList, level);
1308 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1328 it->second->getLcylinder(cylindersList, level);
1349 unsigned int level)
const
1351 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1353 it->second->getLcylinder(cylindersList, level);
1356 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1377 it->second->getLline(linesList, level);
1398 unsigned int level)
const
1400 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1402 it->second->getLline(linesList, level);
1405 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1441 bool displayFullModel)
1446 return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1452 return std::vector<std::vector<double> >();
1481 const std::map<std::string, unsigned int> &mapOfwidths,
1482 const std::map<std::string, unsigned int> &mapOfheights,
1483 const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
1484 const std::map<std::string, vpCameraParameters> &mapOfCams,
1485 bool displayFullModel)
1488 mapOfModels.clear();
1490 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1492 std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
1493 std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
1494 std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
1495 std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
1497 if (findWidth != mapOfwidths.end() && findHeight != mapOfheights.end() && findcMo != mapOfcMos.end() &&
1498 findCam != mapOfCams.end()) {
1499 mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second, findcMo->second,
1500 findCam->second, displayFullModel);
1515 return it->second->getMovingEdge();
1535 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1536 it->second->getMovingEdge(me1);
1539 it->second->getMovingEdge(me2);
1542 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1554 mapOfMovingEdges.clear();
1556 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1558 TrackerWrapper *tracker = it->second;
1559 mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1584 unsigned int nbGoodPoints = 0;
1587 nbGoodPoints = it->second->getNbPoints(level);
1593 return nbGoodPoints;
1612 mapOfNbPoints.clear();
1614 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1616 TrackerWrapper *tracker = it->second;
1617 mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1630 return it->second->getNbPolygon();
1644 mapOfNbPolygons.clear();
1646 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1648 TrackerWrapper *tracker = it->second;
1649 mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1667 return it->second->getPolygon(index);
1687 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1689 return it->second->getPolygon(index);
1692 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1711 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1714 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1718 TrackerWrapper *tracker = it->second;
1719 polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1725 return polygonFaces;
1746 std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1747 bool orderPolygons,
bool useVisibility,
bool clipPolygon)
1749 mapOfPolygons.clear();
1750 mapOfPoints.clear();
1752 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1754 TrackerWrapper *tracker = it->second;
1755 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1756 tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1758 mapOfPolygons[it->first] = polygonFaces.first;
1759 mapOfPoints[it->first] = polygonFaces.second;
1776 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1777 it->second->getPose(c1Mo);
1780 it->second->getPose(c2Mo);
1783 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1796 mapOfCameraPoses.clear();
1798 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1800 TrackerWrapper *tracker = it->second;
1801 tracker->getPose(mapOfCameraPoses[it->first]);
1817 TrackerWrapper *tracker = it->second;
1818 return tracker->getTrackerType();
1828 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1830 TrackerWrapper *tracker = it->second;
1837 double ,
int ,
const std::string & )
1842 #ifdef VISP_HAVE_MODULE_GUI
1887 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1891 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1892 TrackerWrapper *tracker = it->second;
1893 tracker->initClick(I1, initFile1, displayHelp, T1);
1897 tracker = it->second;
1898 tracker->initClick(I2, initFile2, displayHelp, T2);
1902 tracker = it->second;
1905 tracker->getPose(
m_cMo);
1910 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1957 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1961 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1962 TrackerWrapper *tracker = it->second;
1963 tracker->initClick(I_color1, initFile1, displayHelp, T1);
1967 tracker = it->second;
1968 tracker->initClick(I_color2, initFile2, displayHelp, T2);
1972 tracker = it->second;
1975 tracker->getPose(
m_cMo);
1980 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
2027 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2028 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2031 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2033 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2035 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
2036 TrackerWrapper *tracker = it_tracker->second;
2037 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2038 if (it_T != mapOfT.end())
2039 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2041 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2042 tracker->getPose(
m_cMo);
2049 std::vector<std::string> vectorOfMissingCameraPoses;
2054 it_img = mapOfImages.find(it_tracker->first);
2055 it_initFile = mapOfInitFiles.find(it_tracker->first);
2057 if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
2059 TrackerWrapper *tracker = it_tracker->second;
2060 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2063 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2069 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2070 it != vectorOfMissingCameraPoses.end(); ++it) {
2071 it_img = mapOfImages.find(*it);
2072 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2082 "Missing image or missing camera transformation "
2083 "matrix! Cannot set the pose for camera: %s!",
2132 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2133 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2136 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2137 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2139 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2140 TrackerWrapper *tracker = it_tracker->second;
2141 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2142 if (it_T != mapOfT.end())
2143 tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2145 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2146 tracker->getPose(
m_cMo);
2153 std::vector<std::string> vectorOfMissingCameraPoses;
2158 it_img = mapOfColorImages.find(it_tracker->first);
2159 it_initFile = mapOfInitFiles.find(it_tracker->first);
2161 if (it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2163 TrackerWrapper *tracker = it_tracker->second;
2164 tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2167 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2173 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2174 it != vectorOfMissingCameraPoses.end(); ++it) {
2175 it_img = mapOfColorImages.find(*it);
2176 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2187 "Missing image or missing camera transformation "
2188 "matrix! Cannot set the pose for camera: %s!",
2196 const int ,
const std::string & )
2241 const std::string &initFile1,
const std::string &initFile2)
2244 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2245 TrackerWrapper *tracker = it->second;
2246 tracker->initFromPoints(I1, initFile1);
2250 tracker = it->second;
2251 tracker->initFromPoints(I2, initFile2);
2255 tracker = it->second;
2258 tracker->getPose(
m_cMo);
2261 tracker->getCameraParameters(
m_cam);
2266 "Cannot initFromPoints()! Require two cameras but "
2267 "there are %d cameras!",
2302 const std::string &initFile1,
const std::string &initFile2)
2305 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2306 TrackerWrapper *tracker = it->second;
2307 tracker->initFromPoints(I_color1, initFile1);
2311 tracker = it->second;
2312 tracker->initFromPoints(I_color2, initFile2);
2316 tracker = it->second;
2319 tracker->getPose(
m_cMo);
2322 tracker->getCameraParameters(
m_cam);
2327 "Cannot initFromPoints()! Require two cameras but "
2328 "there are %d cameras!",
2334 const std::map<std::string, std::string> &mapOfInitPoints)
2338 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2340 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2342 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2343 TrackerWrapper *tracker = it_tracker->second;
2344 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2345 tracker->getPose(
m_cMo);
2352 std::vector<std::string> vectorOfMissingCameraPoints;
2356 it_img = mapOfImages.find(it_tracker->first);
2357 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2359 if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2361 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2364 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2368 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2369 it != vectorOfMissingCameraPoints.end(); ++it) {
2370 it_img = mapOfImages.find(*it);
2371 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2380 "Missing image or missing camera transformation "
2381 "matrix! Cannot init the pose for camera: %s!",
2388 const std::map<std::string, std::string> &mapOfInitPoints)
2392 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2393 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2395 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() &&
2396 it_initPoints != mapOfInitPoints.end()) {
2397 TrackerWrapper *tracker = it_tracker->second;
2398 tracker->initFromPoints(*it_img->second, it_initPoints->second);
2399 tracker->getPose(
m_cMo);
2406 std::vector<std::string> vectorOfMissingCameraPoints;
2410 it_img = mapOfColorImages.find(it_tracker->first);
2411 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2413 if (it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2415 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2418 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2422 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2423 it != vectorOfMissingCameraPoints.end(); ++it) {
2424 it_img = mapOfColorImages.find(*it);
2425 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2434 "Missing image or missing camera transformation "
2435 "matrix! Cannot init the pose for camera: %s!",
2458 const std::string &initFile1,
const std::string &initFile2)
2461 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2462 TrackerWrapper *tracker = it->second;
2463 tracker->initFromPose(I1, initFile1);
2467 tracker = it->second;
2468 tracker->initFromPose(I2, initFile2);
2472 tracker = it->second;
2475 tracker->getPose(
m_cMo);
2478 tracker->getCameraParameters(
m_cam);
2483 "Cannot initFromPose()! Require two cameras but there "
2501 const std::string &initFile1,
const std::string &initFile2)
2504 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2505 TrackerWrapper *tracker = it->second;
2506 tracker->initFromPose(I_color1, initFile1);
2510 tracker = it->second;
2511 tracker->initFromPose(I_color2, initFile2);
2515 tracker = it->second;
2518 tracker->getPose(
m_cMo);
2521 tracker->getCameraParameters(
m_cam);
2526 "Cannot initFromPose()! Require two cameras but there "
2547 const std::map<std::string, std::string> &mapOfInitPoses)
2551 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2553 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2555 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2556 TrackerWrapper *tracker = it_tracker->second;
2557 tracker->initFromPose(*it_img->second, it_initPose->second);
2558 tracker->getPose(
m_cMo);
2565 std::vector<std::string> vectorOfMissingCameraPoses;
2569 it_img = mapOfImages.find(it_tracker->first);
2570 it_initPose = mapOfInitPoses.find(it_tracker->first);
2572 if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2574 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2577 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2581 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2582 it != vectorOfMissingCameraPoses.end(); ++it) {
2583 it_img = mapOfImages.find(*it);
2584 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2593 "Missing image or missing camera transformation "
2594 "matrix! Cannot init the pose for camera: %s!",
2615 const std::map<std::string, std::string> &mapOfInitPoses)
2619 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2620 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2622 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2623 TrackerWrapper *tracker = it_tracker->second;
2624 tracker->initFromPose(*it_img->second, it_initPose->second);
2625 tracker->getPose(
m_cMo);
2632 std::vector<std::string> vectorOfMissingCameraPoses;
2636 it_img = mapOfColorImages.find(it_tracker->first);
2637 it_initPose = mapOfInitPoses.find(it_tracker->first);
2639 if (it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2641 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2644 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2648 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2649 it != vectorOfMissingCameraPoses.end(); ++it) {
2650 it_img = mapOfColorImages.find(*it);
2651 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2660 "Missing image or missing camera transformation "
2661 "matrix! Cannot init the pose for camera: %s!",
2681 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2682 it->second->initFromPose(I1, c1Mo);
2686 it->second->initFromPose(I2, c2Mo);
2692 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2710 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2711 it->second->initFromPose(I_color1, c1Mo);
2715 it->second->initFromPose(I_color2, c2Mo);
2721 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2739 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2743 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2745 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2747 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2748 TrackerWrapper *tracker = it_tracker->second;
2749 tracker->initFromPose(*it_img->second, it_camPose->second);
2750 tracker->getPose(
m_cMo);
2757 std::vector<std::string> vectorOfMissingCameraPoses;
2761 it_img = mapOfImages.find(it_tracker->first);
2762 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2764 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2766 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2769 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2773 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2774 it != vectorOfMissingCameraPoses.end(); ++it) {
2775 it_img = mapOfImages.find(*it);
2776 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2785 "Missing image or missing camera transformation "
2786 "matrix! Cannot set the pose for camera: %s!",
2806 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2810 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2811 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2813 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2814 TrackerWrapper *tracker = it_tracker->second;
2815 tracker->initFromPose(*it_img->second, it_camPose->second);
2816 tracker->getPose(
m_cMo);
2823 std::vector<std::string> vectorOfMissingCameraPoses;
2827 it_img = mapOfColorImages.find(it_tracker->first);
2828 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2830 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2832 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2835 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2839 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2840 it != vectorOfMissingCameraPoses.end(); ++it) {
2841 it_img = mapOfColorImages.find(*it);
2842 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2851 "Missing image or missing camera transformation "
2852 "matrix! Cannot set the pose for camera: %s!",
2872 if (extension ==
".xml") {
2875 #ifdef VISP_HAVE_NLOHMANN_JSON
2876 else if (extension ==
".json") {
2898 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2900 TrackerWrapper *tracker = it->second;
2901 tracker->loadConfigFile(configFile, verbose);
2914 #ifdef VISP_HAVE_NLOHMANN_JSON
2926 std::ifstream jsonFile(settingsFile);
2927 if (!jsonFile.good()) {
2932 settings = json::parse(jsonFile);
2934 catch (json::parse_error &e) {
2935 std::stringstream msg;
2936 msg <<
"Could not parse JSON file : \n";
2938 msg << e.what() << std::endl;
2939 msg <<
"Byte position of error: " << e.byte;
2944 if (!settings.contains(
"version")) {
2947 else if (settings[
"version"].get<std::string>() != MBT_JSON_SETTINGS_VERSION) {
2954 trackersJson = settings.at(
"trackers");
2959 bool refCameraFound =
false;
2961 for (
const auto &it : trackersJson.items()) {
2962 const std::string cameraName = it.key();
2963 const json trackerJson = it.value();
2967 if (trackerJson.contains(
"camTref")) {
2974 std::cout <<
"Loading tracker " << cameraName << std::endl <<
" with settings: " << std::endl << trackerJson.dump(2);
2978 std::cout <<
"Updating an already existing tracker with JSON configuration." << std::endl;
2984 std::cout <<
"Creating a new tracker from JSON configuration." << std::endl;
2986 TrackerWrapper *tw =
new TrackerWrapper();
2990 const auto unusedCamIt = std::remove(unusedCameraNames.begin(), unusedCameraNames.end(), cameraName);
2991 unusedCameraNames.erase(unusedCamIt, unusedCameraNames.end());
2993 if (!refCameraFound) {
2998 for (
const std::string &oldCameraName : unusedCameraNames) {
3006 refTracker->getCameraParameters(
m_cam);
3010 this->
distNearClip = refTracker->getNearClippingDistance();
3011 this->
distFarClip = refTracker->getFarClippingDistance();
3014 if (settings.contains(
"display")) {
3015 const json displayJson = settings[
"display"];
3019 if (settings.contains(
"visibilityTest")) {
3020 const json visJson = settings[
"visibilityTest"];
3025 if (settings.contains(
"model")) {
3026 loadModel(settings.at(
"model").get<std::string>(), verbose);
3041 j[
"version"] = MBT_JSON_SETTINGS_VERSION;
3045 trackers[kv.first] = *(kv.second);
3048 trackers[kv.first][
"camTref"] = itTransformation->second;
3051 j[
"trackers"] = trackers;
3052 std::ofstream f(settingsFile);
3054 const unsigned indentLevel = 4;
3055 f << j.dump(indentLevel);
3085 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3086 TrackerWrapper *tracker = it_tracker->second;
3087 tracker->loadConfigFile(configFile1, verbose);
3090 tracker = it_tracker->second;
3091 tracker->loadConfigFile(configFile2, verbose);
3118 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3120 TrackerWrapper *tracker = it_tracker->second;
3122 std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
3123 if (it_config != mapOfConfigFiles.end()) {
3124 tracker->loadConfigFile(it_config->second, verbose);
3128 it_tracker->first.c_str());
3135 TrackerWrapper *tracker = it->second;
3136 tracker->getCameraParameters(
m_cam);
3169 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3171 TrackerWrapper *tracker = it->second;
3172 tracker->loadModel(modelFile, verbose, T);
3205 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3206 TrackerWrapper *tracker = it_tracker->second;
3207 tracker->loadModel(modelFile1, verbose, T1);
3210 tracker = it_tracker->second;
3211 tracker->loadModel(modelFile2, verbose, T2);
3234 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3236 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3238 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
3240 if (it_model != mapOfModelFiles.end()) {
3241 TrackerWrapper *tracker = it_tracker->second;
3242 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3244 if (it_T != mapOfT.end())
3245 tracker->loadModel(it_model->second, verbose, it_T->second);
3247 tracker->loadModel(it_model->second, verbose);
3251 it_tracker->first.c_str());
3256 #ifdef VISP_HAVE_PCL
3258 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
3260 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3262 TrackerWrapper *tracker = it->second;
3263 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
3269 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
3270 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
3271 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
3273 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3275 TrackerWrapper *tracker = it->second;
3276 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
3277 mapOfPointCloudHeights[it->first]);
3302 TrackerWrapper *tracker = it_tracker->second;
3303 tracker->reInitModel(I, cad_name, cMo, verbose, T);
3306 tracker->getPose(
m_cMo);
3336 TrackerWrapper *tracker = it_tracker->second;
3337 tracker->reInitModel(I_color, cad_name, cMo, verbose, T);
3340 tracker->getPose(
m_cMo);
3370 const std::string &cad_name1,
const std::string &cad_name2,
3375 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3377 it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
3381 it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
3386 it_tracker->second->getPose(
m_cMo);
3417 const std::string &cad_name1,
const std::string &cad_name2,
3422 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3424 it_tracker->second->reInitModel(I_color1, cad_name1, c1Mo, verbose, T1);
3428 it_tracker->second->reInitModel(I_color2, cad_name2, c2Mo, verbose, T2);
3433 it_tracker->second->getPose(
m_cMo);
3458 const std::map<std::string, std::string> &mapOfModelFiles,
3459 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
bool verbose,
3460 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3463 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3465 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3466 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3468 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3469 it_camPose != mapOfCameraPoses.end()) {
3470 TrackerWrapper *tracker = it_tracker->second;
3471 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3472 if (it_T != mapOfT.end())
3473 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3475 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3478 tracker->getPose(
m_cMo);
3484 std::vector<std::string> vectorOfMissingCameras;
3487 it_img = mapOfImages.find(it_tracker->first);
3488 it_model = mapOfModelFiles.find(it_tracker->first);
3489 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3491 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3492 TrackerWrapper *tracker = it_tracker->second;
3493 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3496 vectorOfMissingCameras.push_back(it_tracker->first);
3501 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3503 it_img = mapOfImages.find(*it);
3504 it_model = mapOfModelFiles.find(*it);
3505 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3508 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3511 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3533 const std::map<std::string, std::string> &mapOfModelFiles,
3534 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
bool verbose,
3535 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3538 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
3539 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3540 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3542 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3543 it_camPose != mapOfCameraPoses.end()) {
3544 TrackerWrapper *tracker = it_tracker->second;
3545 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3546 if (it_T != mapOfT.end())
3547 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3549 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3552 tracker->getPose(
m_cMo);
3558 std::vector<std::string> vectorOfMissingCameras;
3561 it_img = mapOfColorImages.find(it_tracker->first);
3562 it_model = mapOfModelFiles.find(it_tracker->first);
3563 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3565 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3566 it_camPose != mapOfCameraPoses.end()) {
3567 TrackerWrapper *tracker = it_tracker->second;
3568 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3571 vectorOfMissingCameras.push_back(it_tracker->first);
3576 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3578 it_img = mapOfColorImages.find(*it);
3579 it_model = mapOfModelFiles.find(*it);
3580 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3583 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3586 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3603 #ifdef VISP_HAVE_OGRE
3630 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3637 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3639 TrackerWrapper *tracker = it->second;
3640 tracker->resetTracker();
3657 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3659 TrackerWrapper *tracker = it->second;
3660 tracker->setAngleAppear(a);
3679 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3680 it->second->setAngleAppear(a1);
3683 it->second->setAngleAppear(a2);
3710 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3711 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3714 TrackerWrapper *tracker = it_tracker->second;
3715 tracker->setAngleAppear(it->second);
3737 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3739 TrackerWrapper *tracker = it->second;
3740 tracker->setAngleDisappear(a);
3759 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3760 it->second->setAngleDisappear(a1);
3763 it->second->setAngleDisappear(a2);
3790 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3791 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3794 TrackerWrapper *tracker = it_tracker->second;
3795 tracker->setAngleDisappear(it->second);
3813 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3815 TrackerWrapper *tracker = it->second;
3816 tracker->setCameraParameters(camera);
3831 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3832 it->second->setCameraParameters(camera1);
3835 it->second->setCameraParameters(camera2);
3839 it->second->getCameraParameters(
m_cam);
3861 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
3862 it != mapOfCameraParameters.end(); ++it) {
3863 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3866 TrackerWrapper *tracker = it_tracker->second;
3867 tracker->setCameraParameters(it->second);
3890 it->second = cameraTransformationMatrix;
3905 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
3907 for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
3908 it != mapOfTransformationMatrix.end(); ++it) {
3909 std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
3913 it_camTrans->second = it->second;
3931 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3933 TrackerWrapper *tracker = it->second;
3934 tracker->setClipping(flags);
3951 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3952 it->second->setClipping(flags1);
3955 it->second->setClipping(flags2);
3966 std::stringstream ss;
3967 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
3981 for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
3982 it != mapOfClippingFlags.end(); ++it) {
3983 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3986 TrackerWrapper *tracker = it_tracker->second;
3987 tracker->setClipping(it->second);
4007 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4009 TrackerWrapper *tracker = it->second;
4010 tracker->setDepthDenseFilteringMaxDistance(maxDistance);
4024 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4026 TrackerWrapper *tracker = it->second;
4027 tracker->setDepthDenseFilteringMethod(method);
4042 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4044 TrackerWrapper *tracker = it->second;
4045 tracker->setDepthDenseFilteringMinDistance(minDistance);
4060 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4062 TrackerWrapper *tracker = it->second;
4063 tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
4077 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4079 TrackerWrapper *tracker = it->second;
4080 tracker->setDepthDenseSamplingStep(stepX, stepY);
4093 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4095 TrackerWrapper *tracker = it->second;
4096 tracker->setDepthNormalFaceCentroidMethod(method);
4110 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4112 TrackerWrapper *tracker = it->second;
4113 tracker->setDepthNormalFeatureEstimationMethod(method);
4126 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4128 TrackerWrapper *tracker = it->second;
4129 tracker->setDepthNormalPclPlaneEstimationMethod(method);
4142 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4144 TrackerWrapper *tracker = it->second;
4145 tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
4158 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4160 TrackerWrapper *tracker = it->second;
4161 tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
4175 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4177 TrackerWrapper *tracker = it->second;
4178 tracker->setDepthNormalSamplingStep(stepX, stepY);
4204 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4206 TrackerWrapper *tracker = it->second;
4207 tracker->setDisplayFeatures(displayF);
4222 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4224 TrackerWrapper *tracker = it->second;
4225 tracker->setFarClippingDistance(dist);
4240 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4241 it->second->setFarClippingDistance(dist1);
4244 it->second->setFarClippingDistance(dist2);
4248 distFarClip = it->second->getFarClippingDistance();
4267 for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
4269 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4272 TrackerWrapper *tracker = it_tracker->second;
4273 tracker->setFarClippingDistance(it->second);
4292 std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
4293 if (it_factor != mapOfFeatureFactors.end()) {
4294 it->second = it_factor->second;
4318 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4320 TrackerWrapper *tracker = it->second;
4321 tracker->setGoodMovingEdgesRatioThreshold(threshold);
4325 #ifdef VISP_HAVE_OGRE
4339 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4341 TrackerWrapper *tracker = it->second;
4342 tracker->setGoodNbRayCastingAttemptsRatio(ratio);
4359 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4361 TrackerWrapper *tracker = it->second;
4362 tracker->setNbRayCastingAttemptsForVisibility(attempts);
4367 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4377 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4379 TrackerWrapper *tracker = it->second;
4380 tracker->setKltOpencv(t);
4395 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4396 it->second->setKltOpencv(t1);
4399 it->second->setKltOpencv(t2);
4414 for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
4415 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4418 TrackerWrapper *tracker = it_tracker->second;
4419 tracker->setKltOpencv(it->second);
4435 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4437 TrackerWrapper *tracker = it->second;
4438 tracker->setKltThresholdAcceptation(th);
4457 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4459 TrackerWrapper *tracker = it->second;
4460 tracker->setLod(useLod, name);
4464 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4474 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4476 TrackerWrapper *tracker = it->second;
4477 tracker->setKltMaskBorder(e);
4492 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4493 it->second->setKltMaskBorder(e1);
4497 it->second->setKltMaskBorder(e2);
4512 for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
4514 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4517 TrackerWrapper *tracker = it_tracker->second;
4518 tracker->setKltMaskBorder(it->second);
4533 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4535 TrackerWrapper *tracker = it->second;
4536 tracker->setMask(mask);
4553 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4555 TrackerWrapper *tracker = it->second;
4556 tracker->setMinLineLengthThresh(minLineLengthThresh, name);
4572 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4574 TrackerWrapper *tracker = it->second;
4575 tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
4588 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4590 TrackerWrapper *tracker = it->second;
4591 tracker->setMovingEdge(me);
4607 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4608 it->second->setMovingEdge(me1);
4612 it->second->setMovingEdge(me2);
4627 for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
4628 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4631 TrackerWrapper *tracker = it_tracker->second;
4632 tracker->setMovingEdge(it->second);
4648 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4650 TrackerWrapper *tracker = it->second;
4651 tracker->setNearClippingDistance(dist);
4666 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4667 it->second->setNearClippingDistance(dist1);
4671 it->second->setNearClippingDistance(dist2);
4694 for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
4695 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4698 TrackerWrapper *tracker = it_tracker->second;
4699 tracker->setNearClippingDistance(it->second);
4724 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4726 TrackerWrapper *tracker = it->second;
4727 tracker->setOgreShowConfigDialog(showConfigDialog);
4745 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4747 TrackerWrapper *tracker = it->second;
4748 tracker->setOgreVisibilityTest(v);
4751 #ifdef VISP_HAVE_OGRE
4752 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4754 TrackerWrapper *tracker = it->second;
4755 tracker->faces.getOgreContext()->setWindowName(
"Multi Generic MBT (" + it->first +
")");
4771 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4773 TrackerWrapper *tracker = it->second;
4774 tracker->setOptimizationMethod(opt);
4794 "to be configured with only one camera!");
4801 TrackerWrapper *tracker = it->second;
4802 tracker->setPose(I, cdMo);
4826 "to be configured with only one camera!");
4833 TrackerWrapper *tracker = it->second;
4835 tracker->setPose(
m_I, cdMo);
4858 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4859 it->second->setPose(I1, c1Mo);
4863 it->second->setPose(I2, c2Mo);
4868 it->second->getPose(
m_cMo);
4896 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4897 it->second->setPose(I_color1, c1Mo);
4901 it->second->setPose(I_color2, c2Mo);
4906 it->second->getPose(
m_cMo);
4935 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4939 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
4941 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4943 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4944 TrackerWrapper *tracker = it_tracker->second;
4945 tracker->setPose(*it_img->second, it_camPose->second);
4946 tracker->getPose(
m_cMo);
4953 std::vector<std::string> vectorOfMissingCameraPoses;
4958 it_img = mapOfImages.find(it_tracker->first);
4959 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4961 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4963 TrackerWrapper *tracker = it_tracker->second;
4964 tracker->setPose(*it_img->second, it_camPose->second);
4967 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4972 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4973 it != vectorOfMissingCameraPoses.end(); ++it) {
4974 it_img = mapOfImages.find(*it);
4975 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4984 "Missing image or missing camera transformation "
4985 "matrix! Cannot set pose for camera: %s!",
5007 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
5011 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
5012 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
5014 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
5015 TrackerWrapper *tracker = it_tracker->second;
5016 tracker->setPose(*it_img->second, it_camPose->second);
5017 tracker->getPose(
m_cMo);
5024 std::vector<std::string> vectorOfMissingCameraPoses;
5029 it_img = mapOfColorImages.find(it_tracker->first);
5030 it_camPose = mapOfCameraPoses.find(it_tracker->first);
5032 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
5034 TrackerWrapper *tracker = it_tracker->second;
5035 tracker->setPose(*it_img->second, it_camPose->second);
5038 vectorOfMissingCameraPoses.push_back(it_tracker->first);
5043 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
5044 it != vectorOfMissingCameraPoses.end(); ++it) {
5045 it_img = mapOfColorImages.find(*it);
5046 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
5055 "Missing image or missing camera transformation "
5056 "matrix! Cannot set pose for camera: %s!",
5080 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5082 TrackerWrapper *tracker = it->second;
5083 tracker->setProjectionErrorComputation(flag);
5094 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5096 TrackerWrapper *tracker = it->second;
5097 tracker->setProjectionErrorDisplay(
display);
5108 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5110 TrackerWrapper *tracker = it->second;
5111 tracker->setProjectionErrorDisplayArrowLength(length);
5119 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5121 TrackerWrapper *tracker = it->second;
5122 tracker->setProjectionErrorDisplayArrowThickness(thickness);
5133 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(referenceCameraName);
5138 std::cerr <<
"The reference camera: " << referenceCameraName <<
" does not exist!";
5146 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5148 TrackerWrapper *tracker = it->second;
5149 tracker->setScanLineVisibilityTest(v);
5166 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5168 TrackerWrapper *tracker = it->second;
5169 tracker->setTrackerType(type);
5184 for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
5185 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
5187 TrackerWrapper *tracker = it_tracker->second;
5188 tracker->setTrackerType(it->second);
5204 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5206 TrackerWrapper *tracker = it->second;
5207 tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
5222 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5224 TrackerWrapper *tracker = it->second;
5225 tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
5240 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5242 TrackerWrapper *tracker = it->second;
5243 tracker->setUseEdgeTracking(name, useEdgeTracking);
5247 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5259 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5261 TrackerWrapper *tracker = it->second;
5262 tracker->setUseKltTracking(name, useKltTracking);
5270 bool isOneTestTrackingOk =
false;
5271 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5273 TrackerWrapper *tracker = it->second;
5275 tracker->testTracking();
5276 isOneTestTrackingOk =
true;
5282 if (!isOneTestTrackingOk) {
5283 std::ostringstream oss;
5284 oss <<
"Not enough moving edges to track the object. Try to reduce the "
5302 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5305 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5306 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5308 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5322 std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
5325 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5326 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5328 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5344 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5345 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5346 mapOfImages[it->first] = &I1;
5349 mapOfImages[it->first] = &I2;
5351 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5352 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5354 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5357 std::stringstream ss;
5358 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5376 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5377 std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
5378 mapOfImages[it->first] = &I_color1;
5381 mapOfImages[it->first] = &_colorI2;
5383 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5384 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5386 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5389 std::stringstream ss;
5390 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5404 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5405 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5407 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5419 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5420 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5422 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5425 #ifdef VISP_HAVE_PCL
5435 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5437 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5439 TrackerWrapper *tracker = it->second;
5442 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5450 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5454 mapOfImages[it->first] == NULL) {
5459 !mapOfPointClouds[it->first]) {
5476 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5478 TrackerWrapper *tracker = it->second;
5481 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5484 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5487 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5489 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5494 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5511 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5513 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5514 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5516 TrackerWrapper *tracker = it->second;
5519 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5527 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5531 mapOfImages[it->first] == NULL) {
5535 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5539 mapOfImages[it->first] != NULL) {
5541 mapOfImages[it->first] = &tracker->m_I;
5545 !mapOfPointClouds[it->first]) {
5562 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5564 TrackerWrapper *tracker = it->second;
5567 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5570 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5573 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5575 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5580 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5600 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5601 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5602 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5604 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5606 TrackerWrapper *tracker = it->second;
5609 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5617 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5621 mapOfImages[it->first] == NULL) {
5626 (mapOfPointClouds[it->first] == NULL)) {
5631 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5643 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5645 TrackerWrapper *tracker = it->second;
5648 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5651 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5654 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5656 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5661 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5680 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5681 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5682 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5684 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5685 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5687 TrackerWrapper *tracker = it->second;
5690 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5698 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5702 mapOfColorImages[it->first] == NULL) {
5706 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5710 mapOfColorImages[it->first] != NULL) {
5712 mapOfImages[it->first] = &tracker->m_I;
5716 (mapOfPointClouds[it->first] == NULL)) {
5721 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5733 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5735 TrackerWrapper *tracker = it->second;
5738 tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5741 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5744 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5746 tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5751 tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5760 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
5761 : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
5766 #ifdef VISP_HAVE_OGRE
5773 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
int trackerType)
5774 : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
5777 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5787 #ifdef VISP_HAVE_OGRE
5794 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
5799 computeVVSInit(ptr_I);
5801 if (m_error.getRows() < 4) {
5806 double normRes_1 = -1;
5807 unsigned int iter = 0;
5809 double factorEdge = 1.0;
5810 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5811 double factorKlt = 1.0;
5813 double factorDepth = 1.0;
5814 double factorDepthDense = 1.0;
5820 double mu = m_initialMu;
5822 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5825 bool isoJoIdentity = m_isoJoIdentity;
5833 unsigned int nb_edge_features = m_error_edge.
getRows();
5834 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5835 unsigned int nb_klt_features = m_error_klt.getRows();
5837 unsigned int nb_depth_features = m_error_depthNormal.getRows();
5838 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5840 while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
5841 computeVVSInteractionMatrixAndResidu(ptr_I);
5843 bool reStartFromLastIncrement =
false;
5844 computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
5846 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5847 if (reStartFromLastIncrement) {
5848 if (m_trackerType & KLT_TRACKER) {
5854 if (!reStartFromLastIncrement) {
5855 computeVVSWeights();
5857 if (computeCovariance) {
5859 if (!isoJoIdentity) {
5862 LVJ_true = (m_L * cVo * oJo);
5872 if (isoJoIdentity) {
5876 unsigned int rank = (m_L * cVo).kernel(K);
5886 isoJoIdentity =
false;
5895 unsigned int start_index = 0;
5896 if (m_trackerType & EDGE_TRACKER) {
5897 for (
unsigned int i = 0; i < nb_edge_features; i++) {
5898 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5900 m_weightedError[i] = wi * m_error[i];
5905 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5910 start_index += nb_edge_features;
5913 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5914 if (m_trackerType & KLT_TRACKER) {
5915 for (
unsigned int i = 0; i < nb_klt_features; i++) {
5916 double wi = m_w_klt[i] * factorKlt;
5917 W_true[start_index + i] = wi;
5918 m_weightedError[start_index + i] = wi * m_error_klt[i];
5920 num += wi *
vpMath::sqr(m_error[start_index + i]);
5923 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5924 m_L[start_index + i][j] *= wi;
5928 start_index += nb_klt_features;
5932 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5933 for (
unsigned int i = 0; i < nb_depth_features; i++) {
5934 double wi = m_w_depthNormal[i] * factorDepth;
5935 m_w[start_index + i] = m_w_depthNormal[i];
5936 m_weightedError[start_index + i] = wi * m_error[start_index + i];
5938 num += wi *
vpMath::sqr(m_error[start_index + i]);
5941 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5942 m_L[start_index + i][j] *= wi;
5946 start_index += nb_depth_features;
5949 if (m_trackerType & DEPTH_DENSE_TRACKER) {
5950 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
5951 double wi = m_w_depthDense[i] * factorDepthDense;
5952 m_w[start_index + i] = m_w_depthDense[i];
5953 m_weightedError[start_index + i] = wi * m_error[start_index + i];
5955 num += wi *
vpMath::sqr(m_error[start_index + i]);
5958 for (
unsigned int j = 0; j < m_L.getCols(); j++) {
5959 m_L[start_index + i][j] *= wi;
5966 computeVVSPoseEstimation(isoJoIdentity, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
5969 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5970 if (m_trackerType & KLT_TRACKER) {
5977 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5978 if (m_trackerType & KLT_TRACKER) {
5982 normRes_1 = normRes;
5984 normRes = sqrt(num / den);
5990 computeCovarianceMatrixVVS(isoJoIdentity, W_true, cMo_prev, L_true, LVJ_true, m_error);
5992 if (m_trackerType & EDGE_TRACKER) {
5997 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
6000 "TrackerWrapper::computeVVSInit("
6001 ") should not be called!");
6006 initMbtTracking(ptr_I);
6008 unsigned int nbFeatures = 0;
6010 if (m_trackerType & EDGE_TRACKER) {
6011 nbFeatures += m_error_edge.getRows();
6014 m_error_edge.clear();
6015 m_weightedError_edge.clear();
6020 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6021 if (m_trackerType & KLT_TRACKER) {
6023 nbFeatures += m_error_klt.getRows();
6026 m_error_klt.clear();
6027 m_weightedError_klt.clear();
6033 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6035 nbFeatures += m_error_depthNormal.getRows();
6038 m_error_depthNormal.clear();
6039 m_weightedError_depthNormal.clear();
6040 m_L_depthNormal.clear();
6041 m_w_depthNormal.clear();
6044 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6046 nbFeatures += m_error_depthDense.getRows();
6049 m_error_depthDense.clear();
6050 m_weightedError_depthDense.clear();
6051 m_L_depthDense.clear();
6052 m_w_depthDense.clear();
6055 m_L.resize(nbFeatures, 6,
false,
false);
6056 m_error.resize(nbFeatures,
false);
6058 m_weightedError.resize(nbFeatures,
false);
6059 m_w.resize(nbFeatures,
false);
6067 "computeVVSInteractionMatrixAndR"
6068 "esidu() should not be called!");
6073 if (m_trackerType & EDGE_TRACKER) {
6077 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6078 if (m_trackerType & KLT_TRACKER) {
6083 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6087 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6091 unsigned int start_index = 0;
6092 if (m_trackerType & EDGE_TRACKER) {
6093 m_L.insert(m_L_edge, start_index, 0);
6094 m_error.insert(start_index, m_error_edge);
6096 start_index += m_error_edge.getRows();
6099 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6100 if (m_trackerType & KLT_TRACKER) {
6101 m_L.insert(m_L_klt, start_index, 0);
6102 m_error.insert(start_index, m_error_klt);
6104 start_index += m_error_klt.getRows();
6108 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6109 m_L.insert(m_L_depthNormal, start_index, 0);
6110 m_error.insert(start_index, m_error_depthNormal);
6112 start_index += m_error_depthNormal.getRows();
6115 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6116 m_L.insert(m_L_depthDense, start_index, 0);
6117 m_error.insert(start_index, m_error_depthDense);
6125 unsigned int start_index = 0;
6127 if (m_trackerType & EDGE_TRACKER) {
6129 m_w.insert(start_index, m_w_edge);
6131 start_index += m_w_edge.getRows();
6134 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6135 if (m_trackerType & KLT_TRACKER) {
6137 m_w.insert(start_index, m_w_klt);
6139 start_index += m_w_klt.getRows();
6143 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6144 if (m_depthNormalUseRobust) {
6146 m_w.insert(start_index, m_w_depthNormal);
6149 start_index += m_w_depthNormal.getRows();
6152 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6154 m_w.insert(start_index, m_w_depthDense);
6162 unsigned int thickness,
bool displayFullModel)
6164 if (displayFeatures) {
6165 std::vector<std::vector<double> > features = getFeaturesForDisplay();
6166 for (
size_t i = 0; i < features.size(); i++) {
6169 int state =
static_cast<int>(features[i][3]);
6201 double id = features[i][5];
6202 std::stringstream ss;
6207 vpImagePoint im_centroid(features[i][1], features[i][2]);
6208 vpImagePoint im_extremity(features[i][3], features[i][4]);
6215 std::vector<std::vector<double> > models =
6217 for (
size_t i = 0; i < models.size(); i++) {
6225 double n20 = models[i][3];
6226 double n11 = models[i][4];
6227 double n02 = models[i][5];
6232 #ifdef VISP_HAVE_OGRE
6233 if ((m_trackerType & EDGE_TRACKER)
6234 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6235 || (m_trackerType & KLT_TRACKER)
6239 faces.displayOgre(cMo);
6246 unsigned int thickness,
bool displayFullModel)
6248 if (displayFeatures) {
6249 std::vector<std::vector<double> > features = getFeaturesForDisplay();
6250 for (
size_t i = 0; i < features.size(); i++) {
6253 int state =
static_cast<int>(features[i][3]);
6285 double id = features[i][5];
6286 std::stringstream ss;
6291 vpImagePoint im_centroid(features[i][1], features[i][2]);
6292 vpImagePoint im_extremity(features[i][3], features[i][4]);
6299 std::vector<std::vector<double> > models =
6301 for (
size_t i = 0; i < models.size(); i++) {
6309 double n20 = models[i][3];
6310 double n11 = models[i][4];
6311 double n02 = models[i][5];
6316 #ifdef VISP_HAVE_OGRE
6317 if ((m_trackerType & EDGE_TRACKER)
6318 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6319 || (m_trackerType & KLT_TRACKER)
6323 faces.displayOgre(cMo);
6328 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6330 std::vector<std::vector<double> > features;
6332 if (m_trackerType & EDGE_TRACKER) {
6334 features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6337 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6338 if (m_trackerType & KLT_TRACKER) {
6340 features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6344 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6346 features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(),
6347 m_featuresToBeDisplayedDepthNormal.end());
6353 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(
unsigned int width,
6354 unsigned int height,
6357 bool displayFullModel)
6359 std::vector<std::vector<double> > models;
6362 if (m_trackerType == EDGE_TRACKER) {
6365 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6366 else if (m_trackerType == KLT_TRACKER) {
6370 else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
6373 else if (m_trackerType == DEPTH_DENSE_TRACKER) {
6378 if (m_trackerType & EDGE_TRACKER) {
6379 std::vector<std::vector<double> > edgeModels =
6381 models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6385 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6386 std::vector<std::vector<double> > depthDenseModels =
6388 models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6397 if (!modelInitialised) {
6401 if (useScanLine || clippingFlag > 3)
6404 bool reInitialisation =
false;
6406 faces.setVisible(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6409 #ifdef VISP_HAVE_OGRE
6410 if (!faces.isOgreInitialised()) {
6413 faces.setOgreShowConfigDialog(ogreShowConfigDialog);
6414 faces.initOgre(m_cam);
6418 ogreShowConfigDialog =
false;
6421 faces.setVisibleOgre(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6423 faces.setVisible(I.
getWidth(), I.
getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6428 faces.computeClippedPolygons(m_cMo, m_cam);
6432 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6433 if (m_trackerType & KLT_TRACKER)
6437 if (m_trackerType & EDGE_TRACKER) {
6446 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6449 if (m_trackerType & DEPTH_DENSE_TRACKER)
6453 void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
6454 double radius,
int idFace,
const std::string &name)
6456 if (m_trackerType & EDGE_TRACKER)
6459 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6460 if (m_trackerType & KLT_TRACKER)
6465 void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
int idFace,
6466 const std::string &name)
6468 if (m_trackerType & EDGE_TRACKER)
6471 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6472 if (m_trackerType & KLT_TRACKER)
6477 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
6479 if (m_trackerType & EDGE_TRACKER)
6482 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6483 if (m_trackerType & KLT_TRACKER)
6487 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6490 if (m_trackerType & DEPTH_DENSE_TRACKER)
6494 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
6496 if (m_trackerType & EDGE_TRACKER)
6499 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6500 if (m_trackerType & KLT_TRACKER)
6504 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6507 if (m_trackerType & DEPTH_DENSE_TRACKER)
6513 if (m_trackerType & EDGE_TRACKER) {
6519 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile,
bool verbose)
6525 xmlp.setVerbose(verbose);
6526 xmlp.setCameraParameters(m_cam);
6528 xmlp.setAngleDisappear(
vpMath::deg(angleDisappears));
6534 xmlp.setKltMaxFeatures(10000);
6535 xmlp.setKltWindowSize(5);
6536 xmlp.setKltQuality(0.01);
6537 xmlp.setKltMinDistance(5);
6538 xmlp.setKltHarrisParam(0.01);
6539 xmlp.setKltBlockSize(3);
6540 xmlp.setKltPyramidLevels(3);
6541 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6542 xmlp.setKltMaskBorder(maskBorder);
6546 xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
6547 xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
6548 xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
6549 xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
6550 xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
6551 xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
6554 xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
6555 xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
6559 std::cout <<
" *********** Parsing XML for";
6562 std::vector<std::string> tracker_names;
6563 if (m_trackerType & EDGE_TRACKER)
6564 tracker_names.push_back(
"Edge");
6565 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6566 if (m_trackerType & KLT_TRACKER)
6567 tracker_names.push_back(
"Klt");
6569 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6570 tracker_names.push_back(
"Depth Normal");
6571 if (m_trackerType & DEPTH_DENSE_TRACKER)
6572 tracker_names.push_back(
"Depth Dense");
6575 for (
size_t i = 0; i < tracker_names.size(); i++) {
6576 std::cout <<
" " << tracker_names[i];
6577 if (i == tracker_names.size() - 1) {
6582 std::cout <<
"Model-Based Tracker ************ " << std::endl;
6585 xmlp.parse(configFile);
6592 xmlp.getCameraParameters(camera);
6593 setCameraParameters(camera);
6595 angleAppears =
vpMath::rad(xmlp.getAngleAppear());
6596 angleDisappears =
vpMath::rad(xmlp.getAngleDisappear());
6598 if (xmlp.hasNearClippingDistance())
6599 setNearClippingDistance(xmlp.getNearClippingDistance());
6601 if (xmlp.hasFarClippingDistance())
6602 setFarClippingDistance(xmlp.getFarClippingDistance());
6604 if (xmlp.getFovClipping()) {
6608 useLodGeneral = xmlp.getLodState();
6609 minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
6610 minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
6612 applyLodSettingInConfig =
false;
6613 if (this->getNbPolygon() > 0) {
6614 applyLodSettingInConfig =
true;
6615 setLod(useLodGeneral);
6616 setMinLineLengthThresh(minLineLengthThresholdGeneral);
6617 setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
6622 xmlp.getEdgeMe(meParser);
6626 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6627 tracker.setMaxFeatures((
int)xmlp.getKltMaxFeatures());
6628 tracker.setWindowSize((
int)xmlp.getKltWindowSize());
6629 tracker.setQuality(xmlp.getKltQuality());
6630 tracker.setMinDistance(xmlp.getKltMinDistance());
6631 tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
6632 tracker.setBlockSize((
int)xmlp.getKltBlockSize());
6633 tracker.setPyramidLevels((
int)xmlp.getKltPyramidLevels());
6634 maskBorder = xmlp.getKltMaskBorder();
6637 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
6641 setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod());
6642 setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod());
6643 setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter());
6644 setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold());
6645 setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY());
6648 setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
6651 #ifdef VISP_HAVE_PCL
6653 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6655 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6657 if (m_trackerType & KLT_TRACKER) {
6665 if (m_trackerType & EDGE_TRACKER) {
6666 bool newvisibleface =
false;
6670 faces.computeClippedPolygons(m_cMo, m_cam);
6676 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6680 if (m_trackerType & DEPTH_DENSE_TRACKER)
6684 if (m_trackerType & EDGE_TRACKER) {
6691 if (computeProjError) {
6698 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6700 if (m_trackerType & EDGE_TRACKER) {
6705 std::cerr <<
"Error in moving edge tracking" << std::endl;
6710 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6711 if (m_trackerType & KLT_TRACKER) {
6716 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6722 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6727 std::cerr <<
"Error in Depth normal tracking" << std::endl;
6732 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6737 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6745 const unsigned int pointcloud_width,
6746 const unsigned int pointcloud_height)
6748 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6750 if (m_trackerType & KLT_TRACKER) {
6758 if (m_trackerType & EDGE_TRACKER) {
6759 bool newvisibleface =
false;
6763 faces.computeClippedPolygons(m_cMo, m_cam);
6769 if (m_trackerType & DEPTH_NORMAL_TRACKER)
6773 if (m_trackerType & DEPTH_DENSE_TRACKER)
6777 if (m_trackerType & EDGE_TRACKER) {
6784 if (computeProjError) {
6791 const std::vector<vpColVector> *
const point_cloud,
6792 const unsigned int pointcloud_width,
6793 const unsigned int pointcloud_height)
6795 if (m_trackerType & EDGE_TRACKER) {
6800 std::cerr <<
"Error in moving edge tracking" << std::endl;
6805 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6806 if (m_trackerType & KLT_TRACKER) {
6811 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6817 if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6822 std::cerr <<
"Error in Depth tracking" << std::endl;
6827 if (m_trackerType & DEPTH_DENSE_TRACKER) {
6832 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6850 for (
unsigned int i = 0; i < scales.size(); i++) {
6852 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
6859 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
6867 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
6875 cylinders[i].clear();
6883 nbvisiblepolygone = 0;
6886 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6887 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
6889 cvReleaseImage(&cur);
6895 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
6897 if (kltpoly != NULL) {
6902 kltPolygons.clear();
6904 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
6907 if (kltPolyCylinder != NULL) {
6908 delete kltPolyCylinder;
6910 kltPolyCylinder = NULL;
6912 kltCylinders.clear();
6915 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
6922 circles_disp.clear();
6924 firstInitialisation =
true;
6929 for (
size_t i = 0; i < m_depthNormalFaces.size(); i++) {
6930 delete m_depthNormalFaces[i];
6931 m_depthNormalFaces[i] = NULL;
6933 m_depthNormalFaces.clear();
6936 for (
size_t i = 0; i < m_depthDenseFaces.size(); i++) {
6937 delete m_depthDenseFaces[i];
6938 m_depthDenseFaces[i] = NULL;
6940 m_depthDenseFaces.clear();
6944 loadModel(cad_name, verbose, T);
6946 initFromPose(*I, cMo);
6949 initFromPose(*I_color, cMo);
6957 reInitModel(&I, NULL, cad_name, cMo, verbose, T);
6964 reInitModel(NULL, &I_color, cad_name, cMo, verbose, T);
6967 void vpMbGenericTracker::TrackerWrapper::resetTracker()
6970 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6977 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(
const vpCameraParameters &cam)
6982 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6991 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(
const double &dist)
6996 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(
const double &dist)
7001 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(
const bool &v)
7004 #ifdef VISP_HAVE_OGRE
7005 faces.getOgreContext()->setWindowName(
"TrackerWrapper");
7012 bool performKltSetPose =
false;
7017 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
7018 if (m_trackerType & KLT_TRACKER) {
7019 performKltSetPose =
true;
7021 if (useScanLine || clippingFlag > 3) {
7022 m_cam.computeFov(I ? I->
getWidth() : m_I.getWidth(), I ? I->
getHeight() : m_I.getHeight());
7029 if (!performKltSetPose) {
7035 if (m_trackerType & EDGE_TRACKER) {
7040 faces.computeClippedPolygons(m_cMo, m_cam);
7041 faces.computeScanLineRender(m_cam, I ? I->
getWidth() : m_I.getWidth(), I ? I->
getHeight() : m_I.getHeight());
7045 if (m_trackerType & EDGE_TRACKER) {
7046 initPyramid(I, Ipyramid);
7048 unsigned int i = (
unsigned int)scales.size();
7058 cleanPyramid(Ipyramid);
7061 if (m_trackerType & EDGE_TRACKER) {
7075 setPose(&I, NULL, cdMo);
7080 setPose(NULL, &I_color, cdMo);
7083 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(
const bool &flag)
7088 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(
const bool &v)
7091 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
7098 void vpMbGenericTracker::TrackerWrapper::setTrackerType(
int type)
7100 if ((type & (EDGE_TRACKER |
7101 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
7104 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
7108 m_trackerType = type;
7111 void vpMbGenericTracker::TrackerWrapper::testTracking()
7113 if (m_trackerType & EDGE_TRACKER) {
7119 #
if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
7124 if ((m_trackerType & (EDGE_TRACKER
7125 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
7129 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
7133 #if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
7143 #ifdef VISP_HAVE_PCL
7145 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
7147 if ((m_trackerType & (EDGE_TRACKER |
7148 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
7151 DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
7152 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
7156 if (m_trackerType & (EDGE_TRACKER
7157 #
if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
7165 if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && !point_cloud) {
7172 preTracking(ptr_I, point_cloud);
7178 covarianceMatrix = -1;
7182 if (m_trackerType == EDGE_TRACKER)
7185 postTracking(ptr_I, point_cloud);
7189 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.
@ notInitialized
Used to indicate that a parameter is not initialized.
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
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 threshold=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 void saveConfigFile(const std::string &settingsFile) const
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)
virtual void loadConfigFileJSON(const std::string &configFile, bool verbose=true)
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 void loadConfigFileXML(const std::string &configFile, bool verbose=true)
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)
friend void from_json(const nlohmann::json &j, TrackerWrapper &t)
Load configuration settings from a JSON object for a tracker wrapper.
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 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)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual void setMask(const vpImage< bool > &mask)