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