Visual Servoing Platform  version 3.3.1 under development (2020-08-12)
vpMbGenericTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Generic model-based tracker.
33  *
34  *****************************************************************************/
35 
36 #include <visp3/mbt/vpMbGenericTracker.h>
37 
38 #include <visp3/core/vpDisplay.h>
39 #include <visp3/core/vpExponentialMap.h>
40 #include <visp3/core/vpTrackingException.h>
41 #include <visp3/mbt/vpMbtXmlGenericParser.h>
42 
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 {
47  m_mapOfTrackers["Camera"] = new TrackerWrapper(EDGE_TRACKER);
48 
49  // Add default camera transformation matrix
51 
52  // Add default ponderation between each feature type
54 
55 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
57 #endif
58 
61 }
62 
63 vpMbGenericTracker::vpMbGenericTracker(unsigned int nbCameras, int trackerType)
66 {
67  if (nbCameras == 0) {
68  throw vpException(vpTrackingException::fatalError, "Cannot use no camera!");
69  } else if (nbCameras == 1) {
70  m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerType);
71 
72  // Add default camera transformation matrix
74  } else {
75  for (unsigned int i = 1; i <= nbCameras; i++) {
76  std::stringstream ss;
77  ss << "Camera" << i;
78  m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerType);
79 
80  // Add default camera transformation matrix
82  }
83 
84  // Set by default the reference camera to the first one
85  m_referenceCameraName = m_mapOfTrackers.begin()->first;
86  }
87 
88  // Add default ponderation between each feature type
90 
91 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
93 #endif
94 
97 }
98 
99 vpMbGenericTracker::vpMbGenericTracker(const std::vector<int> &trackerTypes)
102 {
103  if (trackerTypes.empty()) {
104  throw vpException(vpException::badValue, "There is no camera!");
105  }
106 
107  if (trackerTypes.size() == 1) {
108  m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerTypes[0]);
109 
110  // Add default camera transformation matrix
112  } else {
113  for (size_t i = 1; i <= trackerTypes.size(); i++) {
114  std::stringstream ss;
115  ss << "Camera" << i;
116  m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerTypes[i - 1]);
117 
118  // Add default camera transformation matrix
120  }
121 
122  // Set by default the reference camera to the first one
123  m_referenceCameraName = m_mapOfTrackers.begin()->first;
124  }
125 
126  // Add default ponderation between each feature type
128 
129 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
131 #endif
132 
135 }
136 
137 vpMbGenericTracker::vpMbGenericTracker(const std::vector<std::string> &cameraNames,
138  const std::vector<int> &trackerTypes)
141 {
142  if (cameraNames.size() != trackerTypes.size() || cameraNames.empty()) {
144  "cameraNames.size() != trackerTypes.size() || cameraNames.empty()");
145  }
146 
147  for (size_t i = 0; i < cameraNames.size(); i++) {
148  m_mapOfTrackers[cameraNames[i]] = new TrackerWrapper(trackerTypes[i]);
149 
150  // Add default camera transformation matrix
152  }
153 
154  // Set by default the reference camera to the first one
155  m_referenceCameraName = m_mapOfTrackers.begin()->first;
156 
157  // Add default ponderation between each feature type
159 
160 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
162 #endif
163 
166 }
167 
169 {
170  for (std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end();
171  ++it) {
172  delete it->second;
173  it->second = NULL;
174  }
175 }
176 
195  const vpCameraParameters &cam)
196 {
197  double rawTotalProjectionError = 0.0;
198  unsigned int nbTotalFeaturesUsed = 0;
199 
200  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
201  it != m_mapOfTrackers.end(); ++it) {
202  TrackerWrapper *tracker = it->second;
203 
204  unsigned int nbFeaturesUsed = 0;
205  double curProjError = tracker->computeProjectionErrorImpl(I, cMo, cam, nbFeaturesUsed);
206 
207  if (nbFeaturesUsed > 0) {
208  nbTotalFeaturesUsed += nbFeaturesUsed;
209  rawTotalProjectionError += curProjError;
210  }
211  }
212 
213  if (nbTotalFeaturesUsed > 0) {
214  return vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
215  }
216 
217  return 90.0;
218 }
219 
238  const vpCameraParameters &_cam)
239 {
241  vpImageConvert::convert(I_color, I); // FS: Shoudn't we use here m_I that was converted in track() ?
242 
243  return computeCurrentProjectionError(I, _cMo, _cam);
244 }
245 
247 {
248  if (computeProjError) {
249  double rawTotalProjectionError = 0.0;
250  unsigned int nbTotalFeaturesUsed = 0;
251 
252  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
253  it != m_mapOfTrackers.end(); ++it) {
254  TrackerWrapper *tracker = it->second;
255 
256  double curProjError = tracker->getProjectionError();
257  unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
258 
259  if (nbFeaturesUsed > 0) {
260  nbTotalFeaturesUsed += nbFeaturesUsed;
261  rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed);
262  }
263  }
264 
265  if (nbTotalFeaturesUsed > 0) {
266  projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
267  } else {
268  projectionError = 90.0;
269  }
270  } else {
271  projectionError = 90.0;
272  }
273 }
274 
275 void vpMbGenericTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
276 {
277  computeVVSInit(mapOfImages);
278 
279  if (m_error.getRows() < 4) {
280  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
281  }
282 
283  double normRes = 0;
284  double normRes_1 = -1;
285  unsigned int iter = 0;
286 
287  vpMatrix LTL;
288  vpColVector LTR, v;
289  vpColVector error_prev;
290 
291  double mu = m_initialMu;
292  vpHomogeneousMatrix cMo_prev;
293 
294  bool isoJoIdentity_ = true;
295 
296  // Covariance
297  vpColVector W_true(m_error.getRows());
298  vpMatrix L_true, LVJ_true;
299 
300  // Create the map of VelocityTwistMatrices
301  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
302  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
303  it != m_mapOfCameraTransformationMatrix.end(); ++it) {
305  cVo.buildFrom(it->second);
306  mapOfVelocityTwist[it->first] = cVo;
307  }
308 
309  double factorEdge = m_mapOfFeatureFactors[EDGE_TRACKER];
310 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
311  double factorKlt = m_mapOfFeatureFactors[KLT_TRACKER];
312 #endif
313  double factorDepth = m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER];
314  double factorDepthDense = m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER];
315 
316  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
317  computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
318 
319  bool reStartFromLastIncrement = false;
320  computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
321  if (reStartFromLastIncrement) {
322  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
323  it != m_mapOfTrackers.end(); ++it) {
324  TrackerWrapper *tracker = it->second;
325 
326  tracker->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo_prev;
327 
328 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
329  vpHomogeneousMatrix c_curr_tTc_curr0 =
330  m_mapOfCameraTransformationMatrix[it->first] * cMo_prev * tracker->c0Mo.inverse();
331  tracker->ctTc0 = c_curr_tTc_curr0;
332 #endif
333  }
334  }
335 
336  if (!reStartFromLastIncrement) {
338 
339  if (computeCovariance) {
340  L_true = m_L;
341  if (!isoJoIdentity_) {
343  cVo.buildFrom(m_cMo);
344  LVJ_true = (m_L * (cVo * oJo));
345  }
346  }
347 
349  if (iter == 0) {
350  isoJoIdentity_ = true;
351  oJo.eye();
352 
353  // If all the 6 dof should be estimated, we check if the interaction
354  // matrix is full rank. If not we remove automatically the dof that
355  // cannot be estimated This is particularly useful when consering
356  // circles (rank 5) and cylinders (rank 4)
357  if (isoJoIdentity_) {
358  cVo.buildFrom(m_cMo);
359 
360  vpMatrix K; // kernel
361  unsigned int rank = (m_L * cVo).kernel(K);
362  if (rank == 0) {
363  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
364  }
365 
366  if (rank != 6) {
367  vpMatrix I; // Identity
368  I.eye(6);
369  oJo = I - K.AtA();
370 
371  isoJoIdentity_ = false;
372  }
373  }
374  }
375 
376  // Weighting
377  double num = 0;
378  double den = 0;
379 
380  unsigned int start_index = 0;
381  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
382  it != m_mapOfTrackers.end(); ++it) {
383  TrackerWrapper *tracker = it->second;
384 
385  if (tracker->m_trackerType & EDGE_TRACKER) {
386  for (unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
387  double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
388  W_true[start_index + i] = wi;
389  m_weightedError[start_index + i] = wi * m_error[start_index + i];
390 
391  num += wi * vpMath::sqr(m_error[start_index + i]);
392  den += wi;
393 
394  for (unsigned int j = 0; j < m_L.getCols(); j++) {
395  m_L[start_index + i][j] *= wi;
396  }
397  }
398 
399  start_index += tracker->m_error_edge.getRows();
400  }
401 
402 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
403  if (tracker->m_trackerType & KLT_TRACKER) {
404  for (unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
405  double wi = tracker->m_w_klt[i] * factorKlt;
406  W_true[start_index + i] = wi;
407  m_weightedError[start_index + i] = wi * m_error[start_index + i];
408 
409  num += wi * vpMath::sqr(m_error[start_index + i]);
410  den += wi;
411 
412  for (unsigned int j = 0; j < m_L.getCols(); j++) {
413  m_L[start_index + i][j] *= wi;
414  }
415  }
416 
417  start_index += tracker->m_error_klt.getRows();
418  }
419 #endif
420 
421  if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
422  for (unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
423  double wi = tracker->m_w_depthNormal[i] * factorDepth;
424  W_true[start_index + i] = wi;
425  m_weightedError[start_index + i] = wi * m_error[start_index + i];
426 
427  num += wi * vpMath::sqr(m_error[start_index + i]);
428  den += wi;
429 
430  for (unsigned int j = 0; j < m_L.getCols(); j++) {
431  m_L[start_index + i][j] *= wi;
432  }
433  }
434 
435  start_index += tracker->m_error_depthNormal.getRows();
436  }
437 
438  if (tracker->m_trackerType & DEPTH_DENSE_TRACKER) {
439  for (unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
440  double wi = tracker->m_w_depthDense[i] * factorDepthDense;
441  W_true[start_index + i] = wi;
442  m_weightedError[start_index + i] = wi * m_error[start_index + i];
443 
444  num += wi * vpMath::sqr(m_error[start_index + i]);
445  den += wi;
446 
447  for (unsigned int j = 0; j < m_L.getCols(); j++) {
448  m_L[start_index + i][j] *= wi;
449  }
450  }
451 
452  start_index += tracker->m_error_depthDense.getRows();
453  }
454  }
455 
456  normRes_1 = normRes;
457  normRes = sqrt(num / den);
458 
459  computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
460 
461  cMo_prev = m_cMo;
462 
464 
465 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
466  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
467  it != m_mapOfTrackers.end(); ++it) {
468  TrackerWrapper *tracker = it->second;
469 
470  vpHomogeneousMatrix c_curr_tTc_curr0 =
471  m_mapOfCameraTransformationMatrix[it->first] * m_cMo * tracker->c0Mo.inverse();
472  tracker->ctTc0 = c_curr_tTc_curr0;
473  }
474 #endif
475 
476  // Update cMo
477  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
478  it != m_mapOfTrackers.end(); ++it) {
479  TrackerWrapper *tracker = it->second;
480  tracker->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;
481  }
482  }
483 
484  iter++;
485  }
486 
487  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
488 
489  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
490  it != m_mapOfTrackers.end(); ++it) {
491  TrackerWrapper *tracker = it->second;
492 
493  if (tracker->m_trackerType & EDGE_TRACKER) {
494  tracker->updateMovingEdgeWeights();
495  }
496  }
497 }
498 
500 {
501  throw vpException(vpException::fatalError, "vpMbGenericTracker::computeVVSInit() should not be called!");
502 }
503 
504 void vpMbGenericTracker::computeVVSInit(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
505 {
506  unsigned int nbFeatures = 0;
507 
508  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
509  it != m_mapOfTrackers.end(); ++it) {
510  TrackerWrapper *tracker = it->second;
511  tracker->computeVVSInit(mapOfImages[it->first]);
512 
513  nbFeatures += tracker->m_error.getRows();
514  }
515 
516  m_L.resize(nbFeatures, 6, false, false);
517  m_error.resize(nbFeatures, false);
518 
519  m_weightedError.resize(nbFeatures, false);
520  m_w.resize(nbFeatures, false);
521  m_w = 1;
522 }
523 
525 {
526  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
527  "computeVVSInteractionMatrixAndR"
528  "esidu() should not be called");
529 }
530 
532  std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
533  std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
534 {
535  unsigned int start_index = 0;
536 
537  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
538  it != m_mapOfTrackers.end(); ++it) {
539  TrackerWrapper *tracker = it->second;
540 
541  tracker->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;
542 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
543  vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * m_cMo * tracker->c0Mo.inverse();
544  tracker->ctTc0 = c_curr_tTc_curr0;
545 #endif
546 
547  tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
548 
549  m_L.insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
550  m_error.insert(start_index, tracker->m_error);
551 
552  start_index += tracker->m_error.getRows();
553  }
554 }
555 
557 {
558  unsigned int start_index = 0;
559 
560  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
561  it != m_mapOfTrackers.end(); ++it) {
562  TrackerWrapper *tracker = it->second;
563  tracker->computeVVSWeights();
564 
565  m_w.insert(start_index, tracker->m_w);
566  start_index += tracker->m_w.getRows();
567  }
568 }
569 
584  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
585  bool displayFullModel)
586 {
587  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
588  if (it != m_mapOfTrackers.end()) {
589  TrackerWrapper *tracker = it->second;
590  tracker->display(I, cMo, cam, col, thickness, displayFullModel);
591  } else {
592  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
593  }
594 }
595 
610  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
611  bool displayFullModel)
612 {
613  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
614  if (it != m_mapOfTrackers.end()) {
615  TrackerWrapper *tracker = it->second;
616  tracker->display(I, cMo, cam, col, thickness, displayFullModel);
617  } else {
618  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
619  }
620 }
621 
639  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
640  const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &color,
641  unsigned int thickness, bool displayFullModel)
642 {
643  if (m_mapOfTrackers.size() == 2) {
644  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
645  it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
646  ++it;
647 
648  it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
649  } else {
650  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
651  << std::endl;
652  }
653 }
654 
672  const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1,
673  const vpCameraParameters &cam2, const vpColor &color, unsigned int thickness,
674  bool displayFullModel)
675 {
676  if (m_mapOfTrackers.size() == 2) {
677  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
678  it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
679  ++it;
680 
681  it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
682  } else {
683  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
684  << std::endl;
685  }
686 }
687 
699 void vpMbGenericTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
700  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
701  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
702  const vpColor &col, unsigned int thickness, bool displayFullModel)
703 {
704  // Display only for the given images
705  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
706  it_img != mapOfImages.end(); ++it_img) {
707  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
708  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
709  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
710 
711  if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
712  it_cam != mapOfCameraParameters.end()) {
713  TrackerWrapper *tracker = it_tracker->second;
714  tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
715  } else {
716  std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
717  }
718  }
719 }
720 
732 void vpMbGenericTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
733  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
734  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
735  const vpColor &col, unsigned int thickness, bool displayFullModel)
736 {
737  // Display only for the given images
738  for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
739  it_img != mapOfImages.end(); ++it_img) {
740  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
741  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
742  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
743 
744  if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
745  it_cam != mapOfCameraParameters.end()) {
746  TrackerWrapper *tracker = it_tracker->second;
747  tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
748  } else {
749  std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
750  }
751  }
752 }
753 
759 std::vector<std::string> vpMbGenericTracker::getCameraNames() const
760 {
761  std::vector<std::string> cameraNames;
762 
763  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
764  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
765  cameraNames.push_back(it_tracker->first);
766  }
767 
768  return cameraNames;
769 }
770 
772 {
774 }
775 
785 {
786  if (m_mapOfTrackers.size() == 2) {
787  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
788  it->second->getCameraParameters(cam1);
789  ++it;
790 
791  it->second->getCameraParameters(cam2);
792  } else {
793  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
794  << std::endl;
795  }
796 }
797 
803 void vpMbGenericTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
804 {
805  // Clear the input map
806  mapOfCameraParameters.clear();
807 
808  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
809  it != m_mapOfTrackers.end(); ++it) {
810  vpCameraParameters cam_;
811  it->second->getCameraParameters(cam_);
812  mapOfCameraParameters[it->first] = cam_;
813  }
814 }
815 
822 std::map<std::string, int> vpMbGenericTracker::getCameraTrackerTypes() const
823 {
824  std::map<std::string, int> trackingTypes;
825 
826  TrackerWrapper *traker;
827  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
828  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
829  traker = it_tracker->second;
830  trackingTypes[it_tracker->first] = traker->getTrackerType();
831  }
832 
833  return trackingTypes;
834 }
835 
844 void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const
845 {
846  if (m_mapOfTrackers.size() == 2) {
847  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
848  clippingFlag1 = it->second->getClipping();
849  ++it;
850 
851  clippingFlag2 = it->second->getClipping();
852  } else {
853  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
854  << std::endl;
855  }
856 }
857 
863 void vpMbGenericTracker::getClipping(std::map<std::string, unsigned int> &mapOfClippingFlags) const
864 {
865  mapOfClippingFlags.clear();
866 
867  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
868  it != m_mapOfTrackers.end(); ++it) {
869  TrackerWrapper *tracker = it->second;
870  mapOfClippingFlags[it->first] = tracker->getClipping();
871  }
872 }
873 
880 {
881  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
882  if (it != m_mapOfTrackers.end()) {
883  return it->second->getFaces();
884  }
885 
886  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found!" << std::endl;
887  return faces;
888 }
889 
896 {
897  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
898  if (it != m_mapOfTrackers.end()) {
899  return it->second->getFaces();
900  }
901 
902  std::cerr << "The camera: " << cameraName << " cannot be found!" << std::endl;
903  return faces;
904 }
905 
906 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
907 
910 std::list<vpMbtDistanceCircle *> &vpMbGenericTracker::getFeaturesCircle()
911 {
912  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
913  if (it != m_mapOfTrackers.end()) {
914  TrackerWrapper *tracker = it->second;
915  return tracker->getFeaturesCircle();
916  } else {
917  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
918  m_referenceCameraName.c_str());
919  }
920 }
921 
925 std::list<vpMbtDistanceKltCylinder *> &vpMbGenericTracker::getFeaturesKltCylinder()
926 {
927  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
928  if (it != m_mapOfTrackers.end()) {
929  TrackerWrapper *tracker = it->second;
930  return tracker->getFeaturesKltCylinder();
931  } else {
932  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
933  m_referenceCameraName.c_str());
934  }
935 }
936 
940 std::list<vpMbtDistanceKltPoints *> &vpMbGenericTracker::getFeaturesKlt()
941 {
942  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
943  if (it != m_mapOfTrackers.end()) {
944  TrackerWrapper *tracker = it->second;
945  return tracker->getFeaturesKlt();
946  } else {
947  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
948  m_referenceCameraName.c_str());
949  }
950 }
951 #endif
952 
978 std::vector<std::vector<double> > vpMbGenericTracker::getFeaturesForDisplay()
979 {
980  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
981 
982  if (it != m_mapOfTrackers.end()) {
983  return it->second->getFeaturesForDisplay();
984  } else {
985  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
986  }
987 
988  return std::vector<std::vector<double> >();
989 }
990 
1014 void vpMbGenericTracker::getFeaturesForDisplay(std::map<std::string, std::vector<std::vector<double> > > &mapOfFeatures)
1015 {
1016  // Clear the input map
1017  mapOfFeatures.clear();
1018 
1019  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1020  it != m_mapOfTrackers.end(); ++it) {
1021  mapOfFeatures[it->first] = it->second->getFeaturesForDisplay();
1022  }
1023 }
1024 
1035 
1036 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
1037 
1045 std::vector<vpImagePoint> vpMbGenericTracker::getKltImagePoints() const
1046 {
1047  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1048  if (it != m_mapOfTrackers.end()) {
1049  TrackerWrapper *tracker = it->second;
1050  return tracker->getKltImagePoints();
1051  } else {
1052  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1053  }
1054 
1055  return std::vector<vpImagePoint>();
1056 }
1057 
1066 std::map<int, vpImagePoint> vpMbGenericTracker::getKltImagePointsWithId() const
1067 {
1068  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1069  if (it != m_mapOfTrackers.end()) {
1070  TrackerWrapper *tracker = it->second;
1071  return tracker->getKltImagePointsWithId();
1072  } else {
1073  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1074  }
1075 
1076  return std::map<int, vpImagePoint>();
1077 }
1078 
1085 {
1086  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1087  if (it != m_mapOfTrackers.end()) {
1088  TrackerWrapper *tracker = it->second;
1089  return tracker->getKltMaskBorder();
1090  } else {
1091  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1092  }
1093 
1094  return 0;
1095 }
1096 
1103 {
1104  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1105  if (it != m_mapOfTrackers.end()) {
1106  TrackerWrapper *tracker = it->second;
1107  return tracker->getKltNbPoints();
1108  } else {
1109  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1110  }
1111 
1112  return 0;
1113 }
1114 
1121 {
1122  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
1123 
1124  if (it_tracker != m_mapOfTrackers.end()) {
1125  TrackerWrapper *tracker;
1126  tracker = it_tracker->second;
1127  return tracker->getKltOpencv();
1128  } else {
1129  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1130  }
1131 
1132  return vpKltOpencv();
1133 }
1134 
1144 {
1145  if (m_mapOfTrackers.size() == 2) {
1146  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1147  klt1 = it->second->getKltOpencv();
1148  ++it;
1149 
1150  klt2 = it->second->getKltOpencv();
1151  } else {
1152  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
1153  << std::endl;
1154  }
1155 }
1156 
1162 void vpMbGenericTracker::getKltOpencv(std::map<std::string, vpKltOpencv> &mapOfKlts) const
1163 {
1164  mapOfKlts.clear();
1165 
1166  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1167  it != m_mapOfTrackers.end(); ++it) {
1168  TrackerWrapper *tracker = it->second;
1169  mapOfKlts[it->first] = tracker->getKltOpencv();
1170  }
1171 }
1172 
1173 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
1174 
1179 std::vector<cv::Point2f> vpMbGenericTracker::getKltPoints() const
1180 {
1181  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1182  if (it != m_mapOfTrackers.end()) {
1183  TrackerWrapper *tracker = it->second;
1184  return tracker->getKltPoints();
1185  } else {
1186  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1187  }
1188 
1189  return std::vector<cv::Point2f>();
1190 }
1191 #endif
1192 
1200 #endif
1201 
1214 void vpMbGenericTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList,
1215  unsigned int level) const
1216 {
1217  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1218  if (it != m_mapOfTrackers.end()) {
1219  it->second->getLcircle(circlesList, level);
1220  } else {
1221  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1222  }
1223 }
1224 
1238 void vpMbGenericTracker::getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
1239  unsigned int level) const
1240 {
1241  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1242  if (it != m_mapOfTrackers.end()) {
1243  it->second->getLcircle(circlesList, level);
1244  } else {
1245  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1246  }
1247 }
1248 
1261 void vpMbGenericTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList,
1262  unsigned int level) const
1263 {
1264  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1265  if (it != m_mapOfTrackers.end()) {
1266  it->second->getLcylinder(cylindersList, level);
1267  } else {
1268  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1269  }
1270 }
1271 
1285 void vpMbGenericTracker::getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
1286  unsigned int level) const
1287 {
1288  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1289  if (it != m_mapOfTrackers.end()) {
1290  it->second->getLcylinder(cylindersList, level);
1291  } else {
1292  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1293  }
1294 }
1295 
1308 void vpMbGenericTracker::getLline(std::list<vpMbtDistanceLine *> &linesList,
1309  unsigned int level) const
1310 {
1311  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1312 
1313  if (it != m_mapOfTrackers.end()) {
1314  it->second->getLline(linesList, level);
1315  } else {
1316  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1317  }
1318 }
1319 
1333 void vpMbGenericTracker::getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
1334  unsigned int level) const
1335 {
1336  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1337  if (it != m_mapOfTrackers.end()) {
1338  it->second->getLline(linesList, level);
1339  } else {
1340  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1341  }
1342 }
1343 
1370 std::vector<std::vector<double> > vpMbGenericTracker::getModelForDisplay(unsigned int width, unsigned int height,
1371  const vpHomogeneousMatrix &cMo,
1372  const vpCameraParameters &cam,
1373  bool displayFullModel)
1374 {
1375  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1376 
1377  if (it != m_mapOfTrackers.end()) {
1378  return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1379  } else {
1380  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1381  }
1382 
1383  return std::vector<std::vector<double> >();
1384 }
1385 
1413 void vpMbGenericTracker::getModelForDisplay(std::map<std::string, std::vector<std::vector<double> > > &mapOfModels,
1414  const std::map<std::string, unsigned int> &mapOfwidths,
1415  const std::map<std::string, unsigned int> &mapOfheights,
1416  const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
1417  const std::map<std::string, vpCameraParameters> &mapOfCams,
1418  bool displayFullModel)
1419 {
1420  // Clear the input map
1421  mapOfModels.clear();
1422 
1423  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1424  it != m_mapOfTrackers.end(); ++it) {
1425  std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
1426  std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
1427  std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
1428  std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
1429 
1430  if (findWidth != mapOfwidths.end() &&
1431  findHeight != mapOfheights.end() &&
1432  findcMo != mapOfcMos.end() &&
1433  findCam != mapOfCams.end()) {
1434  mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second,
1435  findcMo->second, findCam->second, displayFullModel);
1436  }
1437  }
1438 }
1439 
1446 {
1447  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1448 
1449  if (it != m_mapOfTrackers.end()) {
1450  return it->second->getMovingEdge();
1451  } else {
1452  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1453  }
1454 
1455  return vpMe();
1456 }
1457 
1467 {
1468  if (m_mapOfTrackers.size() == 2) {
1469  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1470  it->second->getMovingEdge(me1);
1471  ++it;
1472 
1473  it->second->getMovingEdge(me2);
1474  } else {
1475  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
1476  << std::endl;
1477  }
1478 }
1479 
1485 void vpMbGenericTracker::getMovingEdge(std::map<std::string, vpMe> &mapOfMovingEdges) const
1486 {
1487  mapOfMovingEdges.clear();
1488 
1489  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1490  it != m_mapOfTrackers.end(); ++it) {
1491  TrackerWrapper *tracker = it->second;
1492  mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1493  }
1494 }
1495 
1511 unsigned int vpMbGenericTracker::getNbPoints(unsigned int level) const
1512 {
1513  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1514 
1515  unsigned int nbGoodPoints = 0;
1516  if (it != m_mapOfTrackers.end()) {
1517 
1518  nbGoodPoints = it->second->getNbPoints(level);
1519  } else {
1520  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1521  }
1522 
1523  return nbGoodPoints;
1524 }
1525 
1540 void vpMbGenericTracker::getNbPoints(std::map<std::string, unsigned int> &mapOfNbPoints, unsigned int level) const
1541 {
1542  mapOfNbPoints.clear();
1543 
1544  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1545  it != m_mapOfTrackers.end(); ++it) {
1546  TrackerWrapper *tracker = it->second;
1547  mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1548  }
1549 }
1550 
1557 {
1558  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1559  if (it != m_mapOfTrackers.end()) {
1560  return it->second->getNbPolygon();
1561  }
1562 
1563  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1564  return 0;
1565 }
1566 
1572 void vpMbGenericTracker::getNbPolygon(std::map<std::string, unsigned int> &mapOfNbPolygons) const
1573 {
1574  mapOfNbPolygons.clear();
1575 
1576  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1577  it != m_mapOfTrackers.end(); ++it) {
1578  TrackerWrapper *tracker = it->second;
1579  mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1580  }
1581 }
1582 
1594 {
1595  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1596  if (it != m_mapOfTrackers.end()) {
1597  return it->second->getPolygon(index);
1598  }
1599 
1600  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1601  return NULL;
1602 }
1603 
1615 vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, unsigned int index)
1616 {
1617  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1618  if (it != m_mapOfTrackers.end()) {
1619  return it->second->getPolygon(index);
1620  }
1621 
1622  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1623  return NULL;
1624 }
1625 
1641 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1642 vpMbGenericTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
1643 {
1644  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1645 
1646  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1647  if (it != m_mapOfTrackers.end()) {
1648  TrackerWrapper *tracker = it->second;
1649  polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1650  } else {
1651  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1652  }
1653 
1654  return polygonFaces;
1655 }
1656 
1674 void vpMbGenericTracker::getPolygonFaces(std::map<std::string, std::vector<vpPolygon> > &mapOfPolygons,
1675  std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1676  bool orderPolygons, bool useVisibility, bool clipPolygon)
1677 {
1678  mapOfPolygons.clear();
1679  mapOfPoints.clear();
1680 
1681  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1682  it != m_mapOfTrackers.end(); ++it) {
1683  TrackerWrapper *tracker = it->second;
1684  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1685  tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1686 
1687  mapOfPolygons[it->first] = polygonFaces.first;
1688  mapOfPoints[it->first] = polygonFaces.second;
1689  }
1690 }
1691 
1693 {
1694  vpMbTracker::getPose(cMo);
1695 }
1696 
1706 {
1707  if (m_mapOfTrackers.size() == 2) {
1708  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1709  it->second->getPose(c1Mo);
1710  ++it;
1711 
1712  it->second->getPose(c2Mo);
1713  } else {
1714  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
1715  << std::endl;
1716  }
1717 }
1718 
1724 void vpMbGenericTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
1725 {
1726  // Clear the map
1727  mapOfCameraPoses.clear();
1728 
1729  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1730  it != m_mapOfTrackers.end(); ++it) {
1731  TrackerWrapper *tracker = it->second;
1732  tracker->getPose(mapOfCameraPoses[it->first]);
1733  }
1734 }
1735 
1740 {
1741  return m_referenceCameraName;
1742 }
1743 
1748 {
1749  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1750  if (it != m_mapOfTrackers.end()) {
1751  TrackerWrapper *tracker = it->second;
1752  return tracker->getTrackerType();
1753  } else {
1754  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
1755  m_referenceCameraName.c_str());
1756  }
1757 }
1758 
1760 {
1761  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1762  it != m_mapOfTrackers.end(); ++it) {
1763  TrackerWrapper *tracker = it->second;
1764  tracker->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;
1765  tracker->init(I);
1766  }
1767 }
1768 
1769 void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
1770  double /*radius*/, int /*idFace*/, const std::string & /*name*/)
1771 {
1772  throw vpException(vpException::fatalError, "vpMbGenericTracker::initCircle() should not be called!");
1773 }
1774 
1775 #ifdef VISP_HAVE_MODULE_GUI
1776 
1820  const std::string &initFile1, const std::string &initFile2, bool displayHelp,
1821  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
1822 {
1823  if (m_mapOfTrackers.size() == 2) {
1824  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1825  TrackerWrapper *tracker = it->second;
1826  tracker->initClick(I1, initFile1, displayHelp, T1);
1827 
1828  ++it;
1829 
1830  tracker = it->second;
1831  tracker->initClick(I2, initFile2, displayHelp, T2);
1832 
1834  if (it != m_mapOfTrackers.end()) {
1835  tracker = it->second;
1836 
1837  // Set the reference cMo
1838  tracker->getPose(m_cMo);
1839  }
1840  } else {
1842  "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size());
1843  }
1844 }
1845 
1888 void vpMbGenericTracker::initClick(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
1889  const std::string &initFile1, const std::string &initFile2, bool displayHelp,
1890  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
1891 {
1892  if (m_mapOfTrackers.size() == 2) {
1893  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1894  TrackerWrapper *tracker = it->second;
1895  tracker->initClick(I_color1, initFile1, displayHelp, T1);
1896 
1897  ++it;
1898 
1899  tracker = it->second;
1900  tracker->initClick(I_color2, initFile2, displayHelp, T2);
1901 
1903  if (it != m_mapOfTrackers.end()) {
1904  tracker = it->second;
1905 
1906  // Set the reference cMo
1907  tracker->getPose(m_cMo);
1908  }
1909  } else {
1911  "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size());
1912  }
1913 }
1914 
1957 void vpMbGenericTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1958  const std::map<std::string, std::string> &mapOfInitFiles, bool displayHelp,
1959  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
1960 {
1961  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
1962  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1963  mapOfImages.find(m_referenceCameraName);
1964  std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
1965 
1966  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1967  TrackerWrapper *tracker = it_tracker->second;
1968  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
1969  if (it_T != mapOfT.end())
1970  tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
1971  else
1972  tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1973  tracker->getPose(m_cMo);
1974  } else {
1975  throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera!");
1976  }
1977 
1978  // Vector of missing initFile for cameras
1979  std::vector<std::string> vectorOfMissingCameraPoses;
1980 
1981  // Set pose for the specified cameras
1982  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
1983  if (it_tracker->first != m_referenceCameraName) {
1984  it_img = mapOfImages.find(it_tracker->first);
1985  it_initFile = mapOfInitFiles.find(it_tracker->first);
1986 
1987  if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1988  // InitClick for the current camera
1989  TrackerWrapper *tracker = it_tracker->second;
1990  tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1991  } else {
1992  vectorOfMissingCameraPoses.push_back(it_tracker->first);
1993  }
1994  }
1995  }
1996 
1997  // Init for cameras that do not have an initFile
1998  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
1999  it != vectorOfMissingCameraPoses.end(); ++it) {
2000  it_img = mapOfImages.find(*it);
2001  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2003 
2004  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2005  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2006  m_mapOfTrackers[*it]->m_cMo = cCurrentMo;
2007  m_mapOfTrackers[*it]->init(*it_img->second);
2008  } else {
2010  "Missing image or missing camera transformation "
2011  "matrix! Cannot set the pose for camera: %s!",
2012  it->c_str());
2013  }
2014  }
2015 }
2016 
2059 void vpMbGenericTracker::initClick(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
2060  const std::map<std::string, std::string> &mapOfInitFiles, bool displayHelp,
2061  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2062 {
2063  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2064  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2065  mapOfColorImages.find(m_referenceCameraName);
2066  std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
2067 
2068  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2069  TrackerWrapper *tracker = it_tracker->second;
2070  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2071  if (it_T != mapOfT.end())
2072  tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
2073  else
2074  tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2075  tracker->getPose(m_cMo);
2076  } else {
2077  throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera!");
2078  }
2079 
2080  // Vector of missing initFile for cameras
2081  std::vector<std::string> vectorOfMissingCameraPoses;
2082 
2083  // Set pose for the specified cameras
2084  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2085  if (it_tracker->first != m_referenceCameraName) {
2086  it_img = mapOfColorImages.find(it_tracker->first);
2087  it_initFile = mapOfInitFiles.find(it_tracker->first);
2088 
2089  if (it_img != mapOfColorImages.end() && it_initFile != mapOfInitFiles.end()) {
2090  // InitClick for the current camera
2091  TrackerWrapper *tracker = it_tracker->second;
2092  tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
2093  } else {
2094  vectorOfMissingCameraPoses.push_back(it_tracker->first);
2095  }
2096  }
2097  }
2098 
2099  // Init for cameras that do not have an initFile
2100  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2101  it != vectorOfMissingCameraPoses.end(); ++it) {
2102  it_img = mapOfColorImages.find(*it);
2103  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2105 
2106  if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2107  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2108  m_mapOfTrackers[*it]->m_cMo = cCurrentMo;
2109  vpImageConvert::convert(*it_img->second, m_mapOfTrackers[*it]->m_I);
2110  m_mapOfTrackers[*it]->init(m_mapOfTrackers[*it]->m_I);
2111  } else {
2113  "Missing image or missing camera transformation "
2114  "matrix! Cannot set the pose for camera: %s!",
2115  it->c_str());
2116  }
2117  }
2118 }
2119 #endif
2120 
2121 void vpMbGenericTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
2122  const int /*idFace*/, const std::string & /*name*/)
2123 {
2124  throw vpException(vpException::fatalError, "vpMbGenericTracker::initCylinder() should not be called!");
2125 }
2126 
2128 {
2129  throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromCorners() should not be called!");
2130 }
2131 
2133 {
2134  throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromLines() should not be called!");
2135 }
2136 
2167  const std::string &initFile1, const std::string &initFile2)
2168 {
2169  if (m_mapOfTrackers.size() == 2) {
2170  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2171  TrackerWrapper *tracker = it->second;
2172  tracker->initFromPoints(I1, initFile1);
2173 
2174  ++it;
2175 
2176  tracker = it->second;
2177  tracker->initFromPoints(I2, initFile2);
2178 
2180  if (it != m_mapOfTrackers.end()) {
2181  tracker = it->second;
2182 
2183  // Set the reference cMo
2184  tracker->getPose(m_cMo);
2185 
2186  // Set the reference camera parameters
2187  tracker->getCameraParameters(m_cam);
2188  }
2189  } else {
2191  "Cannot initFromPoints()! Require two cameras but "
2192  "there are %d cameras!",
2193  m_mapOfTrackers.size());
2194  }
2195 }
2196 
2227  const std::string &initFile1, const std::string &initFile2)
2228 {
2229  if (m_mapOfTrackers.size() == 2) {
2230  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2231  TrackerWrapper *tracker = it->second;
2232  tracker->initFromPoints(I_color1, initFile1);
2233 
2234  ++it;
2235 
2236  tracker = it->second;
2237  tracker->initFromPoints(I_color2, initFile2);
2238 
2240  if (it != m_mapOfTrackers.end()) {
2241  tracker = it->second;
2242 
2243  // Set the reference cMo
2244  tracker->getPose(m_cMo);
2245 
2246  // Set the reference camera parameters
2247  tracker->getCameraParameters(m_cam);
2248  }
2249  } else {
2251  "Cannot initFromPoints()! Require two cameras but "
2252  "there are %d cameras!",
2253  m_mapOfTrackers.size());
2254  }
2255 }
2256 
2257 void vpMbGenericTracker::initFromPoints(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2258  const std::map<std::string, std::string> &mapOfInitPoints)
2259 {
2260  // Set the reference cMo
2261  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2262  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2263  mapOfImages.find(m_referenceCameraName);
2264  std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(m_referenceCameraName);
2265 
2266  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2267  TrackerWrapper *tracker = it_tracker->second;
2268  tracker->initFromPoints(*it_img->second, it_initPoints->second);
2269  tracker->getPose(m_cMo);
2270  } else {
2271  throw vpException(vpTrackingException::initializationError, "Cannot initFromPoints() for the reference camera!");
2272  }
2273 
2274  // Vector of missing initPoints for cameras
2275  std::vector<std::string> vectorOfMissingCameraPoints;
2276 
2277  // Set pose for the specified cameras
2278  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2279  it_img = mapOfImages.find(it_tracker->first);
2280  it_initPoints = mapOfInitPoints.find(it_tracker->first);
2281 
2282  if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
2283  // Set pose
2284  it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2285  } else {
2286  vectorOfMissingCameraPoints.push_back(it_tracker->first);
2287  }
2288  }
2289 
2290  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2291  it != vectorOfMissingCameraPoints.end(); ++it) {
2292  it_img = mapOfImages.find(*it);
2293  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2295 
2296  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2297  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2298  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2299  } else {
2301  "Missing image or missing camera transformation "
2302  "matrix! Cannot init the pose for camera: %s!",
2303  it->c_str());
2304  }
2305  }
2306 }
2307 
2308 void vpMbGenericTracker::initFromPoints(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
2309  const std::map<std::string, std::string> &mapOfInitPoints)
2310 {
2311  // Set the reference cMo
2312  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2313  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2314  mapOfColorImages.find(m_referenceCameraName);
2315  std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(m_referenceCameraName);
2316 
2317  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2318  TrackerWrapper *tracker = it_tracker->second;
2319  tracker->initFromPoints(*it_img->second, it_initPoints->second);
2320  tracker->getPose(m_cMo);
2321  } else {
2322  throw vpException(vpTrackingException::initializationError, "Cannot initFromPoints() for the reference camera!");
2323  }
2324 
2325  // Vector of missing initPoints for cameras
2326  std::vector<std::string> vectorOfMissingCameraPoints;
2327 
2328  // Set pose for the specified cameras
2329  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2330  it_img = mapOfColorImages.find(it_tracker->first);
2331  it_initPoints = mapOfInitPoints.find(it_tracker->first);
2332 
2333  if (it_img != mapOfColorImages.end() && it_initPoints != mapOfInitPoints.end()) {
2334  // Set pose
2335  it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
2336  } else {
2337  vectorOfMissingCameraPoints.push_back(it_tracker->first);
2338  }
2339  }
2340 
2341  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
2342  it != vectorOfMissingCameraPoints.end(); ++it) {
2343  it_img = mapOfColorImages.find(*it);
2344  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2346 
2347  if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2348  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2349  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2350  } else {
2352  "Missing image or missing camera transformation "
2353  "matrix! Cannot init the pose for camera: %s!",
2354  it->c_str());
2355  }
2356  }
2357 }
2358 
2360 {
2361  vpMbTracker::initFromPose(I, cMo);
2362 }
2363 
2376  const std::string &initFile1, const std::string &initFile2)
2377 {
2378  if (m_mapOfTrackers.size() == 2) {
2379  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2380  TrackerWrapper *tracker = it->second;
2381  tracker->initFromPose(I1, initFile1);
2382 
2383  ++it;
2384 
2385  tracker = it->second;
2386  tracker->initFromPose(I2, initFile2);
2387 
2389  if (it != m_mapOfTrackers.end()) {
2390  tracker = it->second;
2391 
2392  // Set the reference cMo
2393  tracker->getPose(m_cMo);
2394 
2395  // Set the reference camera parameters
2396  tracker->getCameraParameters(m_cam);
2397  }
2398  } else {
2400  "Cannot initFromPose()! Require two cameras but there "
2401  "are %d cameras!",
2402  m_mapOfTrackers.size());
2403  }
2404 }
2405 
2418  const std::string &initFile1, const std::string &initFile2)
2419 {
2420  if (m_mapOfTrackers.size() == 2) {
2421  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2422  TrackerWrapper *tracker = it->second;
2423  tracker->initFromPose(I_color1, initFile1);
2424 
2425  ++it;
2426 
2427  tracker = it->second;
2428  tracker->initFromPose(I_color2, initFile2);
2429 
2431  if (it != m_mapOfTrackers.end()) {
2432  tracker = it->second;
2433 
2434  // Set the reference cMo
2435  tracker->getPose(m_cMo);
2436 
2437  // Set the reference camera parameters
2438  tracker->getCameraParameters(m_cam);
2439  }
2440  } else {
2442  "Cannot initFromPose()! Require two cameras but there "
2443  "are %d cameras!",
2444  m_mapOfTrackers.size());
2445  }
2446 }
2447 
2462 void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2463  const std::map<std::string, std::string> &mapOfInitPoses)
2464 {
2465  // Set the reference cMo
2466  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2467  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2468  mapOfImages.find(m_referenceCameraName);
2469  std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(m_referenceCameraName);
2470 
2471  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2472  TrackerWrapper *tracker = it_tracker->second;
2473  tracker->initFromPose(*it_img->second, it_initPose->second);
2474  tracker->getPose(m_cMo);
2475  } else {
2476  throw vpException(vpTrackingException::initializationError, "Cannot initFromPose() for the reference camera!");
2477  }
2478 
2479  // Vector of missing pose matrices for cameras
2480  std::vector<std::string> vectorOfMissingCameraPoses;
2481 
2482  // Set pose for the specified cameras
2483  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2484  it_img = mapOfImages.find(it_tracker->first);
2485  it_initPose = mapOfInitPoses.find(it_tracker->first);
2486 
2487  if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
2488  // Set pose
2489  it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2490  } else {
2491  vectorOfMissingCameraPoses.push_back(it_tracker->first);
2492  }
2493  }
2494 
2495  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2496  it != vectorOfMissingCameraPoses.end(); ++it) {
2497  it_img = mapOfImages.find(*it);
2498  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2500 
2501  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2502  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2503  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2504  } else {
2506  "Missing image or missing camera transformation "
2507  "matrix! Cannot init the pose for camera: %s!",
2508  it->c_str());
2509  }
2510  }
2511 }
2512 
2527 void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
2528  const std::map<std::string, std::string> &mapOfInitPoses)
2529 {
2530  // Set the reference cMo
2531  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2532  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2533  mapOfColorImages.find(m_referenceCameraName);
2534  std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(m_referenceCameraName);
2535 
2536  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2537  TrackerWrapper *tracker = it_tracker->second;
2538  tracker->initFromPose(*it_img->second, it_initPose->second);
2539  tracker->getPose(m_cMo);
2540  } else {
2541  throw vpException(vpTrackingException::initializationError, "Cannot initFromPose() for the reference camera!");
2542  }
2543 
2544  // Vector of missing pose matrices for cameras
2545  std::vector<std::string> vectorOfMissingCameraPoses;
2546 
2547  // Set pose for the specified cameras
2548  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2549  it_img = mapOfColorImages.find(it_tracker->first);
2550  it_initPose = mapOfInitPoses.find(it_tracker->first);
2551 
2552  if (it_img != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
2553  // Set pose
2554  it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
2555  } else {
2556  vectorOfMissingCameraPoses.push_back(it_tracker->first);
2557  }
2558  }
2559 
2560  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2561  it != vectorOfMissingCameraPoses.end(); ++it) {
2562  it_img = mapOfColorImages.find(*it);
2563  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2565 
2566  if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2567  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2568  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2569  } else {
2571  "Missing image or missing camera transformation "
2572  "matrix! Cannot init the pose for camera: %s!",
2573  it->c_str());
2574  }
2575  }
2576 }
2577 
2589  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
2590 {
2591  if (m_mapOfTrackers.size() == 2) {
2592  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2593  it->second->initFromPose(I1, c1Mo);
2594 
2595  ++it;
2596 
2597  it->second->initFromPose(I2, c2Mo);
2598 
2599  m_cMo = c1Mo;
2600  } else {
2602  "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size());
2603  }
2604 }
2605 
2617  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
2618 {
2619  if (m_mapOfTrackers.size() == 2) {
2620  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2621  it->second->initFromPose(I_color1, c1Mo);
2622 
2623  ++it;
2624 
2625  it->second->initFromPose(I_color2, c2Mo);
2626 
2627  m_cMo = c1Mo;
2628  } else {
2630  "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size());
2631  }
2632 }
2633 
2647 void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2648  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2649 {
2650  // Set the reference cMo
2651  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2652  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2653  mapOfImages.find(m_referenceCameraName);
2654  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
2655 
2656  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2657  TrackerWrapper *tracker = it_tracker->second;
2658  tracker->initFromPose(*it_img->second, it_camPose->second);
2659  tracker->getPose(m_cMo);
2660  } else {
2661  throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera!");
2662  }
2663 
2664  // Vector of missing pose matrices for cameras
2665  std::vector<std::string> vectorOfMissingCameraPoses;
2666 
2667  // Set pose for the specified cameras
2668  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2669  it_img = mapOfImages.find(it_tracker->first);
2670  it_camPose = mapOfCameraPoses.find(it_tracker->first);
2671 
2672  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2673  // Set pose
2674  it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2675  } else {
2676  vectorOfMissingCameraPoses.push_back(it_tracker->first);
2677  }
2678  }
2679 
2680  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2681  it != vectorOfMissingCameraPoses.end(); ++it) {
2682  it_img = mapOfImages.find(*it);
2683  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2685 
2686  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2687  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2688  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2689  } else {
2691  "Missing image or missing camera transformation "
2692  "matrix! Cannot set the pose for camera: %s!",
2693  it->c_str());
2694  }
2695  }
2696 }
2697 
2711 void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
2712  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2713 {
2714  // Set the reference cMo
2715  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2716  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
2717  mapOfColorImages.find(m_referenceCameraName);
2718  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
2719 
2720  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2721  TrackerWrapper *tracker = it_tracker->second;
2722  tracker->initFromPose(*it_img->second, it_camPose->second);
2723  tracker->getPose(m_cMo);
2724  } else {
2725  throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera!");
2726  }
2727 
2728  // Vector of missing pose matrices for cameras
2729  std::vector<std::string> vectorOfMissingCameraPoses;
2730 
2731  // Set pose for the specified cameras
2732  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2733  it_img = mapOfColorImages.find(it_tracker->first);
2734  it_camPose = mapOfCameraPoses.find(it_tracker->first);
2735 
2736  if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
2737  // Set pose
2738  it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2739  } else {
2740  vectorOfMissingCameraPoses.push_back(it_tracker->first);
2741  }
2742  }
2743 
2744  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2745  it != vectorOfMissingCameraPoses.end(); ++it) {
2746  it_img = mapOfColorImages.find(*it);
2747  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2749 
2750  if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2751  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
2752  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2753  } else {
2755  "Missing image or missing camera transformation "
2756  "matrix! Cannot set the pose for camera: %s!",
2757  it->c_str());
2758  }
2759  }
2760 }
2761 
2772 void vpMbGenericTracker::loadConfigFile(const std::string &configFile)
2773 {
2774  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2775  it != m_mapOfTrackers.end(); ++it) {
2776  TrackerWrapper *tracker = it->second;
2777  tracker->loadConfigFile(configFile);
2778  }
2779 
2781  throw vpException(vpException::fatalError, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str());
2782  }
2783 
2784  m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(m_cam);
2785  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
2786  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
2787  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
2788 }
2789 
2803 void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2)
2804 {
2805  if (m_mapOfTrackers.size() != 2) {
2806  throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
2807  }
2808 
2809  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2810  TrackerWrapper *tracker = it_tracker->second;
2811  tracker->loadConfigFile(configFile1);
2812 
2813  ++it_tracker;
2814  tracker = it_tracker->second;
2815  tracker->loadConfigFile(configFile2);
2816 
2818  throw vpException(vpException::fatalError, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str());
2819  }
2820 
2821  m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(m_cam);
2822  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
2823  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
2824  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
2825 }
2826 
2839 void vpMbGenericTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
2840 {
2841  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2842  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2843  TrackerWrapper *tracker = it_tracker->second;
2844 
2845  std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
2846  if (it_config != mapOfConfigFiles.end()) {
2847  tracker->loadConfigFile(it_config->second);
2848  } else {
2849  throw vpException(vpTrackingException::initializationError, "Missing configuration file for camera: %s!",
2850  it_tracker->first.c_str());
2851  }
2852  }
2853 
2854  // Set the reference camera parameters
2855  std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.find(m_referenceCameraName);
2856  if (it != m_mapOfTrackers.end()) {
2857  TrackerWrapper *tracker = it->second;
2858  tracker->getCameraParameters(m_cam);
2859 
2860  // Set clipping
2861  this->clippingFlag = tracker->getClipping();
2862  this->angleAppears = tracker->getAngleAppear();
2863  this->angleDisappears = tracker->getAngleDisappear();
2864  } else {
2865  throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
2866  m_referenceCameraName.c_str());
2867  }
2868 }
2869 
2900 void vpMbGenericTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &T)
2901 {
2902  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2903  it != m_mapOfTrackers.end(); ++it) {
2904  TrackerWrapper *tracker = it->second;
2905  tracker->loadModel(modelFile, verbose, T);
2906  }
2907 }
2908 
2943 void vpMbGenericTracker::loadModel(const std::string &modelFile1, const std::string &modelFile2, bool verbose,
2944  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
2945 {
2946  if (m_mapOfTrackers.size() != 2) {
2947  throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
2948  }
2949 
2950  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2951  TrackerWrapper *tracker = it_tracker->second;
2952  tracker->loadModel(modelFile1, verbose, T1);
2953 
2954  ++it_tracker;
2955  tracker = it_tracker->second;
2956  tracker->loadModel(modelFile2, verbose, T2);
2957 }
2958 
2990 void vpMbGenericTracker::loadModel(const std::map<std::string, std::string> &mapOfModelFiles, bool verbose,
2991  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2992 {
2993  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2994  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2995  std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
2996 
2997  if (it_model != mapOfModelFiles.end()) {
2998  TrackerWrapper *tracker = it_tracker->second;
2999  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3000 
3001  if (it_T != mapOfT.end())
3002  tracker->loadModel(it_model->second, verbose, it_T->second);
3003  else
3004  tracker->loadModel(it_model->second, verbose);
3005  } else {
3006  throw vpException(vpTrackingException::initializationError, "Cannot load model for camera: %s",
3007  it_tracker->first.c_str());
3008  }
3009  }
3010 }
3011 
3012 #ifdef VISP_HAVE_PCL
3013 void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
3014  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
3015 {
3016  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3017  it != m_mapOfTrackers.end(); ++it) {
3018  TrackerWrapper *tracker = it->second;
3019  tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
3020  }
3021 }
3022 #endif
3023 
3024 void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
3025  std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
3026  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
3027  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
3028 {
3029  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3030  it != m_mapOfTrackers.end(); ++it) {
3031  TrackerWrapper *tracker = it->second;
3032  tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
3033  mapOfPointCloudHeights[it->first]);
3034  }
3035 }
3036 
3048 void vpMbGenericTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
3049  const vpHomogeneousMatrix &cMo, bool verbose,
3050  const vpHomogeneousMatrix &T)
3051 {
3052  if (m_mapOfTrackers.size() != 1) {
3053  throw vpException(vpTrackingException::fatalError, "This method requires exactly one camera, there are %d cameras!",
3054  m_mapOfTrackers.size());
3055  }
3056 
3057  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3058  if (it_tracker != m_mapOfTrackers.end()) {
3059  TrackerWrapper *tracker = it_tracker->second;
3060  tracker->reInitModel(I, cad_name, cMo, verbose, T);
3061 
3062  // Set reference pose
3063  tracker->getPose(m_cMo);
3064  } else {
3065  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!");
3066  }
3067 
3068  modelInitialised = true;
3069 }
3070 
3082 void vpMbGenericTracker::reInitModel(const vpImage<vpRGBa> &I_color, const std::string &cad_name,
3083  const vpHomogeneousMatrix &cMo, bool verbose,
3084  const vpHomogeneousMatrix &T)
3085 {
3086  if (m_mapOfTrackers.size() != 1) {
3087  throw vpException(vpTrackingException::fatalError, "This method requires exactly one camera, there are %d cameras!",
3088  m_mapOfTrackers.size());
3089  }
3090 
3091  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3092  if (it_tracker != m_mapOfTrackers.end()) {
3093  TrackerWrapper *tracker = it_tracker->second;
3094  tracker->reInitModel(I_color, cad_name, cMo, verbose, T);
3095 
3096  // Set reference pose
3097  tracker->getPose(m_cMo);
3098  } else {
3099  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!");
3100  }
3101 
3102  modelInitialised = true;
3103 }
3104 
3126  const std::string &cad_name1, const std::string &cad_name2,
3127  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
3128  bool verbose,
3129  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
3130 {
3131  if (m_mapOfTrackers.size() == 2) {
3132  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
3133 
3134  it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
3135 
3136  ++it_tracker;
3137 
3138  it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
3139 
3140  it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3141  if (it_tracker != m_mapOfTrackers.end()) {
3142  // Set reference pose
3143  it_tracker->second->getPose(m_cMo);
3144  }
3145  } else {
3146  throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!");
3147  }
3148 
3149  modelInitialised = true;
3150 }
3151 
3173  const std::string &cad_name1, const std::string &cad_name2,
3174  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
3175  bool verbose,
3176  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
3177 {
3178  if (m_mapOfTrackers.size() == 2) {
3179  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
3180 
3181  it_tracker->second->reInitModel(I_color1, cad_name1, c1Mo, verbose, T1);
3182 
3183  ++it_tracker;
3184 
3185  it_tracker->second->reInitModel(I_color2, cad_name2, c2Mo, verbose, T2);
3186 
3187  it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3188  if (it_tracker != m_mapOfTrackers.end()) {
3189  // Set reference pose
3190  it_tracker->second->getPose(m_cMo);
3191  }
3192  } else {
3193  throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!");
3194  }
3195 
3196  modelInitialised = true;
3197 }
3198 
3213 void vpMbGenericTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
3214  const std::map<std::string, std::string> &mapOfModelFiles,
3215  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
3216  bool verbose,
3217  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3218 {
3219  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3220  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3221  mapOfImages.find(m_referenceCameraName);
3222  std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(m_referenceCameraName);
3223  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
3224 
3225  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3226  it_camPose != mapOfCameraPoses.end()) {
3227  TrackerWrapper *tracker = it_tracker->second;
3228  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3229  if (it_T != mapOfT.end())
3230  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3231  else
3232  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3233 
3234  // Set reference pose
3235  tracker->getPose(m_cMo);
3236  } else {
3237  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() for reference camera!");
3238  }
3239 
3240  std::vector<std::string> vectorOfMissingCameras;
3241  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
3242  if (it_tracker->first != m_referenceCameraName) {
3243  it_img = mapOfImages.find(it_tracker->first);
3244  it_model = mapOfModelFiles.find(it_tracker->first);
3245  it_camPose = mapOfCameraPoses.find(it_tracker->first);
3246 
3247  if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3248  TrackerWrapper *tracker = it_tracker->second;
3249  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3250  } else {
3251  vectorOfMissingCameras.push_back(it_tracker->first);
3252  }
3253  }
3254  }
3255 
3256  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3257  ++it) {
3258  it_img = mapOfImages.find(*it);
3259  it_model = mapOfModelFiles.find(*it);
3260  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3262 
3263  if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
3264  it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
3265  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
3266  m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3267  }
3268  }
3269 
3270  modelInitialised = true;
3271 }
3272 
3287 void vpMbGenericTracker::reInitModel(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
3288  const std::map<std::string, std::string> &mapOfModelFiles,
3289  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
3290  bool verbose,
3291  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
3292 {
3293  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3294  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
3295  mapOfColorImages.find(m_referenceCameraName);
3296  std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(m_referenceCameraName);
3297  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
3298 
3299  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3300  it_camPose != mapOfCameraPoses.end()) {
3301  TrackerWrapper *tracker = it_tracker->second;
3302  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
3303  if (it_T != mapOfT.end())
3304  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
3305  else
3306  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3307 
3308  // Set reference pose
3309  tracker->getPose(m_cMo);
3310  } else {
3311  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() for reference camera!");
3312  }
3313 
3314  std::vector<std::string> vectorOfMissingCameras;
3315  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
3316  if (it_tracker->first != m_referenceCameraName) {
3317  it_img = mapOfColorImages.find(it_tracker->first);
3318  it_model = mapOfModelFiles.find(it_tracker->first);
3319  it_camPose = mapOfCameraPoses.find(it_tracker->first);
3320 
3321  if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
3322  TrackerWrapper *tracker = it_tracker->second;
3323  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
3324  } else {
3325  vectorOfMissingCameras.push_back(it_tracker->first);
3326  }
3327  }
3328  }
3329 
3330  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
3331  ++it) {
3332  it_img = mapOfColorImages.find(*it);
3333  it_model = mapOfModelFiles.find(*it);
3334  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3336 
3337  if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
3338  it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
3339  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
3340  m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
3341  }
3342  }
3343 
3344  modelInitialised = true;
3345 }
3346 
3352 {
3353  m_cMo.eye();
3354 
3355  useScanLine = false;
3356 
3357 #ifdef VISP_HAVE_OGRE
3358  useOgre = false;
3359 #endif
3360 
3361  m_computeInteraction = true;
3362  m_lambda = 1.0;
3363 
3364  angleAppears = vpMath::rad(89);
3367  distNearClip = 0.001;
3368  distFarClip = 100;
3369 
3371  m_maxIter = 30;
3372  m_stopCriteriaEpsilon = 1e-8;
3373  m_initialMu = 0.01;
3374 
3375  // Only for Edge
3376  m_percentageGdPt = 0.4;
3377 
3378  // Only for KLT
3379  m_thresholdOutlier = 0.5;
3380 
3381  // Reset default ponderation between each feature type
3383 
3384 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3386 #endif
3387 
3390 
3391  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3392  it != m_mapOfTrackers.end(); ++it) {
3393  TrackerWrapper *tracker = it->second;
3394  tracker->resetTracker();
3395  }
3396 }
3397 
3408 {
3410 
3411  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3412  it != m_mapOfTrackers.end(); ++it) {
3413  TrackerWrapper *tracker = it->second;
3414  tracker->setAngleAppear(a);
3415  }
3416 }
3417 
3430 void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2)
3431 {
3432  if (m_mapOfTrackers.size() == 2) {
3433  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3434  it->second->setAngleAppear(a1);
3435 
3436  ++it;
3437  it->second->setAngleAppear(a2);
3438 
3440  if (it != m_mapOfTrackers.end()) {
3441  angleAppears = it->second->getAngleAppear();
3442  } else {
3443  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3444  }
3445  } else {
3446  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3447  m_mapOfTrackers.size());
3448  }
3449 }
3450 
3460 void vpMbGenericTracker::setAngleAppear(const std::map<std::string, double> &mapOfAngles)
3461 {
3462  for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3463  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3464 
3465  if (it_tracker != m_mapOfTrackers.end()) {
3466  TrackerWrapper *tracker = it_tracker->second;
3467  tracker->setAngleAppear(it->second);
3468 
3469  if (it->first == m_referenceCameraName) {
3470  angleAppears = it->second;
3471  }
3472  }
3473  }
3474 }
3475 
3486 {
3488 
3489  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3490  it != m_mapOfTrackers.end(); ++it) {
3491  TrackerWrapper *tracker = it->second;
3492  tracker->setAngleDisappear(a);
3493  }
3494 }
3495 
3508 void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2)
3509 {
3510  if (m_mapOfTrackers.size() == 2) {
3511  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3512  it->second->setAngleDisappear(a1);
3513 
3514  ++it;
3515  it->second->setAngleDisappear(a2);
3516 
3518  if (it != m_mapOfTrackers.end()) {
3519  angleDisappears = it->second->getAngleDisappear();
3520  } else {
3521  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3522  }
3523  } else {
3524  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3525  m_mapOfTrackers.size());
3526  }
3527 }
3528 
3538 void vpMbGenericTracker::setAngleDisappear(const std::map<std::string, double> &mapOfAngles)
3539 {
3540  for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
3541  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3542 
3543  if (it_tracker != m_mapOfTrackers.end()) {
3544  TrackerWrapper *tracker = it_tracker->second;
3545  tracker->setAngleDisappear(it->second);
3546 
3547  if (it->first == m_referenceCameraName) {
3548  angleDisappears = it->second;
3549  }
3550  }
3551  }
3552 }
3553 
3560 {
3562 
3563  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3564  it != m_mapOfTrackers.end(); ++it) {
3565  TrackerWrapper *tracker = it->second;
3566  tracker->setCameraParameters(camera);
3567  }
3568 }
3569 
3579 {
3580  if (m_mapOfTrackers.size() == 2) {
3581  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3582  it->second->setCameraParameters(camera1);
3583 
3584  ++it;
3585  it->second->setCameraParameters(camera2);
3586 
3588  if (it != m_mapOfTrackers.end()) {
3589  it->second->getCameraParameters(m_cam);
3590  } else {
3591  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3592  }
3593  } else {
3594  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3595  m_mapOfTrackers.size());
3596  }
3597 }
3598 
3607 void vpMbGenericTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
3608 {
3609  for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
3610  it != mapOfCameraParameters.end(); ++it) {
3611  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3612 
3613  if (it_tracker != m_mapOfTrackers.end()) {
3614  TrackerWrapper *tracker = it_tracker->second;
3615  tracker->setCameraParameters(it->second);
3616 
3617  if (it->first == m_referenceCameraName) {
3618  m_cam = it->second;
3619  }
3620  }
3621  }
3622 }
3623 
3632 void vpMbGenericTracker::setCameraTransformationMatrix(const std::string &cameraName,
3633  const vpHomogeneousMatrix &cameraTransformationMatrix)
3634 {
3635  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
3636 
3637  if (it != m_mapOfCameraTransformationMatrix.end()) {
3638  it->second = cameraTransformationMatrix;
3639  } else {
3640  throw vpException(vpTrackingException::fatalError, "Cannot find camera: %s!", cameraName.c_str());
3641  }
3642 }
3643 
3652  const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
3653 {
3654  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
3655  it != mapOfTransformationMatrix.end(); ++it) {
3656  std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
3657  m_mapOfCameraTransformationMatrix.find(it->first);
3658 
3659  if (it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
3660  it_camTrans->second = it->second;
3661  }
3662  }
3663 }
3664 
3674 void vpMbGenericTracker::setClipping(const unsigned int &flags)
3675 {
3676  vpMbTracker::setClipping(flags);
3677 
3678  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3679  it != m_mapOfTrackers.end(); ++it) {
3680  TrackerWrapper *tracker = it->second;
3681  tracker->setClipping(flags);
3682  }
3683 }
3684 
3695 void vpMbGenericTracker::setClipping(const unsigned int &flags1, const unsigned int &flags2)
3696 {
3697  if (m_mapOfTrackers.size() == 2) {
3698  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3699  it->second->setClipping(flags1);
3700 
3701  ++it;
3702  it->second->setClipping(flags2);
3703 
3705  if (it != m_mapOfTrackers.end()) {
3706  clippingFlag = it->second->getClipping();
3707  } else {
3708  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3709  }
3710  } else {
3711  std::stringstream ss;
3712  ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
3714  }
3715 }
3716 
3724 void vpMbGenericTracker::setClipping(const std::map<std::string, unsigned int> &mapOfClippingFlags)
3725 {
3726  for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
3727  it != mapOfClippingFlags.end(); ++it) {
3728  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3729 
3730  if (it_tracker != m_mapOfTrackers.end()) {
3731  TrackerWrapper *tracker = it_tracker->second;
3732  tracker->setClipping(it->second);
3733 
3734  if (it->first == m_referenceCameraName) {
3735  clippingFlag = it->second;
3736  }
3737  }
3738  }
3739 }
3740 
3751 {
3752  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3753  it != m_mapOfTrackers.end(); ++it) {
3754  TrackerWrapper *tracker = it->second;
3755  tracker->setDepthDenseFilteringMaxDistance(maxDistance);
3756  }
3757 }
3758 
3768 {
3769  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3770  it != m_mapOfTrackers.end(); ++it) {
3771  TrackerWrapper *tracker = it->second;
3772  tracker->setDepthDenseFilteringMethod(method);
3773  }
3774 }
3775 
3786 {
3787  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3788  it != m_mapOfTrackers.end(); ++it) {
3789  TrackerWrapper *tracker = it->second;
3790  tracker->setDepthDenseFilteringMinDistance(minDistance);
3791  }
3792 }
3793 
3804 {
3805  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3806  it != m_mapOfTrackers.end(); ++it) {
3807  TrackerWrapper *tracker = it->second;
3808  tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
3809  }
3810 }
3811 
3820 void vpMbGenericTracker::setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
3821 {
3822  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3823  it != m_mapOfTrackers.end(); ++it) {
3824  TrackerWrapper *tracker = it->second;
3825  tracker->setDepthDenseSamplingStep(stepX, stepY);
3826  }
3827 }
3828 
3837 {
3838  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3839  it != m_mapOfTrackers.end(); ++it) {
3840  TrackerWrapper *tracker = it->second;
3841  tracker->setDepthNormalFaceCentroidMethod(method);
3842  }
3843 }
3844 
3854 {
3855  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3856  it != m_mapOfTrackers.end(); ++it) {
3857  TrackerWrapper *tracker = it->second;
3858  tracker->setDepthNormalFeatureEstimationMethod(method);
3859  }
3860 }
3861 
3870 {
3871  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3872  it != m_mapOfTrackers.end(); ++it) {
3873  TrackerWrapper *tracker = it->second;
3874  tracker->setDepthNormalPclPlaneEstimationMethod(method);
3875  }
3876 }
3877 
3886 {
3887  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3888  it != m_mapOfTrackers.end(); ++it) {
3889  TrackerWrapper *tracker = it->second;
3890  tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
3891  }
3892 }
3893 
3902 {
3903  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3904  it != m_mapOfTrackers.end(); ++it) {
3905  TrackerWrapper *tracker = it->second;
3906  tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
3907  }
3908 }
3909 
3918 void vpMbGenericTracker::setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
3919 {
3920  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3921  it != m_mapOfTrackers.end(); ++it) {
3922  TrackerWrapper *tracker = it->second;
3923  tracker->setDepthNormalSamplingStep(stepX, stepY);
3924  }
3925 }
3926 
3946 {
3948 
3949  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3950  it != m_mapOfTrackers.end(); ++it) {
3951  TrackerWrapper *tracker = it->second;
3952  tracker->setDisplayFeatures(displayF);
3953  }
3954 }
3955 
3964 {
3966 
3967  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3968  it != m_mapOfTrackers.end(); ++it) {
3969  TrackerWrapper *tracker = it->second;
3970  tracker->setFarClippingDistance(dist);
3971  }
3972 }
3973 
3982 void vpMbGenericTracker::setFarClippingDistance(const double &dist1, const double &dist2)
3983 {
3984  if (m_mapOfTrackers.size() == 2) {
3985  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3986  it->second->setFarClippingDistance(dist1);
3987 
3988  ++it;
3989  it->second->setFarClippingDistance(dist2);
3990 
3992  if (it != m_mapOfTrackers.end()) {
3993  distFarClip = it->second->getFarClippingDistance();
3994  } else {
3995  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3996  }
3997  } else {
3998  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3999  m_mapOfTrackers.size());
4000  }
4001 }
4002 
4008 void vpMbGenericTracker::setFarClippingDistance(const std::map<std::string, double> &mapOfClippingDists)
4009 {
4010  for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
4011  ++it) {
4012  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
4013 
4014  if (it_tracker != m_mapOfTrackers.end()) {
4015  TrackerWrapper *tracker = it_tracker->second;
4016  tracker->setFarClippingDistance(it->second);
4017 
4018  if (it->first == m_referenceCameraName) {
4019  distFarClip = it->second;
4020  }
4021  }
4022  }
4023 }
4024 
4031 void vpMbGenericTracker::setFeatureFactors(const std::map<vpTrackerType, double> &mapOfFeatureFactors)
4032 {
4033  for (std::map<vpTrackerType, double>::iterator it = m_mapOfFeatureFactors.begin(); it != m_mapOfFeatureFactors.end();
4034  ++it) {
4035  std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
4036  if (it_factor != mapOfFeatureFactors.end()) {
4037  it->second = it_factor->second;
4038  }
4039  }
4040 }
4041 
4058 {
4059  m_percentageGdPt = threshold;
4060 
4061  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4062  it != m_mapOfTrackers.end(); ++it) {
4063  TrackerWrapper *tracker = it->second;
4064  tracker->setGoodMovingEdgesRatioThreshold(threshold);
4065  }
4066 }
4067 
4068 #ifdef VISP_HAVE_OGRE
4069 
4081 {
4082  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4083  it != m_mapOfTrackers.end(); ++it) {
4084  TrackerWrapper *tracker = it->second;
4085  tracker->setGoodNbRayCastingAttemptsRatio(ratio);
4086  }
4087 }
4088 
4101 {
4102  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4103  it != m_mapOfTrackers.end(); ++it) {
4104  TrackerWrapper *tracker = it->second;
4105  tracker->setNbRayCastingAttemptsForVisibility(attempts);
4106  }
4107 }
4108 #endif
4109 
4110 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4111 
4119 {
4120  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4121  it != m_mapOfTrackers.end(); ++it) {
4122  TrackerWrapper *tracker = it->second;
4123  tracker->setKltOpencv(t);
4124  }
4125 }
4126 
4136 {
4137  if (m_mapOfTrackers.size() == 2) {
4138  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4139  it->second->setKltOpencv(t1);
4140 
4141  ++it;
4142  it->second->setKltOpencv(t2);
4143  } else {
4144  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
4145  m_mapOfTrackers.size());
4146  }
4147 }
4148 
4154 void vpMbGenericTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfKlts)
4155 {
4156  for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
4157  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
4158 
4159  if (it_tracker != m_mapOfTrackers.end()) {
4160  TrackerWrapper *tracker = it_tracker->second;
4161  tracker->setKltOpencv(it->second);
4162  }
4163  }
4164 }
4165 
4174 {
4175  m_thresholdOutlier = th;
4176 
4177  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4178  it != m_mapOfTrackers.end(); ++it) {
4179  TrackerWrapper *tracker = it->second;
4180  tracker->setKltThresholdAcceptation(th);
4181  }
4182 }
4183 #endif
4184 
4197 void vpMbGenericTracker::setLod(bool useLod, const std::string &name)
4198 {
4199  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4200  it != m_mapOfTrackers.end(); ++it) {
4201  TrackerWrapper *tracker = it->second;
4202  tracker->setLod(useLod, name);
4203  }
4204 }
4205 
4206 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4207 
4214 void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e)
4215 {
4216  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4217  it != m_mapOfTrackers.end(); ++it) {
4218  TrackerWrapper *tracker = it->second;
4219  tracker->setKltMaskBorder(e);
4220  }
4221 }
4222 
4231 void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, const unsigned int &e2)
4232 {
4233  if (m_mapOfTrackers.size() == 2) {
4234  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4235  it->second->setKltMaskBorder(e1);
4236 
4237  ++it;
4238 
4239  it->second->setKltMaskBorder(e2);
4240  } else {
4241  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
4242  m_mapOfTrackers.size());
4243  }
4244 }
4245 
4251 void vpMbGenericTracker::setKltMaskBorder(const std::map<std::string, unsigned int> &mapOfErosions)
4252 {
4253  for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
4254  ++it) {
4255  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
4256 
4257  if (it_tracker != m_mapOfTrackers.end()) {
4258  TrackerWrapper *tracker = it_tracker->second;
4259  tracker->setKltMaskBorder(it->second);
4260  }
4261  }
4262 }
4263 #endif
4264 
4271 {
4272  vpMbTracker::setMask(mask);
4273 
4274  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4275  it != m_mapOfTrackers.end(); ++it) {
4276  TrackerWrapper *tracker = it->second;
4277  tracker->setMask(mask);
4278  }
4279 }
4280 
4281 
4293 void vpMbGenericTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
4294 {
4295  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4296  it != m_mapOfTrackers.end(); ++it) {
4297  TrackerWrapper *tracker = it->second;
4298  tracker->setMinLineLengthThresh(minLineLengthThresh, name);
4299  }
4300 }
4301 
4312 void vpMbGenericTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
4313 {
4314  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4315  it != m_mapOfTrackers.end(); ++it) {
4316  TrackerWrapper *tracker = it->second;
4317  tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
4318  }
4319 }
4320 
4329 {
4330  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4331  it != m_mapOfTrackers.end(); ++it) {
4332  TrackerWrapper *tracker = it->second;
4333  tracker->setMovingEdge(me);
4334  }
4335 }
4336 
4346 void vpMbGenericTracker::setMovingEdge(const vpMe &me1, const vpMe &me2)
4347 {
4348  if (m_mapOfTrackers.size() == 2) {
4349  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4350  it->second->setMovingEdge(me1);
4351 
4352  ++it;
4353 
4354  it->second->setMovingEdge(me2);
4355  } else {
4356  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
4357  m_mapOfTrackers.size());
4358  }
4359 }
4360 
4366 void vpMbGenericTracker::setMovingEdge(const std::map<std::string, vpMe> &mapOfMe)
4367 {
4368  for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
4369  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
4370 
4371  if (it_tracker != m_mapOfTrackers.end()) {
4372  TrackerWrapper *tracker = it_tracker->second;
4373  tracker->setMovingEdge(it->second);
4374  }
4375  }
4376 }
4377 
4386 {
4388 
4389  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4390  it != m_mapOfTrackers.end(); ++it) {
4391  TrackerWrapper *tracker = it->second;
4392  tracker->setNearClippingDistance(dist);
4393  }
4394 }
4395 
4404 void vpMbGenericTracker::setNearClippingDistance(const double &dist1, const double &dist2)
4405 {
4406  if (m_mapOfTrackers.size() == 2) {
4407  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4408  it->second->setNearClippingDistance(dist1);
4409 
4410  ++it;
4411 
4412  it->second->setNearClippingDistance(dist2);
4413 
4415  if (it != m_mapOfTrackers.end()) {
4416  distNearClip = it->second->getNearClippingDistance();
4417  } else {
4418  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
4419  }
4420  } else {
4421  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
4422  m_mapOfTrackers.size());
4423  }
4424 }
4425 
4431 void vpMbGenericTracker::setNearClippingDistance(const std::map<std::string, double> &mapOfDists)
4432 {
4433  for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
4434  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
4435 
4436  if (it_tracker != m_mapOfTrackers.end()) {
4437  TrackerWrapper *tracker = it_tracker->second;
4438  tracker->setNearClippingDistance(it->second);
4439 
4440  if (it->first == m_referenceCameraName) {
4441  distNearClip = it->second;
4442  }
4443  }
4444  }
4445 }
4446 
4460 {
4461  vpMbTracker::setOgreShowConfigDialog(showConfigDialog);
4462 
4463  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4464  it != m_mapOfTrackers.end(); ++it) {
4465  TrackerWrapper *tracker = it->second;
4466  tracker->setOgreShowConfigDialog(showConfigDialog);
4467  }
4468 }
4469 
4481 {
4483 
4484  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4485  it != m_mapOfTrackers.end(); ++it) {
4486  TrackerWrapper *tracker = it->second;
4487  tracker->setOgreVisibilityTest(v);
4488  }
4489 
4490 #ifdef VISP_HAVE_OGRE
4491  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4492  it != m_mapOfTrackers.end(); ++it) {
4493  TrackerWrapper *tracker = it->second;
4494  tracker->faces.getOgreContext()->setWindowName("Multi Generic MBT (" + it->first + ")");
4495  }
4496 #endif
4497 }
4498 
4507 {
4509 
4510  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4511  it != m_mapOfTrackers.end(); ++it) {
4512  TrackerWrapper *tracker = it->second;
4513  tracker->setOptimizationMethod(opt);
4514  }
4515 }
4516 
4530 {
4531  if (m_mapOfTrackers.size() > 1) {
4532  throw vpException(vpTrackingException::initializationError, "The function setPose() requires the generic tracker "
4533  "to be configured with only one camera!");
4534  }
4535 
4536  m_cMo = cdMo;
4537 
4538  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
4539  if (it != m_mapOfTrackers.end()) {
4540  TrackerWrapper *tracker = it->second;
4541  tracker->setPose(I, cdMo);
4542  } else {
4543  throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
4544  m_referenceCameraName.c_str());
4545  }
4546 }
4547 
4561 {
4562  if (m_mapOfTrackers.size() > 1) {
4563  throw vpException(vpTrackingException::initializationError, "The function setPose() requires the generic tracker "
4564  "to be configured with only one camera!");
4565  }
4566 
4567  m_cMo = cdMo;
4568 
4569  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
4570  if (it != m_mapOfTrackers.end()) {
4571  TrackerWrapper *tracker = it->second;
4572  vpImageConvert::convert(I_color, m_I);
4573  tracker->setPose(m_I, cdMo);
4574  } else {
4575  throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
4576  m_referenceCameraName.c_str());
4577  }
4578 }
4579 
4592  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
4593 {
4594  if (m_mapOfTrackers.size() == 2) {
4595  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4596  it->second->setPose(I1, c1Mo);
4597 
4598  ++it;
4599 
4600  it->second->setPose(I2, c2Mo);
4601 
4603  if (it != m_mapOfTrackers.end()) {
4604  // Set reference pose
4605  it->second->getPose(m_cMo);
4606  } else {
4607  throw vpException(vpTrackingException::fatalError, "The reference camera: %s does not exist!",
4608  m_referenceCameraName.c_str());
4609  }
4610  } else {
4611  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
4612  m_mapOfTrackers.size());
4613  }
4614 }
4615 
4627 void vpMbGenericTracker::setPose(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
4628  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
4629 {
4630  if (m_mapOfTrackers.size() == 2) {
4631  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4632  it->second->setPose(I_color1, c1Mo);
4633 
4634  ++it;
4635 
4636  it->second->setPose(I_color2, c2Mo);
4637 
4639  if (it != m_mapOfTrackers.end()) {
4640  // Set reference pose
4641  it->second->getPose(m_cMo);
4642  } else {
4643  throw vpException(vpTrackingException::fatalError, "The reference camera: %s does not exist!",
4644  m_referenceCameraName.c_str());
4645  }
4646  } else {
4647  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
4648  m_mapOfTrackers.size());
4649  }
4650 }
4651 
4667 void vpMbGenericTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
4668  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4669 {
4670  // Set the reference cMo
4671  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
4672  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
4673  mapOfImages.find(m_referenceCameraName);
4674  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
4675 
4676  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4677  TrackerWrapper *tracker = it_tracker->second;
4678  tracker->setPose(*it_img->second, it_camPose->second);
4679  tracker->getPose(m_cMo);
4680  } else {
4681  throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera!");
4682  }
4683 
4684  // Vector of missing pose matrices for cameras
4685  std::vector<std::string> vectorOfMissingCameraPoses;
4686 
4687  // Set pose for the specified cameras
4688  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
4689  if (it_tracker->first != m_referenceCameraName) {
4690  it_img = mapOfImages.find(it_tracker->first);
4691  it_camPose = mapOfCameraPoses.find(it_tracker->first);
4692 
4693  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
4694  // Set pose
4695  TrackerWrapper *tracker = it_tracker->second;
4696  tracker->setPose(*it_img->second, it_camPose->second);
4697  } else {
4698  vectorOfMissingCameraPoses.push_back(it_tracker->first);
4699  }
4700  }
4701  }
4702 
4703  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4704  it != vectorOfMissingCameraPoses.end(); ++it) {
4705  it_img = mapOfImages.find(*it);
4706  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4708 
4709  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
4710  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
4711  m_mapOfTrackers[*it]->setPose(*it_img->second, cCurrentMo);
4712  } else {
4714  "Missing image or missing camera transformation "
4715  "matrix! Cannot set pose for camera: %s!",
4716  it->c_str());
4717  }
4718  }
4719 }
4720 
4736 void vpMbGenericTracker::setPose(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
4737  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
4738 {
4739  // Set the reference cMo
4740  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
4741  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img =
4742  mapOfColorImages.find(m_referenceCameraName);
4743  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
4744 
4745  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4746  TrackerWrapper *tracker = it_tracker->second;
4747  tracker->setPose(*it_img->second, it_camPose->second);
4748  tracker->getPose(m_cMo);
4749  } else {
4750  throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera!");
4751  }
4752 
4753  // Vector of missing pose matrices for cameras
4754  std::vector<std::string> vectorOfMissingCameraPoses;
4755 
4756  // Set pose for the specified cameras
4757  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
4758  if (it_tracker->first != m_referenceCameraName) {
4759  it_img = mapOfColorImages.find(it_tracker->first);
4760  it_camPose = mapOfCameraPoses.find(it_tracker->first);
4761 
4762  if (it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
4763  // Set pose
4764  TrackerWrapper *tracker = it_tracker->second;
4765  tracker->setPose(*it_img->second, it_camPose->second);
4766  } else {
4767  vectorOfMissingCameraPoses.push_back(it_tracker->first);
4768  }
4769  }
4770  }
4771 
4772  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
4773  it != vectorOfMissingCameraPoses.end(); ++it) {
4774  it_img = mapOfColorImages.find(*it);
4775  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
4777 
4778  if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
4779  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
4780  m_mapOfTrackers[*it]->setPose(*it_img->second, cCurrentMo);
4781  } else {
4783  "Missing image or missing camera transformation "
4784  "matrix! Cannot set pose for camera: %s!",
4785  it->c_str());
4786  }
4787  }
4788 }
4789 
4805 {
4807 
4808  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4809  it != m_mapOfTrackers.end(); ++it) {
4810  TrackerWrapper *tracker = it->second;
4811  tracker->setProjectionErrorComputation(flag);
4812  }
4813 }
4814 
4819 {
4821 
4822  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4823  it != m_mapOfTrackers.end(); ++it) {
4824  TrackerWrapper *tracker = it->second;
4825  tracker->setProjectionErrorDisplay(display);
4826  }
4827 }
4828 
4833 {
4835 
4836  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4837  it != m_mapOfTrackers.end(); ++it) {
4838  TrackerWrapper *tracker = it->second;
4839  tracker->setProjectionErrorDisplayArrowLength(length);
4840  }
4841 }
4842 
4844 {
4846 
4847  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4848  it != m_mapOfTrackers.end(); ++it) {
4849  TrackerWrapper *tracker = it->second;
4850  tracker->setProjectionErrorDisplayArrowThickness(thickness);
4851  }
4852 }
4853 
4859 void vpMbGenericTracker::setReferenceCameraName(const std::string &referenceCameraName)
4860 {
4861  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(referenceCameraName);
4862  if (it != m_mapOfTrackers.end()) {
4863  m_referenceCameraName = referenceCameraName;
4864  } else {
4865  std::cerr << "The reference camera: " << referenceCameraName << " does not exist!";
4866  }
4867 }
4868 
4870 {
4872 
4873  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4874  it != m_mapOfTrackers.end(); ++it) {
4875  TrackerWrapper *tracker = it->second;
4876  tracker->setScanLineVisibilityTest(v);
4877  }
4878 }
4879 
4892 {
4893  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4894  it != m_mapOfTrackers.end(); ++it) {
4895  TrackerWrapper *tracker = it->second;
4896  tracker->setTrackerType(type);
4897  }
4898 }
4899 
4909 void vpMbGenericTracker::setTrackerType(const std::map<std::string, int> &mapOfTrackerTypes)
4910 {
4911  for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
4912  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
4913  if (it_tracker != m_mapOfTrackers.end()) {
4914  TrackerWrapper *tracker = it_tracker->second;
4915  tracker->setTrackerType(it->second);
4916  }
4917  }
4918 }
4919 
4929 void vpMbGenericTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
4930 {
4931  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4932  it != m_mapOfTrackers.end(); ++it) {
4933  TrackerWrapper *tracker = it->second;
4934  tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
4935  }
4936 }
4937 
4947 void vpMbGenericTracker::setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
4948 {
4949  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4950  it != m_mapOfTrackers.end(); ++it) {
4951  TrackerWrapper *tracker = it->second;
4952  tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
4953  }
4954 }
4955 
4965 void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
4966 {
4967  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4968  it != m_mapOfTrackers.end(); ++it) {
4969  TrackerWrapper *tracker = it->second;
4970  tracker->setUseEdgeTracking(name, useEdgeTracking);
4971  }
4972 }
4973 
4974 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4975 
4984 void vpMbGenericTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
4985 {
4986  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4987  it != m_mapOfTrackers.end(); ++it) {
4988  TrackerWrapper *tracker = it->second;
4989  tracker->setUseKltTracking(name, useKltTracking);
4990  }
4991 }
4992 #endif
4993 
4995 {
4996  // Test tracking fails only if all testTracking have failed
4997  bool isOneTestTrackingOk = false;
4998  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4999  it != m_mapOfTrackers.end(); ++it) {
5000  TrackerWrapper *tracker = it->second;
5001  try {
5002  tracker->testTracking();
5003  isOneTestTrackingOk = true;
5004  } catch (...) {
5005  }
5006  }
5007 
5008  if (!isOneTestTrackingOk) {
5009  std::ostringstream oss;
5010  oss << "Not enough moving edges to track the object. Try to reduce the "
5011  "threshold="
5012  << m_percentageGdPt << " using setGoodMovingEdgesRatioThreshold()";
5014  }
5015 }
5016 
5027 {
5028  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5029  mapOfImages[m_referenceCameraName] = &I;
5030 
5031  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5032  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5033 
5034  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5035 }
5036 
5047 {
5048  std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
5049  mapOfColorImages[m_referenceCameraName] = &I_color;
5050 
5051  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5052  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5053 
5054  track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5055 }
5056 
5068 {
5069  if (m_mapOfTrackers.size() == 2) {
5070  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5071  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5072  mapOfImages[it->first] = &I1;
5073  ++it;
5074 
5075  mapOfImages[it->first] = &I2;
5076 
5077  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5078  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5079 
5080  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5081  } else {
5082  std::stringstream ss;
5083  ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
5084  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
5085  }
5086 }
5087 
5098 void vpMbGenericTracker::track(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &_colorI2)
5099 {
5100  if (m_mapOfTrackers.size() == 2) {
5101  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5102  std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
5103  mapOfImages[it->first] = &I_color1;
5104  ++it;
5105 
5106  mapOfImages[it->first] = &_colorI2;
5107 
5108  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5109  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5110 
5111  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5112  } else {
5113  std::stringstream ss;
5114  ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
5115  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
5116  }
5117 }
5118 
5126 void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
5127 {
5128  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5129  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5130 
5131  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5132 }
5133 
5141 void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages)
5142 {
5143  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
5144  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
5145 
5146  track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
5147 }
5148 
5149 #ifdef VISP_HAVE_PCL
5150 
5158 void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
5159  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5160 {
5161  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5162  it != m_mapOfTrackers.end(); ++it) {
5163  TrackerWrapper *tracker = it->second;
5164 
5165  if ((tracker->m_trackerType & (EDGE_TRACKER |
5166 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5167  KLT_TRACKER |
5168 #endif
5170  throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
5171  }
5172 
5173  if (tracker->m_trackerType & (EDGE_TRACKER
5174 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5175  | KLT_TRACKER
5176 #endif
5177  ) &&
5178  mapOfImages[it->first] == NULL) {
5179  throw vpException(vpException::fatalError, "Image pointer is NULL!");
5180  }
5181 
5182  if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
5183  ! mapOfPointClouds[it->first]) { // mapOfPointClouds[it->first] == nullptr
5184  throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
5185  }
5186  }
5187 
5188  preTracking(mapOfImages, mapOfPointClouds);
5189 
5190  try {
5191  computeVVS(mapOfImages);
5192  } catch (...) {
5193  covarianceMatrix = -1;
5194  throw; // throw the original exception
5195  }
5196 
5197  testTracking();
5198 
5199  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5200  it != m_mapOfTrackers.end(); ++it) {
5201  TrackerWrapper *tracker = it->second;
5202 
5203  if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
5204  tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5205  }
5206 
5207  tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5208 
5209  if (displayFeatures) {
5210 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5211  if (tracker->m_trackerType & KLT_TRACKER) {
5212  tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5213  }
5214 #endif
5215 
5216  if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
5217  tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5218  }
5219  }
5220  }
5221 
5223 }
5224 
5233 void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
5234  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
5235 {
5236  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5237  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5238  it != m_mapOfTrackers.end(); ++it) {
5239  TrackerWrapper *tracker = it->second;
5240 
5241  if ((tracker->m_trackerType & (EDGE_TRACKER |
5242 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5243  KLT_TRACKER |
5244 #endif
5246  throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
5247  }
5248 
5249  if (tracker->m_trackerType & (EDGE_TRACKER
5250 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5251  | KLT_TRACKER
5252 #endif
5253  ) && mapOfImages[it->first] == NULL) {
5254  throw vpException(vpException::fatalError, "Image pointer is NULL!");
5255  } else if (tracker->m_trackerType & (EDGE_TRACKER
5256 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5257  | KLT_TRACKER
5258 #endif
5259  ) && mapOfImages[it->first] != NULL) {
5260  vpImageConvert::convert(*mapOfColorImages[it->first], tracker->m_I);
5261  mapOfImages[it->first] = &tracker->m_I; //update grayscale image buffer
5262  }
5263 
5264  if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
5265  ! mapOfPointClouds[it->first]) { // mapOfPointClouds[it->first] == nullptr
5266  throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
5267  }
5268  }
5269 
5270  preTracking(mapOfImages, mapOfPointClouds);
5271 
5272  try {
5273  computeVVS(mapOfImages);
5274  } catch (...) {
5275  covarianceMatrix = -1;
5276  throw; // throw the original exception
5277  }
5278 
5279  testTracking();
5280 
5281  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5282  it != m_mapOfTrackers.end(); ++it) {
5283  TrackerWrapper *tracker = it->second;
5284 
5285  if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
5286  tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5287  }
5288 
5289  tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
5290 
5291  if (displayFeatures) {
5292 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5293  if (tracker->m_trackerType & KLT_TRACKER) {
5294  tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5295  }
5296 #endif
5297 
5298  if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
5299  tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5300  }
5301  }
5302  }
5303 
5305 }
5306 #endif
5307 
5318 void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
5319  std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
5320  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5321  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5322 {
5323  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5324  it != m_mapOfTrackers.end(); ++it) {
5325  TrackerWrapper *tracker = it->second;
5326 
5327  if ((tracker->m_trackerType & (EDGE_TRACKER |
5328 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5329  KLT_TRACKER |
5330 #endif
5332  throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
5333  }
5334 
5335  if (tracker->m_trackerType & (EDGE_TRACKER
5336 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5337  | KLT_TRACKER
5338 #endif
5339  ) &&
5340  mapOfImages[it->first] == NULL) {
5341  throw vpException(vpException::fatalError, "Image pointer is NULL!");
5342  }
5343 
5344  if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
5345  (mapOfPointClouds[it->first] == NULL)) {
5346  throw vpException(vpException::fatalError, "Pointcloud is NULL!");
5347  }
5348  }
5349 
5350  preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5351 
5352  try {
5353  computeVVS(mapOfImages);
5354  } catch (...) {
5355  covarianceMatrix = -1;
5356  throw; // throw the original exception
5357  }
5358 
5359  testTracking();
5360 
5361  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5362  it != m_mapOfTrackers.end(); ++it) {
5363  TrackerWrapper *tracker = it->second;
5364 
5365  if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
5366  tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5367  }
5368 
5369  tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5370 
5371  if (displayFeatures) {
5372 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5373  if (tracker->m_trackerType & KLT_TRACKER) {
5374  tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5375  }
5376 #endif
5377 
5378  if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
5379  tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5380  }
5381  }
5382  }
5383 
5385 }
5386 
5397 void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
5398  std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
5399  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
5400  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
5401 {
5402  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
5403  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5404  it != m_mapOfTrackers.end(); ++it) {
5405  TrackerWrapper *tracker = it->second;
5406 
5407  if ((tracker->m_trackerType & (EDGE_TRACKER |
5408 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5409  KLT_TRACKER |
5410 #endif
5412  throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
5413  }
5414 
5415  if (tracker->m_trackerType & (EDGE_TRACKER
5416 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5417  | KLT_TRACKER
5418 #endif
5419  ) && mapOfColorImages[it->first] == NULL) {
5420  throw vpException(vpException::fatalError, "Image pointer is NULL!");
5421  } else if (tracker->m_trackerType & (EDGE_TRACKER
5422 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5423  | KLT_TRACKER
5424 #endif
5425  ) && mapOfColorImages[it->first] != NULL) {
5426  vpImageConvert::convert(*mapOfColorImages[it->first], tracker->m_I);
5427  mapOfImages[it->first] = &tracker->m_I; //update grayscale image buffer
5428  }
5429 
5430  if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
5431  (mapOfPointClouds[it->first] == NULL)) {
5432  throw vpException(vpException::fatalError, "Pointcloud is NULL!");
5433  }
5434  }
5435 
5436  preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
5437 
5438  try {
5439  computeVVS(mapOfImages);
5440  } catch (...) {
5441  covarianceMatrix = -1;
5442  throw; // throw the original exception
5443  }
5444 
5445  testTracking();
5446 
5447  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
5448  it != m_mapOfTrackers.end(); ++it) {
5449  TrackerWrapper *tracker = it->second;
5450 
5451  if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
5452  tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
5453  }
5454 
5455  tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
5456 
5457  if (displayFeatures) {
5458 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5459  if (tracker->m_trackerType & KLT_TRACKER) {
5460  tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
5461  }
5462 #endif
5463 
5464  if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
5465  tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
5466  }
5467  }
5468  }
5469 
5471 }
5472 
5474 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
5475  : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
5476 {
5477  m_lambda = 1.0;
5478  m_maxIter = 30;
5479 
5480 #ifdef VISP_HAVE_OGRE
5481  faces.getOgreContext()->setWindowName("MBT TrackerWrapper");
5482 
5483  m_projectionErrorFaces.getOgreContext()->setWindowName("MBT TrackerWrapper (projection error)");
5484 #endif
5485 }
5486 
5487 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(int trackerType)
5488  : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
5489 {
5490  if ((m_trackerType & (EDGE_TRACKER |
5491 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5492  KLT_TRACKER |
5493 #endif
5495  throw vpException(vpTrackingException::badValue, "Bad value for tracker type: %d!", m_trackerType);
5496  }
5497 
5498  m_lambda = 1.0;
5499  m_maxIter = 30;
5500 
5501 #ifdef VISP_HAVE_OGRE
5502  faces.getOgreContext()->setWindowName("MBT TrackerWrapper");
5503 
5504  m_projectionErrorFaces.getOgreContext()->setWindowName("MBT TrackerWrapper (projection error)");
5505 #endif
5506 }
5507 
5508 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
5509 
5510 // Implemented only for debugging purposes: use TrackerWrapper as a standalone tracker
5511 void vpMbGenericTracker::TrackerWrapper::computeVVS(const vpImage<unsigned char> *const ptr_I)
5512 {
5513  computeVVSInit(ptr_I);
5514 
5515  if (m_error.getRows() < 4) {
5516  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
5517  }
5518 
5519  double normRes = 0;
5520  double normRes_1 = -1;
5521  unsigned int iter = 0;
5522 
5523  double factorEdge = 1.0;
5524 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5525  double factorKlt = 1.0;
5526 #endif
5527  double factorDepth = 1.0;
5528  double factorDepthDense = 1.0;
5529 
5530  vpMatrix LTL;
5531  vpColVector LTR, v;
5532  vpColVector error_prev;
5533 
5534  double mu = m_initialMu;
5535  vpHomogeneousMatrix cMo_prev;
5536 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5537  vpHomogeneousMatrix ctTc0_Prev; // Only for KLT
5538 #endif
5539  bool isoJoIdentity_ = true;
5540 
5541  // Covariance
5542  vpColVector W_true(m_error.getRows());
5543  vpMatrix L_true, LVJ_true;
5544 
5545  unsigned int nb_edge_features = m_error_edge.getRows();
5546 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5547  unsigned int nb_klt_features = m_error_klt.getRows();
5548 #endif
5549  unsigned int nb_depth_features = m_error_depthNormal.getRows();
5550  unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
5551 
5552  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
5554 
5555  bool reStartFromLastIncrement = false;
5556  computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
5557 
5558 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5559  if (reStartFromLastIncrement) {
5560  if (m_trackerType & KLT_TRACKER) {
5561  ctTc0 = ctTc0_Prev;
5562  }
5563  }
5564 #endif
5565 
5566  if (!reStartFromLastIncrement) {
5568 
5569  if (computeCovariance) {
5570  L_true = m_L;
5571  if (!isoJoIdentity_) {
5573  cVo.buildFrom(m_cMo);
5574  LVJ_true = (m_L * cVo * oJo);
5575  }
5576  }
5577 
5579  if (iter == 0) {
5580  isoJoIdentity_ = true;
5581  oJo.eye();
5582 
5583  // If all the 6 dof should be estimated, we check if the interaction
5584  // matrix is full rank. If not we remove automatically the dof that
5585  // cannot be estimated This is particularly useful when consering
5586  // circles (rank 5) and cylinders (rank 4)
5587  if (isoJoIdentity_) {
5588  cVo.buildFrom(m_cMo);
5589 
5590  vpMatrix K; // kernel
5591  unsigned int rank = (m_L * cVo).kernel(K);
5592  if (rank == 0) {
5593  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
5594  }
5595 
5596  if (rank != 6) {
5597  vpMatrix I; // Identity
5598  I.eye(6);
5599  oJo = I - K.AtA();
5600 
5601  isoJoIdentity_ = false;
5602  }
5603  }
5604  }
5605 
5606  // Weighting
5607  double num = 0;
5608  double den = 0;
5609 
5610  unsigned int start_index = 0;
5611  if (m_trackerType & EDGE_TRACKER) {
5612  for (unsigned int i = 0; i < nb_edge_features; i++) {
5613  double wi = m_w_edge[i] * m_factor[i] * factorEdge;
5614  W_true[i] = wi;
5615  m_weightedError[i] = wi * m_error[i];
5616 
5617  num += wi * vpMath::sqr(m_error[i]);
5618  den += wi;
5619 
5620  for (unsigned int j = 0; j < m_L.getCols(); j++) {
5621  m_L[i][j] *= wi;
5622  }
5623  }
5624 
5625  start_index += nb_edge_features;
5626  }
5627 
5628 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5629  if (m_trackerType & KLT_TRACKER) {
5630  for (unsigned int i = 0; i < nb_klt_features; i++) {
5631  double wi = m_w_klt[i] * factorKlt;
5632  W_true[start_index + i] = wi;
5633  m_weightedError[start_index + i] = wi * m_error_klt[i];
5634 
5635  num += wi * vpMath::sqr(m_error[start_index + i]);
5636  den += wi;
5637 
5638  for (unsigned int j = 0; j < m_L.getCols(); j++) {
5639  m_L[start_index + i][j] *= wi;
5640  }
5641  }
5642 
5643  start_index += nb_klt_features;
5644  }
5645 #endif
5646 
5647  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5648  for (unsigned int i = 0; i < nb_depth_features; i++) {
5649  double wi = m_w_depthNormal[i] * factorDepth;
5650  m_w[start_index + i] = m_w_depthNormal[i];
5651  m_weightedError[start_index + i] = wi * m_error[start_index + i];
5652 
5653  num += wi * vpMath::sqr(m_error[start_index + i]);
5654  den += wi;
5655 
5656  for (unsigned int j = 0; j < m_L.getCols(); j++) {
5657  m_L[start_index + i][j] *= wi;
5658  }
5659  }
5660 
5661  start_index += nb_depth_features;
5662  }
5663 
5664  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5665  for (unsigned int i = 0; i < nb_depth_dense_features; i++) {
5666  double wi = m_w_depthDense[i] * factorDepthDense;
5667  m_w[start_index + i] = m_w_depthDense[i];
5668  m_weightedError[start_index + i] = wi * m_error[start_index + i];
5669 
5670  num += wi * vpMath::sqr(m_error[start_index + i]);
5671  den += wi;
5672 
5673  for (unsigned int j = 0; j < m_L.getCols(); j++) {
5674  m_L[start_index + i][j] *= wi;
5675  }
5676  }
5677 
5678  // start_index += nb_depth_dense_features;
5679  }
5680 
5681  computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
5682 
5683  cMo_prev = m_cMo;
5684 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5685  if (m_trackerType & KLT_TRACKER) {
5686  ctTc0_Prev = ctTc0;
5687  }
5688 #endif
5689 
5691 
5692 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5693  if (m_trackerType & KLT_TRACKER) {
5694  ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
5695  }
5696 #endif
5697  normRes_1 = normRes;
5698 
5699  normRes = sqrt(num / den);
5700  }
5701 
5702  iter++;
5703  }
5704 
5705  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
5706 
5707  if (m_trackerType & EDGE_TRACKER) {
5709  }
5710 }
5711 
5712 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
5713 {
5714  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
5715  "TrackerWrapper::computeVVSInit("
5716  ") should not be called!");
5717 }
5718 
5719 void vpMbGenericTracker::TrackerWrapper::computeVVSInit(const vpImage<unsigned char> *const ptr_I)
5720 {
5721  initMbtTracking(ptr_I);
5722 
5723  unsigned int nbFeatures = 0;
5724 
5725  if (m_trackerType & EDGE_TRACKER) {
5726  nbFeatures += m_error_edge.getRows();
5727  } else {
5728  m_error_edge.clear();
5729  m_weightedError_edge.clear();
5730  m_L_edge.clear();
5731  m_w_edge.clear();
5732  }
5733 
5734 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5735  if (m_trackerType & KLT_TRACKER) {
5737  nbFeatures += m_error_klt.getRows();
5738  } else {
5739  m_error_klt.clear();
5740  m_weightedError_klt.clear();
5741  m_L_klt.clear();
5742  m_w_klt.clear();
5743  }
5744 #endif
5745 
5746  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5748  nbFeatures += m_error_depthNormal.getRows();
5749  } else {
5750  m_error_depthNormal.clear();
5751  m_weightedError_depthNormal.clear();
5752  m_L_depthNormal.clear();
5753  m_w_depthNormal.clear();
5754  }
5755 
5756  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5758  nbFeatures += m_error_depthDense.getRows();
5759  } else {
5760  m_error_depthDense.clear();
5761  m_weightedError_depthDense.clear();
5762  m_L_depthDense.clear();
5763  m_w_depthDense.clear();
5764  }
5765 
5766  m_L.resize(nbFeatures, 6, false, false);
5767  m_error.resize(nbFeatures, false);
5768 
5769  m_weightedError.resize(nbFeatures, false);
5770  m_w.resize(nbFeatures, false);
5771  m_w = 1;
5772 }
5773 
5774 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu()
5775 {
5776  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
5777  "TrackerWrapper::"
5778  "computeVVSInteractionMatrixAndR"
5779  "esidu() should not be called!");
5780 }
5781 
5782 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> *const ptr_I)
5783 {
5784  if (m_trackerType & EDGE_TRACKER) {
5786  }
5787 
5788 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5789  if (m_trackerType & KLT_TRACKER) {
5791  }
5792 #endif
5793 
5794  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5796  }
5797 
5798  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5800  }
5801 
5802  unsigned int start_index = 0;
5803  if (m_trackerType & EDGE_TRACKER) {
5804  m_L.insert(m_L_edge, start_index, 0);
5805  m_error.insert(start_index, m_error_edge);
5806 
5807  start_index += m_error_edge.getRows();
5808  }
5809 
5810 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5811  if (m_trackerType & KLT_TRACKER) {
5812  m_L.insert(m_L_klt, start_index, 0);
5813  m_error.insert(start_index, m_error_klt);
5814 
5815  start_index += m_error_klt.getRows();
5816  }
5817 #endif
5818 
5819  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5820  m_L.insert(m_L_depthNormal, start_index, 0);
5821  m_error.insert(start_index, m_error_depthNormal);
5822 
5823  start_index += m_error_depthNormal.getRows();
5824  }
5825 
5826  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5827  m_L.insert(m_L_depthDense, start_index, 0);
5828  m_error.insert(start_index, m_error_depthDense);
5829 
5830  // start_index += m_error_depthDense.getRows();
5831  }
5832 }
5833 
5835 {
5836  unsigned int start_index = 0;
5837 
5838  if (m_trackerType & EDGE_TRACKER) {
5840  m_w.insert(start_index, m_w_edge);
5841 
5842  start_index += m_w_edge.getRows();
5843  }
5844 
5845 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5846  if (m_trackerType & KLT_TRACKER) {
5847  vpMbTracker::computeVVSWeights(m_robust_klt, m_error_klt, m_w_klt);
5848  m_w.insert(start_index, m_w_klt);
5849 
5850  start_index += m_w_klt.getRows();
5851  }
5852 #endif
5853 
5854  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5855  if (m_depthNormalUseRobust) {
5856  vpMbTracker::computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal);
5857  m_w.insert(start_index, m_w_depthNormal);
5858  }
5859 
5860  start_index += m_w_depthNormal.getRows();
5861  }
5862 
5863  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5865  m_w.insert(start_index, m_w_depthDense);
5866 
5867  // start_index += m_w_depthDense.getRows();
5868  }
5869 }
5870 
5871 void vpMbGenericTracker::TrackerWrapper::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
5872  const vpCameraParameters &cam, const vpColor &col,
5873  unsigned int thickness, bool displayFullModel)
5874 {
5875  if (displayFeatures) {
5876  std::vector<std::vector<double> > features = getFeaturesForDisplay();
5877  for (size_t i = 0; i < features.size(); i++) {
5878  if (vpMath::equal(features[i][0], 0)) {
5879  vpImagePoint ip(features[i][1], features[i][2]);
5880  int state = static_cast<int>(features[i][3]);
5881 
5882  switch (state) {
5885  break;
5886 
5887  case vpMeSite::CONSTRAST:
5888  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
5889  break;
5890 
5891  case vpMeSite::THRESHOLD:
5893  break;
5894 
5895  case vpMeSite::M_ESTIMATOR:
5896  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
5897  break;
5898 
5899  case vpMeSite::TOO_NEAR:
5900  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
5901  break;
5902 
5903  default:
5905  }
5906  } else if (vpMath::equal(features[i][0], 1)) {
5907  vpImagePoint ip1(features[i][1], features[i][2]);
5909 
5910  vpImagePoint ip2(features[i][3], features[i][4]);
5911  double id = features[i][5];
5912  std::stringstream ss;
5913  ss << id;
5914  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
5915  } else if (vpMath::equal(features[i][0], 2)) {
5916  vpImagePoint im_centroid(features[i][1], features[i][2]);
5917  vpImagePoint im_extremity(features[i][3], features[i][4]);
5918  bool desired = vpMath::equal(features[i][0], 2);
5919  vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
5920  }
5921  }
5922  }
5923 
5924  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
5925  for (size_t i = 0; i < models.size(); i++) {
5926  if (vpMath::equal(models[i][0], 0)) {
5927  vpImagePoint ip1(models[i][1], models[i][2]);
5928  vpImagePoint ip2(models[i][3], models[i][4]);
5929  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
5930  } else if (vpMath::equal(models[i][0], 1)) {
5931  vpImagePoint center(models[i][1], models[i][2]);
5932  double n20 = models[i][3];
5933  double n11 = models[i][4];
5934  double n02 = models[i][5];
5935  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
5936  }
5937  }
5938 
5939 #ifdef VISP_HAVE_OGRE
5940  if ((m_trackerType & EDGE_TRACKER)
5941  #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5942  || (m_trackerType & KLT_TRACKER)
5943  #endif
5944  ) {
5945  if (useOgre)
5946  faces.displayOgre(cMo);
5947  }
5948 #endif
5949 }
5950 
5951 void vpMbGenericTracker::TrackerWrapper::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
5952  const vpCameraParameters &cam, const vpColor &col,
5953  unsigned int thickness, bool displayFullModel)
5954 {
5955  if (displayFeatures) {
5956  std::vector<std::vector<double> > features = getFeaturesForDisplay();
5957  for (size_t i = 0; i < features.size(); i++) {
5958  if (vpMath::equal(features[i][0], 0)) {
5959  vpImagePoint ip(features[i][1], features[i][2]);
5960  int state = static_cast<int>(features[i][3]);
5961 
5962  switch (state) {
5965  break;
5966 
5967  case vpMeSite::CONSTRAST:
5968  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
5969  break;
5970 
5971  case vpMeSite::THRESHOLD:
5973  break;
5974 
5975  case vpMeSite::M_ESTIMATOR:
5976  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
5977  break;
5978 
5979  case vpMeSite::TOO_NEAR:
5980  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
5981  break;
5982 
5983  default:
5985  }
5986  } else if (vpMath::equal(features[i][0], 1)) {
5987  vpImagePoint ip1(features[i][1], features[i][2]);
5989 
5990  vpImagePoint ip2(features[i][3], features[i][4]);
5991  double id = features[i][5];
5992  std::stringstream ss;
5993  ss << id;
5994  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
5995  } else if (vpMath::equal(features[i][0], 2)) {
5996  vpImagePoint im_centroid(features[i][1], features[i][2]);
5997  vpImagePoint im_extremity(features[i][3], features[i][4]);
5998  bool desired = vpMath::equal(features[i][0], 2);
5999  vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
6000  }
6001  }
6002  }
6003 
6004  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
6005  for (size_t i = 0; i < models.size(); i++) {
6006  if (vpMath::equal(models[i][0], 0)) {
6007  vpImagePoint ip1(models[i][1], models[i][2]);
6008  vpImagePoint ip2(models[i][3], models[i][4]);
6009  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
6010  } else if (vpMath::equal(models[i][0], 1)) {
6011  vpImagePoint center(models[i][1], models[i][2]);
6012  double n20 = models[i][3];
6013  double n11 = models[i][4];
6014  double n02 = models[i][5];
6015  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
6016  }
6017  }
6018 
6019 #ifdef VISP_HAVE_OGRE
6020  if ((m_trackerType & EDGE_TRACKER)
6021  #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6022  || (m_trackerType & KLT_TRACKER)
6023  #endif
6024  ) {
6025  if (useOgre)
6026  faces.displayOgre(cMo);
6027  }
6028 #endif
6029 }
6030 
6031 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
6032 {
6033  std::vector<std::vector<double> > features;
6034 
6035  if (m_trackerType & EDGE_TRACKER) {
6036  //m_featuresToBeDisplayedEdge updated after computeVVS()
6037  features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
6038  }
6039 
6040 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6041  if (m_trackerType & KLT_TRACKER) {
6042  //m_featuresToBeDisplayedKlt updated after postTracking()
6043  features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
6044  }
6045 #endif
6046 
6047  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6048  //m_featuresToBeDisplayedDepthNormal updated after postTracking()
6049  features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(), m_featuresToBeDisplayedDepthNormal.end());
6050  }
6051 
6052  return features;
6053 }
6054 
6055 std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(unsigned int width, unsigned int height,
6056  const vpHomogeneousMatrix &cMo,
6057  const vpCameraParameters &cam,
6058  bool displayFullModel)
6059 {
6060  std::vector<std::vector<double> > models;
6061 
6062  //Do not add multiple times the same models
6063  if (m_trackerType == EDGE_TRACKER) {
6064  models = vpMbEdgeTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
6065  }
6066 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6067  else if (m_trackerType == KLT_TRACKER) {
6068  models = vpMbKltTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
6069  }
6070 #endif
6071  else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
6072  models = vpMbDepthNormalTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
6073  } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
6074  models = vpMbDepthDenseTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
6075  } else {
6076  //Edge and KLT trackers use the same primitives
6077  if (m_trackerType & EDGE_TRACKER) {
6078  std::vector<std::vector<double> > edgeModels = vpMbEdgeTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
6079  models.insert(models.end(), edgeModels.begin(), edgeModels.end());
6080  }
6081 
6082  //Depth dense and depth normal trackers use the same primitives
6083  if (m_trackerType & DEPTH_DENSE_TRACKER) {
6084  std::vector<std::vector<double> > depthDenseModels = vpMbDepthDenseTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
6085  models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
6086  }
6087  }
6088 
6089  return models;
6090 }
6091 
6092 void vpMbGenericTracker::TrackerWrapper::init(const vpImage<unsigned char> &I)
6093 {
6094  if (!modelInitialised) {
6095  throw vpException(vpException::fatalError, "model not initialized");
6096  }
6097 
6098  if (useScanLine || clippingFlag > 3)
6099  m_cam.computeFov(I.getWidth(), I.getHeight());
6100 
6101  bool reInitialisation = false;
6102  if (!useOgre) {
6103  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6104  } else {
6105 #ifdef VISP_HAVE_OGRE
6106  if (!faces.isOgreInitialised()) {
6108 
6110  faces.initOgre(m_cam);
6111  // Turn off Ogre config dialog display for the next call to this
6112  // function since settings are saved in the ogre.cfg file and used
6113  // during the next call
6114  ogreShowConfigDialog = false;
6115  }
6116 
6118 #else
6119  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
6120 #endif
6121  }
6122 
6123  if (useScanLine) {
6126  }
6127 
6128 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6129  if (m_trackerType & KLT_TRACKER)
6131 #endif
6132 
6133  if (m_trackerType & EDGE_TRACKER) {
6135 
6136  bool a = false;
6137  vpMbEdgeTracker::visibleFace(I, m_cMo, a); // should be useless, but keep it for nbvisiblepolygone
6138 
6139  initMovingEdge(I, m_cMo);
6140  }
6141 
6142  if (m_trackerType & DEPTH_NORMAL_TRACKER)
6143  vpMbDepthNormalTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthNormalTracker::init(I);
6144 
6145  if (m_trackerType & DEPTH_DENSE_TRACKER)
6146  vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthDenseTracker::init(I);
6147 }
6148 
6149 void vpMbGenericTracker::TrackerWrapper::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
6150  double radius, int idFace, const std::string &name)
6151 {
6152  if (m_trackerType & EDGE_TRACKER)
6153  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
6154 
6155 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6156  if (m_trackerType & KLT_TRACKER)
6157  vpMbKltTracker::initCircle(p1, p2, p3, radius, idFace, name);
6158 #endif
6159 }
6160 
6161 void vpMbGenericTracker::TrackerWrapper::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius,
6162  int idFace, const std::string &name)
6163 {
6164  if (m_trackerType & EDGE_TRACKER)
6165  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
6166 
6167 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6168  if (m_trackerType & KLT_TRACKER)
6169  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
6170 #endif
6171 }
6172 
6173 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(vpMbtPolygon &polygon)
6174 {
6175  if (m_trackerType & EDGE_TRACKER)
6177 
6178 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6179  if (m_trackerType & KLT_TRACKER)
6181 #endif
6182 
6183  if (m_trackerType & DEPTH_NORMAL_TRACKER)
6185 
6186  if (m_trackerType & DEPTH_DENSE_TRACKER)
6188 }
6189 
6190 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(vpMbtPolygon &polygon)
6191 {
6192  if (m_trackerType & EDGE_TRACKER)
6194 
6195 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6196  if (m_trackerType & KLT_TRACKER)
6198 #endif
6199 
6200  if (m_trackerType & DEPTH_NORMAL_TRACKER)
6202 
6203  if (m_trackerType & DEPTH_DENSE_TRACKER)
6205 }
6206 
6207 void vpMbGenericTracker::TrackerWrapper::initMbtTracking(const vpImage<unsigned char> *const ptr_I)
6208 {
6209  if (m_trackerType & EDGE_TRACKER) {
6212  }
6213 }
6214 
6215 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(const std::string &configFile)
6216 {
6217  // Load projection error config
6218  vpMbTracker::loadConfigFile(configFile);
6219 
6220 #ifdef VISP_HAVE_PUGIXML
6222 
6223  xmlp.setCameraParameters(m_cam);
6226 
6227  // Edge
6228  xmlp.setEdgeMe(me);
6229 
6230  // KLT
6231  xmlp.setKltMaxFeatures(10000);
6232  xmlp.setKltWindowSize(5);
6233  xmlp.setKltQuality(0.01);
6234  xmlp.setKltMinDistance(5);
6235  xmlp.setKltHarrisParam(0.01);
6236  xmlp.setKltBlockSize(3);
6237  xmlp.setKltPyramidLevels(3);
6238 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6239  xmlp.setKltMaskBorder(maskBorder);
6240 #endif
6241 
6242  // Depth normal
6243  xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
6244  xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
6245  xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
6246  xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
6247  xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
6248  xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
6249 
6250  // Depth dense
6251  xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
6252  xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
6253 
6254  try {
6255  std::cout << " *********** Parsing XML for";
6256 
6257  std::vector<std::string> tracker_names;
6258  if (m_trackerType & EDGE_TRACKER)
6259  tracker_names.push_back("Edge");
6260 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6261  if (m_trackerType & KLT_TRACKER)
6262  tracker_names.push_back("Klt");
6263 #endif
6264  if (m_trackerType & DEPTH_NORMAL_TRACKER)
6265  tracker_names.push_back("Depth Normal");
6266  if (m_trackerType & DEPTH_DENSE_TRACKER)
6267  tracker_names.push_back("Depth Dense");
6268 
6269  for (size_t i = 0; i < tracker_names.size(); i++) {
6270  std::cout << " " << tracker_names[i];
6271  if (i == tracker_names.size() - 1) {
6272  std::cout << " ";
6273  }
6274  }
6275 
6276  std::cout << "Model-Based Tracker ************ " << std::endl;
6277  xmlp.parse(configFile);
6278  } catch (...) {
6279  throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
6280  }
6281 
6282  vpCameraParameters camera;
6283  xmlp.getCameraParameters(camera);
6284  setCameraParameters(camera);
6285 
6288 
6289  if (xmlp.hasNearClippingDistance())
6291 
6292  if (xmlp.hasFarClippingDistance())
6294 
6295  if (xmlp.getFovClipping()) {
6297  }
6298 
6299  useLodGeneral = xmlp.getLodState();
6302 
6303  applyLodSettingInConfig = false;
6304  if (this->getNbPolygon() > 0) {
6305  applyLodSettingInConfig = true;
6309  }
6310 
6311  // Edge
6312  vpMe meParser;
6313  xmlp.getEdgeMe(meParser);
6315 
6316 // KLT
6317 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6318  tracker.setMaxFeatures((int)xmlp.getKltMaxFeatures());
6319  tracker.setWindowSize((int)xmlp.getKltWindowSize());
6320  tracker.setQuality(xmlp.getKltQuality());
6321  tracker.setMinDistance(xmlp.getKltMinDistance());
6322  tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
6323  tracker.setBlockSize((int)xmlp.getKltBlockSize());
6324  tracker.setPyramidLevels((int)xmlp.getKltPyramidLevels());
6325  maskBorder = xmlp.getKltMaskBorder();
6326 
6327  // if(useScanLine)
6328  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
6329 #endif
6330 
6331  // Depth normal
6337 
6338  // Depth dense
6340 #else
6341  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
6342 #endif
6343 }
6344 
6345 #ifdef VISP_HAVE_PCL
6346 void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
6347  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6348 {
6349 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6350  // KLT
6351  if (m_trackerType & KLT_TRACKER) {
6352  if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) {
6353  vpMbKltTracker::reinit(*ptr_I);
6354  }
6355  }
6356 #endif
6357 
6358  // Looking for new visible face
6359  if (m_trackerType & EDGE_TRACKER) {
6360  bool newvisibleface = false;
6361  vpMbEdgeTracker::visibleFace(*ptr_I, m_cMo, newvisibleface);
6362 
6363  if (useScanLine) {
6365  faces.computeScanLineRender(m_cam, ptr_I->getWidth(), ptr_I->getHeight());
6366  }
6367  }
6368 
6369  // Depth normal
6370  if (m_trackerType & DEPTH_NORMAL_TRACKER)
6371  vpMbDepthNormalTracker::computeVisibility(point_cloud->width, point_cloud->height);
6372 
6373  // Depth dense
6374  if (m_trackerType & DEPTH_DENSE_TRACKER)
6375  vpMbDepthDenseTracker::computeVisibility(point_cloud->width, point_cloud->height);
6376 
6377  // Edge
6378  if (m_trackerType & EDGE_TRACKER) {
6380 
6382  // Reinit the moving edge for the lines which need it.
6384 
6385  if (computeProjError) {
6387  }
6388  }
6389 }
6390 
6391 void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
6392  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6393 {
6394  if (m_trackerType & EDGE_TRACKER) {
6395  try {
6397  } catch (...) {
6398  std::cerr << "Error in moving edge tracking" << std::endl;
6399  throw;
6400  }
6401  }
6402 
6403 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6404  if (m_trackerType & KLT_TRACKER) {
6405  try {
6407  } catch (const vpException &e) {
6408  std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
6409  throw;
6410  }
6411  }
6412 #endif
6413 
6414  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6415  try {
6417  } catch (...) {
6418  std::cerr << "Error in Depth normal tracking" << std::endl;
6419  throw;
6420  }
6421  }
6422 
6423  if (m_trackerType & DEPTH_DENSE_TRACKER) {
6424  try {
6426  } catch (...) {
6427  std::cerr << "Error in Depth dense tracking" << std::endl;
6428  throw;
6429  }
6430  }
6431 }
6432 #endif
6433 
6434 void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
6435  const unsigned int pointcloud_width,
6436  const unsigned int pointcloud_height)
6437 {
6438 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6439  // KLT
6440  if (m_trackerType & KLT_TRACKER) {
6441  if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) {
6442  vpMbKltTracker::reinit(*ptr_I);
6443  }
6444  }
6445 #endif
6446 
6447  // Looking for new visible face
6448  if (m_trackerType & EDGE_TRACKER) {
6449  bool newvisibleface = false;
6450  vpMbEdgeTracker::visibleFace(*ptr_I, m_cMo, newvisibleface);
6451 
6452  if (useScanLine) {
6454  faces.computeScanLineRender(m_cam, ptr_I->getWidth(), ptr_I->getHeight());
6455  }
6456  }
6457 
6458  // Depth normal
6459  if (m_trackerType & DEPTH_NORMAL_TRACKER)
6460  vpMbDepthNormalTracker::computeVisibility(pointcloud_width, pointcloud_height);
6461 
6462  // Depth dense
6463  if (m_trackerType & DEPTH_DENSE_TRACKER)
6464  vpMbDepthDenseTracker::computeVisibility(pointcloud_width, pointcloud_height);
6465 
6466  // Edge
6467  if (m_trackerType & EDGE_TRACKER) {
6469 
6471  // Reinit the moving edge for the lines which need it.
6473 
6474  if (computeProjError) {
6476  }
6477  }
6478 }
6479 
6480 void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> * const ptr_I,
6481  const std::vector<vpColVector> * const point_cloud,
6482  const unsigned int pointcloud_width,
6483  const unsigned int pointcloud_height)
6484 {
6485  if (m_trackerType & EDGE_TRACKER) {
6486  try {
6488  } catch (...) {
6489  std::cerr << "Error in moving edge tracking" << std::endl;
6490  throw;
6491  }
6492  }
6493 
6494 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6495  if (m_trackerType & KLT_TRACKER) {
6496  try {
6498  } catch (const vpException &e) {
6499  std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
6500  throw;
6501  }
6502  }
6503 #endif
6504 
6505  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
6506  try {
6507  vpMbDepthNormalTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
6508  } catch (...) {
6509  std::cerr << "Error in Depth tracking" << std::endl;
6510  throw;
6511  }
6512  }
6513 
6514  if (m_trackerType & DEPTH_DENSE_TRACKER) {
6515  try {
6516  vpMbDepthDenseTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
6517  } catch (...) {
6518  std::cerr << "Error in Depth dense tracking" << std::endl;
6519  throw;
6520  }
6521  }
6522 }
6523 
6524 void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
6525  const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose,
6526  const vpHomogeneousMatrix &T)
6527 {
6528  m_cMo.eye();
6529 
6530  // Edge
6531  vpMbtDistanceLine *l;
6533  vpMbtDistanceCircle *ci;
6534 
6535  for (unsigned int i = 0; i < scales.size(); i++) {
6536  if (scales[i]) {
6537  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
6538  l = *it;
6539  if (l != NULL)
6540  delete l;
6541  l = NULL;
6542  }
6543 
6544  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
6545  ++it) {
6546  cy = *it;
6547  if (cy != NULL)
6548  delete cy;
6549  cy = NULL;
6550  }
6551 
6552  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
6553  ci = *it;
6554  if (ci != NULL)
6555  delete ci;
6556  ci = NULL;
6557  }
6558 
6559  lines[i].clear();
6560  cylinders[i].clear();
6561  circles[i].clear();
6562  }
6563  }
6564 
6565  nline = 0;
6566  ncylinder = 0;
6567  ncircle = 0;
6568  nbvisiblepolygone = 0;
6569 
6570 // KLT
6571 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6572 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
6573  if (cur != NULL) {
6574  cvReleaseImage(&cur);
6575  cur = NULL;
6576  }
6577 #endif
6578 
6579  // delete the Klt Polygon features
6580  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
6581  vpMbtDistanceKltPoints *kltpoly = *it;
6582  if (kltpoly != NULL) {
6583  delete kltpoly;
6584  }
6585  kltpoly = NULL;
6586  }
6587  kltPolygons.clear();
6588 
6589  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
6590  ++it) {
6591  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
6592  if (kltPolyCylinder != NULL) {
6593  delete kltPolyCylinder;
6594  }
6595  kltPolyCylinder = NULL;
6596  }
6597  kltCylinders.clear();
6598 
6599  // delete the structures used to display circles
6600  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
6601  ci = *it;
6602  if (ci != NULL) {
6603  delete ci;
6604  }
6605  ci = NULL;
6606  }
6607  circles_disp.clear();
6608 
6609  firstInitialisation = true;
6610 
6611 #endif
6612 
6613  // Depth normal
6614  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
6615  delete m_depthNormalFaces[i];
6616  m_depthNormalFaces[i] = NULL;
6617  }
6618  m_depthNormalFaces.clear();
6619 
6620  // Depth dense
6621  for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
6622  delete m_depthDenseFaces[i];
6623  m_depthDenseFaces[i] = NULL;
6624  }
6625  m_depthDenseFaces.clear();
6626 
6627  faces.reset();
6628 
6629  loadModel(cad_name, verbose, T);
6630  if (I) {
6631  initFromPose(*I, cMo);
6632  } else {
6633  initFromPose(*I_color, cMo);
6634  }
6635 }
6636 
6637 void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
6638  const vpHomogeneousMatrix &cMo, bool verbose,
6639  const vpHomogeneousMatrix &T)
6640 {
6641  reInitModel(&I, NULL, cad_name, cMo, verbose, T);
6642 }
6643 
6644 void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<vpRGBa> &I_color, const std::string &cad_name,
6645  const vpHomogeneousMatrix &cMo, bool verbose,
6646  const vpHomogeneousMatrix &T)
6647 {
6648  reInitModel(NULL, &I_color, cad_name, cMo, verbose, T);
6649 }
6650 
6651 void vpMbGenericTracker::TrackerWrapper::resetTracker()
6652 {
6654 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6656 #endif
6659 }
6660 
6661 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(const vpCameraParameters &cam)
6662 {
6663  m_cam = cam;
6664 
6666 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6668 #endif
6671 }
6672 
6673 void vpMbGenericTracker::TrackerWrapper::setClipping(const unsigned int &flags)
6674 {
6676 }
6677 
6678 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(const double &dist)
6679 {
6681 }
6682 
6683 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(const double &dist)
6684 {
6686 }
6687 
6688 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(const bool &v)
6689 {
6691 #ifdef VISP_HAVE_OGRE
6692  faces.getOgreContext()->setWindowName("TrackerWrapper");
6693 #endif
6694 }
6695 
6696 void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
6697  const vpHomogeneousMatrix &cdMo)
6698 {
6699  bool performKltSetPose = false;
6700  if (I_color) {
6701  vpImageConvert::convert(*I_color, m_I);
6702  }
6703 
6704 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6705  if (m_trackerType & KLT_TRACKER) {
6706  performKltSetPose = true;
6707 
6708  if (useScanLine || clippingFlag > 3) {
6709  m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
6710  }
6711 
6712  vpMbKltTracker::setPose(I ? *I : m_I, cdMo);
6713  }
6714 #endif
6715 
6716  if (!performKltSetPose) {
6717  m_cMo = cdMo;
6718  init(I ? *I : m_I);
6719  return;
6720  }
6721 
6722  if (m_trackerType & EDGE_TRACKER)
6723  resetMovingEdge();
6724 
6725  if (useScanLine) {
6728  }
6729 
6730 #if 0
6731  if (m_trackerType & EDGE_TRACKER) {
6732  initPyramid(I, Ipyramid);
6733 
6734  unsigned int i = (unsigned int) scales.size();
6735  do {
6736  i--;
6737  if(scales[i]){
6738  downScale(i);
6739  initMovingEdge(*Ipyramid[i], cMo);
6740  upScale(i);
6741  }
6742  } while(i != 0);
6743 
6744  cleanPyramid(Ipyramid);
6745  }
6746 #else
6747  if (m_trackerType & EDGE_TRACKER)
6748  initMovingEdge(I ? *I : m_I, m_cMo);
6749 #endif
6750 
6751  // Depth normal
6752  vpMbDepthNormalTracker::computeVisibility(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
6753 
6754  // Depth dense
6755  vpMbDepthDenseTracker::computeVisibility(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
6756 }
6757 
6758 void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
6759 {
6760  setPose(&I, NULL, cdMo);
6761 }
6762 
6763 void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo)
6764 {
6765  setPose(NULL, &I_color, cdMo);
6766 }
6767 
6768 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(const bool &flag)
6769 {
6771 }
6772 
6773 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(const bool &v)
6774 {
6776 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6778 #endif
6781 }
6782 
6783 void vpMbGenericTracker::TrackerWrapper::setTrackerType(int type)
6784 {
6785  if ((type & (EDGE_TRACKER |
6786 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6787  KLT_TRACKER |
6788 #endif
6790  throw vpException(vpTrackingException::badValue, "bad value for tracker type: !", type);
6791  }
6792 
6793  m_trackerType = type;
6794 }
6795 
6796 void vpMbGenericTracker::TrackerWrapper::testTracking()
6797 {
6798  if (m_trackerType & EDGE_TRACKER) {
6800  }
6801 }
6802 
6803 void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> &
6804 #if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6805  I
6806 #endif
6807 )
6808 {
6809  if ((m_trackerType & (EDGE_TRACKER
6810 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6811  | KLT_TRACKER
6812 #endif
6813  )) == 0) {
6814  std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
6815  return;
6816  }
6817 
6818 #if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
6819  track(&I, nullptr);
6820 #endif
6821 }
6822 
6823 void vpMbGenericTracker::TrackerWrapper::track(const vpImage<vpRGBa> &)
6824 {
6825  //not exposed to the public API, only for debug
6826 }
6827 
6828 #ifdef VISP_HAVE_PCL
6829 void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> *const ptr_I,
6830  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
6831 {
6832  if ((m_trackerType & (EDGE_TRACKER |
6833 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6834  KLT_TRACKER |
6835 #endif
6837  std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
6838  return;
6839  }
6840 
6841  if (m_trackerType & (EDGE_TRACKER
6842 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
6843  | KLT_TRACKER
6844 #endif
6845  ) &&
6846  ptr_I == NULL) {
6847  throw vpException(vpException::fatalError, "Image pointer is NULL!");
6848  }
6849 
6850  if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && ! point_cloud) { // point_cloud == nullptr
6851  throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
6852  }
6853 
6854  // Back-up cMo in case of exception
6855  vpHomogeneousMatrix cMo_1 = m_cMo;
6856  try {
6857  preTracking(ptr_I, point_cloud);
6858 
6859  try {
6860  computeVVS(ptr_I);
6861  } catch (...) {
6862  covarianceMatrix = -1;
6863  throw; // throw the original exception
6864  }
6865 
6866  if (m_trackerType == EDGE_TRACKER)
6867  testTracking();
6868 
6869  postTracking(ptr_I, point_cloud);
6870 
6871  } catch (const vpException &e) {
6872  std::cerr << "Exception: " << e.what() << std::endl;
6873  m_cMo = cMo_1;
6874  throw; // rethrowing the original exception
6875  }
6876 }
6877 #endif
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:252
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
unsigned int getDepthNormalSamplingStepY() const
virtual void setFeatureFactors(const std::map< vpTrackerType, double > &mapOfFeatureFactors)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeProjError
Definition: vpMbTracker.h:133
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
virtual void loadConfigFile(const std::string &configFile)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
void setMovingEdge(const vpMe &me)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setScanLineVisibilityTest(const bool &v)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
virtual void setScanLineVisibilityTest(const bool &v)
virtual void track(const vpImage< unsigned char > &I)
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual vpMbtPolygon * getPolygon(unsigned int index)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double threshold)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpColVector m_w
Robust weights.
int getDepthNormalPclPlaneEstimationMethod() const
void setKltQuality(const double &q)
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:476
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual std::list< vpMbtDistanceKltPoints * > & getFeaturesKlt()
void getCameraParameters(vpCameraParameters &cam) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual std::map< std::string, int > getCameraTrackerTypes() const
void setKltPyramidLevels(const unsigned int &pL)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void setTrackerType(int type)
virtual int getTrackerType() const
virtual void setDepthDenseFilteringMethod(int method)
Point removed because too near image borders.
Definition: vpMeSite.h:82
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void setDepthNormalSamplingStepY(unsigned int stepY)
vpMatrix AtA() const
Definition: vpMatrix.cpp:762
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void loadConfigFile(const std::string &configFile)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
Definition: vpMbTracker.h:553
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:300
Point removed due to a threshold problem.
Definition: vpMeSite.h:80
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
virtual unsigned int getNbPoints(unsigned int level=0) const
virtual void setClipping(const unsigned int &flags)
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
virtual void setDisplayFeatures(bool displayF)
double getDepthNormalPclPlaneEstimationRansacThreshold() const
bool modelInitialised
Definition: vpMbTracker.h:123
virtual void setOgreShowConfigDialog(bool showConfigDialog)
Definition: vpMbTracker.h:639
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void initClick(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2, bool displayHelp=false, const vpHomogeneousMatrix &T1=vpHomogeneousMatrix(), const vpHomogeneousMatrix &T2=vpHomogeneousMatrix())
virtual void setAngleDisappear(const double &a)
virtual void computeVVSInit()
virtual void setGoodMovingEdgesRatioThreshold(double threshold)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Manage a cylinder used in the model-based tracker.
vpMbScanLine & getMbScanLineRenderer()
unsigned int getRows() const
Definition: vpArray2D.h:289
Definition: vpMe.h:60
vpHomogeneousMatrix inverse() const
virtual void setMask(const vpImage< bool > &mask)
Definition: vpMbTracker.h:559
Manage the line of a polygon used in the model-based tracker.
virtual double getGoodMovingEdgesRatioThreshold() const
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void computeVVSInteractionMatrixAndResidu()
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
unsigned int getKltBlockSize() const
void setKltMaskBorder(const unsigned int &mb)
virtual void setMovingEdge(const vpMe &me)
virtual void setLod(bool useLod, const std::string &name="")
virtual int getKltNbPoints() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void setEdgeMe(const vpMe &ecm)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
static const vpColor green
Definition: vpColor.h:220
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void updateMovingEdge(const vpImage< unsigned char > &I)
std::map< vpTrackerType, double > m_mapOfFeatureFactors
Ponderation between each feature type in the VVS stage.
std::map< std::string, TrackerWrapper * > m_mapOfTrackers
virtual void computeVVS(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages)
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
unsigned int getDepthDenseSamplingStepY() const
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
Definition: vpMbTracker.h:590
virtual void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
virtual std::string getReferenceCameraName() const
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpMatrix m_L
Interaction matrix.
unsigned int getCols() const
Definition: vpArray2D.h:279
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
double projectionError
Definition: vpMbTracker.h:136
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:419
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void getLcylinder(std::list< vpMbtDistanceCylinder *> &cylindersList, unsigned int level=0) const
Manage a circle used in the model-based tracker.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setKltMaskBorder(const unsigned int &e)
void computeVisibility(unsigned int width, unsigned int height)
vpColVector m_weightedError
Weighted error.
virtual void setCameraParameters(const vpCameraParameters &cam)
Definition: vpMbTracker.h:483
Error that can be emited by the vpTracker class and its derivates.
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
Point used by the tracker.
Definition: vpMeSite.h:78
static const vpColor cyan
Definition: vpColor.h:226
vp_deprecated void init()
virtual void setScanLineVisibilityTest(const bool &v)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setAngleDisappear(const double &adisappear)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void setProjectionErrorComputation(const bool &flag)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual std::vector< cv::Point2f > getKltPoints() const
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
virtual std::list< vpMbtDistanceCircle * > & getFeaturesCircle()
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setKltMinDistance(const double &mD)
static double sqr(double x)
Definition: vpMath.h:116
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
Point removed due to a contrast problem.
Definition: vpMeSite.h:79
virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual unsigned int getNbPolygon() const
void setDepthNormalSamplingStepX(unsigned int stepX)
virtual void testTracking()
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
Generic class defining intrinsic camera parameters.
virtual unsigned int getKltMaskBorder() const
unsigned int getKltWindowSize() const
void setOgreShowConfigDialog(bool showConfigDialog)
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
unsigned int getKltMaxFeatures() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void setKltThresholdAcceptation(double th)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void setAngleAppear(const double &a)
virtual double getKltThresholdAcceptation() const
virtual void computeVVSInteractionMatrixAndResidu()
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void setFarClippingDistance(const double &dist)
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:465
virtual void getLline(std::list< vpMbtDistanceLine *> &linesList, unsigned int level=0) const
void setDepthDenseSamplingStepY(unsigned int stepY)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getDepthNormalSamplingStepX() const
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
virtual std::vector< std::vector< double > > getFeaturesForDisplay()
void computeVisibility(unsigned int width, unsigned int height)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
void setKltWindowSize(const unsigned int &w)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
virtual void setScanLineVisibilityTest(const bool &v)
virtual void computeVVSWeights()
void setAngleAppear(const double &aappear)
const char * what() const
static double rad(double deg)
Definition: vpMath.h:110
virtual void computeVVSInteractionMatrixAndResidu()
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
virtual void setProjectionErrorDisplay(bool display)
Definition: vpMbTracker.h:585
void setDepthDenseSamplingStepX(unsigned int stepX)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
void computeProjectionError(const vpImage< unsigned char > &_I)
void insert(unsigned int i, const vpColVector &v)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setNearClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
Definition: vpMbTracker.h:595
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
virtual void setScanLineVisibilityTest(const bool &v)
void getEdgeMe(vpMe &ecm) const
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:191
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
static double deg(double rad)
Definition: vpMath.h:103
virtual void setOgreVisibilityTest(const bool &v)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:188
virtual void initFromPoints(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2)
void setCameraParameters(const vpCameraParameters &cam)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
void preTracking(const vpImage< unsigned char > &I)
vpColVector m_error
(s - s*)
virtual void init(const vpImage< unsigned char > &I)
virtual void setDepthDenseFilteringMinDistance(double minDistance)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementatio