Visual Servoing Platform  version 3.2.0
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)
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:256
bool m_computeInteraction
Definition: vpMbTracker.h:191
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
unsigned int getDepthNormalSamplingStepY() const
virtual void setFeatureFactors(const std::map< vpTrackerType, double > &mapOfFeatureFactors)
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)
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)
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)
vpColVector m_w
Robust weights.
int getDepthNormalPclPlaneEstimationMethod() const
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()
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual std::map< std::string, int > getCameraTrackerTypes() const
void setKltPyramidLevels(const unsigned int &pL)
virtual int getTrackerType() const
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void loadConfigFile(const std::string &configFile)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
Definition: vpMbTracker.h:543
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
double getFarClippingDistance() const
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)
double getDepthNormalPclPlaneEstimationRansacThreshold() 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()
unsigned int getRows() const
Definition: vpArray2D.h:156
Definition: vpMe.h:60
vpHomogeneousMatrix inverse() const
virtual void setMask(const vpImage< bool > &mask)
Definition: vpMbTracker.h:549
Manage the line of a polygon used in the model-based tracker.
virtual double getGoodMovingEdgesRatioThreshold() const
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void computeVVSInteractionMatrixAndResidu()
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:161
void getCameraParameters(vpCameraParameters &_cam) const
virtual void setDepthDenseFilteringMethod(const int method)
unsigned int getKltBlockSize() const
void setKltMaskBorder(const unsigned int &mb)
double getNearClippingDistance() 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)
virtual int getKltNbPoints() const
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 computeVVS(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages)
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)
unsigned int getDepthDenseSamplingStepY() const
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.
unsigned int getCols() const
Definition: vpArray2D.h:146
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)
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:423
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
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)
virtual void getLcylinder(std::list< vpMbtDistanceCylinder *> &cylindersList, const unsigned int level=0) const
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)
void setEdgeMe(const vpMe &_ecm)
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual std::vector< cv::Point2f > getKltPoints() const
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 unsigned int getNbPolygon() const
virtual vpMbtPolygon * getPolygon(const unsigned int index)
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
Generic class defining intrinsic camera parameters.
virtual unsigned int getKltMaskBorder() const
unsigned int getKltWindowSize() const
virtual unsigned int getNbPoints(const unsigned int level=0) const
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
unsigned int getKltMaxFeatures() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void setAngleAppear(const double &a)
virtual double getKltThresholdAcceptation() const
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 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 getDepthNormalSamplingStepX() const
void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:151
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 >())
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 void setDepthDenseFilteringMaxDistance(const double maxDistance)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void computeVVSWeights()
void setAngleAppear(const double &aappear)
const char * what() const
static double rad(double deg)
Definition: vpMath.h: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 setCameraParameters(const vpCameraParameters &camera)
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="")
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
unsigned int getHeight() const
Definition: vpImage.h:178
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.
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 preTracking(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages, std::map< std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr > &mapOfPointClouds)
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
double getLodMinLineLengthThreshold() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
virtual std::vector< std::string > getCameraNames() const
virtual void getLline(std::list< vpMbtDistanceLine *> &linesList, const unsigned int level=0) 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 void getLcircle(std::list< vpMbtDistanceCircle *> &circlesList, const unsigned int level=0) 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)
virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
unsigned int getDepthDenseSamplingStepX() const
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()
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
virtual void computeVVSInteractionMatrixAndResidu()
virtual std::map< int, vpImagePoint > getKltImagePointsWithId() const
virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
unsigned int getKltMaskBorder() const
virtual void setTrackerType(const int type)
void setCameraParameters(const vpCameraParameters &_cam)
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:587
virtual void setClipping(const unsigned int &flags)
void computeVisibility(const unsigned int width, const unsigned int height)
virtual void setProjectionErrorDisplayArrowThickness(const unsigned int thickness)
Definition: vpMbTracker.h:585
void getEdgeMe(vpMe &_ecm) const
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
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 void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
virtual vpKltOpencv getKltOpencv() const
virtual std::vector< vpImagePoint > getKltImagePoints() const
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)
unsigned int getWidth() const
Definition: vpImage.h:239
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
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()
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)
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
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 vpMe getMovingEdge() const
virtual void setLod(const bool useLod, const std::string &name="")
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:208
double getLodMinPolygonAreaThreshold() const
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
unsigned int getKltPyramidLevels() const
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)