Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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(const unsigned int nbCameras, const 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 
221 {
222  if (computeProjError) {
223  double rawTotalProjectionError = 0.0;
224  unsigned int nbTotalFeaturesUsed = 0;
225 
226  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
227  it != m_mapOfTrackers.end(); ++it) {
228  TrackerWrapper *tracker = it->second;
229 
230  double curProjError = tracker->getProjectionError();
231  unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
232 
233  if (nbFeaturesUsed > 0) {
234  nbTotalFeaturesUsed += nbFeaturesUsed;
235  rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed);
236  }
237  }
238 
239  if (nbTotalFeaturesUsed > 0) {
240  projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
241  } else {
242  projectionError = 90.0;
243  }
244  } else {
245  projectionError = 90.0;
246  }
247 }
248 
249 void vpMbGenericTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
250 {
251  computeVVSInit(mapOfImages);
252 
253  if (m_error.getRows() < 4) {
254  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
255  }
256 
257  double normRes = 0;
258  double normRes_1 = -1;
259  unsigned int iter = 0;
260 
261  vpMatrix LTL;
262  vpColVector LTR, v;
263  vpColVector error_prev;
264 
265  double mu = m_initialMu;
266  vpHomogeneousMatrix cMo_prev;
267 
268  bool isoJoIdentity_ = true;
269 
270  // Covariance
271  vpColVector W_true(m_error.getRows());
272  vpMatrix L_true, LVJ_true;
273 
274  // Create the map of VelocityTwistMatrices
275  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
276  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
277  it != m_mapOfCameraTransformationMatrix.end(); ++it) {
279  cVo.buildFrom(it->second);
280  mapOfVelocityTwist[it->first] = cVo;
281  }
282 
283  double factorEdge = m_mapOfFeatureFactors[EDGE_TRACKER];
284 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
285  double factorKlt = m_mapOfFeatureFactors[KLT_TRACKER];
286 #endif
287  double factorDepth = m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER];
288  double factorDepthDense = m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER];
289 
290  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
291  computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
292 
293  bool reStartFromLastIncrement = false;
294  computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
295  if (reStartFromLastIncrement) {
296  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
297  it != m_mapOfTrackers.end(); ++it) {
298  TrackerWrapper *tracker = it->second;
299 
300  tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo_prev;
301 
302 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
303  vpHomogeneousMatrix c_curr_tTc_curr0 =
304  m_mapOfCameraTransformationMatrix[it->first] * cMo_prev * tracker->c0Mo.inverse();
305  tracker->ctTc0 = c_curr_tTc_curr0;
306 #endif
307  }
308  }
309 
310  if (!reStartFromLastIncrement) {
312 
313  if (computeCovariance) {
314  L_true = m_L;
315  if (!isoJoIdentity_) {
317  cVo.buildFrom(cMo);
318  LVJ_true = (m_L * (cVo * oJo));
319  }
320  }
321 
323  if (iter == 0) {
324  isoJoIdentity_ = true;
325  oJo.eye();
326 
327  // If all the 6 dof should be estimated, we check if the interaction
328  // matrix is full rank. If not we remove automatically the dof that
329  // cannot be estimated This is particularly useful when consering
330  // circles (rank 5) and cylinders (rank 4)
331  if (isoJoIdentity_) {
332  cVo.buildFrom(cMo);
333 
334  vpMatrix K; // kernel
335  unsigned int rank = (m_L * cVo).kernel(K);
336  if (rank == 0) {
337  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
338  }
339 
340  if (rank != 6) {
341  vpMatrix I; // Identity
342  I.eye(6);
343  oJo = I - K.AtA();
344 
345  isoJoIdentity_ = false;
346  }
347  }
348  }
349 
350  // Weighting
351  double num = 0;
352  double den = 0;
353 
354  unsigned int start_index = 0;
355  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
356  it != m_mapOfTrackers.end(); ++it) {
357  TrackerWrapper *tracker = it->second;
358 
359  if (tracker->m_trackerType & EDGE_TRACKER) {
360  for (unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
361  double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
362  W_true[start_index + i] = wi;
363  m_weightedError[start_index + i] = wi * m_error[start_index + i];
364 
365  num += wi * vpMath::sqr(m_error[start_index + i]);
366  den += wi;
367 
368  for (unsigned int j = 0; j < m_L.getCols(); j++) {
369  m_L[start_index + i][j] *= wi;
370  }
371  }
372 
373  start_index += tracker->m_error_edge.getRows();
374  }
375 
376 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
377  if (tracker->m_trackerType & KLT_TRACKER) {
378  for (unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
379  double wi = tracker->m_w_klt[i] * factorKlt;
380  W_true[start_index + i] = wi;
381  m_weightedError[start_index + i] = wi * m_error[start_index + i];
382 
383  num += wi * vpMath::sqr(m_error[start_index + i]);
384  den += wi;
385 
386  for (unsigned int j = 0; j < m_L.getCols(); j++) {
387  m_L[start_index + i][j] *= wi;
388  }
389  }
390 
391  start_index += tracker->m_error_klt.getRows();
392  }
393 #endif
394 
395  if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
396  for (unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
397  double wi = tracker->m_w_depthNormal[i] * factorDepth;
398  W_true[start_index + i] = wi;
399  m_weightedError[start_index + i] = wi * m_error[start_index + i];
400 
401  num += wi * vpMath::sqr(m_error[start_index + i]);
402  den += wi;
403 
404  for (unsigned int j = 0; j < m_L.getCols(); j++) {
405  m_L[start_index + i][j] *= wi;
406  }
407  }
408 
409  start_index += tracker->m_error_depthNormal.getRows();
410  }
411 
412  if (tracker->m_trackerType & DEPTH_DENSE_TRACKER) {
413  for (unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
414  double wi = tracker->m_w_depthDense[i] * factorDepthDense;
415  W_true[start_index + i] = wi;
416  m_weightedError[start_index + i] = wi * m_error[start_index + i];
417 
418  num += wi * vpMath::sqr(m_error[start_index + i]);
419  den += wi;
420 
421  for (unsigned int j = 0; j < m_L.getCols(); j++) {
422  m_L[start_index + i][j] *= wi;
423  }
424  }
425 
426  start_index += tracker->m_error_depthDense.getRows();
427  }
428  }
429 
430  normRes_1 = normRes;
431  normRes = sqrt(num / den);
432 
433  computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
434 
435  cMo_prev = cMo;
436 
438 
439 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
440  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
441  it != m_mapOfTrackers.end(); ++it) {
442  TrackerWrapper *tracker = it->second;
443 
444  vpHomogeneousMatrix c_curr_tTc_curr0 =
445  m_mapOfCameraTransformationMatrix[it->first] * cMo * tracker->c0Mo.inverse();
446  tracker->ctTc0 = c_curr_tTc_curr0;
447  }
448 #endif
449 
450  // Update cMo
451  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
452  it != m_mapOfTrackers.end(); ++it) {
453  TrackerWrapper *tracker = it->second;
454  tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
455  }
456  }
457 
458  iter++;
459  }
460 
461  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
462 
463  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
464  it != m_mapOfTrackers.end(); ++it) {
465  TrackerWrapper *tracker = it->second;
466 
467  if (tracker->m_trackerType & EDGE_TRACKER) {
468  tracker->updateMovingEdgeWeights();
469  }
470  }
471 }
472 
474 {
475  throw vpException(vpException::fatalError, "vpMbGenericTracker::computeVVSInit() should not be called!");
476 }
477 
478 void vpMbGenericTracker::computeVVSInit(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
479 {
480  unsigned int nbFeatures = 0;
481 
482  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
483  it != m_mapOfTrackers.end(); ++it) {
484  TrackerWrapper *tracker = it->second;
485  tracker->computeVVSInit(mapOfImages[it->first]);
486 
487  nbFeatures += tracker->m_error.getRows();
488  }
489 
490  m_L.resize(nbFeatures, 6, false, false);
491  m_error.resize(nbFeatures, false);
492 
493  m_weightedError.resize(nbFeatures, false);
494  m_w.resize(nbFeatures, false);
495  m_w = 1;
496 }
497 
499 {
500  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
501  "computeVVSInteractionMatrixAndR"
502  "esidu() should not be called");
503 }
504 
506  std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
507  std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
508 {
509  unsigned int start_index = 0;
510 
511  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
512  it != m_mapOfTrackers.end(); ++it) {
513  TrackerWrapper *tracker = it->second;
514 
515  tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
516 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
517  vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * cMo * tracker->c0Mo.inverse();
518  tracker->ctTc0 = c_curr_tTc_curr0;
519 #endif
520 
521  tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
522 
523  m_L.insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
524  m_error.insert(start_index, tracker->m_error);
525 
526  start_index += tracker->m_error.getRows();
527  }
528 }
529 
531 {
532  unsigned int start_index = 0;
533 
534  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
535  it != m_mapOfTrackers.end(); ++it) {
536  TrackerWrapper *tracker = it->second;
537  tracker->computeVVSWeights();
538 
539  m_w.insert(start_index, tracker->m_w);
540  start_index += tracker->m_w.getRows();
541  }
542 }
543 
558  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
559  const bool displayFullModel)
560 {
561  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
562  if (it != m_mapOfTrackers.end()) {
563  TrackerWrapper *tracker = it->second;
564  tracker->display(I, cMo_, cam_, col, thickness, displayFullModel);
565  } else {
566  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
567  }
568 }
569 
584  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
585  const 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 
613  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
614  const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &color,
615  const unsigned int thickness, const bool displayFullModel)
616 {
617  if (m_mapOfTrackers.size() == 2) {
618  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
619  it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
620  ++it;
621 
622  it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
623  } else {
624  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
625  << std::endl;
626  }
627 }
628 
646  const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1,
647  const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness,
648  const bool displayFullModel)
649 {
650  if (m_mapOfTrackers.size() == 2) {
651  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
652  it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
653  ++it;
654 
655  it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
656  } else {
657  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
658  << std::endl;
659  }
660 }
661 
673 void vpMbGenericTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
674  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
675  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
676  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
677 {
678  // Display only for the given images
679  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
680  it_img != mapOfImages.end(); ++it_img) {
681  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
682  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
683  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
684 
685  if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
686  it_cam != mapOfCameraParameters.end()) {
687  TrackerWrapper *tracker = it_tracker->second;
688  tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
689  } else {
690  std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
691  }
692  }
693 }
694 
706 void vpMbGenericTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
707  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
708  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
709  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
710 {
711  // Display only for the given images
712  for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
713  it_img != mapOfImages.end(); ++it_img) {
714  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
715  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
716  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
717 
718  if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
719  it_cam != mapOfCameraParameters.end()) {
720  TrackerWrapper *tracker = it_tracker->second;
721  tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
722  } else {
723  std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
724  }
725  }
726 }
727 
733 std::vector<std::string> vpMbGenericTracker::getCameraNames() const
734 {
735  std::vector<std::string> cameraNames;
736 
737  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
738  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
739  cameraNames.push_back(it_tracker->first);
740  }
741 
742  return cameraNames;
743 }
744 
754 {
755  if (m_mapOfTrackers.size() == 2) {
756  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
757  it->second->getCameraParameters(cam1);
758  ++it;
759 
760  it->second->getCameraParameters(cam2);
761  } else {
762  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
763  << std::endl;
764  }
765 }
766 
772 void vpMbGenericTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
773 {
774  // Clear the input map
775  mapOfCameraParameters.clear();
776 
777  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
778  it != m_mapOfTrackers.end(); ++it) {
779  vpCameraParameters cam_;
780  it->second->getCameraParameters(cam_);
781  mapOfCameraParameters[it->first] = cam_;
782  }
783 }
784 
791 std::map<std::string, int> vpMbGenericTracker::getCameraTrackerTypes() const
792 {
793  std::map<std::string, int> trackingTypes;
794 
795  TrackerWrapper *traker;
796  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
797  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
798  traker = it_tracker->second;
799  trackingTypes[it_tracker->first] = traker->getTrackerType();
800  }
801 
802  return trackingTypes;
803 }
804 
813 void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const
814 {
815  if (m_mapOfTrackers.size() == 2) {
816  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
817  clippingFlag1 = it->second->getClipping();
818  ++it;
819 
820  clippingFlag2 = it->second->getClipping();
821  } else {
822  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
823  << std::endl;
824  }
825 }
826 
832 void vpMbGenericTracker::getClipping(std::map<std::string, unsigned int> &mapOfClippingFlags) const
833 {
834  mapOfClippingFlags.clear();
835 
836  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
837  it != m_mapOfTrackers.end(); ++it) {
838  TrackerWrapper *tracker = it->second;
839  mapOfClippingFlags[it->first] = tracker->getClipping();
840  }
841 }
842 
849 {
850  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
851  if (it != m_mapOfTrackers.end()) {
852  return it->second->getFaces();
853  }
854 
855  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found!" << std::endl;
856  return faces;
857 }
858 
865 {
866  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
867  if (it != m_mapOfTrackers.end()) {
868  return it->second->getFaces();
869  }
870 
871  std::cerr << "The camera: " << cameraName << " cannot be found!" << std::endl;
872  return faces;
873 }
874 
875 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
876 
879 std::list<vpMbtDistanceCircle *> &vpMbGenericTracker::getFeaturesCircle()
880 {
881  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
882  if (it != m_mapOfTrackers.end()) {
883  TrackerWrapper *tracker = it->second;
884  return tracker->getFeaturesCircle();
885  } else {
886  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
887  m_referenceCameraName.c_str());
888  }
889 }
890 
894 std::list<vpMbtDistanceKltCylinder *> &vpMbGenericTracker::getFeaturesKltCylinder()
895 {
896  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
897  if (it != m_mapOfTrackers.end()) {
898  TrackerWrapper *tracker = it->second;
899  return tracker->getFeaturesKltCylinder();
900  } else {
901  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
902  m_referenceCameraName.c_str());
903  }
904 }
905 
909 std::list<vpMbtDistanceKltPoints *> &vpMbGenericTracker::getFeaturesKlt()
910 {
911  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
912  if (it != m_mapOfTrackers.end()) {
913  TrackerWrapper *tracker = it->second;
914  return tracker->getFeaturesKlt();
915  } else {
916  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
917  m_referenceCameraName.c_str());
918  }
919 }
920 #endif
921 
932 
933 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
934 
942 std::vector<vpImagePoint> vpMbGenericTracker::getKltImagePoints() const
943 {
944  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
945  if (it != m_mapOfTrackers.end()) {
946  TrackerWrapper *tracker = it->second;
947  return tracker->getKltImagePoints();
948  } else {
949  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
950  }
951 
952  return std::vector<vpImagePoint>();
953 }
954 
963 std::map<int, vpImagePoint> vpMbGenericTracker::getKltImagePointsWithId() const
964 {
965  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
966  if (it != m_mapOfTrackers.end()) {
967  TrackerWrapper *tracker = it->second;
968  return tracker->getKltImagePointsWithId();
969  } else {
970  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
971  }
972 
973  return std::map<int, vpImagePoint>();
974 }
975 
982 {
983  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
984  if (it != m_mapOfTrackers.end()) {
985  TrackerWrapper *tracker = it->second;
986  return tracker->getKltMaskBorder();
987  } else {
988  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
989  }
990 
991  return 0;
992 }
993 
1000 {
1001  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1002  if (it != m_mapOfTrackers.end()) {
1003  TrackerWrapper *tracker = it->second;
1004  return tracker->getKltNbPoints();
1005  } else {
1006  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1007  }
1008 
1009  return 0;
1010 }
1011 
1018 {
1019  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
1020 
1021  if (it_tracker != m_mapOfTrackers.end()) {
1022  TrackerWrapper *tracker;
1023  tracker = it_tracker->second;
1024  return tracker->getKltOpencv();
1025  } else {
1026  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1027  }
1028 
1029  return vpKltOpencv();
1030 }
1031 
1041 {
1042  if (m_mapOfTrackers.size() == 2) {
1043  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1044  klt1 = it->second->getKltOpencv();
1045  ++it;
1046 
1047  klt2 = it->second->getKltOpencv();
1048  } else {
1049  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
1050  << std::endl;
1051  }
1052 }
1053 
1059 void vpMbGenericTracker::getKltOpencv(std::map<std::string, vpKltOpencv> &mapOfKlts) const
1060 {
1061  mapOfKlts.clear();
1062 
1063  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1064  it != m_mapOfTrackers.end(); ++it) {
1065  TrackerWrapper *tracker = it->second;
1066  mapOfKlts[it->first] = tracker->getKltOpencv();
1067  }
1068 }
1069 
1070 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
1071 
1076 std::vector<cv::Point2f> vpMbGenericTracker::getKltPoints() const
1077 {
1078  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1079  if (it != m_mapOfTrackers.end()) {
1080  TrackerWrapper *tracker = it->second;
1081  return tracker->getKltPoints();
1082  } else {
1083  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1084  }
1085 
1086  return std::vector<cv::Point2f>();
1087 }
1088 #endif
1089 
1097 #endif
1098 
1111 void vpMbGenericTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList,
1112  const unsigned int level) const
1113 {
1114  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1115  if (it != m_mapOfTrackers.end()) {
1116  it->second->getLcircle(circlesList, level);
1117  } else {
1118  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1119  }
1120 }
1121 
1135 void vpMbGenericTracker::getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
1136  const unsigned int level) const
1137 {
1138  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1139  if (it != m_mapOfTrackers.end()) {
1140  it->second->getLcircle(circlesList, level);
1141  } else {
1142  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1143  }
1144 }
1145 
1158 void vpMbGenericTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList,
1159  const unsigned int level) const
1160 {
1161  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1162  if (it != m_mapOfTrackers.end()) {
1163  it->second->getLcylinder(cylindersList, level);
1164  } else {
1165  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1166  }
1167 }
1168 
1182 void vpMbGenericTracker::getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
1183  const unsigned int level) const
1184 {
1185  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1186  if (it != m_mapOfTrackers.end()) {
1187  it->second->getLcylinder(cylindersList, level);
1188  } else {
1189  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1190  }
1191 }
1192 
1205 void vpMbGenericTracker::getLline(std::list<vpMbtDistanceLine *> &linesList,
1206  const unsigned int level) const
1207 {
1208  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1209 
1210  if (it != m_mapOfTrackers.end()) {
1211  it->second->getLline(linesList, level);
1212  } else {
1213  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1214  }
1215 }
1216 
1230 void vpMbGenericTracker::getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
1231  const unsigned int level) const
1232 {
1233  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1234  if (it != m_mapOfTrackers.end()) {
1235  it->second->getLline(linesList, level);
1236  } else {
1237  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1238  }
1239 }
1240 
1247 {
1248  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1249 
1250  if (it != m_mapOfTrackers.end()) {
1251  return it->second->getMovingEdge();
1252  } else {
1253  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1254  }
1255 
1256  return vpMe();
1257 }
1258 
1268 {
1269  if (m_mapOfTrackers.size() == 2) {
1270  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1271  it->second->getMovingEdge(me1);
1272  ++it;
1273 
1274  it->second->getMovingEdge(me2);
1275  } else {
1276  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
1277  << std::endl;
1278  }
1279 }
1280 
1286 void vpMbGenericTracker::getMovingEdge(std::map<std::string, vpMe> &mapOfMovingEdges) const
1287 {
1288  mapOfMovingEdges.clear();
1289 
1290  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1291  it != m_mapOfTrackers.end(); ++it) {
1292  TrackerWrapper *tracker = it->second;
1293  mapOfMovingEdges[it->first] = tracker->getMovingEdge();
1294  }
1295 }
1296 
1312 unsigned int vpMbGenericTracker::getNbPoints(const unsigned int level) const
1313 {
1314  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1315 
1316  unsigned int nbGoodPoints = 0;
1317  if (it != m_mapOfTrackers.end()) {
1318 
1319  nbGoodPoints = it->second->getNbPoints(level);
1320  } else {
1321  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1322  }
1323 
1324  return nbGoodPoints;
1325 }
1326 
1341 void vpMbGenericTracker::getNbPoints(std::map<std::string, unsigned int> &mapOfNbPoints, const unsigned int level) const
1342 {
1343  mapOfNbPoints.clear();
1344 
1345  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1346  it != m_mapOfTrackers.end(); ++it) {
1347  TrackerWrapper *tracker = it->second;
1348  mapOfNbPoints[it->first] = tracker->getNbPoints(level);
1349  }
1350 }
1351 
1358 {
1359  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1360  if (it != m_mapOfTrackers.end()) {
1361  return it->second->getNbPolygon();
1362  }
1363 
1364  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1365  return 0;
1366 }
1367 
1373 void vpMbGenericTracker::getNbPolygon(std::map<std::string, unsigned int> &mapOfNbPolygons) const
1374 {
1375  mapOfNbPolygons.clear();
1376 
1377  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1378  it != m_mapOfTrackers.end(); ++it) {
1379  TrackerWrapper *tracker = it->second;
1380  mapOfNbPolygons[it->first] = tracker->getNbPolygon();
1381  }
1382 }
1383 
1395 {
1396  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1397  if (it != m_mapOfTrackers.end()) {
1398  return it->second->getPolygon(index);
1399  }
1400 
1401  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
1402  return NULL;
1403 }
1404 
1416 vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, const unsigned int index)
1417 {
1418  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
1419  if (it != m_mapOfTrackers.end()) {
1420  return it->second->getPolygon(index);
1421  }
1422 
1423  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
1424  return NULL;
1425 }
1426 
1442 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
1443 vpMbGenericTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
1444 {
1445  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
1446 
1447  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1448  if (it != m_mapOfTrackers.end()) {
1449  TrackerWrapper *tracker = it->second;
1450  polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1451  } else {
1452  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
1453  }
1454 
1455  return polygonFaces;
1456 }
1457 
1475 void vpMbGenericTracker::getPolygonFaces(std::map<std::string, std::vector<vpPolygon> > &mapOfPolygons,
1476  std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
1477  const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
1478 {
1479  mapOfPolygons.clear();
1480  mapOfPoints.clear();
1481 
1482  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1483  it != m_mapOfTrackers.end(); ++it) {
1484  TrackerWrapper *tracker = it->second;
1485  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
1486  tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
1487 
1488  mapOfPolygons[it->first] = polygonFaces.first;
1489  mapOfPoints[it->first] = polygonFaces.second;
1490  }
1491 }
1492 
1502 {
1503  if (m_mapOfTrackers.size() == 2) {
1504  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1505  it->second->getPose(c1Mo);
1506  ++it;
1507 
1508  it->second->getPose(c2Mo);
1509  } else {
1510  std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
1511  << std::endl;
1512  }
1513 }
1514 
1520 void vpMbGenericTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
1521 {
1522  // Clear the map
1523  mapOfCameraPoses.clear();
1524 
1525  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1526  it != m_mapOfTrackers.end(); ++it) {
1527  TrackerWrapper *tracker = it->second;
1528  tracker->getPose(mapOfCameraPoses[it->first]);
1529  }
1530 }
1531 
1536 {
1537  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
1538  if (it != m_mapOfTrackers.end()) {
1539  TrackerWrapper *tracker = it->second;
1540  return tracker->getTrackerType();
1541  } else {
1542  throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
1543  m_referenceCameraName.c_str());
1544  }
1545 }
1546 
1548 {
1549  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1550  it != m_mapOfTrackers.end(); ++it) {
1551  TrackerWrapper *tracker = it->second;
1552  tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
1553  tracker->init(I);
1554  }
1555 }
1556 
1557 void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
1558  const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
1559 {
1560  throw vpException(vpException::fatalError, "vpMbGenericTracker::initCircle() should not be called!");
1561 }
1562 
1563 #ifdef VISP_HAVE_MODULE_GUI
1564 
1608  const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
1609  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
1610 {
1611  if (m_mapOfTrackers.size() == 2) {
1612  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1613  TrackerWrapper *tracker = it->second;
1614  tracker->initClick(I1, initFile1, displayHelp, T1);
1615 
1616  ++it;
1617 
1618  tracker = it->second;
1619  tracker->initClick(I2, initFile2, displayHelp, T2);
1620 
1622  if (it != m_mapOfTrackers.end()) {
1623  tracker = it->second;
1624 
1625  // Set the reference cMo
1626  tracker->getPose(cMo);
1627  }
1628  } else {
1630  "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size());
1631  }
1632 }
1633 
1675 void vpMbGenericTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1676  const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp,
1677  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
1678 {
1679  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
1680  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1681  mapOfImages.find(m_referenceCameraName);
1682  std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
1683 
1684  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1685  TrackerWrapper *tracker = it_tracker->second;
1686  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
1687  if (it_T != mapOfT.end())
1688  tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
1689  else
1690  tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1691  tracker->getPose(cMo);
1692  } else {
1693  throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera!");
1694  }
1695 
1696  // Vector of missing initFile for cameras
1697  std::vector<std::string> vectorOfMissingCameraPoses;
1698 
1699  // Set pose for the specified cameras
1700  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
1701  if (it_tracker->first != m_referenceCameraName) {
1702  it_img = mapOfImages.find(it_tracker->first);
1703  it_initFile = mapOfInitFiles.find(it_tracker->first);
1704 
1705  if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1706  // InitClick for the current camera
1707  TrackerWrapper *tracker = it_tracker->second;
1708  tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
1709  } else {
1710  vectorOfMissingCameraPoses.push_back(it_tracker->first);
1711  }
1712  }
1713  }
1714 
1715  // Init for cameras that do not have an initFile
1716  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
1717  it != vectorOfMissingCameraPoses.end(); ++it) {
1718  it_img = mapOfImages.find(*it);
1719  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1721 
1722  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1723  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1724  m_mapOfTrackers[*it]->cMo = cCurrentMo;
1725  m_mapOfTrackers[*it]->init(*it_img->second);
1726  } else {
1728  "Missing image or missing camera transformation "
1729  "matrix! Cannot set the pose for camera: %s!",
1730  it->c_str());
1731  }
1732  }
1733 }
1734 #endif
1735 
1736 void vpMbGenericTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
1737  const int /*idFace*/, const std::string & /*name*/)
1738 {
1739  throw vpException(vpException::fatalError, "vpMbGenericTracker::initCylinder() should not be called!");
1740 }
1741 
1743 {
1744  throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromCorners() should not be called!");
1745 }
1746 
1748 {
1749  throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromLines() should not be called!");
1750 }
1751 
1782  const std::string &initFile1, const std::string &initFile2)
1783 {
1784  if (m_mapOfTrackers.size() == 2) {
1785  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1786  TrackerWrapper *tracker = it->second;
1787  tracker->initFromPoints(I1, initFile1);
1788 
1789  ++it;
1790 
1791  tracker = it->second;
1792  tracker->initFromPoints(I2, initFile2);
1793 
1795  if (it != m_mapOfTrackers.end()) {
1796  tracker = it->second;
1797 
1798  // Set the reference cMo
1799  tracker->getPose(cMo);
1800 
1801  // Set the reference camera parameters
1802  tracker->getCameraParameters(cam);
1803  }
1804  } else {
1806  "Cannot initFromPoints()! Require two cameras but "
1807  "there are %d cameras!",
1808  m_mapOfTrackers.size());
1809  }
1810 }
1811 
1812 void vpMbGenericTracker::initFromPoints(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1813  const std::map<std::string, std::string> &mapOfInitPoints)
1814 {
1815  // Set the reference cMo
1816  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
1817  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1818  mapOfImages.find(m_referenceCameraName);
1819  std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(m_referenceCameraName);
1820 
1821  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
1822  TrackerWrapper *tracker = it_tracker->second;
1823  tracker->initFromPoints(*it_img->second, it_initPoints->second);
1824  tracker->getPose(cMo);
1825  } else {
1826  throw vpException(vpTrackingException::initializationError, "Cannot initFromPoints() for the reference camera!");
1827  }
1828 
1829  // Vector of missing initPoints for cameras
1830  std::vector<std::string> vectorOfMissingCameraPoints;
1831 
1832  // Set pose for the specified cameras
1833  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
1834  it_img = mapOfImages.find(it_tracker->first);
1835  it_initPoints = mapOfInitPoints.find(it_tracker->first);
1836 
1837  if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
1838  // Set pose
1839  it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
1840  } else {
1841  vectorOfMissingCameraPoints.push_back(it_tracker->first);
1842  }
1843  }
1844 
1845  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
1846  it != vectorOfMissingCameraPoints.end(); ++it) {
1847  it_img = mapOfImages.find(*it);
1848  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1850 
1851  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1852  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1853  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
1854  } else {
1856  "Missing image or missing camera transformation "
1857  "matrix! Cannot init the pose for camera: %s!",
1858  it->c_str());
1859  }
1860  }
1861 }
1862 
1875  const std::string &initFile1, const std::string &initFile2)
1876 {
1877  if (m_mapOfTrackers.size() == 2) {
1878  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1879  TrackerWrapper *tracker = it->second;
1880  tracker->initFromPose(I1, initFile1);
1881 
1882  ++it;
1883 
1884  tracker = it->second;
1885  tracker->initFromPose(I2, initFile2);
1886 
1888  if (it != m_mapOfTrackers.end()) {
1889  tracker = it->second;
1890 
1891  // Set the reference cMo
1892  tracker->getPose(cMo);
1893 
1894  // Set the reference camera parameters
1895  tracker->getCameraParameters(cam);
1896  }
1897  } else {
1899  "Cannot initFromPose()! Require two cameras but there "
1900  "are %d cameras!",
1901  m_mapOfTrackers.size());
1902  }
1903 }
1904 
1919 void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1920  const std::map<std::string, std::string> &mapOfInitPoses)
1921 {
1922  // Set the reference cMo
1923  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
1924  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1925  mapOfImages.find(m_referenceCameraName);
1926  std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(m_referenceCameraName);
1927 
1928  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
1929  TrackerWrapper *tracker = it_tracker->second;
1930  tracker->initFromPose(*it_img->second, it_initPose->second);
1931  tracker->getPose(cMo);
1932  } else {
1933  throw vpException(vpTrackingException::initializationError, "Cannot initFromPose() for the reference camera!");
1934  }
1935 
1936  // Vector of missing pose matrices for cameras
1937  std::vector<std::string> vectorOfMissingCameraPoses;
1938 
1939  // Set pose for the specified cameras
1940  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
1941  it_img = mapOfImages.find(it_tracker->first);
1942  it_initPose = mapOfInitPoses.find(it_tracker->first);
1943 
1944  if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
1945  // Set pose
1946  it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
1947  } else {
1948  vectorOfMissingCameraPoses.push_back(it_tracker->first);
1949  }
1950  }
1951 
1952  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
1953  it != vectorOfMissingCameraPoses.end(); ++it) {
1954  it_img = mapOfImages.find(*it);
1955  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1957 
1958  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1959  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1960  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
1961  } else {
1963  "Missing image or missing camera transformation "
1964  "matrix! Cannot init the pose for camera: %s!",
1965  it->c_str());
1966  }
1967  }
1968 }
1969 
1981  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
1982 {
1983  if (m_mapOfTrackers.size() == 2) {
1984  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
1985  it->second->initFromPose(I1, c1Mo);
1986 
1987  ++it;
1988 
1989  it->second->initFromPose(I2, c2Mo);
1990 
1991  this->cMo = c1Mo;
1992  } else {
1994  "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size());
1995  }
1996 }
1997 
2011 void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2012  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2013 {
2014  // Set the reference cMo
2015  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2016  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2017  mapOfImages.find(m_referenceCameraName);
2018  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
2019 
2020  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2021  TrackerWrapper *tracker = it_tracker->second;
2022  tracker->initFromPose(*it_img->second, it_camPose->second);
2023  tracker->getPose(cMo);
2024  } else {
2025  throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera!");
2026  }
2027 
2028  // Vector of missing pose matrices for cameras
2029  std::vector<std::string> vectorOfMissingCameraPoses;
2030 
2031  // Set pose for the specified cameras
2032  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2033  it_img = mapOfImages.find(it_tracker->first);
2034  it_camPose = mapOfCameraPoses.find(it_tracker->first);
2035 
2036  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2037  // Set pose
2038  it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
2039  } else {
2040  vectorOfMissingCameraPoses.push_back(it_tracker->first);
2041  }
2042  }
2043 
2044  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
2045  it != vectorOfMissingCameraPoses.end(); ++it) {
2046  it_img = mapOfImages.find(*it);
2047  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2049 
2050  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2051  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
2052  m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
2053  } else {
2055  "Missing image or missing camera transformation "
2056  "matrix! Cannot set the pose for camera: %s!",
2057  it->c_str());
2058  }
2059  }
2060 }
2061 
2077 void vpMbGenericTracker::loadConfigFile(const std::string &configFile)
2078 {
2079  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2080  it != m_mapOfTrackers.end(); ++it) {
2081  TrackerWrapper *tracker = it->second;
2082  tracker->loadConfigFile(configFile);
2083  }
2084 
2086  throw vpException(vpException::fatalError, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str());
2087  }
2088 
2089  m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam);
2090  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
2091  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
2092  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
2093 }
2094 
2110 void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2)
2111 {
2112  if (m_mapOfTrackers.size() != 2) {
2113  throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
2114  }
2115 
2116  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2117  TrackerWrapper *tracker = it_tracker->second;
2118  tracker->loadConfigFile(configFile1);
2119 
2120  ++it_tracker;
2121  tracker = it_tracker->second;
2122  tracker->loadConfigFile(configFile2);
2123 
2125  throw vpException(vpException::fatalError, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str());
2126  }
2127 
2128  m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam);
2129  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
2130  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
2131  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
2132 }
2133 
2148 void vpMbGenericTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
2149 {
2150  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2151  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2152  TrackerWrapper *tracker = it_tracker->second;
2153 
2154  std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
2155  if (it_config != mapOfConfigFiles.end()) {
2156  tracker->loadConfigFile(it_config->second);
2157  } else {
2158  throw vpException(vpTrackingException::initializationError, "Missing configuration file for camera: %s!",
2159  it_tracker->first.c_str());
2160  }
2161  }
2162 
2163  // Set the reference camera parameters
2164  std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.find(m_referenceCameraName);
2165  if (it != m_mapOfTrackers.end()) {
2166  TrackerWrapper *tracker = it->second;
2167  tracker->getCameraParameters(cam);
2168 
2169  // Set clipping
2170  this->clippingFlag = tracker->getClipping();
2171  this->angleAppears = tracker->getAngleAppear();
2172  this->angleDisappears = tracker->getAngleDisappear();
2173  } else {
2174  throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
2175  m_referenceCameraName.c_str());
2176  }
2177 }
2178 
2209 void vpMbGenericTracker::loadModel(const std::string &modelFile, const bool verbose, const vpHomogeneousMatrix &T)
2210 {
2211  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2212  it != m_mapOfTrackers.end(); ++it) {
2213  TrackerWrapper *tracker = it->second;
2214  tracker->loadModel(modelFile, verbose, T);
2215  }
2216 }
2217 
2252 void vpMbGenericTracker::loadModel(const std::string &modelFile1, const std::string &modelFile2, const bool verbose,
2253  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
2254 {
2255  if (m_mapOfTrackers.size() != 2) {
2256  throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
2257  }
2258 
2259  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2260  TrackerWrapper *tracker = it_tracker->second;
2261  tracker->loadModel(modelFile1, verbose, T1);
2262 
2263  ++it_tracker;
2264  tracker = it_tracker->second;
2265  tracker->loadModel(modelFile2, verbose, T2);
2266 }
2267 
2299 void vpMbGenericTracker::loadModel(const std::map<std::string, std::string> &mapOfModelFiles, const bool verbose,
2300  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2301 {
2302  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2303  it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2304  std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);
2305 
2306  if (it_model != mapOfModelFiles.end()) {
2307  TrackerWrapper *tracker = it_tracker->second;
2308  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2309 
2310  if (it_T != mapOfT.end())
2311  tracker->loadModel(it_model->second, verbose, it_T->second);
2312  else
2313  tracker->loadModel(it_model->second, verbose);
2314  } else {
2315  throw vpException(vpTrackingException::initializationError, "Cannot load model for camera: %s",
2316  it_tracker->first.c_str());
2317  }
2318  }
2319 }
2320 
2321 #ifdef VISP_HAVE_PCL
2322 void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2323  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
2324 {
2325  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2326  it != m_mapOfTrackers.end(); ++it) {
2327  TrackerWrapper *tracker = it->second;
2328  tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
2329  }
2330 }
2331 #endif
2332 
2333 void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2334  std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
2335  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
2336  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
2337 {
2338  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2339  it != m_mapOfTrackers.end(); ++it) {
2340  TrackerWrapper *tracker = it->second;
2341  tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
2342  mapOfPointCloudHeights[it->first]);
2343  }
2344 }
2345 
2357 void vpMbGenericTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2358  const vpHomogeneousMatrix &cMo_, const bool verbose,
2359  const vpHomogeneousMatrix &T)
2360 {
2361  if (m_mapOfTrackers.size() != 1) {
2362  throw vpException(vpTrackingException::fatalError, "This method requires exactly one camera, there are %d cameras!",
2363  m_mapOfTrackers.size());
2364  }
2365 
2366  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2367  if (it_tracker != m_mapOfTrackers.end()) {
2368  TrackerWrapper *tracker = it_tracker->second;
2369  tracker->reInitModel(I, cad_name, cMo_, verbose, T);
2370 
2371  // Set reference pose
2372  tracker->getPose(cMo);
2373  } else {
2374  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!");
2375  }
2376 
2377  modelInitialised = true;
2378 }
2379 
2401  const std::string &cad_name1, const std::string &cad_name2,
2402  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
2403  const bool verbose,
2404  const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
2405 {
2406  if (m_mapOfTrackers.size() == 2) {
2407  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
2408 
2409  it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);
2410 
2411  ++it_tracker;
2412 
2413  it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);
2414 
2415  it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2416  if (it_tracker != m_mapOfTrackers.end()) {
2417  // Set reference pose
2418  it_tracker->second->getPose(cMo);
2419  }
2420  } else {
2421  throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!");
2422  }
2423 
2424  modelInitialised = true;
2425 }
2426 
2441 void vpMbGenericTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2442  const std::map<std::string, std::string> &mapOfModelFiles,
2443  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
2444  const bool verbose,
2445  const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
2446 {
2447  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
2448  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2449  mapOfImages.find(m_referenceCameraName);
2450  std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(m_referenceCameraName);
2451  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
2452 
2453  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
2454  it_camPose != mapOfCameraPoses.end()) {
2455  TrackerWrapper *tracker = it_tracker->second;
2456  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
2457  if (it_T != mapOfT.end())
2458  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
2459  else
2460  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
2461 
2462  // Set reference pose
2463  tracker->getPose(cMo);
2464  } else {
2465  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() for reference camera!");
2466  }
2467 
2468  std::vector<std::string> vectorOfMissingCameras;
2469  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
2470  if (it_tracker->first != m_referenceCameraName) {
2471  it_img = mapOfImages.find(it_tracker->first);
2472  it_model = mapOfModelFiles.find(it_tracker->first);
2473  it_camPose = mapOfCameraPoses.find(it_tracker->first);
2474 
2475  if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
2476  TrackerWrapper *tracker = it_tracker->second;
2477  tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
2478  } else {
2479  vectorOfMissingCameras.push_back(it_tracker->first);
2480  }
2481  }
2482  }
2483 
2484  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
2485  ++it) {
2486  it_img = mapOfImages.find(*it);
2487  it_model = mapOfModelFiles.find(*it);
2488  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2490 
2491  if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
2492  it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2493  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
2494  m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
2495  }
2496  }
2497 
2498  modelInitialised = true;
2499 }
2500 
2506 {
2507  cMo.eye();
2508 
2509  useScanLine = false;
2510 
2511 #ifdef VISP_HAVE_OGRE
2512  useOgre = false;
2513 #endif
2514 
2515  m_computeInteraction = true;
2516  m_lambda = 1.0;
2517 
2518  angleAppears = vpMath::rad(89);
2521  distNearClip = 0.001;
2522  distFarClip = 100;
2523 
2525  m_maxIter = 30;
2526  m_stopCriteriaEpsilon = 1e-8;
2527  m_initialMu = 0.01;
2528 
2529  // Only for Edge
2530  m_percentageGdPt = 0.4;
2531 
2532  // Only for KLT
2533  m_thresholdOutlier = 0.5;
2534 
2535  // Reset default ponderation between each feature type
2537 
2538 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
2540 #endif
2541 
2544 
2545  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2546  it != m_mapOfTrackers.end(); ++it) {
2547  TrackerWrapper *tracker = it->second;
2548  tracker->resetTracker();
2549  }
2550 }
2551 
2562 {
2564 
2565  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2566  it != m_mapOfTrackers.end(); ++it) {
2567  TrackerWrapper *tracker = it->second;
2568  tracker->setAngleAppear(a);
2569  }
2570 }
2571 
2584 void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2)
2585 {
2586  if (m_mapOfTrackers.size() == 2) {
2587  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2588  it->second->setAngleAppear(a1);
2589 
2590  ++it;
2591  it->second->setAngleAppear(a2);
2592 
2594  if (it != m_mapOfTrackers.end()) {
2595  angleAppears = it->second->getAngleAppear();
2596  } else {
2597  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
2598  }
2599  } else {
2600  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
2601  m_mapOfTrackers.size());
2602  }
2603 }
2604 
2614 void vpMbGenericTracker::setAngleAppear(const std::map<std::string, double> &mapOfAngles)
2615 {
2616  for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
2617  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
2618 
2619  if (it_tracker != m_mapOfTrackers.end()) {
2620  TrackerWrapper *tracker = it_tracker->second;
2621  tracker->setAngleAppear(it->second);
2622 
2623  if (it->first == m_referenceCameraName) {
2624  angleAppears = it->second;
2625  }
2626  }
2627  }
2628 }
2629 
2640 {
2642 
2643  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2644  it != m_mapOfTrackers.end(); ++it) {
2645  TrackerWrapper *tracker = it->second;
2646  tracker->setAngleDisappear(a);
2647  }
2648 }
2649 
2662 void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2)
2663 {
2664  if (m_mapOfTrackers.size() == 2) {
2665  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2666  it->second->setAngleDisappear(a1);
2667 
2668  ++it;
2669  it->second->setAngleDisappear(a2);
2670 
2672  if (it != m_mapOfTrackers.end()) {
2673  angleDisappears = it->second->getAngleDisappear();
2674  } else {
2675  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
2676  }
2677  } else {
2678  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
2679  m_mapOfTrackers.size());
2680  }
2681 }
2682 
2692 void vpMbGenericTracker::setAngleDisappear(const std::map<std::string, double> &mapOfAngles)
2693 {
2694  for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
2695  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
2696 
2697  if (it_tracker != m_mapOfTrackers.end()) {
2698  TrackerWrapper *tracker = it_tracker->second;
2699  tracker->setAngleDisappear(it->second);
2700 
2701  if (it->first == m_referenceCameraName) {
2702  angleDisappears = it->second;
2703  }
2704  }
2705  }
2706 }
2707 
2714 {
2716 
2717  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2718  it != m_mapOfTrackers.end(); ++it) {
2719  TrackerWrapper *tracker = it->second;
2720  tracker->setCameraParameters(camera);
2721  }
2722 }
2723 
2733 {
2734  if (m_mapOfTrackers.size() == 2) {
2735  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2736  it->second->setCameraParameters(camera1);
2737 
2738  ++it;
2739  it->second->setCameraParameters(camera2);
2740 
2742  if (it != m_mapOfTrackers.end()) {
2743  it->second->getCameraParameters(cam);
2744  } else {
2745  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
2746  }
2747  } else {
2748  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
2749  m_mapOfTrackers.size());
2750  }
2751 }
2752 
2761 void vpMbGenericTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
2762 {
2763  for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
2764  it != mapOfCameraParameters.end(); ++it) {
2765  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
2766 
2767  if (it_tracker != m_mapOfTrackers.end()) {
2768  TrackerWrapper *tracker = it_tracker->second;
2769  tracker->setCameraParameters(it->second);
2770 
2771  if (it->first == m_referenceCameraName) {
2772  cam = it->second;
2773  }
2774  }
2775  }
2776 }
2777 
2786 void vpMbGenericTracker::setCameraTransformationMatrix(const std::string &cameraName,
2787  const vpHomogeneousMatrix &cameraTransformationMatrix)
2788 {
2789  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
2790 
2791  if (it != m_mapOfCameraTransformationMatrix.end()) {
2792  it->second = cameraTransformationMatrix;
2793  } else {
2794  throw vpException(vpTrackingException::fatalError, "Cannot find camera: %s!", cameraName.c_str());
2795  }
2796 }
2797 
2806  const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
2807 {
2808  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
2809  it != mapOfTransformationMatrix.end(); ++it) {
2810  std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
2811  m_mapOfCameraTransformationMatrix.find(it->first);
2812 
2813  if (it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2814  it_camTrans->second = it->second;
2815  }
2816  }
2817 }
2818 
2828 void vpMbGenericTracker::setClipping(const unsigned int &flags)
2829 {
2830  vpMbTracker::setClipping(flags);
2831 
2832  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2833  it != m_mapOfTrackers.end(); ++it) {
2834  TrackerWrapper *tracker = it->second;
2835  tracker->setClipping(flags);
2836  }
2837 }
2838 
2849 void vpMbGenericTracker::setClipping(const unsigned int &flags1, const unsigned int &flags2)
2850 {
2851  if (m_mapOfTrackers.size() == 2) {
2852  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2853  it->second->setClipping(flags1);
2854 
2855  ++it;
2856  it->second->setClipping(flags2);
2857 
2859  if (it != m_mapOfTrackers.end()) {
2860  clippingFlag = it->second->getClipping();
2861  } else {
2862  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
2863  }
2864  } else {
2865  std::stringstream ss;
2866  ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
2868  }
2869 }
2870 
2878 void vpMbGenericTracker::setClipping(const std::map<std::string, unsigned int> &mapOfClippingFlags)
2879 {
2880  for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
2881  it != mapOfClippingFlags.end(); ++it) {
2882  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
2883 
2884  if (it_tracker != m_mapOfTrackers.end()) {
2885  TrackerWrapper *tracker = it_tracker->second;
2886  tracker->setClipping(it->second);
2887 
2888  if (it->first == m_referenceCameraName) {
2889  clippingFlag = it->second;
2890  }
2891  }
2892  }
2893 }
2894 
2905 {
2906  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2907  it != m_mapOfTrackers.end(); ++it) {
2908  TrackerWrapper *tracker = it->second;
2909  tracker->setDepthDenseFilteringMaxDistance(maxDistance);
2910  }
2911 }
2912 
2922 {
2923  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2924  it != m_mapOfTrackers.end(); ++it) {
2925  TrackerWrapper *tracker = it->second;
2926  tracker->setDepthDenseFilteringMethod(method);
2927  }
2928 }
2929 
2940 {
2941  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2942  it != m_mapOfTrackers.end(); ++it) {
2943  TrackerWrapper *tracker = it->second;
2944  tracker->setDepthDenseFilteringMinDistance(minDistance);
2945  }
2946 }
2947 
2958 {
2959  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2960  it != m_mapOfTrackers.end(); ++it) {
2961  TrackerWrapper *tracker = it->second;
2962  tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
2963  }
2964 }
2965 
2974 void vpMbGenericTracker::setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
2975 {
2976  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2977  it != m_mapOfTrackers.end(); ++it) {
2978  TrackerWrapper *tracker = it->second;
2979  tracker->setDepthDenseSamplingStep(stepX, stepY);
2980  }
2981 }
2982 
2991 {
2992  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
2993  it != m_mapOfTrackers.end(); ++it) {
2994  TrackerWrapper *tracker = it->second;
2995  tracker->setDepthNormalFaceCentroidMethod(method);
2996  }
2997 }
2998 
3008 {
3009  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3010  it != m_mapOfTrackers.end(); ++it) {
3011  TrackerWrapper *tracker = it->second;
3012  tracker->setDepthNormalFeatureEstimationMethod(method);
3013  }
3014 }
3015 
3024 {
3025  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3026  it != m_mapOfTrackers.end(); ++it) {
3027  TrackerWrapper *tracker = it->second;
3028  tracker->setDepthNormalPclPlaneEstimationMethod(method);
3029  }
3030 }
3031 
3040 {
3041  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3042  it != m_mapOfTrackers.end(); ++it) {
3043  TrackerWrapper *tracker = it->second;
3044  tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
3045  }
3046 }
3047 
3056 {
3057  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3058  it != m_mapOfTrackers.end(); ++it) {
3059  TrackerWrapper *tracker = it->second;
3060  tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
3061  }
3062 }
3063 
3072 void vpMbGenericTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
3073 {
3074  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3075  it != m_mapOfTrackers.end(); ++it) {
3076  TrackerWrapper *tracker = it->second;
3077  tracker->setDepthNormalSamplingStep(stepX, stepY);
3078  }
3079 }
3080 
3100 {
3102 
3103  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3104  it != m_mapOfTrackers.end(); ++it) {
3105  TrackerWrapper *tracker = it->second;
3106  tracker->setDisplayFeatures(displayF);
3107  }
3108 }
3109 
3118 {
3120 
3121  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3122  it != m_mapOfTrackers.end(); ++it) {
3123  TrackerWrapper *tracker = it->second;
3124  tracker->setFarClippingDistance(dist);
3125  }
3126 }
3127 
3136 void vpMbGenericTracker::setFarClippingDistance(const double &dist1, const double &dist2)
3137 {
3138  if (m_mapOfTrackers.size() == 2) {
3139  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3140  it->second->setFarClippingDistance(dist1);
3141 
3142  ++it;
3143  it->second->setFarClippingDistance(dist2);
3144 
3146  if (it != m_mapOfTrackers.end()) {
3147  distFarClip = it->second->getFarClippingDistance();
3148  } else {
3149  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3150  }
3151  } else {
3152  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3153  m_mapOfTrackers.size());
3154  }
3155 }
3156 
3162 void vpMbGenericTracker::setFarClippingDistance(const std::map<std::string, double> &mapOfClippingDists)
3163 {
3164  for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
3165  ++it) {
3166  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3167 
3168  if (it_tracker != m_mapOfTrackers.end()) {
3169  TrackerWrapper *tracker = it_tracker->second;
3170  tracker->setFarClippingDistance(it->second);
3171 
3172  if (it->first == m_referenceCameraName) {
3173  distFarClip = it->second;
3174  }
3175  }
3176  }
3177 }
3178 
3185 void vpMbGenericTracker::setFeatureFactors(const std::map<vpTrackerType, double> &mapOfFeatureFactors)
3186 {
3187  for (std::map<vpTrackerType, double>::iterator it = m_mapOfFeatureFactors.begin(); it != m_mapOfFeatureFactors.end();
3188  ++it) {
3189  std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
3190  if (it_factor != mapOfFeatureFactors.end()) {
3191  it->second = it_factor->second;
3192  }
3193  }
3194 }
3195 
3212 {
3213  m_percentageGdPt = threshold;
3214 
3215  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3216  it != m_mapOfTrackers.end(); ++it) {
3217  TrackerWrapper *tracker = it->second;
3218  tracker->setGoodMovingEdgesRatioThreshold(threshold);
3219  }
3220 }
3221 
3222 #ifdef VISP_HAVE_OGRE
3223 
3235 {
3236  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3237  it != m_mapOfTrackers.end(); ++it) {
3238  TrackerWrapper *tracker = it->second;
3239  tracker->setGoodNbRayCastingAttemptsRatio(ratio);
3240  }
3241 }
3242 
3255 {
3256  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3257  it != m_mapOfTrackers.end(); ++it) {
3258  TrackerWrapper *tracker = it->second;
3259  tracker->setNbRayCastingAttemptsForVisibility(attempts);
3260  }
3261 }
3262 #endif
3263 
3264 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3265 
3273 {
3274  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3275  it != m_mapOfTrackers.end(); ++it) {
3276  TrackerWrapper *tracker = it->second;
3277  tracker->setKltOpencv(t);
3278  }
3279 }
3280 
3290 {
3291  if (m_mapOfTrackers.size() == 2) {
3292  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3293  it->second->setKltOpencv(t1);
3294 
3295  ++it;
3296  it->second->setKltOpencv(t2);
3297  } else {
3298  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3299  m_mapOfTrackers.size());
3300  }
3301 }
3302 
3308 void vpMbGenericTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfKlts)
3309 {
3310  for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
3311  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3312 
3313  if (it_tracker != m_mapOfTrackers.end()) {
3314  TrackerWrapper *tracker = it_tracker->second;
3315  tracker->setKltOpencv(it->second);
3316  }
3317  }
3318 }
3319 
3328 {
3329  m_thresholdOutlier = th;
3330 
3331  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3332  it != m_mapOfTrackers.end(); ++it) {
3333  TrackerWrapper *tracker = it->second;
3334  tracker->setKltThresholdAcceptation(th);
3335  }
3336 }
3337 #endif
3338 
3351 void vpMbGenericTracker::setLod(const bool useLod, const std::string &name)
3352 {
3353  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3354  it != m_mapOfTrackers.end(); ++it) {
3355  TrackerWrapper *tracker = it->second;
3356  tracker->setLod(useLod, name);
3357  }
3358 }
3359 
3360 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3361 
3368 void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e)
3369 {
3370  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3371  it != m_mapOfTrackers.end(); ++it) {
3372  TrackerWrapper *tracker = it->second;
3373  tracker->setKltMaskBorder(e);
3374  }
3375 }
3376 
3385 void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, const unsigned int &e2)
3386 {
3387  if (m_mapOfTrackers.size() == 2) {
3388  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3389  it->second->setKltMaskBorder(e1);
3390 
3391  ++it;
3392 
3393  it->second->setKltMaskBorder(e2);
3394  } else {
3395  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3396  m_mapOfTrackers.size());
3397  }
3398 }
3399 
3405 void vpMbGenericTracker::setKltMaskBorder(const std::map<std::string, unsigned int> &mapOfErosions)
3406 {
3407  for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
3408  ++it) {
3409  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3410 
3411  if (it_tracker != m_mapOfTrackers.end()) {
3412  TrackerWrapper *tracker = it_tracker->second;
3413  tracker->setKltMaskBorder(it->second);
3414  }
3415  }
3416 }
3417 #endif
3418 
3425 {
3426  vpMbTracker::setMask(mask);
3427 
3428  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3429  it != m_mapOfTrackers.end(); ++it) {
3430  TrackerWrapper *tracker = it->second;
3431  tracker->setMask(mask);
3432  }
3433 }
3434 
3435 
3447 void vpMbGenericTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
3448 {
3449  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3450  it != m_mapOfTrackers.end(); ++it) {
3451  TrackerWrapper *tracker = it->second;
3452  tracker->setMinLineLengthThresh(minLineLengthThresh, name);
3453  }
3454 }
3455 
3466 void vpMbGenericTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
3467 {
3468  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3469  it != m_mapOfTrackers.end(); ++it) {
3470  TrackerWrapper *tracker = it->second;
3471  tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
3472  }
3473 }
3474 
3483 {
3484  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3485  it != m_mapOfTrackers.end(); ++it) {
3486  TrackerWrapper *tracker = it->second;
3487  tracker->setMovingEdge(me);
3488  }
3489 }
3490 
3500 void vpMbGenericTracker::setMovingEdge(const vpMe &me1, const vpMe &me2)
3501 {
3502  if (m_mapOfTrackers.size() == 2) {
3503  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3504  it->second->setMovingEdge(me1);
3505 
3506  ++it;
3507 
3508  it->second->setMovingEdge(me2);
3509  } else {
3510  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3511  m_mapOfTrackers.size());
3512  }
3513 }
3514 
3520 void vpMbGenericTracker::setMovingEdge(const std::map<std::string, vpMe> &mapOfMe)
3521 {
3522  for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
3523  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3524 
3525  if (it_tracker != m_mapOfTrackers.end()) {
3526  TrackerWrapper *tracker = it_tracker->second;
3527  tracker->setMovingEdge(it->second);
3528  }
3529  }
3530 }
3531 
3540 {
3542 
3543  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3544  it != m_mapOfTrackers.end(); ++it) {
3545  TrackerWrapper *tracker = it->second;
3546  tracker->setNearClippingDistance(dist);
3547  }
3548 }
3549 
3558 void vpMbGenericTracker::setNearClippingDistance(const double &dist1, const double &dist2)
3559 {
3560  if (m_mapOfTrackers.size() == 2) {
3561  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3562  it->second->setNearClippingDistance(dist1);
3563 
3564  ++it;
3565 
3566  it->second->setNearClippingDistance(dist2);
3567 
3569  if (it != m_mapOfTrackers.end()) {
3570  distNearClip = it->second->getNearClippingDistance();
3571  } else {
3572  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
3573  }
3574  } else {
3575  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3576  m_mapOfTrackers.size());
3577  }
3578 }
3579 
3585 void vpMbGenericTracker::setNearClippingDistance(const std::map<std::string, double> &mapOfDists)
3586 {
3587  for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
3588  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3589 
3590  if (it_tracker != m_mapOfTrackers.end()) {
3591  TrackerWrapper *tracker = it_tracker->second;
3592  tracker->setNearClippingDistance(it->second);
3593 
3594  if (it->first == m_referenceCameraName) {
3595  distNearClip = it->second;
3596  }
3597  }
3598  }
3599 }
3600 
3613 void vpMbGenericTracker::setOgreShowConfigDialog(const bool showConfigDialog)
3614 {
3615  vpMbTracker::setOgreShowConfigDialog(showConfigDialog);
3616 
3617  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3618  it != m_mapOfTrackers.end(); ++it) {
3619  TrackerWrapper *tracker = it->second;
3620  tracker->setOgreShowConfigDialog(showConfigDialog);
3621  }
3622 }
3623 
3635 {
3637 
3638  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3639  it != m_mapOfTrackers.end(); ++it) {
3640  TrackerWrapper *tracker = it->second;
3641  tracker->setOgreVisibilityTest(v);
3642  }
3643 
3644 #ifdef VISP_HAVE_OGRE
3645  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3646  it != m_mapOfTrackers.end(); ++it) {
3647  TrackerWrapper *tracker = it->second;
3648  tracker->faces.getOgreContext()->setWindowName("Multi Generic MBT (" + it->first + ")");
3649  }
3650 #endif
3651 }
3652 
3661 {
3663 
3664  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3665  it != m_mapOfTrackers.end(); ++it) {
3666  TrackerWrapper *tracker = it->second;
3667  tracker->setOptimizationMethod(opt);
3668  }
3669 }
3670 
3684 {
3685  if (m_mapOfTrackers.size() > 1) {
3686  throw vpException(vpTrackingException::initializationError, "The function setPose() requires the generic tracker "
3687  "to be configured with only one camera!");
3688  }
3689 
3690  cMo = cdMo;
3691 
3692  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
3693  if (it != m_mapOfTrackers.end()) {
3694  TrackerWrapper *tracker = it->second;
3695  tracker->setPose(I, cdMo);
3696  } else {
3697  throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
3698  m_referenceCameraName.c_str());
3699  }
3700 }
3701 
3714  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
3715 {
3716  if (m_mapOfTrackers.size() == 2) {
3717  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3718  it->second->setPose(I1, c1Mo);
3719 
3720  ++it;
3721 
3722  it->second->setPose(I2, c2Mo);
3723 
3725  if (it != m_mapOfTrackers.end()) {
3726  // Set reference pose
3727  it->second->getPose(cMo);
3728  } else {
3729  throw vpException(vpTrackingException::fatalError, "The reference camera: %s does not exist!",
3730  m_referenceCameraName.c_str());
3731  }
3732  } else {
3733  throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
3734  m_mapOfTrackers.size());
3735  }
3736 }
3737 
3753 void vpMbGenericTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
3754  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
3755 {
3756  // Set the reference cMo
3757  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
3758  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
3759  mapOfImages.find(m_referenceCameraName);
3760  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
3761 
3762  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
3763  TrackerWrapper *tracker = it_tracker->second;
3764  tracker->setPose(*it_img->second, it_camPose->second);
3765  tracker->getPose(cMo);
3766  } else {
3767  throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera!");
3768  }
3769 
3770  // Vector of missing pose matrices for cameras
3771  std::vector<std::string> vectorOfMissingCameraPoses;
3772 
3773  // Set pose for the specified cameras
3774  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
3775  if (it_tracker->first != m_referenceCameraName) {
3776  it_img = mapOfImages.find(it_tracker->first);
3777  it_camPose = mapOfCameraPoses.find(it_tracker->first);
3778 
3779  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
3780  // Set pose
3781  TrackerWrapper *tracker = it_tracker->second;
3782  tracker->setPose(*it_img->second, it_camPose->second);
3783  } else {
3784  vectorOfMissingCameraPoses.push_back(it_tracker->first);
3785  }
3786  }
3787  }
3788 
3789  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
3790  it != vectorOfMissingCameraPoses.end(); ++it) {
3791  it_img = mapOfImages.find(*it);
3792  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
3794 
3795  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
3796  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
3797  m_mapOfTrackers[*it]->setPose(*it_img->second, cCurrentMo);
3798  } else {
3800  "Missing image or missing camera transformation "
3801  "matrix! Cannot set pose for camera: %s!",
3802  it->c_str());
3803  }
3804  }
3805 }
3806 
3822 {
3824 
3825  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3826  it != m_mapOfTrackers.end(); ++it) {
3827  TrackerWrapper *tracker = it->second;
3828  tracker->setProjectionErrorComputation(flag);
3829  }
3830 }
3831 
3836 {
3838 
3839  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3840  it != m_mapOfTrackers.end(); ++it) {
3841  TrackerWrapper *tracker = it->second;
3842  tracker->setProjectionErrorDisplay(display);
3843  }
3844 }
3845 
3850 {
3852 
3853  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3854  it != m_mapOfTrackers.end(); ++it) {
3855  TrackerWrapper *tracker = it->second;
3856  tracker->setProjectionErrorDisplayArrowLength(length);
3857  }
3858 }
3859 
3861 {
3863 
3864  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3865  it != m_mapOfTrackers.end(); ++it) {
3866  TrackerWrapper *tracker = it->second;
3867  tracker->setProjectionErrorDisplayArrowThickness(thickness);
3868  }
3869 }
3870 
3876 void vpMbGenericTracker::setReferenceCameraName(const std::string &referenceCameraName)
3877 {
3878  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(referenceCameraName);
3879  if (it != m_mapOfTrackers.end()) {
3880  m_referenceCameraName = referenceCameraName;
3881  } else {
3882  std::cerr << "The reference camera: " << referenceCameraName << " does not exist!";
3883  }
3884 }
3885 
3887 {
3889 
3890  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3891  it != m_mapOfTrackers.end(); ++it) {
3892  TrackerWrapper *tracker = it->second;
3893  tracker->setScanLineVisibilityTest(v);
3894  }
3895 }
3896 
3909 {
3910  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3911  it != m_mapOfTrackers.end(); ++it) {
3912  TrackerWrapper *tracker = it->second;
3913  tracker->setTrackerType(type);
3914  }
3915 }
3916 
3926 void vpMbGenericTracker::setTrackerType(const std::map<std::string, int> &mapOfTrackerTypes)
3927 {
3928  for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
3929  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
3930  if (it_tracker != m_mapOfTrackers.end()) {
3931  TrackerWrapper *tracker = it_tracker->second;
3932  tracker->setTrackerType(it->second);
3933  }
3934  }
3935 }
3936 
3946 void vpMbGenericTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
3947 {
3948  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3949  it != m_mapOfTrackers.end(); ++it) {
3950  TrackerWrapper *tracker = it->second;
3951  tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
3952  }
3953 }
3954 
3964 void vpMbGenericTracker::setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
3965 {
3966  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3967  it != m_mapOfTrackers.end(); ++it) {
3968  TrackerWrapper *tracker = it->second;
3969  tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
3970  }
3971 }
3972 
3982 void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
3983 {
3984  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
3985  it != m_mapOfTrackers.end(); ++it) {
3986  TrackerWrapper *tracker = it->second;
3987  tracker->setUseEdgeTracking(name, useEdgeTracking);
3988  }
3989 }
3990 
3991 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
3992 
4001 void vpMbGenericTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
4002 {
4003  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4004  it != m_mapOfTrackers.end(); ++it) {
4005  TrackerWrapper *tracker = it->second;
4006  tracker->setUseKltTracking(name, useKltTracking);
4007  }
4008 }
4009 #endif
4010 
4012 {
4013  // Test tracking fails only if all testTracking have failed
4014  bool isOneTestTrackingOk = false;
4015  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4016  it != m_mapOfTrackers.end(); ++it) {
4017  TrackerWrapper *tracker = it->second;
4018  try {
4019  tracker->testTracking();
4020  isOneTestTrackingOk = true;
4021  } catch (...) {
4022  }
4023  }
4024 
4025  if (!isOneTestTrackingOk) {
4026  std::ostringstream oss;
4027  oss << "Not enough moving edges to track the object. Try to reduce the "
4028  "threshold="
4029  << m_percentageGdPt << " using setGoodMovingEdgesRatioThreshold()";
4031  }
4032 }
4033 
4044 {
4045  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
4046  mapOfImages[m_referenceCameraName] = &I;
4047 
4048  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
4049  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
4050 
4051  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
4052 }
4053 
4065 {
4066  if (m_mapOfTrackers.size() == 2) {
4067  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4068  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
4069  mapOfImages[it->first] = &I1;
4070  ++it;
4071 
4072  mapOfImages[it->first] = &I2;
4073 
4074  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
4075  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
4076 
4077  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
4078  } else {
4079  std::stringstream ss;
4080  ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
4081  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
4082  }
4083 }
4084 
4092 void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
4093 {
4094  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
4095  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
4096 
4097  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
4098 }
4099 
4100 #ifdef VISP_HAVE_PCL
4101 
4109 void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
4110  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
4111 {
4112  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4113  it != m_mapOfTrackers.end(); ++it) {
4114  TrackerWrapper *tracker = it->second;
4115 
4116  if ((tracker->m_trackerType & (EDGE_TRACKER |
4117 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4118  KLT_TRACKER |
4119 #endif
4121  throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
4122  }
4123 
4124  if (tracker->m_trackerType & (EDGE_TRACKER
4125 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4126  | KLT_TRACKER
4127 #endif
4128  ) &&
4129  mapOfImages[it->first] == NULL) {
4130  throw vpException(vpException::fatalError, "Image pointer is NULL!");
4131  }
4132 
4133  if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
4134  mapOfPointClouds[it->first] == nullptr) {
4135  throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
4136  }
4137  }
4138 
4139  preTracking(mapOfImages, mapOfPointClouds);
4140 
4141  try {
4142  computeVVS(mapOfImages);
4143  } catch (...) {
4144  covarianceMatrix = -1;
4145  throw; // throw the original exception
4146  }
4147 
4148  testTracking();
4149 
4150  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4151  it != m_mapOfTrackers.end(); ++it) {
4152  TrackerWrapper *tracker = it->second;
4153 
4154  tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
4155  }
4156 
4158 }
4159 #endif
4160 
4171 void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
4172  std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
4173  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
4174  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
4175 {
4176  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4177  it != m_mapOfTrackers.end(); ++it) {
4178  TrackerWrapper *tracker = it->second;
4179 
4180  if ((tracker->m_trackerType & (EDGE_TRACKER |
4181 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4182  KLT_TRACKER |
4183 #endif
4185  throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
4186  }
4187 
4188  if (tracker->m_trackerType & (EDGE_TRACKER
4189 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4190  | KLT_TRACKER
4191 #endif
4192  ) &&
4193  mapOfImages[it->first] == NULL) {
4194  throw vpException(vpException::fatalError, "Image pointer is NULL!");
4195  }
4196 
4197  if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
4198  (mapOfPointClouds[it->first] == NULL)) {
4199  throw vpException(vpException::fatalError, "Pointcloud is NULL!");
4200  }
4201  }
4202 
4203  preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
4204 
4205  try {
4206  computeVVS(mapOfImages);
4207  } catch (...) {
4208  covarianceMatrix = -1;
4209  throw; // throw the original exception
4210  }
4211 
4212  testTracking();
4213 
4214  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
4215  it != m_mapOfTrackers.end(); ++it) {
4216  TrackerWrapper *tracker = it->second;
4217 
4218  tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
4219  }
4220 
4222 }
4223 
4225 vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
4226  : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
4227 {
4228  m_lambda = 1.0;
4229  m_maxIter = 30;
4230 
4231 #ifdef VISP_HAVE_OGRE
4232  faces.getOgreContext()->setWindowName("MBT TrackerWrapper");
4233 
4234  m_projectionErrorFaces.getOgreContext()->setWindowName("MBT TrackerWrapper (projection error)");
4235 #endif
4236 }
4237 
4238 vpMbGenericTracker::TrackerWrapper::TrackerWrapper(const int trackerType)
4239  : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
4240 {
4241  if ((m_trackerType & (EDGE_TRACKER |
4242 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4243  KLT_TRACKER |
4244 #endif
4246  throw vpException(vpTrackingException::badValue, "Bad value for tracker type: %d!", m_trackerType);
4247  }
4248 
4249  m_lambda = 1.0;
4250  m_maxIter = 30;
4251 
4252 #ifdef VISP_HAVE_OGRE
4253  faces.getOgreContext()->setWindowName("MBT TrackerWrapper");
4254 
4255  m_projectionErrorFaces.getOgreContext()->setWindowName("MBT TrackerWrapper (projection error)");
4256 #endif
4257 }
4258 
4259 vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }
4260 
4261 // Implemented only for debugging purposes: use TrackerWrapper as a standalone tracker
4262 void vpMbGenericTracker::TrackerWrapper::computeVVS(const vpImage<unsigned char> *const ptr_I)
4263 {
4264  computeVVSInit(ptr_I);
4265 
4266  if (m_error.getRows() < 4) {
4267  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
4268  }
4269 
4270  double normRes = 0;
4271  double normRes_1 = -1;
4272  unsigned int iter = 0;
4273 
4274  double factorEdge = 1.0;
4275 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4276  double factorKlt = 1.0;
4277 #endif
4278  double factorDepth = 1.0;
4279  double factorDepthDense = 1.0;
4280 
4281  vpMatrix LTL;
4282  vpColVector LTR, v;
4283  vpColVector error_prev;
4284 
4285  double mu = m_initialMu;
4286  vpHomogeneousMatrix cMo_prev;
4287 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4288  vpHomogeneousMatrix ctTc0_Prev; // Only for KLT
4289 #endif
4290  bool isoJoIdentity_ = true;
4291 
4292  // Covariance
4293  vpColVector W_true(m_error.getRows());
4294  vpMatrix L_true, LVJ_true;
4295 
4296  unsigned int nb_edge_features = m_error_edge.getRows();
4297 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4298  unsigned int nb_klt_features = m_error_klt.getRows();
4299 #endif
4300  unsigned int nb_depth_features = m_error_depthNormal.getRows();
4301  unsigned int nb_depth_dense_features = m_error_depthDense.getRows();
4302 
4303  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
4305 
4306  bool reStartFromLastIncrement = false;
4307  computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
4308 
4309 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4310  if (reStartFromLastIncrement) {
4311  if (m_trackerType & KLT_TRACKER) {
4312  ctTc0 = ctTc0_Prev;
4313  }
4314  }
4315 #endif
4316 
4317  if (!reStartFromLastIncrement) {
4319 
4320  if (computeCovariance) {
4321  L_true = m_L;
4322  if (!isoJoIdentity_) {
4324  cVo.buildFrom(cMo);
4325  LVJ_true = (m_L * cVo * oJo);
4326  }
4327  }
4328 
4330  if (iter == 0) {
4331  isoJoIdentity_ = true;
4332  oJo.eye();
4333 
4334  // If all the 6 dof should be estimated, we check if the interaction
4335  // matrix is full rank. If not we remove automatically the dof that
4336  // cannot be estimated This is particularly useful when consering
4337  // circles (rank 5) and cylinders (rank 4)
4338  if (isoJoIdentity_) {
4339  cVo.buildFrom(cMo);
4340 
4341  vpMatrix K; // kernel
4342  unsigned int rank = (m_L * cVo).kernel(K);
4343  if (rank == 0) {
4344  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
4345  }
4346 
4347  if (rank != 6) {
4348  vpMatrix I; // Identity
4349  I.eye(6);
4350  oJo = I - K.AtA();
4351 
4352  isoJoIdentity_ = false;
4353  }
4354  }
4355  }
4356 
4357  // Weighting
4358  double num = 0;
4359  double den = 0;
4360 
4361  unsigned int start_index = 0;
4362  if (m_trackerType & EDGE_TRACKER) {
4363  for (unsigned int i = 0; i < nb_edge_features; i++) {
4364  double wi = m_w_edge[i] * m_factor[i] * factorEdge;
4365  W_true[i] = wi;
4366  m_weightedError[i] = wi * m_error[i];
4367 
4368  num += wi * vpMath::sqr(m_error[i]);
4369  den += wi;
4370 
4371  for (unsigned int j = 0; j < m_L.getCols(); j++) {
4372  m_L[i][j] *= wi;
4373  }
4374  }
4375 
4376  start_index += nb_edge_features;
4377  }
4378 
4379 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4380  if (m_trackerType & KLT_TRACKER) {
4381  for (unsigned int i = 0; i < nb_klt_features; i++) {
4382  double wi = m_w_klt[i] * factorKlt;
4383  W_true[start_index + i] = wi;
4384  m_weightedError[start_index + i] = wi * m_error_klt[i];
4385 
4386  num += wi * vpMath::sqr(m_error[start_index + i]);
4387  den += wi;
4388 
4389  for (unsigned int j = 0; j < m_L.getCols(); j++) {
4390  m_L[start_index + i][j] *= wi;
4391  }
4392  }
4393 
4394  start_index += nb_klt_features;
4395  }
4396 #endif
4397 
4398  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4399  for (unsigned int i = 0; i < nb_depth_features; i++) {
4400  double wi = m_w_depthNormal[i] * factorDepth;
4401  m_w[start_index + i] = m_w_depthNormal[i];
4402  m_weightedError[start_index + i] = wi * m_error[start_index + i];
4403 
4404  num += wi * vpMath::sqr(m_error[start_index + i]);
4405  den += wi;
4406 
4407  for (unsigned int j = 0; j < m_L.getCols(); j++) {
4408  m_L[start_index + i][j] *= wi;
4409  }
4410  }
4411 
4412  start_index += nb_depth_features;
4413  }
4414 
4415  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4416  for (unsigned int i = 0; i < nb_depth_dense_features; i++) {
4417  double wi = m_w_depthDense[i] * factorDepthDense;
4418  m_w[start_index + i] = m_w_depthDense[i];
4419  m_weightedError[start_index + i] = wi * m_error[start_index + i];
4420 
4421  num += wi * vpMath::sqr(m_error[start_index + i]);
4422  den += wi;
4423 
4424  for (unsigned int j = 0; j < m_L.getCols(); j++) {
4425  m_L[start_index + i][j] *= wi;
4426  }
4427  }
4428 
4429  // start_index += nb_depth_dense_features;
4430  }
4431 
4432  computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);
4433 
4434  cMo_prev = cMo;
4435 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4436  if (m_trackerType & KLT_TRACKER) {
4437  ctTc0_Prev = ctTc0;
4438  }
4439 #endif
4440 
4442 
4443 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4444  if (m_trackerType & KLT_TRACKER) {
4445  ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
4446  }
4447 #endif
4448  normRes_1 = normRes;
4449 
4450  normRes = sqrt(num / den);
4451  }
4452 
4453  iter++;
4454  }
4455 
4456  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
4457 
4458  if (m_trackerType & EDGE_TRACKER) {
4460  }
4461 }
4462 
4463 void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
4464 {
4465  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
4466  "TrackerWrapper::computeVVSInit("
4467  ") should not be called!");
4468 }
4469 
4470 void vpMbGenericTracker::TrackerWrapper::computeVVSInit(const vpImage<unsigned char> *const ptr_I)
4471 {
4472  initMbtTracking(ptr_I);
4473 
4474  unsigned int nbFeatures = 0;
4475 
4476  if (m_trackerType & EDGE_TRACKER) {
4477  nbFeatures += m_error_edge.getRows();
4478  } else {
4479  m_error_edge.clear();
4480  m_weightedError_edge.clear();
4481  m_L_edge.clear();
4482  m_w_edge.clear();
4483  }
4484 
4485 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4486  if (m_trackerType & KLT_TRACKER) {
4488  nbFeatures += m_error_klt.getRows();
4489  } else {
4490  m_error_klt.clear();
4491  m_weightedError_klt.clear();
4492  m_L_klt.clear();
4493  m_w_klt.clear();
4494  }
4495 #endif
4496 
4497  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4499  nbFeatures += m_error_depthNormal.getRows();
4500  } else {
4501  m_error_depthNormal.clear();
4502  m_weightedError_depthNormal.clear();
4503  m_L_depthNormal.clear();
4504  m_w_depthNormal.clear();
4505  }
4506 
4507  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4509  nbFeatures += m_error_depthDense.getRows();
4510  } else {
4511  m_error_depthDense.clear();
4512  m_weightedError_depthDense.clear();
4513  m_L_depthDense.clear();
4514  m_w_depthDense.clear();
4515  }
4516 
4517  m_L.resize(nbFeatures, 6, false, false);
4518  m_error.resize(nbFeatures, false);
4519 
4520  m_weightedError.resize(nbFeatures, false);
4521  m_w.resize(nbFeatures, false);
4522  m_w = 1;
4523 }
4524 
4525 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu()
4526 {
4527  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
4528  "TrackerWrapper::"
4529  "computeVVSInteractionMatrixAndR"
4530  "esidu() should not be called!");
4531 }
4532 
4533 void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> *const ptr_I)
4534 {
4535  if (m_trackerType & EDGE_TRACKER) {
4537  }
4538 
4539 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4540  if (m_trackerType & KLT_TRACKER) {
4542  }
4543 #endif
4544 
4545  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4547  }
4548 
4549  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4551  }
4552 
4553  unsigned int start_index = 0;
4554  if (m_trackerType & EDGE_TRACKER) {
4555  m_L.insert(m_L_edge, start_index, 0);
4556  m_error.insert(start_index, m_error_edge);
4557 
4558  start_index += m_error_edge.getRows();
4559  }
4560 
4561 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4562  if (m_trackerType & KLT_TRACKER) {
4563  m_L.insert(m_L_klt, start_index, 0);
4564  m_error.insert(start_index, m_error_klt);
4565 
4566  start_index += m_error_klt.getRows();
4567  }
4568 #endif
4569 
4570  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4571  m_L.insert(m_L_depthNormal, start_index, 0);
4572  m_error.insert(start_index, m_error_depthNormal);
4573 
4574  start_index += m_error_depthNormal.getRows();
4575  }
4576 
4577  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4578  m_L.insert(m_L_depthDense, start_index, 0);
4579  m_error.insert(start_index, m_error_depthDense);
4580 
4581  // start_index += m_error_depthDense.getRows();
4582  }
4583 }
4584 
4586 {
4587  unsigned int start_index = 0;
4588 
4589  if (m_trackerType & EDGE_TRACKER) {
4591  m_w.insert(start_index, m_w_edge);
4592 
4593  start_index += m_w_edge.getRows();
4594  }
4595 
4596 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4597  if (m_trackerType & KLT_TRACKER) {
4598  vpMbTracker::computeVVSWeights(m_robust_klt, m_error_klt, m_w_klt);
4599  m_w.insert(start_index, m_w_klt);
4600 
4601  start_index += m_w_klt.getRows();
4602  }
4603 #endif
4604 
4605  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4606  if (m_depthNormalUseRobust) {
4607  vpMbTracker::computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal);
4608  m_w.insert(start_index, m_w_depthNormal);
4609  }
4610 
4611  start_index += m_w_depthNormal.getRows();
4612  }
4613 
4614  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4616  m_w.insert(start_index, m_w_depthDense);
4617 
4618  // start_index += m_w_depthDense.getRows();
4619  }
4620 }
4621 
4622 void vpMbGenericTracker::TrackerWrapper::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
4623  const vpCameraParameters &camera, const vpColor &col,
4624  const unsigned int thickness, const bool displayFullModel)
4625 {
4626  if (m_trackerType == EDGE_TRACKER) {
4627  vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4628 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4629  } else if (m_trackerType == KLT_TRACKER) {
4630  vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4631 #endif
4632  } else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
4633  vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4634  } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
4635  vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4636  } else {
4637  if (m_trackerType & EDGE_TRACKER) {
4638  for (unsigned int i = 0; i < scales.size(); i += 1) {
4639  if (scales[i]) {
4640  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
4641  it != lines[scaleLevel].end(); ++it) {
4642  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4643  }
4644 
4645  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
4646  it != cylinders[scaleLevel].end(); ++it) {
4647  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4648  }
4649 
4650  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
4651  it != circles[scaleLevel].end(); ++it) {
4652  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4653  }
4654 
4655  break; // display model on one scale only
4656  }
4657  }
4658  }
4659 
4660 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4661  if (m_trackerType & KLT_TRACKER) {
4662  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
4663  ++it) {
4664  vpMbtDistanceKltPoints *kltpoly = *it;
4665  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
4666  kltpoly->displayPrimitive(I);
4667  }
4668  }
4669 
4670  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
4671  ++it) {
4672  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
4673  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
4674  kltPolyCylinder->displayPrimitive(I);
4675  }
4676  }
4677 #endif
4678 
4679  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4680  vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4681  }
4682 
4683  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4684  vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4685  }
4686 
4687 #ifdef VISP_HAVE_OGRE
4688  if (useOgre)
4689  faces.displayOgre(cMo_);
4690 #endif
4691  }
4692 }
4693 
4694 void vpMbGenericTracker::TrackerWrapper::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
4695  const vpCameraParameters &camera, const vpColor &col,
4696  const unsigned int thickness, const bool displayFullModel)
4697 {
4698  if (m_trackerType == EDGE_TRACKER) {
4699  vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4700 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4701  } else if (m_trackerType == KLT_TRACKER) {
4702  vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4703 #endif
4704  } else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
4705  vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4706  } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
4707  vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4708  } else {
4709  if (m_trackerType & EDGE_TRACKER) {
4710  for (unsigned int i = 0; i < scales.size(); i += 1) {
4711  if (scales[i]) {
4712  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
4713  it != lines[scaleLevel].end(); ++it) {
4714  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4715  }
4716 
4717  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
4718  it != cylinders[scaleLevel].end(); ++it) {
4719  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4720  }
4721 
4722  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
4723  it != circles[scaleLevel].end(); ++it) {
4724  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
4725  }
4726 
4727  break; // display model on one scale only
4728  }
4729  }
4730  }
4731 
4732 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4733  if (m_trackerType & KLT_TRACKER) {
4734  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
4735  ++it) {
4736  vpMbtDistanceKltPoints *kltpoly = *it;
4737  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
4738  kltpoly->displayPrimitive(I);
4739  }
4740  }
4741 
4742  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
4743  ++it) {
4744  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
4745  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
4746  kltPolyCylinder->displayPrimitive(I);
4747  }
4748  }
4749 #endif
4750 
4751  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
4752  vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4753  }
4754 
4755  if (m_trackerType & DEPTH_DENSE_TRACKER) {
4756  vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
4757  }
4758 
4759 #ifdef VISP_HAVE_OGRE
4760  if (useOgre)
4761  faces.displayOgre(cMo_);
4762 #endif
4763  }
4764 }
4765 
4766 void vpMbGenericTracker::TrackerWrapper::init(const vpImage<unsigned char> &I)
4767 {
4768  if (!modelInitialised) {
4769  throw vpException(vpException::fatalError, "model not initialized");
4770  }
4771 
4772  if (useScanLine || clippingFlag > 3)
4773  cam.computeFov(I.getWidth(), I.getHeight());
4774 
4775  bool reInitialisation = false;
4776  if (!useOgre) {
4777  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
4778  } else {
4779 #ifdef VISP_HAVE_OGRE
4780  if (!faces.isOgreInitialised()) {
4782 
4784  faces.initOgre(cam);
4785  // Turn off Ogre config dialog display for the next call to this
4786  // function since settings are saved in the ogre.cfg file and used
4787  // during the next call
4788  ogreShowConfigDialog = false;
4789  }
4790 
4791  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
4792 #else
4793  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
4794 #endif
4795  }
4796 
4797  if (useScanLine) {
4800  }
4801 
4802 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4803  if (m_trackerType & KLT_TRACKER)
4805 #endif
4806 
4807  if (m_trackerType & EDGE_TRACKER) {
4809 
4810  bool a = false;
4811  vpMbEdgeTracker::visibleFace(I, cMo, a); // should be useless, but keep it for nbvisiblepolygone
4812 
4813  initMovingEdge(I, cMo);
4814  }
4815 
4816  if (m_trackerType & DEPTH_NORMAL_TRACKER)
4817  vpMbDepthNormalTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthNormalTracker::init(I);
4818 
4819  if (m_trackerType & DEPTH_DENSE_TRACKER)
4820  vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthDenseTracker::init(I);
4821 }
4822 
4823 void vpMbGenericTracker::TrackerWrapper::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
4824  const double radius, const int idFace, const std::string &name)
4825 {
4826  if (m_trackerType & EDGE_TRACKER)
4827  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
4828 
4829 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4830  if (m_trackerType & KLT_TRACKER)
4831  vpMbKltTracker::initCircle(p1, p2, p3, radius, idFace, name);
4832 #endif
4833 }
4834 
4835 void vpMbGenericTracker::TrackerWrapper::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius,
4836  const int idFace, const std::string &name)
4837 {
4838  if (m_trackerType & EDGE_TRACKER)
4839  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
4840 
4841 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4842  if (m_trackerType & KLT_TRACKER)
4843  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
4844 #endif
4845 }
4846 
4847 void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(vpMbtPolygon &polygon)
4848 {
4849  if (m_trackerType & EDGE_TRACKER)
4851 
4852 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4853  if (m_trackerType & KLT_TRACKER)
4855 #endif
4856 
4857  if (m_trackerType & DEPTH_NORMAL_TRACKER)
4859 
4860  if (m_trackerType & DEPTH_DENSE_TRACKER)
4862 }
4863 
4864 void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(vpMbtPolygon &polygon)
4865 {
4866  if (m_trackerType & EDGE_TRACKER)
4868 
4869 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4870  if (m_trackerType & KLT_TRACKER)
4872 #endif
4873 
4874  if (m_trackerType & DEPTH_NORMAL_TRACKER)
4876 
4877  if (m_trackerType & DEPTH_DENSE_TRACKER)
4879 }
4880 
4881 void vpMbGenericTracker::TrackerWrapper::initMbtTracking(const vpImage<unsigned char> *const ptr_I)
4882 {
4883  if (m_trackerType & EDGE_TRACKER) {
4886  }
4887 }
4888 
4889 void vpMbGenericTracker::TrackerWrapper::loadConfigFile(const std::string &configFile)
4890 {
4891  // Load projection error config
4892  vpMbTracker::loadConfigFile(configFile);
4893 
4894 #ifdef VISP_HAVE_XML2
4896 
4897  xmlp.setCameraParameters(cam);
4900 
4901  // Edge
4902  xmlp.setEdgeMe(me);
4903 
4904  // KLT
4905  xmlp.setKltMaxFeatures(10000);
4906  xmlp.setKltWindowSize(5);
4907  xmlp.setKltQuality(0.01);
4908  xmlp.setKltMinDistance(5);
4909  xmlp.setKltHarrisParam(0.01);
4910  xmlp.setKltBlockSize(3);
4911  xmlp.setKltPyramidLevels(3);
4912 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4913  xmlp.setKltMaskBorder(maskBorder);
4914 #endif
4915 
4916  // Depth normal
4917  xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
4918  xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
4919  xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
4920  xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
4921  xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
4922  xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
4923 
4924  // Depth dense
4925  xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
4926  xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
4927 
4928  try {
4929 
4930  std::cout << " *********** Parsing XML for";
4931 
4932  std::vector<std::string> tracker_names;
4933  if (m_trackerType & EDGE_TRACKER)
4934  tracker_names.push_back("Edge");
4935 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4936  if (m_trackerType & KLT_TRACKER)
4937  tracker_names.push_back("Klt");
4938 #endif
4939  if (m_trackerType & DEPTH_NORMAL_TRACKER)
4940  tracker_names.push_back("Depth Normal");
4941  if (m_trackerType & DEPTH_DENSE_TRACKER)
4942  tracker_names.push_back("Depth Dense");
4943 
4944  for (size_t i = 0; i < tracker_names.size(); i++) {
4945  std::cout << " " << tracker_names[i];
4946  if (i == tracker_names.size() - 1) {
4947  std::cout << " ";
4948  }
4949  }
4950 
4951  std::cout << "Model-Based Tracker ************ " << std::endl;
4952  xmlp.parse(configFile);
4953  } catch (...) {
4954  throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
4955  }
4956 
4957  vpCameraParameters camera;
4958  xmlp.getCameraParameters(camera);
4959  setCameraParameters(camera);
4960 
4963 
4964  if (xmlp.hasNearClippingDistance())
4966 
4967  if (xmlp.hasFarClippingDistance())
4969 
4970  if (xmlp.getFovClipping()) {
4972  }
4973 
4974  useLodGeneral = xmlp.getLodState();
4977 
4978  applyLodSettingInConfig = false;
4979  if (this->getNbPolygon() > 0) {
4980  applyLodSettingInConfig = true;
4984  }
4985 
4986  // Edge
4987  vpMe meParser;
4988  xmlp.getEdgeMe(meParser);
4990 
4991 // KLT
4992 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
4993  tracker.setMaxFeatures((int)xmlp.getKltMaxFeatures());
4994  tracker.setWindowSize((int)xmlp.getKltWindowSize());
4995  tracker.setQuality(xmlp.getKltQuality());
4996  tracker.setMinDistance(xmlp.getKltMinDistance());
4997  tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
4998  tracker.setBlockSize((int)xmlp.getKltBlockSize());
4999  tracker.setPyramidLevels((int)xmlp.getKltPyramidLevels());
5000  maskBorder = xmlp.getKltMaskBorder();
5001 
5002  // if(useScanLine)
5003  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
5004 #endif
5005 
5006  // Depth normal
5012 
5013  // Depth dense
5015 #else
5016  std::cerr << "You need the libXML2 to read the config file: " << configFile << std::endl;
5017 #endif
5018 }
5019 
5020 #ifdef VISP_HAVE_PCL
5021 void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
5022  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
5023 {
5024  if (displayFeatures) {
5025  if (m_trackerType & EDGE_TRACKER) {
5027  }
5028  }
5029 
5030 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5031  // KLT
5032  if (m_trackerType & KLT_TRACKER) {
5033  if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) {
5034  vpMbKltTracker::reinit(*ptr_I);
5035  }
5036  }
5037 #endif
5038 
5039  // Looking for new visible face
5040  if (m_trackerType & EDGE_TRACKER) {
5041  bool newvisibleface = false;
5042  vpMbEdgeTracker::visibleFace(*ptr_I, cMo, newvisibleface);
5043 
5044  if (useScanLine) {
5046  faces.computeScanLineRender(cam, ptr_I->getWidth(), ptr_I->getHeight());
5047  }
5048  }
5049 
5050  // Depth normal
5051  if (m_trackerType & DEPTH_NORMAL_TRACKER)
5052  vpMbDepthNormalTracker::computeVisibility(point_cloud->width, point_cloud->height);
5053 
5054  // Depth dense
5055  if (m_trackerType & DEPTH_DENSE_TRACKER)
5056  vpMbDepthDenseTracker::computeVisibility(point_cloud->width, point_cloud->height);
5057 
5058  // Edge
5059  if (m_trackerType & EDGE_TRACKER) {
5061 
5063  // Reinit the moving edge for the lines which need it.
5065 
5066  if (computeProjError) {
5068  }
5069  }
5070 }
5071 
5072 void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
5073  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
5074 {
5075  if (m_trackerType & EDGE_TRACKER) {
5076  try {
5078  } catch (...) {
5079  std::cerr << "Error in moving edge tracking" << std::endl;
5080  throw;
5081  }
5082  }
5083 
5084 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5085  if (m_trackerType & KLT_TRACKER) {
5086  try {
5088  } catch (const vpException &e) {
5089  std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
5090  throw;
5091  }
5092  }
5093 #endif
5094 
5095  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5096  try {
5098  } catch (...) {
5099  std::cerr << "Error in Depth normal tracking" << std::endl;
5100  throw;
5101  }
5102  }
5103 
5104  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5105  try {
5107  } catch (...) {
5108  std::cerr << "Error in Depth dense tracking" << std::endl;
5109  throw;
5110  }
5111  }
5112 }
5113 #endif
5114 
5115 void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
5116  const unsigned int pointcloud_width,
5117  const unsigned int pointcloud_height)
5118 {
5119  if (displayFeatures) {
5120  if (m_trackerType & EDGE_TRACKER) {
5122  }
5123  }
5124 
5125 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5126  // KLT
5127  if (m_trackerType & KLT_TRACKER) {
5128  if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) {
5129  vpMbKltTracker::reinit(*ptr_I);
5130  }
5131  }
5132 #endif
5133 
5134  // Looking for new visible face
5135  if (m_trackerType & EDGE_TRACKER) {
5136  bool newvisibleface = false;
5137  vpMbEdgeTracker::visibleFace(*ptr_I, cMo, newvisibleface);
5138 
5139  if (useScanLine) {
5141  faces.computeScanLineRender(cam, ptr_I->getWidth(), ptr_I->getHeight());
5142  }
5143  }
5144 
5145  // Depth normal
5146  if (m_trackerType & DEPTH_NORMAL_TRACKER)
5147  vpMbDepthNormalTracker::computeVisibility(pointcloud_width, pointcloud_height);
5148 
5149  // Depth dense
5150  if (m_trackerType & DEPTH_DENSE_TRACKER)
5151  vpMbDepthDenseTracker::computeVisibility(pointcloud_width, pointcloud_height);
5152 
5153  // Edge
5154  if (m_trackerType & EDGE_TRACKER) {
5156 
5158  // Reinit the moving edge for the lines which need it.
5160 
5161  if (computeProjError) {
5163  }
5164  }
5165 }
5166 
5167 void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
5168  const std::vector<vpColVector> *const point_cloud,
5169  const unsigned int pointcloud_width,
5170  const unsigned int pointcloud_height)
5171 {
5172  if (m_trackerType & EDGE_TRACKER) {
5173  try {
5175  } catch (...) {
5176  std::cerr << "Error in moving edge tracking" << std::endl;
5177  throw;
5178  }
5179  }
5180 
5181 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5182  if (m_trackerType & KLT_TRACKER) {
5183  try {
5185  } catch (const vpException &e) {
5186  std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
5187  throw;
5188  }
5189  }
5190 #endif
5191 
5192  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
5193  try {
5194  vpMbDepthNormalTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
5195  } catch (...) {
5196  std::cerr << "Error in Depth tracking" << std::endl;
5197  throw;
5198  }
5199  }
5200 
5201  if (m_trackerType & DEPTH_DENSE_TRACKER) {
5202  try {
5203  vpMbDepthDenseTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
5204  } catch (...) {
5205  std::cerr << "Error in Depth dense tracking" << std::endl;
5206  throw;
5207  }
5208  }
5209 }
5210 
5211 void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
5212  const vpHomogeneousMatrix &cMo_, const bool verbose,
5213  const vpHomogeneousMatrix &T)
5214 {
5215  cMo.eye();
5216 
5217  // Edge
5218  vpMbtDistanceLine *l;
5220  vpMbtDistanceCircle *ci;
5221 
5222  for (unsigned int i = 0; i < scales.size(); i++) {
5223  if (scales[i]) {
5224  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
5225  l = *it;
5226  if (l != NULL)
5227  delete l;
5228  l = NULL;
5229  }
5230 
5231  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
5232  ++it) {
5233  cy = *it;
5234  if (cy != NULL)
5235  delete cy;
5236  cy = NULL;
5237  }
5238 
5239  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
5240  ci = *it;
5241  if (ci != NULL)
5242  delete ci;
5243  ci = NULL;
5244  }
5245 
5246  lines[i].clear();
5247  cylinders[i].clear();
5248  circles[i].clear();
5249  }
5250  }
5251 
5252  nline = 0;
5253  ncylinder = 0;
5254  ncircle = 0;
5255  nbvisiblepolygone = 0;
5256 
5257 // KLT
5258 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5259 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
5260  if (cur != NULL) {
5261  cvReleaseImage(&cur);
5262  cur = NULL;
5263  }
5264 #endif
5265 
5266  // delete the Klt Polygon features
5267  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
5268  vpMbtDistanceKltPoints *kltpoly = *it;
5269  if (kltpoly != NULL) {
5270  delete kltpoly;
5271  }
5272  kltpoly = NULL;
5273  }
5274  kltPolygons.clear();
5275 
5276  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
5277  ++it) {
5278  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
5279  if (kltPolyCylinder != NULL) {
5280  delete kltPolyCylinder;
5281  }
5282  kltPolyCylinder = NULL;
5283  }
5284  kltCylinders.clear();
5285 
5286  // delete the structures used to display circles
5287  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
5288  ci = *it;
5289  if (ci != NULL) {
5290  delete ci;
5291  }
5292  ci = NULL;
5293  }
5294  circles_disp.clear();
5295 
5296  firstInitialisation = true;
5297 
5298 #endif
5299 
5300  // Depth normal
5301  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
5302  delete m_depthNormalFaces[i];
5303  m_depthNormalFaces[i] = NULL;
5304  }
5305  m_depthNormalFaces.clear();
5306 
5307  // Depth dense
5308  for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
5309  delete m_depthDenseFaces[i];
5310  m_depthDenseFaces[i] = NULL;
5311  }
5312  m_depthDenseFaces.clear();
5313 
5314  faces.reset();
5315 
5316  loadModel(cad_name, verbose, T);
5317  initFromPose(I, cMo_);
5318 }
5319 
5320 void vpMbGenericTracker::TrackerWrapper::resetTracker()
5321 {
5323 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5325 #endif
5328 }
5329 
5330 void vpMbGenericTracker::TrackerWrapper::setCameraParameters(const vpCameraParameters &camera)
5331 {
5332  this->cam = camera;
5333 
5335 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5337 #endif
5340 }
5341 
5342 void vpMbGenericTracker::TrackerWrapper::setClipping(const unsigned int &flags)
5343 {
5345 }
5346 
5347 void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(const double &dist)
5348 {
5350 }
5351 
5352 void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(const double &dist)
5353 {
5355 }
5356 
5357 void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(const bool &v)
5358 {
5360 #ifdef VISP_HAVE_OGRE
5361  faces.getOgreContext()->setWindowName("TrackerWrapper");
5362 #endif
5363 }
5364 
5365 void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
5366 {
5367  bool performKltSetPose = false;
5368 
5369 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5370  if (m_trackerType & KLT_TRACKER) {
5371  performKltSetPose = true;
5372 
5373  if (useScanLine || clippingFlag > 3)
5374  cam.computeFov(I.getWidth(), I.getHeight());
5375 
5376  vpMbKltTracker::setPose(I, cdMo);
5377  }
5378 #endif
5379 
5380  if (!performKltSetPose) {
5381  cMo = cdMo;
5382  init(I);
5383  return;
5384  }
5385 
5386  if (m_trackerType & EDGE_TRACKER)
5387  resetMovingEdge();
5388 
5389  if (useScanLine) {
5392  }
5393 
5394 #if 0
5395  if (m_trackerType & EDGE_TRACKER) {
5396  initPyramid(I, Ipyramid);
5397 
5398  unsigned int i = (unsigned int) scales.size();
5399  do {
5400  i--;
5401  if(scales[i]){
5402  downScale(i);
5403  initMovingEdge(*Ipyramid[i], cMo);
5404  upScale(i);
5405  }
5406  } while(i != 0);
5407 
5408  cleanPyramid(Ipyramid);
5409  }
5410 #else
5411  if (m_trackerType & EDGE_TRACKER)
5412  initMovingEdge(I, cMo);
5413 #endif
5414 
5415  // Depth normal
5417 
5418  // Depth dense
5420 }
5421 
5422 void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(const bool &flag)
5423 {
5425 }
5426 
5427 void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(const bool &v)
5428 {
5430 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5432 #endif
5435 }
5436 
5437 void vpMbGenericTracker::TrackerWrapper::setTrackerType(const int type)
5438 {
5439  if ((type & (EDGE_TRACKER |
5440 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5441  KLT_TRACKER |
5442 #endif
5444  throw vpException(vpTrackingException::badValue, "bad value for tracker type: !", type);
5445  }
5446 
5447  m_trackerType = type;
5448 }
5449 
5450 void vpMbGenericTracker::TrackerWrapper::testTracking()
5451 {
5452  if (m_trackerType & EDGE_TRACKER) {
5454  }
5455 }
5456 
5457 void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> &
5458 #ifdef VISP_HAVE_PCL
5459  I
5460 #endif
5461 )
5462 {
5463  if ((m_trackerType & (EDGE_TRACKER
5464 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5465  | KLT_TRACKER
5466 #endif
5467  )) == 0) {
5468  std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
5469  return;
5470  }
5471 
5472 #ifdef VISP_HAVE_PCL
5473  track(&I, nullptr);
5474 #endif
5475 }
5476 
5477 #ifdef VISP_HAVE_PCL
5478 void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> *const ptr_I,
5479  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
5480 {
5481  if ((m_trackerType & (EDGE_TRACKER |
5482 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
5483  KLT_TRACKER |
5484 #endif
5486  std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
5487  return;
5488  }
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
5494  ) &&
5495  ptr_I == NULL) {
5496  throw vpException(vpException::fatalError, "Image pointer is NULL!");
5497  }
5498 
5499  if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && point_cloud == nullptr) {
5500  throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
5501  }
5502 
5503  // Back-up cMo in case of exception
5504  vpHomogeneousMatrix cMo_1 = cMo;
5505  try {
5506  preTracking(ptr_I, point_cloud);
5507 
5508  try {
5509  computeVVS(ptr_I);
5510  } catch (...) {
5511  covarianceMatrix = -1;
5512  throw; // throw the original exception
5513  }
5514 
5515  if (m_trackerType == EDGE_TRACKER)
5516  testTracking();
5517 
5518  postTracking(ptr_I, point_cloud);
5519 
5520  } catch (const vpException &e) {
5521  std::cerr << "Exception: " << e.what() << std::endl;
5522  cMo = cMo_1;
5523  throw; // rethrowing the original exception
5524  }
5525 }
5526 #endif
virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
virtual void setDisplayFeatures(const bool displayF)
bool m_computeInteraction
Definition: vpMbTracker.h:191
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
virtual void setFeatureFactors(const std::map< vpTrackerType, double > &mapOfFeatureFactors)
virtual void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0) const
bool computeProjError
Definition: vpMbTracker.h:139
virtual void setDisplayFeatures(const bool displayF)
Definition: vpMbTracker.h:503
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
Definition: vpMbTracker.h:629
virtual void loadConfigFile(const std::string &configFile)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
virtual void setKltThresholdAcceptation(const double th)
virtual unsigned int getKltMaskBorder() const
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:136
void displayPrimitive(const vpImage< unsigned char > &_I)
void setMovingEdge(const vpMe &me)
virtual void setScanLineVisibilityTest(const bool &v)
double getFarClippingDistance() const
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
virtual void setProjectionErrorDisplayArrowLength(const unsigned int length)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void track(const vpImage< unsigned char > &I)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(const bool orderPolygons=true, const bool useVisibility=true, const bool clipPolygon=false)
virtual void setProjectionErrorDisplayArrowThickness(const unsigned int thickness)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
double getKltMinDistance() const
vpColVector m_w
Robust weights.
unsigned int getWidth() const
Definition: vpImage.h:239
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:256
void setKltQuality(const double &q)
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:466
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:149
virtual std::list< vpMbtDistanceKltPoints * > & getFeaturesKlt()
double getNearClippingDistance() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void setReferenceCameraName(const std::string &referenceCameraName)
void setKltPyramidLevels(const unsigned int &pL)
virtual vpMe getMovingEdge() const
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void getCameraParameters(vpCameraParameters &_cam) const
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void loadConfigFile(const std::string &configFile)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
Definition: vpMbTracker.h:543
bool hasNearClippingDistance() const
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
unsigned int getKltBlockSize() const
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:119
virtual void setClipping(const unsigned int &flags)
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
virtual std::vector< cv::Point2f > getKltPoints() const
void setOgreShowConfigDialog(const bool showConfigDialog)
bool modelInitialised
Definition: vpMbTracker.h:129
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void setProjectionErrorDisplay(const bool display)
virtual void setAngleDisappear(const double &a)
virtual void computeVVSInit()
error that can be emited by ViSP classes.
Definition: vpException.h:71
Manage a cylinder used in the model-based tracker.
vpMbScanLine & getMbScanLineRenderer()
Definition: vpMe.h:60
virtual void setMask(const vpImage< bool > &mask)
Definition: vpMbTracker.h:549
Manage the line of a polygon used in the model-based tracker.
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:161
virtual void setDepthDenseFilteringMethod(const int method)
unsigned int getCols() const
Definition: vpArray2D.h:146
double getLodMinLineLengthThreshold() const
virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
void setKltMaskBorder(const unsigned int &mb)
virtual void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0) const
virtual void setMovingEdge(const vpMe &me)
virtual void setCameraParameters(const vpCameraParameters &camera)
void displayFeaturesOnImage(const vpImage< unsigned char > &I, const unsigned int lvl)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:134
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 display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual void computeVVSInteractionMatrixAndResidu()
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...
virtual void setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
Class that defines what is a point.
Definition: vpPoint.h:58
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:117
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
virtual void setNearClippingDistance(const double &dist)
vpMatrix m_L
Interaction matrix.
void setDepthNormalSamplingStepX(const unsigned int stepX)
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:157
double projectionError
Definition: vpMbTracker.h:142
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
const char * what() const
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
bool hasFarClippingDistance() const
void setDepthDenseSamplingStepX(const unsigned int stepX)
Manage a circle used in the model-based tracker.
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4570
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:121
virtual void setKltMaskBorder(const unsigned int &e)
vpColVector m_weightedError
Weighted error.
virtual void initFromPose(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2)
virtual void setDepthNormalPclPlaneEstimationMethod(const int method)
Error that can be emited by the vpTracker class and its derivates.
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 void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbTracker.h:473
void setAngleDisappear(const double &adisappear)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void setProjectionErrorComputation(const bool &flag)
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
void setEdgeMe(const vpMe &_ecm)
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:423
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
void displayPrimitive(const vpImage< unsigned char > &_I)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void initClick(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2, const bool displayHelp=false, const vpHomogeneousMatrix &T1=vpHomogeneousMatrix(), const vpHomogeneousMatrix &T2=vpHomogeneousMatrix())
virtual std::list< vpMbtDistanceCircle * > & getFeaturesCircle()
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:164
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setKltMinDistance(const double &mD)
static double sqr(double x)
Definition: vpMath.h:108
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:183
virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual std::vector< vpImagePoint > getKltImagePoints() const
virtual vpMbtPolygon * getPolygon(const unsigned int index)
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
Generic class defining intrinsic camera parameters.
double getKltHarrisParam() const
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
double getDepthNormalPclPlaneEstimationRansacThreshold() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void getEdgeMe(vpMe &_ecm) const
virtual void setAngleAppear(const double &a)
virtual void computeVVSInteractionMatrixAndResidu()
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:199
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:193
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual void setFarClippingDistance(const double &dist)
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:455
virtual std::map< std::string, int > getCameraTrackerTypes() const
virtual void setProjectionErrorDisplay(const bool display)
Definition: vpMbTracker.h:575
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:146
unsigned int getRows() const
Definition: vpArray2D.h:156
void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
double getAngleDisappear() const
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:151
virtual unsigned int getNbPoints(const unsigned int level=0) const
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
unsigned int getDepthNormalSamplingStepX() const
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:195
virtual double getGoodMovingEdgesRatioThreshold() const
virtual void setDepthDenseFilteringMaxDistance(const double maxDistance)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void computeVVSWeights()
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:102
virtual void computeVVSInteractionMatrixAndResidu()
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
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 getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0) const
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void preTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages, std::map< std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr > &mapOfPointClouds)
virtual std::vector< std::string > getCameraNames() const
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
void setDepthDenseSamplingStepY(const unsigned int stepY)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
virtual void setScanLineVisibilityTest(const bool &v)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
unsigned int getKltPyramidLevels() const
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:197
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:185
static double deg(double rad)
Definition: vpMath.h:95
void computeVisibility(const unsigned int width, const unsigned int height)
virtual void setOgreVisibilityTest(const bool &v)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:144
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:162
void preTracking(const vpImage< unsigned char > &I)
vpColVector m_error
(s - s*)
virtual void init(const vpImage< unsigned char > &I)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:181
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
virtual void setOgreVisibilityTest(const bool &v)
std::string m_referenceCameraName
Name of the reference camera.
unsigned int getKltMaskBorder() const
virtual void setFarClippingDistance(const double &dist)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const 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 setProjectionErrorDisplayArrowLength(const unsigned int length)
Definition: vpMbTracker.h:580
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
vpHomogeneousMatrix inverse() const
virtual int getKltNbPoints() const
unsigned int getDepthDenseSamplingStepX() const
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
virtual std::map< int, vpImagePoint > getKltImagePointsWithId() const
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:153
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void computeVVSInit()
virtual void setDepthDenseFilteringMinDistance(const double minDistance)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
virtual void computeVVSInit()
virtual void computeVVSInteractionMatrixAndResidu()
unsigned int getDepthDenseSamplingStepY() const
virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
unsigned int getHeight() const
Definition: vpImage.h:178
virtual void setTrackerType(const int type)
void setCameraParameters(const vpCameraParameters &_cam)
unsigned int getDepthNormalSamplingStepY() const
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:587
unsigned int getKltMaxFeatures() const
virtual void setClipping(const unsigned int &flags)
virtual vpKltOpencv getKltOpencv() const
void computeVisibility(const unsigned int width, const unsigned int height)
virtual void setProjectionErrorDisplayArrowThickness(const unsigned int thickness)
Definition: vpMbTracker.h:585
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual double getKltThresholdAcceptation() const
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:159
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
virtual void setClipping(const unsigned int &flags)
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void setMask(const vpImage< bool > &mask)
virtual unsigned int getNbPolygon() const
virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
virtual void computeVVS(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
void setDepthNormalPclPlaneEstimationMethod(const int method)
virtual void setFarClippingDistance(const double &dist)
void setKltBlockSize(const unsigned int &bs)
void setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold)
void setKltMaxFeatures(const unsigned int &mF)
virtual void setCameraParameters(const vpCameraParameters &camera)
double getLodMinPolygonAreaThreshold() const
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:155
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:178
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
virtual void computeProjectionError()
int getDepthNormalPclPlaneEstimationMethod() const
void setDepthNormalSamplingStepY(const unsigned int stepY)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
void eye()
Definition: vpMatrix.cpp:360
virtual void computeVVSCheckLevenbergMarquardt(const 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 int getTrackerType() const
virtual void setLod(const bool useLod, const std::string &name="")
unsigned int getKltWindowSize() const
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:208
virtual void setGoodMovingEdgesRatioThreshold(const double threshold)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setCameraParameters(const vpCameraParameters &camera)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:570
virtual void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)