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>
46using 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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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()) {
115 throw vpException(vpException::badValue,
"There is no camera!");
118 if (trackerTypes.size() == 1) {
119 m_mapOfTrackers[
"Camera"] = new TrackerWrapper(trackerTypes[0]);
122 m_mapOfCameraTransformationMatrix[
"Camera"] = vpHomogeneousMatrix();
125 for (size_t i = 1; i <= trackerTypes.size(); i++) {
126 std::stringstream ss;
128 m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerTypes[i - 1]);
131 m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
139 m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0;
141#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
142 m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
145 m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
146 m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
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()) {
156 throw vpException(vpTrackingException::badValue,
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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;
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;
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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;
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;
421#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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;
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;
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;
476 normRes = sqrt(num / den);
484#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
528 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
530 TrackerWrapper *tracker = it->second;
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
962 TrackerWrapper *tracker = it->second;
978 TrackerWrapper *tracker = it->second;
994 TrackerWrapper *tracker = it->second;
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
1104 TrackerWrapper *tracker = it->second;
1111 return std::vector<vpImagePoint>();
1126 TrackerWrapper *tracker = it->second;
1133 return std::map<int, vpImagePoint>();
1145 TrackerWrapper *tracker = it->second;
1164 TrackerWrapper *tracker = it->second;
1184 TrackerWrapper *tracker;
1185 tracker = it_tracker->second;
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;
1243 TrackerWrapper *tracker = it->second;
1250 return std::vector<cv::Point2f>();
1278 it->second->getLcircle(circlesList, level);
1299 unsigned int level)
const
1301 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1303 it->second->getLcircle(circlesList, level);
1306 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1326 it->second->getLcylinder(cylindersList, level);
1347 unsigned int level)
const
1349 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1351 it->second->getLcylinder(cylindersList, level);
1354 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1375 it->second->getLline(linesList, level);
1396 unsigned int level)
const
1398 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1400 it->second->getLline(linesList, level);
1403 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1439 bool displayFullModel)
1444 return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1450 return std::vector<std::vector<double> >();
1479 const std::map<std::string, unsigned int> &mapOfwidths,
1480 const std::map<std::string, unsigned int> &mapOfheights,
1481 const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
1482 const std::map<std::string, vpCameraParameters> &mapOfCams,
1483 bool displayFullModel)
1486 mapOfModels.clear();
1488 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1490 std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
1491 std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
1492 std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
1493 std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
1495 if (findWidth != mapOfwidths.end() && findHeight != mapOfheights.end() && findcMo != mapOfcMos.end() &&
1496 findCam != mapOfCams.end()) {
1497 mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second, findcMo->second,
1498 findCam->second, displayFullModel);
1513 return it->second->getMovingEdge();
1533 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1534 it->second->getMovingEdge(me1);
1537 it->second->getMovingEdge(me2);
1540 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1552 mapOfMovingEdges.clear();
1554 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1556 TrackerWrapper *tracker = it->second;
1582 unsigned int nbGoodPoints = 0;
1585 nbGoodPoints = it->second->getNbPoints(level);
1591 return nbGoodPoints;
1610 mapOfNbPoints.clear();
1612 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1614 TrackerWrapper *tracker = it->second;
1615 mapOfNbPoints[it->first] = tracker->
getNbPoints(level);
1628 return it->second->getNbPolygon();
1642 mapOfNbPolygons.clear();
1644 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1646 TrackerWrapper *tracker = it->second;
1665 return it->second->getPolygon(index);
1685 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(cameraName);
1687 return it->second->getPolygon(index);
1690 std::cerr <<
"The camera: " << cameraName <<
" does not exist!" << std::endl;
1709std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1712 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1716 TrackerWrapper *tracker = it->second;
1717 polygonFaces = tracker->
getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1723 return polygonFaces;
1744 std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1745 bool orderPolygons,
bool useVisibility,
bool clipPolygon)
1747 mapOfPolygons.clear();
1748 mapOfPoints.clear();
1750 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1752 TrackerWrapper *tracker = it->second;
1753 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1756 mapOfPolygons[it->first] = polygonFaces.first;
1757 mapOfPoints[it->first] = polygonFaces.second;
1774 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1775 it->second->getPose(c1Mo);
1778 it->second->getPose(c2Mo);
1781 std::cerr <<
"The tracker is not set as a stereo configuration! There are " <<
m_mapOfTrackers.size() <<
" cameras!"
1794 mapOfCameraPoses.clear();
1796 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1798 TrackerWrapper *tracker = it->second;
1799 tracker->
getPose(mapOfCameraPoses[it->first]);
1815 TrackerWrapper *tracker = it->second;
1816 return tracker->getTrackerType();
1826 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1828 TrackerWrapper *tracker = it->second;
1835 double ,
int ,
const std::string & )
1840#ifdef VISP_HAVE_MODULE_GUI
1885 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1889 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1890 TrackerWrapper *tracker = it->second;
1891 tracker->
initClick(I1, initFile1, displayHelp, T1);
1895 tracker = it->second;
1896 tracker->
initClick(I2, initFile2, displayHelp, T2);
1900 tracker = it->second;
1908 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
1955 const std::string &initFile1,
const std::string &initFile2,
bool displayHelp,
1959 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
1960 TrackerWrapper *tracker = it->second;
1961 tracker->
initClick(I_color1, initFile1, displayHelp, T1);
1965 tracker = it->second;
1966 tracker->
initClick(I_color2, initFile2, displayHelp, T2);
1970 tracker = it->second;
1978 "Cannot initClick()! Require two cameras but there are %d cameras!",
m_mapOfTrackers.size());
2025 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2026 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2029 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2031 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2033 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
2034 TrackerWrapper *tracker = it_tracker->second;
2035 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2036 if (it_T != mapOfT.end())
2037 tracker->
initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2039 tracker->
initClick(*it_img->second, it_initFile->second, displayHelp);
2047 std::vector<std::string> vectorOfMissingCameraPoses;
2052 it_img = mapOfImages.find(it_tracker->first);
2053 it_initFile = mapOfInitFiles.find(it_tracker->first);
2055 if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
2057 TrackerWrapper *tracker = it_tracker->second;
2058 tracker->
initClick(*it_img->second, it_initFile->second, displayHelp);
2061 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2067 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2068 it != vectorOfMissingCameraPoses.end(); ++it) {
2069 it_img = mapOfImages.find(*it);
2070 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2080 "Missing image or missing camera transformation "
2081 "matrix! Cannot set the pose for camera: %s!",
2130 const std::map<std::string, std::string> &mapOfInitFiles,
bool displayHelp,
2131 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2134 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2135 std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(
m_referenceCameraName);
2137 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2138 TrackerWrapper *tracker = it_tracker->second;
2139 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2140 if (it_T != mapOfT.end())
2141 tracker->
initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2143 tracker->
initClick(*it_img->second, it_initFile->second, displayHelp);
2151 std::vector<std::string> vectorOfMissingCameraPoses;
2156 it_img = mapOfColorImages.find(it_tracker->first);
2157 it_initFile = mapOfInitFiles.find(it_tracker->first);
2159 if (it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2161 TrackerWrapper *tracker = it_tracker->second;
2162 tracker->
initClick(*it_img->second, it_initFile->second, displayHelp);
2165 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2171 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2172 it != vectorOfMissingCameraPoses.end(); ++it) {
2173 it_img = mapOfColorImages.find(*it);
2174 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2185 "Missing image or missing camera transformation "
2186 "matrix! Cannot set the pose for camera: %s!",
2194 const int ,
const std::string & )
2239 const std::string &initFile1,
const std::string &initFile2)
2242 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2243 TrackerWrapper *tracker = it->second;
2248 tracker = it->second;
2253 tracker = it->second;
2264 "Cannot initFromPoints()! Require two cameras but "
2265 "there are %d cameras!",
2300 const std::string &initFile1,
const std::string &initFile2)
2303 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2304 TrackerWrapper *tracker = it->second;
2309 tracker = it->second;
2314 tracker = it->second;
2325 "Cannot initFromPoints()! Require two cameras but "
2326 "there are %d cameras!",
2332 const std::map<std::string, std::string> &mapOfInitPoints)
2336 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2338 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2340 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2341 TrackerWrapper *tracker = it_tracker->second;
2350 std::vector<std::string> vectorOfMissingCameraPoints;
2354 it_img = mapOfImages.find(it_tracker->first);
2355 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2357 if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2359 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2362 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2366 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2367 it != vectorOfMissingCameraPoints.end(); ++it) {
2368 it_img = mapOfImages.find(*it);
2369 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2378 "Missing image or missing camera transformation "
2379 "matrix! Cannot init the pose for camera: %s!",
2386 const std::map<std::string, std::string> &mapOfInitPoints)
2390 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2391 std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(
m_referenceCameraName);
2393 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() &&
2394 it_initPoints != mapOfInitPoints.end()) {
2395 TrackerWrapper *tracker = it_tracker->second;
2404 std::vector<std::string> vectorOfMissingCameraPoints;
2408 it_img = mapOfColorImages.find(it_tracker->first);
2409 it_initPoints = mapOfInitPoints.find(it_tracker->first);
2411 if (it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2413 it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2416 vectorOfMissingCameraPoints.push_back(it_tracker->first);
2420 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2421 it != vectorOfMissingCameraPoints.end(); ++it) {
2422 it_img = mapOfColorImages.find(*it);
2423 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2432 "Missing image or missing camera transformation "
2433 "matrix! Cannot init the pose for camera: %s!",
2456 const std::string &initFile1,
const std::string &initFile2)
2459 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2460 TrackerWrapper *tracker = it->second;
2465 tracker = it->second;
2470 tracker = it->second;
2481 "Cannot initFromPose()! Require two cameras but there "
2499 const std::string &initFile1,
const std::string &initFile2)
2502 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2503 TrackerWrapper *tracker = it->second;
2508 tracker = it->second;
2513 tracker = it->second;
2524 "Cannot initFromPose()! Require two cameras but there "
2545 const std::map<std::string, std::string> &mapOfInitPoses)
2549 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2551 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2553 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2554 TrackerWrapper *tracker = it_tracker->second;
2555 tracker->
initFromPose(*it_img->second, it_initPose->second);
2563 std::vector<std::string> vectorOfMissingCameraPoses;
2567 it_img = mapOfImages.find(it_tracker->first);
2568 it_initPose = mapOfInitPoses.find(it_tracker->first);
2570 if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2572 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2575 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2579 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2580 it != vectorOfMissingCameraPoses.end(); ++it) {
2581 it_img = mapOfImages.find(*it);
2582 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2591 "Missing image or missing camera transformation "
2592 "matrix! Cannot init the pose for camera: %s!",
2613 const std::map<std::string, std::string> &mapOfInitPoses)
2617 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2618 std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(
m_referenceCameraName);
2620 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2621 TrackerWrapper *tracker = it_tracker->second;
2622 tracker->
initFromPose(*it_img->second, it_initPose->second);
2630 std::vector<std::string> vectorOfMissingCameraPoses;
2634 it_img = mapOfColorImages.find(it_tracker->first);
2635 it_initPose = mapOfInitPoses.find(it_tracker->first);
2637 if (it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2639 it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2642 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2646 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2647 it != vectorOfMissingCameraPoses.end(); ++it) {
2648 it_img = mapOfColorImages.find(*it);
2649 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2658 "Missing image or missing camera transformation "
2659 "matrix! Cannot init the pose for camera: %s!",
2679 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2680 it->second->initFromPose(I1, c1Mo);
2684 it->second->initFromPose(I2, c2Mo);
2690 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2708 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2709 it->second->initFromPose(I_color1, c1Mo);
2713 it->second->initFromPose(I_color2, c2Mo);
2719 "This method requires 2 cameras but there are %d cameras!",
m_mapOfTrackers.size());
2737 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2741 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2743 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2745 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2746 TrackerWrapper *tracker = it_tracker->second;
2747 tracker->
initFromPose(*it_img->second, it_camPose->second);
2755 std::vector<std::string> vectorOfMissingCameraPoses;
2759 it_img = mapOfImages.find(it_tracker->first);
2760 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2762 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2764 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2767 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2771 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2772 it != vectorOfMissingCameraPoses.end(); ++it) {
2773 it_img = mapOfImages.find(*it);
2774 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2783 "Missing image or missing camera transformation "
2784 "matrix! Cannot set the pose for camera: %s!",
2804 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2808 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
2809 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
2811 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2812 TrackerWrapper *tracker = it_tracker->second;
2813 tracker->
initFromPose(*it_img->second, it_camPose->second);
2821 std::vector<std::string> vectorOfMissingCameraPoses;
2825 it_img = mapOfColorImages.find(it_tracker->first);
2826 it_camPose = mapOfCameraPoses.find(it_tracker->first);
2828 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2830 it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2833 vectorOfMissingCameraPoses.push_back(it_tracker->first);
2837 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2838 it != vectorOfMissingCameraPoses.end(); ++it) {
2839 it_img = mapOfColorImages.find(*it);
2840 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2849 "Missing image or missing camera transformation "
2850 "matrix! Cannot set the pose for camera: %s!",
2870 if (extension ==
".xml") {
2873#ifdef VISP_HAVE_NLOHMANN_JSON
2874 else if (extension ==
".json") {
2896 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
2898 TrackerWrapper *tracker = it->second;
2899 tracker->loadConfigFile(configFile, verbose);
2912#ifdef VISP_HAVE_NLOHMANN_JSON
2924 std::ifstream jsonFile(settingsFile);
2925 if (!jsonFile.good()) {
2930 settings = json::parse(jsonFile);
2932 catch (json::parse_error &e) {
2933 std::stringstream msg;
2934 msg <<
"Could not parse JSON file : \n";
2936 msg << e.what() << std::endl;
2937 msg <<
"Byte position of error: " << e.byte;
2942 if (!settings.contains(
"version")) {
2945 else if (settings[
"version"].get<std::string>() != MBT_JSON_SETTINGS_VERSION) {
2952 trackersJson = settings.at(
"trackers");
2957 bool refCameraFound =
false;
2959 for (
const auto &it : trackersJson.items()) {
2960 const std::string cameraName = it.key();
2961 const json trackerJson = it.value();
2965 if (trackerJson.contains(
"camTref")) {
2972 std::cout <<
"Loading tracker " << cameraName << std::endl <<
" with settings: " << std::endl << trackerJson.dump(2);
2976 std::cout <<
"Updating an already existing tracker with JSON configuration." << std::endl;
2982 std::cout <<
"Creating a new tracker from JSON configuration." << std::endl;
2984 TrackerWrapper *tw =
new TrackerWrapper();
2988 const auto unusedCamIt = std::remove(unusedCameraNames.begin(), unusedCameraNames.end(), cameraName);
2989 unusedCameraNames.erase(unusedCamIt, unusedCameraNames.end());
2991 if (!refCameraFound) {
2996 for (
const std::string &oldCameraName : unusedCameraNames) {
3012 if (settings.contains(
"display")) {
3013 const json displayJson = settings[
"display"];
3017 if (settings.contains(
"visibilityTest")) {
3018 const json visJson = settings[
"visibilityTest"];
3023 if (settings.contains(
"model")) {
3024 loadModel(settings.at(
"model").get<std::string>(), verbose);
3039 j[
"version"] = MBT_JSON_SETTINGS_VERSION;
3043 trackers[kv.first] = *(kv.second);
3046 trackers[kv.first][
"camTref"] = itTransformation->second;
3049 j[
"trackers"] = trackers;
3050 std::ofstream f(settingsFile);
3052 const unsigned indentLevel = 4;
3053 f << j.dump(indentLevel);
3083 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3084 TrackerWrapper *tracker = it_tracker->second;
3085 tracker->loadConfigFile(configFile1, verbose);
3088 tracker = it_tracker->second;
3089 tracker->loadConfigFile(configFile2, verbose);
3116 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3118 TrackerWrapper *tracker = it_tracker->second;
3120 std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
3121 if (it_config != mapOfConfigFiles.end()) {
3122 tracker->loadConfigFile(it_config->second, verbose);
3126 it_tracker->first.c_str());
3133 TrackerWrapper *tracker = it->second;
3167 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3169 TrackerWrapper *tracker = it->second;
3170 tracker->
loadModel(modelFile, verbose, T);
3203 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3204 TrackerWrapper *tracker = it_tracker->second;
3205 tracker->
loadModel(modelFile1, verbose, T1);
3208 tracker = it_tracker->second;
3209 tracker->
loadModel(modelFile2, verbose, T2);
3232 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3234 for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3236 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
3238 if (it_model != mapOfModelFiles.end()) {
3239 TrackerWrapper *tracker = it_tracker->second;
3240 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3242 if (it_T != mapOfT.end())
3243 tracker->
loadModel(it_model->second, verbose, it_T->second);
3245 tracker->
loadModel(it_model->second, verbose);
3249 it_tracker->first.c_str());
3256 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
3258 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3260 TrackerWrapper *tracker = it->second;
3261 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
3267 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
3268 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
3269 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
3271 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3273 TrackerWrapper *tracker = it->second;
3274 tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
3275 mapOfPointCloudHeights[it->first]);
3300 TrackerWrapper *tracker = it_tracker->second;
3301 tracker->reInitModel(I, cad_name, cMo, verbose, T);
3334 TrackerWrapper *tracker = it_tracker->second;
3335 tracker->reInitModel(I_color, cad_name, cMo, verbose, T);
3368 const std::string &cad_name1,
const std::string &cad_name2,
3373 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3375 it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
3379 it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
3384 it_tracker->second->getPose(
m_cMo);
3415 const std::string &cad_name1,
const std::string &cad_name2,
3420 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.begin();
3422 it_tracker->second->reInitModel(I_color1, cad_name1, c1Mo, verbose, T1);
3426 it_tracker->second->reInitModel(I_color2, cad_name2, c2Mo, verbose, T2);
3431 it_tracker->second->getPose(
m_cMo);
3456 const std::map<std::string, std::string> &mapOfModelFiles,
3457 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
bool verbose,
3458 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3461 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3463 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3464 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3466 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3467 it_camPose != mapOfCameraPoses.end()) {
3468 TrackerWrapper *tracker = it_tracker->second;
3469 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3470 if (it_T != mapOfT.end())
3471 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3473 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3482 std::vector<std::string> vectorOfMissingCameras;
3485 it_img = mapOfImages.find(it_tracker->first);
3486 it_model = mapOfModelFiles.find(it_tracker->first);
3487 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3489 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3490 TrackerWrapper *tracker = it_tracker->second;
3491 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3494 vectorOfMissingCameras.push_back(it_tracker->first);
3499 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3501 it_img = mapOfImages.find(*it);
3502 it_model = mapOfModelFiles.find(*it);
3503 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3506 if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3509 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3531 const std::map<std::string, std::string> &mapOfModelFiles,
3532 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
bool verbose,
3533 const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3536 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
3537 std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(
m_referenceCameraName);
3538 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
3540 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3541 it_camPose != mapOfCameraPoses.end()) {
3542 TrackerWrapper *tracker = it_tracker->second;
3543 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3544 if (it_T != mapOfT.end())
3545 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3547 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3556 std::vector<std::string> vectorOfMissingCameras;
3559 it_img = mapOfColorImages.find(it_tracker->first);
3560 it_model = mapOfModelFiles.find(it_tracker->first);
3561 it_camPose = mapOfCameraPoses.find(it_tracker->first);
3563 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3564 it_camPose != mapOfCameraPoses.end()) {
3565 TrackerWrapper *tracker = it_tracker->second;
3566 tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3569 vectorOfMissingCameras.push_back(it_tracker->first);
3574 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3576 it_img = mapOfColorImages.find(*it);
3577 it_model = mapOfModelFiles.find(*it);
3578 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3581 if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3584 m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3601#ifdef VISP_HAVE_OGRE
3628#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
3635 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3637 TrackerWrapper *tracker = it->second;
3638 tracker->resetTracker();
3655 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3657 TrackerWrapper *tracker = it->second;
3677 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3678 it->second->setAngleAppear(a1);
3681 it->second->setAngleAppear(a2);
3708 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3709 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3712 TrackerWrapper *tracker = it_tracker->second;
3735 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3737 TrackerWrapper *tracker = it->second;
3757 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3758 it->second->setAngleDisappear(a1);
3761 it->second->setAngleDisappear(a2);
3788 for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3789 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3792 TrackerWrapper *tracker = it_tracker->second;
3811 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3813 TrackerWrapper *tracker = it->second;
3814 tracker->setCameraParameters(camera);
3829 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3830 it->second->setCameraParameters(camera1);
3833 it->second->setCameraParameters(camera2);
3837 it->second->getCameraParameters(
m_cam);
3859 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
3860 it != mapOfCameraParameters.end(); ++it) {
3861 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3864 TrackerWrapper *tracker = it_tracker->second;
3865 tracker->setCameraParameters(it->second);
3888 it->second = cameraTransformationMatrix;
3903 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
3905 for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
3906 it != mapOfTransformationMatrix.end(); ++it) {
3907 std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
3911 it_camTrans->second = it->second;
3929 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3931 TrackerWrapper *tracker = it->second;
3932 tracker->setClipping(flags);
3949 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
3950 it->second->setClipping(flags1);
3953 it->second->setClipping(flags2);
3964 std::stringstream ss;
3965 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
3979 for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
3980 it != mapOfClippingFlags.end(); ++it) {
3981 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
3984 TrackerWrapper *tracker = it_tracker->second;
3985 tracker->setClipping(it->second);
4005 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4007 TrackerWrapper *tracker = it->second;
4022 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4024 TrackerWrapper *tracker = it->second;
4040 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4042 TrackerWrapper *tracker = it->second;
4058 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4060 TrackerWrapper *tracker = it->second;
4075 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4077 TrackerWrapper *tracker = it->second;
4091 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4093 TrackerWrapper *tracker = it->second;
4108 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4110 TrackerWrapper *tracker = it->second;
4124 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4126 TrackerWrapper *tracker = it->second;
4140 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4142 TrackerWrapper *tracker = it->second;
4156 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4158 TrackerWrapper *tracker = it->second;
4173 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4175 TrackerWrapper *tracker = it->second;
4202 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4204 TrackerWrapper *tracker = it->second;
4220 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4222 TrackerWrapper *tracker = it->second;
4223 tracker->setFarClippingDistance(dist);
4238 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4239 it->second->setFarClippingDistance(dist1);
4242 it->second->setFarClippingDistance(dist2);
4246 distFarClip = it->second->getFarClippingDistance();
4265 for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
4267 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4270 TrackerWrapper *tracker = it_tracker->second;
4271 tracker->setFarClippingDistance(it->second);
4290 std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
4291 if (it_factor != mapOfFeatureFactors.end()) {
4292 it->second = it_factor->second;
4316 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4318 TrackerWrapper *tracker = it->second;
4323#ifdef VISP_HAVE_OGRE
4337 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4339 TrackerWrapper *tracker = it->second;
4357 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4359 TrackerWrapper *tracker = it->second;
4365#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
4375 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4377 TrackerWrapper *tracker = it->second;
4393 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4394 it->second->setKltOpencv(t1);
4397 it->second->setKltOpencv(t2);
4412 for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
4413 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4416 TrackerWrapper *tracker = it_tracker->second;
4433 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4435 TrackerWrapper *tracker = it->second;
4455 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4457 TrackerWrapper *tracker = it->second;
4458 tracker->
setLod(useLod, name);
4462#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
4472 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4474 TrackerWrapper *tracker = it->second;
4490 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4491 it->second->setKltMaskBorder(e1);
4495 it->second->setKltMaskBorder(e2);
4510 for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
4512 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4515 TrackerWrapper *tracker = it_tracker->second;
4531 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4533 TrackerWrapper *tracker = it->second;
4551 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4553 TrackerWrapper *tracker = it->second;
4570 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4572 TrackerWrapper *tracker = it->second;
4586 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4588 TrackerWrapper *tracker = it->second;
4605 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4606 it->second->setMovingEdge(me1);
4610 it->second->setMovingEdge(me2);
4625 for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
4626 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4629 TrackerWrapper *tracker = it_tracker->second;
4646 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4648 TrackerWrapper *tracker = it->second;
4649 tracker->setNearClippingDistance(dist);
4664 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4665 it->second->setNearClippingDistance(dist1);
4669 it->second->setNearClippingDistance(dist2);
4692 for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
4693 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
4696 TrackerWrapper *tracker = it_tracker->second;
4697 tracker->setNearClippingDistance(it->second);
4722 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4724 TrackerWrapper *tracker = it->second;
4743 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4745 TrackerWrapper *tracker = it->second;
4746 tracker->setOgreVisibilityTest(v);
4749#ifdef VISP_HAVE_OGRE
4750 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4752 TrackerWrapper *tracker = it->second;
4769 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4771 TrackerWrapper *tracker = it->second;
4792 "to be configured with only one camera!");
4799 TrackerWrapper *tracker = it->second;
4800 tracker->setPose(I, cdMo);
4824 "to be configured with only one camera!");
4831 TrackerWrapper *tracker = it->second;
4833 tracker->setPose(
m_I, cdMo);
4856 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4857 it->second->setPose(I1, c1Mo);
4861 it->second->setPose(I2, c2Mo);
4866 it->second->getPose(
m_cMo);
4894 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
4895 it->second->setPose(I_color1, c1Mo);
4899 it->second->setPose(I_color2, c2Mo);
4904 it->second->getPose(
m_cMo);
4933 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4937 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
4939 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
4941 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4942 TrackerWrapper *tracker = it_tracker->second;
4943 tracker->setPose(*it_img->second, it_camPose->second);
4951 std::vector<std::string> vectorOfMissingCameraPoses;
4956 it_img = mapOfImages.find(it_tracker->first);
4957 it_camPose = mapOfCameraPoses.find(it_tracker->first);
4959 if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4961 TrackerWrapper *tracker = it_tracker->second;
4962 tracker->setPose(*it_img->second, it_camPose->second);
4965 vectorOfMissingCameraPoses.push_back(it_tracker->first);
4970 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4971 it != vectorOfMissingCameraPoses.end(); ++it) {
4972 it_img = mapOfImages.find(*it);
4973 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4982 "Missing image or missing camera transformation "
4983 "matrix! Cannot set pose for camera: %s!",
5005 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
5009 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(
m_referenceCameraName);
5010 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(
m_referenceCameraName);
5012 if (it_tracker !=
m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
5013 TrackerWrapper *tracker = it_tracker->second;
5014 tracker->setPose(*it_img->second, it_camPose->second);
5022 std::vector<std::string> vectorOfMissingCameraPoses;
5027 it_img = mapOfColorImages.find(it_tracker->first);
5028 it_camPose = mapOfCameraPoses.find(it_tracker->first);
5030 if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
5032 TrackerWrapper *tracker = it_tracker->second;
5033 tracker->setPose(*it_img->second, it_camPose->second);
5036 vectorOfMissingCameraPoses.push_back(it_tracker->first);
5041 for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
5042 it != vectorOfMissingCameraPoses.end(); ++it) {
5043 it_img = mapOfColorImages.find(*it);
5044 std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
5053 "Missing image or missing camera transformation "
5054 "matrix! Cannot set pose for camera: %s!",
5078 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5080 TrackerWrapper *tracker = it->second;
5081 tracker->setProjectionErrorComputation(flag);
5092 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5094 TrackerWrapper *tracker = it->second;
5106 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5108 TrackerWrapper *tracker = it->second;
5117 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5119 TrackerWrapper *tracker = it->second;
5131 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.find(referenceCameraName);
5136 std::cerr <<
"The reference camera: " << referenceCameraName <<
" does not exist!";
5144 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5146 TrackerWrapper *tracker = it->second;
5147 tracker->setScanLineVisibilityTest(v);
5164 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5166 TrackerWrapper *tracker = it->second;
5167 tracker->setTrackerType(type);
5182 for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
5183 std::map<std::string, TrackerWrapper *>::const_iterator it_tracker =
m_mapOfTrackers.find(it->first);
5185 TrackerWrapper *tracker = it_tracker->second;
5186 tracker->setTrackerType(it->second);
5202 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5204 TrackerWrapper *tracker = it->second;
5220 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5222 TrackerWrapper *tracker = it->second;
5238 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5240 TrackerWrapper *tracker = it->second;
5245#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5257 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5259 TrackerWrapper *tracker = it->second;
5268 bool isOneTestTrackingOk =
false;
5269 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5271 TrackerWrapper *tracker = it->second;
5273 tracker->testTracking();
5274 isOneTestTrackingOk =
true;
5280 if (!isOneTestTrackingOk) {
5281 std::ostringstream oss;
5282 oss <<
"Not enough moving edges to track the object. Try to reduce the "
5300 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5303 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5304 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5306 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5320 std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
5323 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5324 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5326 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5342 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5343 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5344 mapOfImages[it->first] = &I1;
5347 mapOfImages[it->first] = &I2;
5349 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5350 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5352 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5355 std::stringstream ss;
5356 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5374 std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5375 std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
5376 mapOfImages[it->first] = &I_color1;
5379 mapOfImages[it->first] = &_colorI2;
5381 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5382 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5384 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5387 std::stringstream ss;
5388 ss <<
"Require two cameras! There are " <<
m_mapOfTrackers.size() <<
" cameras!";
5402 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5403 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5405 track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5417 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5418 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5420 track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5433 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5435 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5437 TrackerWrapper *tracker = it->second;
5440#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5448#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5452 mapOfImages[it->first] == NULL) {
5457 !mapOfPointClouds[it->first]) {
5474 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5476 TrackerWrapper *tracker = it->second;
5482 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5485#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5509 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5511 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5512 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5514 TrackerWrapper *tracker = it->second;
5517#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5525#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5529 mapOfImages[it->first] == NULL) {
5533#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5537 mapOfImages[it->first] != NULL) {
5539 mapOfImages[it->first] = &tracker->
m_I;
5543 !mapOfPointClouds[it->first]) {
5560 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5562 TrackerWrapper *tracker = it->second;
5568 tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5571#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5598 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5599 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5600 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5602 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5604 TrackerWrapper *tracker = it->second;
5607#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5615#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5619 mapOfImages[it->first] == NULL) {
5624 (mapOfPointClouds[it->first] == NULL)) {
5629 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5641 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5643 TrackerWrapper *tracker = it->second;
5649 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5652#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5678 std::map<std::string,
const std::vector<vpColVector> *> &mapOfPointClouds,
5679 std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5680 std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5682 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5683 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5685 TrackerWrapper *tracker = it->second;
5688#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5696#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5700 mapOfColorImages[it->first] == NULL) {
5704#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5708 mapOfColorImages[it->first] != NULL) {
5710 mapOfImages[it->first] = &tracker->
m_I;
5714 (mapOfPointClouds[it->first] == NULL)) {
5719 preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5731 for (std::map<std::string, TrackerWrapper *>::const_iterator it =
m_mapOfTrackers.begin();
5733 TrackerWrapper *tracker = it->second;
5739 tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5742#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5758vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
5759 : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
5764#ifdef VISP_HAVE_OGRE
5771vpMbGenericTracker::TrackerWrapper::TrackerWrapper(
int trackerType)
5772 : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
5775#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5785#ifdef VISP_HAVE_OGRE
5792vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
5804 double normRes_1 = -1;
5805 unsigned int iter = 0;
5807 double factorEdge = 1.0;
5808#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5809 double factorKlt = 1.0;
5811 double factorDepth = 1.0;
5812 double factorDepthDense = 1.0;
5820#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5831 unsigned int nb_edge_features = m_error_edge.
getRows();
5832#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5833 unsigned int nb_klt_features = m_error_klt.getRows();
5835 unsigned int nb_depth_features = m_error_depthNormal.getRows();
5836 unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5841 bool reStartFromLastIncrement =
false;
5844#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5845 if (reStartFromLastIncrement) {
5852 if (!reStartFromLastIncrement) {
5857 if (!isoJoIdentity) {
5860 LVJ_true = (
m_L * cVo *
oJo);
5870 if (isoJoIdentity) {
5874 unsigned int rank = (
m_L * cVo).kernel(K);
5884 isoJoIdentity =
false;
5893 unsigned int start_index = 0;
5895 for (
unsigned int i = 0; i < nb_edge_features; i++) {
5896 double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5903 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5908 start_index += nb_edge_features;
5911#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5913 for (
unsigned int i = 0; i < nb_klt_features; i++) {
5914 double wi = m_w_klt[i] * factorKlt;
5915 W_true[start_index + i] = wi;
5921 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5922 m_L[start_index + i][j] *= wi;
5926 start_index += nb_klt_features;
5931 for (
unsigned int i = 0; i < nb_depth_features; i++) {
5932 double wi = m_w_depthNormal[i] * factorDepth;
5933 m_w[start_index + i] = m_w_depthNormal[i];
5939 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5940 m_L[start_index + i][j] *= wi;
5944 start_index += nb_depth_features;
5948 for (
unsigned int i = 0; i < nb_depth_dense_features; i++) {
5949 double wi = m_w_depthDense[i] * factorDepthDense;
5950 m_w[start_index + i] = m_w_depthDense[i];
5956 for (
unsigned int j = 0; j <
m_L.
getCols(); j++) {
5957 m_L[start_index + i][j] *= wi;
5967#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5975#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
5980 normRes_1 = normRes;
5982 normRes = sqrt(num / den);
5995void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
5998 "TrackerWrapper::computeVVSInit("
5999 ") should not be called!");
6004 initMbtTracking(ptr_I);
6006 unsigned int nbFeatures = 0;
6009 nbFeatures += m_error_edge.getRows();
6012 m_error_edge.clear();
6013 m_weightedError_edge.clear();
6018#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6021 nbFeatures += m_error_klt.getRows();
6024 m_error_klt.clear();
6025 m_weightedError_klt.clear();
6033 nbFeatures += m_error_depthNormal.getRows();
6036 m_error_depthNormal.clear();
6037 m_weightedError_depthNormal.clear();
6038 m_L_depthNormal.clear();
6039 m_w_depthNormal.clear();
6044 nbFeatures += m_error_depthDense.getRows();
6047 m_error_depthDense.clear();
6048 m_weightedError_depthDense.clear();
6049 m_L_depthDense.clear();
6050 m_w_depthDense.clear();
6053 m_L.
resize(nbFeatures, 6,
false,
false);
6061void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu()
6065 "computeVVSInteractionMatrixAndR"
6066 "esidu() should not be called!");
6069void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(
const vpImage<unsigned char> *
const ptr_I)
6075#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6089 unsigned int start_index = 0;
6094 start_index += m_error_edge.getRows();
6097#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6102 start_index += m_error_klt.getRows();
6107 m_L.
insert(m_L_depthNormal, start_index, 0);
6110 start_index += m_error_depthNormal.getRows();
6114 m_L.
insert(m_L_depthDense, start_index, 0);
6121void vpMbGenericTracker::TrackerWrapper::computeVVSWeights()
6123 unsigned int start_index = 0;
6129 start_index += m_w_edge.getRows();
6132#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6137 start_index += m_w_klt.getRows();
6142 if (m_depthNormalUseRobust) {
6144 m_w.
insert(start_index, m_w_depthNormal);
6147 start_index += m_w_depthNormal.getRows();
6152 m_w.
insert(start_index, m_w_depthDense);
6160 unsigned int thickness,
bool displayFullModel)
6164 for (
size_t i = 0; i < features.size(); i++) {
6167 int state =
static_cast<int>(features[i][3]);
6199 double id = features[i][5];
6200 std::stringstream ss;
6205 vpImagePoint im_centroid(features[i][1], features[i][2]);
6206 vpImagePoint im_extremity(features[i][3], features[i][4]);
6213 std::vector<std::vector<double> > models =
6215 for (
size_t i = 0; i < models.size(); i++) {
6223 double n20 = models[i][3];
6224 double n11 = models[i][4];
6225 double n02 = models[i][5];
6230#ifdef VISP_HAVE_OGRE
6232#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6244 unsigned int thickness,
bool displayFullModel)
6248 for (
size_t i = 0; i < features.size(); i++) {
6251 int state =
static_cast<int>(features[i][3]);
6283 double id = features[i][5];
6284 std::stringstream ss;
6289 vpImagePoint im_centroid(features[i][1], features[i][2]);
6290 vpImagePoint im_extremity(features[i][3], features[i][4]);
6297 std::vector<std::vector<double> > models =
6299 for (
size_t i = 0; i < models.size(); i++) {
6307 double n20 = models[i][3];
6308 double n11 = models[i][4];
6309 double n02 = models[i][5];
6314#ifdef VISP_HAVE_OGRE
6316#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6326std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6328 std::vector<std::vector<double> > features;
6332 features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6335#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6338 features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6344 features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(),
6345 m_featuresToBeDisplayedDepthNormal.end());
6351std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(
unsigned int width,
6352 unsigned int height,
6355 bool displayFullModel)
6357 std::vector<std::vector<double> > models;
6363#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6377 std::vector<std::vector<double> > edgeModels =
6379 models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6384 std::vector<std::vector<double> > depthDenseModels =
6386 models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6402 bool reInitialisation =
false;
6407#ifdef VISP_HAVE_OGRE
6430#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6451void vpMbGenericTracker::TrackerWrapper::initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
6452 double radius,
int idFace,
const std::string &name)
6457#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6463void vpMbGenericTracker::TrackerWrapper::initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
int idFace,
6464 const std::string &name)
6469#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6475void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(
vpMbtPolygon &polygon)
6480#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6492void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(
vpMbtPolygon &polygon)
6497#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6517void vpMbGenericTracker::TrackerWrapper::loadConfigFile(
const std::string &configFile,
bool verbose)
6523 xmlp.setVerbose(verbose);
6524 xmlp.setCameraParameters(
m_cam);
6532 xmlp.setKltMaxFeatures(10000);
6533 xmlp.setKltWindowSize(5);
6534 xmlp.setKltQuality(0.01);
6535 xmlp.setKltMinDistance(5);
6536 xmlp.setKltHarrisParam(0.01);
6537 xmlp.setKltBlockSize(3);
6538 xmlp.setKltPyramidLevels(3);
6539#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6540 xmlp.setKltMaskBorder(maskBorder);
6544 xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
6545 xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
6546 xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
6547 xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
6548 xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
6549 xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
6552 xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
6553 xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
6557 std::cout <<
" *********** Parsing XML for";
6560 std::vector<std::string> tracker_names;
6562 tracker_names.push_back(
"Edge");
6563#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6565 tracker_names.push_back(
"Klt");
6568 tracker_names.push_back(
"Depth Normal");
6570 tracker_names.push_back(
"Depth Dense");
6573 for (
size_t i = 0; i < tracker_names.size(); i++) {
6574 std::cout <<
" " << tracker_names[i];
6575 if (i == tracker_names.size() - 1) {
6580 std::cout <<
"Model-Based Tracker ************ " << std::endl;
6583 xmlp.parse(configFile);
6590 xmlp.getCameraParameters(camera);
6596 if (xmlp.hasNearClippingDistance())
6599 if (xmlp.hasFarClippingDistance())
6602 if (xmlp.getFovClipping()) {
6620 xmlp.getEdgeMe(meParser);
6624#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6625 tracker.setMaxFeatures((
int)xmlp.getKltMaxFeatures());
6626 tracker.setWindowSize((
int)xmlp.getKltWindowSize());
6627 tracker.setQuality(xmlp.getKltQuality());
6628 tracker.setMinDistance(xmlp.getKltMinDistance());
6629 tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
6630 tracker.setBlockSize((
int)xmlp.getKltBlockSize());
6631 tracker.setPyramidLevels((
int)xmlp.getKltPyramidLevels());
6632 maskBorder = xmlp.getKltMaskBorder();
6651 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6653#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6664 bool newvisibleface =
false;
6696 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6703 std::cerr <<
"Error in moving edge tracking" << std::endl;
6708#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6714 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6725 std::cerr <<
"Error in Depth normal tracking" << std::endl;
6735 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6743 const unsigned int pointcloud_width,
6744 const unsigned int pointcloud_height)
6746#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6757 bool newvisibleface =
false;
6789 const std::vector<vpColVector> *
const point_cloud,
6790 const unsigned int pointcloud_width,
6791 const unsigned int pointcloud_height)
6798 std::cerr <<
"Error in moving edge tracking" << std::endl;
6803#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6809 std::cerr <<
"Error in KLT tracking: " << e.
what() << std::endl;
6820 std::cerr <<
"Error in Depth tracking" << std::endl;
6830 std::cerr <<
"Error in Depth dense tracking" << std::endl;
6848 for (
unsigned int i = 0; i < scales.size(); i++) {
6850 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
6857 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
6865 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
6873 cylinders[i].clear();
6881 nbvisiblepolygone = 0;
6884#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6886 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
6888 if (kltpoly != NULL) {
6893 kltPolygons.clear();
6895 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
6898 if (kltPolyCylinder != NULL) {
6899 delete kltPolyCylinder;
6901 kltPolyCylinder = NULL;
6903 kltCylinders.clear();
6906 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
6913 circles_disp.clear();
6915 firstInitialisation =
true;
6920 for (
size_t i = 0; i < m_depthNormalFaces.size(); i++) {
6921 delete m_depthNormalFaces[i];
6922 m_depthNormalFaces[i] = NULL;
6924 m_depthNormalFaces.clear();
6927 for (
size_t i = 0; i < m_depthDenseFaces.size(); i++) {
6928 delete m_depthDenseFaces[i];
6929 m_depthDenseFaces[i] = NULL;
6931 m_depthDenseFaces.clear();
6944void vpMbGenericTracker::TrackerWrapper::reInitModel(
const vpImage<unsigned char> &I,
const std::string &cad_name,
6951void vpMbGenericTracker::TrackerWrapper::reInitModel(
const vpImage<vpRGBa> &I_color,
const std::string &cad_name,
6955 reInitModel(NULL, &I_color, cad_name, cMo, verbose, T);
6958void vpMbGenericTracker::TrackerWrapper::resetTracker()
6961#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6968void vpMbGenericTracker::TrackerWrapper::setCameraParameters(
const vpCameraParameters &cam)
6973#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
6982void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(
const double &dist)
6987void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(
const double &dist)
6992void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(
const bool &v)
6995#ifdef VISP_HAVE_OGRE
7003 bool performKltSetPose =
false;
7008#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
7010 performKltSetPose =
true;
7020 if (!performKltSetPose) {
7037 initPyramid(I, Ipyramid);
7039 unsigned int i = (
unsigned int)scales.size();
7049 cleanPyramid(Ipyramid);
7071 setPose(NULL, &I_color, cdMo);
7074void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(
const bool &flag)
7079void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(
const bool &v)
7082#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
7089void vpMbGenericTracker::TrackerWrapper::setTrackerType(
int type)
7092#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
7099 m_trackerType = type;
7102void vpMbGenericTracker::TrackerWrapper::testTracking()
7110#
if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
7116#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
7120 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
7124#if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
7129void vpMbGenericTracker::TrackerWrapper::track(
const vpImage<vpRGBa> &)
7136 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
7139#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
7143 std::cerr <<
"Bad tracker type: " << m_trackerType << std::endl;
7148#
if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
7176 postTracking(ptr_I, point_cloud);
7180 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 size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
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 functionalities.
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 emitted 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
vp_deprecated void init()
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
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)
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
void computeVisibility(unsigned int width, unsigned int height)
virtual void setDepthDenseFilteringMethod(int method)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void computeVVSInit()
virtual void resetTracker()
void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
virtual void computeVVSWeights()
virtual void setScanLineVisibilityTest(const bool &v)
vpColVector m_error_depthDense
(s - s*)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setCameraParameters(const vpCameraParameters &camera)
void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
virtual void setDepthDenseFilteringMinDistance(double minDistance)
vpColVector m_w_depthDense
Robust weights.
virtual void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void resetTracker()
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
virtual void computeVVSInit()
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual std::vector< std::vector< double > > getFeaturesForDisplayDepthNormal()
vpColVector m_w_depthNormal
Robust weights.
vpColVector m_error_depthNormal
(s - s*)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
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 setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
void computeVisibility(unsigned int width, unsigned int height)
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
std::vector< std::vector< double > > m_featuresToBeDisplayedDepthNormal
Display features.
void updateMovingEdgeWeights()
vpColVector m_w_edge
Robust weights.
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()
vpColVector m_error_edge
(s - s*)
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
virtual void setClipping(const unsigned int &flags)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual unsigned int getNbPoints(unsigned int level=0) const
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)
void setGoodMovingEdgesRatioThreshold(double threshold)
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)
virtual void getMovingEdge(vpMe &p_me) const
void setMovingEdge(const vpMe &me)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
vpColVector m_factor
Edge VVS variables.
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 setDepthDenseFilteringMaxDistance(double maxDistance)
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual vpHomogeneousMatrix getPose() const
virtual void resetTracker()
virtual std::vector< cv::Point2f > getKltPoints() const
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
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)
friend void from_json(const nlohmann::json &j, TrackerWrapper &t)
Load configuration settings from a JSON object for a tracker wrapper.
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
vpMbGenericTracker()
json namespace shortcut
virtual void setNearClippingDistance(const double &dist)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void loadConfigFileJSON(const std::string &configFile, bool verbose=true)
virtual void getCameraParameters(vpCameraParameters &camera) const
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 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 void initFromPoints(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2)
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 void initClick(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2, bool displayHelp=false, const vpHomogeneousMatrix &T1=vpHomogeneousMatrix(), const vpHomogeneousMatrix &T2=vpHomogeneousMatrix())
virtual 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 double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
virtual void setClipping(const unsigned int &flags)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
virtual void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setOgreVisibilityTest(const bool &v)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
std::string m_referenceCameraName
Name of the reference camera.
virtual void setMask(const vpImage< bool > &mask)
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void track(const vpImage< unsigned char > &I)
virtual unsigned int getClipping() const
virtual std::map< int, vpImagePoint > getKltImagePointsWithId() const
Implementation of the polygons management for the model-based trackers.
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpAROgre * getOgreContext()
void displayOgre(const vpHomogeneousMatrix &cMo)
vpMbScanLine & getMbScanLineRenderer()
void setOgreShowConfigDialog(bool showConfigDialog)
virtual std::list< vpMbtDistanceCircle * > & getFeaturesCircle()
vpColVector m_error_klt
(s - s*)
vpHomogeneousMatrix c0Mo
Initial pose.
virtual void setScanLineVisibilityTest(const bool &v)
vpHomogeneousMatrix ctTc0
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 setKltOpencv(const vpKltOpencv &t)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
std::vector< cv::Point2f > getKltPoints() const
unsigned int getKltMaskBorder() const
std::map< int, vpImagePoint > getKltImagePointsWithId() const
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
void preTracking(const vpImage< unsigned char > &I)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
vpKltOpencv getKltOpencv() const
void setKltThresholdAcceptation(double th)
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void reinit(const vpImage< unsigned char > &I)
int getKltNbPoints() const
std::vector< vpImagePoint > getKltImagePoints() const
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
vpColVector m_w_klt
Robust weights.
void setCameraParameters(const vpCameraParameters &cam)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void computeVVSInit()
void setKltMaskBorder(const unsigned int &e)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual std::list< vpMbtDistanceKltPoints * > & getFeaturesKlt()
virtual double getNearClippingDistance() const
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
double m_lambda
Gain of the virtual visual servoing stage.
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
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 double getAngleAppear() const
virtual void setMask(const vpImage< bool > &mask)
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
virtual void getCameraParameters(vpCameraParameters &cam) const
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
virtual void setDisplayFeatures(bool displayF)
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
virtual vpHomogeneousMatrix getPose() const
bool useLodGeneral
True if LOD mode is enabled.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
bool m_computeInteraction
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
vpMatrix covarianceMatrix
Covariance matrix.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
vpHomogeneousMatrix m_cMo
The current pose.
vpCameraParameters m_cam
The camera parameters.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
bool useOgre
Use Ogre3d for visibility tests.
virtual double getAngleDisappear() const
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setAngleDisappear(const double &a)
virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setOgreVisibilityTest(const bool &v)
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void setLod(bool useLod, const std::string &name="")
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
bool displayFeatures
If true, the features are displayed.
virtual void setProjectionErrorDisplay(bool display)
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
virtual double getProjectionError() const
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
bool useScanLine
Use Scanline for visibility tests.
virtual void setProjectionErrorComputation(const bool &flag)
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void setAngleAppear(const double &a)
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
double distNearClip
Distance for near clipping.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
bool ogreShowConfigDialog
unsigned int clippingFlag
Flags specifying which clipping to used.
virtual double getFarClippingDistance() const
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
virtual unsigned int getClipping() const
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
Manage a cylinder used in the model-based tracker.
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
Manage the line of a polygon used in the model-based tracker.
Implementation of a polygon of the model used by the model-based tracker.
Parse an Xml file to extract configuration parameters of a mbtConfig object.
@ TOO_NEAR
Point removed because too near image borders.
@ THRESHOLD
Point removed due to a threshold problem.
@ CONTRAST
Point removed due to a contrast problem.
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
@ NO_SUPPRESSION
Point used by the tracker.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ initializationError
Tracker initialization error.
@ fatalError
Tracker fatal error.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)