Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpMbEdgeMultiTracker.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  * Model-based edge tracker with multiple cameras.
33  *
34  * Authors:
35  * Souriya Trinh
36  *
37  *****************************************************************************/
38 
44 #include <visp3/core/vpDebug.h>
45 #include <visp3/core/vpExponentialMap.h>
46 #include <visp3/core/vpTrackingException.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
48 #include <visp3/mbt/vpMbEdgeMultiTracker.h>
49 
50 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
51 
56  : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(),
57  m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti()
58 {
59  m_mapOfEdgeTrackers["Camera"] = new vpMbEdgeTracker;
60 
61  // Add default camera transformation matrix
63 }
64 
70 vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const unsigned int nbCameras)
73 {
74 
75  if (nbCameras == 0) {
76  throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !");
77  } else if (nbCameras == 1) {
78  m_mapOfEdgeTrackers["Camera"] = new vpMbEdgeTracker;
79 
80  // Add default camera transformation matrix
82  } else if (nbCameras == 2) {
83  m_mapOfEdgeTrackers["Camera1"] = new vpMbEdgeTracker;
84  m_mapOfEdgeTrackers["Camera2"] = new vpMbEdgeTracker;
85 
86  // Add default camera transformation matrix
89 
90  // Set by default the reference camera to Camera1
91  m_referenceCameraName = "Camera1";
92  } else {
93  for (unsigned int i = 1; i <= nbCameras; i++) {
94  std::stringstream ss;
95  ss << "Camera" << i;
96  m_mapOfEdgeTrackers[ss.str()] = new vpMbEdgeTracker;
97 
98  // Add default camera transformation matrix
100  }
101 
102  // Set by default the reference camera to the first one
104  }
105 }
106 
114 vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const std::vector<std::string> &cameraNames)
117 {
118 
119  if (cameraNames.empty()) {
120  throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !");
121  }
122 
123  for (std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
125  }
126 
127  // Set by default the reference camera
128  m_referenceCameraName = cameraNames.front();
129 }
130 
135 {
136  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
137  it != m_mapOfEdgeTrackers.end(); ++it) {
138  delete it->second;
139  }
140 
141  m_mapOfEdgeTrackers.clear();
142 
144 }
145 
146 void vpMbEdgeMultiTracker::cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid)
147 {
148  for (std::map<std::string, std::vector<const vpImage<unsigned char> *> >::iterator it1 = pyramid.begin();
149  it1 != pyramid.end(); ++it1) {
150  if (it1->second.size() > 0) {
151  it1->second[0] = NULL;
152  for (size_t i = 1; i < it1->second.size(); i++) {
153  if (it1->second[i] != NULL) {
154  delete it1->second[i];
155  it1->second[i] = NULL;
156  }
157  }
158  it1->second.clear();
159  }
160  }
161 }
162 
164 {
165  if (computeProjError) {
166  double rawTotalProjectionError = 0.0;
167  unsigned int nbTotalFeaturesUsed = 0;
168  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
169  it != m_mapOfEdgeTrackers.end(); ++it) {
170  double curProjError = it->second->getProjectionError();
171  unsigned int nbFeaturesUsed = it->second->nbFeaturesForProjErrorComputation;
172 
173  if (nbFeaturesUsed > 0) {
174  nbTotalFeaturesUsed += nbFeaturesUsed;
175  rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed);
176  }
177  }
178 
179  if (nbTotalFeaturesUsed > 0) {
180  nbFeaturesForProjErrorComputation = nbTotalFeaturesUsed;
181  projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
182  } else {
184  projectionError = 90.0;
185  }
186  }
187 }
188 
189 void vpMbEdgeMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
190  const unsigned int lvl)
191 {
192  computeVVSInit();
193  unsigned int nbrow = m_error_edgeMulti.getRows();
194 
195  unsigned int iter = 0;
196  // Parametre pour la premiere phase d'asservissement
197  bool reloop = true;
198 
199  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
200 
201  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
202  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
203  it != m_mapOfCameraTransformationMatrix.end(); ++it) {
205  cVo.buildFrom(it->second);
206  mapOfVelocityTwist[it->first] = cVo;
207  }
208 
209  // std::cout << "\n\n\ncMo used before the first phase=\n" << cMo <<
210  // std::endl;
211 
212  /*** First phase ***/
213 
214  vpMbEdgeTracker *edge;
215  while (reloop == true && iter < 10) {
216  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
217  it != m_mapOfEdgeTrackers.end(); ++it) {
218  it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
219  }
220 
221  double count = 0;
222  reloop = false;
223 
224  unsigned int start_idx = 0;
225  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
226  it != m_mapOfEdgeTrackers.end(); ++it) {
227  double count_tmp = 0.0;
228  edge = it->second;
229  edge->computeVVSFirstPhase(*mapOfImages[it->first], iter, count_tmp, lvl);
230  count += count_tmp;
231 
232  m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0);
233  m_factor.insert(start_idx, edge->m_factor);
234  m_w_edgeMulti.insert(start_idx, edge->m_w_edge);
235  m_error_edgeMulti.insert(start_idx, edge->m_error_edge);
236 
237  start_idx += edge->m_error_edge.getRows();
238  }
239 
240  count = count / (double)nbrow;
241  if (count < 0.85) {
242  reloop = true;
243  }
244 
245  computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);
246 
247  iter++;
248  }
249 
250  // std::cout << "\n\t First minimization in " << iter << " iteration give
251  // as initial cMo: \n" << cMo << std::endl; std::cout << "Residual=" <<
252  // m_error.sum() / m_error.size() << std::endl;
253 
254  /*** Second phase ***/
255 
256  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
257  it != m_mapOfEdgeTrackers.end(); ++it) {
258  edge = it->second;
259  edge->m_w_edge = 1;
260  }
261 
262  iter = 0;
263 
264  vpHomogeneousMatrix cMoPrev;
265  vpColVector W_true(nbrow);
266  vpMatrix L_true;
267  vpMatrix LVJ_true;
268 
269  double mu = m_initialMu;
270  vpColVector m_error_prev;
271  vpColVector m_w_prev;
272 
273  double residu_1 = 1e3;
274  double r = 1e3 - 1;
275 
276  // For computeVVSPoseEstimation
277  vpColVector LTR;
278  vpColVector v;
279  vpMatrix LTL;
280 
281  // while ( ((int)((residu_1 - r)*1e8) != 0 ) && (iter<30))
282  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
283  computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
284 
285  bool reStartFromLastIncrement = false;
286  computeVVSCheckLevenbergMarquardt(iter, m_error_edgeMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
287  &m_w_prev);
288 
289  if (!reStartFromLastIncrement) {
291 
292  L_true = m_L_edgeMulti;
294 
295  if (computeCovariance) {
296  L_true = m_L_edgeMulti;
297  if (!isoJoIdentity_) {
298  cVo.buildFrom(cMo);
299  LVJ_true = (m_L_edgeMulti * cVo * oJo);
300  }
301  }
302 
303  double wi = 0.0, eri = 0.0;
304  double num = 0.0, den = 0.0;
305  if ((iter == 0) || m_computeInteraction) {
306  for (unsigned int i = 0; i < nbrow; i++) {
307  wi = m_w_edgeMulti[i] * m_factor[i];
308  W_true[i] = wi;
309  eri = m_error_edgeMulti[i];
310  num += wi * vpMath::sqr(eri);
311  den += wi;
312 
313  m_weightedError_edgeMulti[i] = wi * eri;
314 
315  for (unsigned int j = 0; j < 6; j++) {
316  m_L_edgeMulti[i][j] = wi * m_L_edgeMulti[i][j];
317  }
318  }
319  } else {
320  for (unsigned int i = 0; i < nbrow; i++) {
321  wi = m_w_edgeMulti[i] * m_factor[i];
322  W_true[i] = wi;
323  eri = m_error_edgeMulti[i];
324  num += wi * vpMath::sqr(eri);
325  den += wi;
326 
327  m_weightedError_edgeMulti[i] = wi * eri;
328  }
329  }
330 
331  residu_1 = r;
332  r = sqrt(num / den); // Le critere d'arret prend en compte le poids
333 
335  m_error_prev, LTR, mu, v, &m_w_edgeMulti, &m_w_prev);
336 
337  cMoPrev = cMo;
339  }
340 
341  iter++;
342  }
343 
344  // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
345 
346  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edgeMulti);
347 
348  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
349  it != m_mapOfEdgeTrackers.end(); ++it) {
350  edge = it->second;
351  edge->updateMovingEdgeWeights();
352  }
353 }
354 
355 void vpMbEdgeMultiTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
356 {
357  unsigned int nerror = m_weightedError_edgeMulti.getRows();
358 
359  double wi, eri;
360  if ((iter == 0) || m_computeInteraction) {
361  for (unsigned int i = 0; i < nerror; i++) {
362  wi = m_w_edgeMulti[i] * m_factor[i];
363  eri = m_error_edgeMulti[i];
364 
365  m_weightedError_edgeMulti[i] = wi * eri;
366 
367  for (unsigned int j = 0; j < 6; j++) {
368  m_L_edgeMulti[i][j] = wi * m_L_edgeMulti[i][j];
369  }
370  }
371  } else {
372  for (unsigned int i = 0; i < nerror; i++) {
373  wi = m_w_edgeMulti[i] * m_factor[i];
374  eri = m_error_edgeMulti[i];
375 
376  m_weightedError_edgeMulti[i] = wi * eri;
377  }
378  }
379 
381 
382  // If all the 6 dof should be estimated, we check if the interaction matrix
383  // is full rank. If not we remove automatically the dof that cannot be
384  // estimated This is particularly useful when consering circles (rank 5) and
385  // cylinders (rank 4)
386  if (isoJoIdentity_) {
387  cVo.buildFrom(cMo);
388 
389  vpMatrix K; // kernel
390  unsigned int rank = (m_L_edgeMulti * cVo).kernel(K);
391  if (rank == 0) {
392  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
393  }
394  if (rank != 6) {
395  vpMatrix I; // Identity
396  I.eye(6);
397  oJo = I - K.AtA();
398 
399  isoJoIdentity_ = false;
400  }
401  }
402 
403  vpColVector v;
404  vpMatrix LTL;
405  vpColVector LTR;
406 
407  if (isoJoIdentity_) {
408  LTL = m_L_edgeMulti.AtA();
410  v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
411  } else {
412  cVo.buildFrom(cMo);
413  vpMatrix LVJ = (m_L_edgeMulti * cVo * oJo);
414  vpMatrix LVJTLVJ = (LVJ).AtA();
415  vpColVector LVJTR;
417  v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
418  v = cVo * v;
419  }
420 
422 }
423 
425 {
426  unsigned int nbrow = 0;
427 
428  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
429  it != m_mapOfEdgeTrackers.end(); ++it) {
430  vpMbEdgeTracker *edge = it->second;
431 
432  try {
433  edge->computeVVSInit();
434  nbrow += edge->m_error_edge.getRows();
435  } catch (...) {
436  }
437  }
438 
439  if (nbrow < 4) {
441  "No data found to compute the interaction matrix...");
442  }
443 
444  // Initialize with correct size
445  m_L_edgeMulti.resize(nbrow, 6, false, false);
446  m_w_edgeMulti.resize(nbrow, false);
447  m_error_edgeMulti.resize(nbrow, false);
448  m_weightedError_edgeMulti.resize(nbrow, false);
449  m_factor.resize(nbrow, false);
450 }
451 
453 {
454  throw vpException(vpException::fatalError, "vpMbEdgeMultiTracker::"
455  "computeVVSInteractionMatrixAndR"
456  "esidu() should not be called!");
457 }
458 
460  std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
461  std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
462 {
463  unsigned int start_idx = 0;
464 
465  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
466  it != m_mapOfEdgeTrackers.end(); ++it) {
467  vpMbEdgeTracker *edge = it->second;
468  edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
469 
470  edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]);
471 
472  m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0);
473  m_error_edgeMulti.insert(start_idx, edge->m_error_edge);
474 
475  start_idx += edge->m_error_edge.getRows();
476  }
477 }
478 
480 {
481  unsigned int start_idx = 0;
482 
483  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
484  it != m_mapOfEdgeTrackers.end(); ++it) {
485  vpMbEdgeTracker *edge = it->second;
486 
487  // Compute weights
488  edge->computeVVSWeights();
489 
490  m_w_edgeMulti.insert(start_idx, edge->m_w_edge);
491  start_idx += edge->m_w_edge.getRows();
492  }
493 }
494 
507  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
508  const bool displayFullModel)
509 {
510 
511  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
512  if (it != m_mapOfEdgeTrackers.end()) {
513  it->second->display(I, cMo_, cam_, col, thickness, displayFullModel);
514  } else {
515  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
516  }
517 }
518 
531  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
532  const bool displayFullModel)
533 {
534 
535  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
536  if (it != m_mapOfEdgeTrackers.end()) {
537  it->second->display(I, cMo_, cam_, col, thickness, displayFullModel);
538  } else {
539  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
540  }
541 }
542 
559  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
560  const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &col,
561  const unsigned int thickness, const bool displayFullModel)
562 {
563 
564  if (m_mapOfEdgeTrackers.size() == 2) {
565  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
566  it->second->display(I1, c1Mo, cam1, col, thickness, displayFullModel);
567  ++it;
568 
569  it->second->display(I2, c2Mo, cam2, col, thickness, displayFullModel);
570  } else {
571  std::cerr << "This display is only for the stereo case ! There are " << m_mapOfEdgeTrackers.size() << " camera !"
572  << std::endl;
573  }
574 }
575 
592  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
593  const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &col,
594  const unsigned int thickness, const bool displayFullModel)
595 {
596 
597  if (m_mapOfEdgeTrackers.size() == 2) {
598  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
599  it->second->display(I1, c1Mo, cam1, col, thickness, displayFullModel);
600  ++it;
601 
602  it->second->display(I2, c2Mo, cam2, col, thickness, displayFullModel);
603  } else {
604  std::cerr << "This display is only for the stereo case ! There are " << m_mapOfEdgeTrackers.size() << " cameras !"
605  << std::endl;
606  }
607 }
608 
621 void vpMbEdgeMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
622  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
623  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
624  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
625 {
626 
627  // Display only for the given images
628  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
629  it_img != mapOfImages.end(); ++it_img) {
630  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(it_img->first);
631  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
632  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
633 
634  if (it_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
635  it_cam != mapOfCameraParameters.end()) {
636  it_edge->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
637  } else {
638  std::cerr << "Missing elements !" << std::endl;
639  }
640  }
641 }
642 
655 void vpMbEdgeMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
656  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
657  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
658  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
659 {
660 
661  // Display only for the given images
662  for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
663  it_img != mapOfImages.end(); ++it_img) {
664  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(it_img->first);
665  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
666  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
667 
668  if (it_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
669  it_cam != mapOfCameraParameters.end()) {
670  it_edge->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
671  } else {
672  std::cerr << "Missing elements !" << std::endl;
673  }
674  }
675 }
676 
682 std::vector<std::string> vpMbEdgeMultiTracker::getCameraNames() const
683 {
684  std::vector<std::string> cameraNames;
685 
686  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
687  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
688  cameraNames.push_back(it_edge->first);
689  }
690 
691  return cameraNames;
692 }
693 
700 {
701  // Get the reference camera parameters
702  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
703  if (it != m_mapOfEdgeTrackers.end()) {
704  it->second->getCameraParameters(camera);
705  } else {
706  std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl;
707  }
708 }
709 
717 {
718  if (m_mapOfEdgeTrackers.size() == 2) {
719  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
720  it->second->getCameraParameters(cam1);
721  ++it;
722 
723  it->second->getCameraParameters(cam2);
724  } else {
725  std::cerr << "Problem with the number of cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !"
726  << std::endl;
727  }
728 }
729 
736 void vpMbEdgeMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const
737 {
738  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
739  if (it != m_mapOfEdgeTrackers.end()) {
740  it->second->getCameraParameters(camera);
741  } else {
742  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
743  }
744 }
745 
751 void vpMbEdgeMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
752 {
753  // Clear the input map
754  mapOfCameraParameters.clear();
755 
756  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
757  it != m_mapOfEdgeTrackers.end(); ++it) {
758  vpCameraParameters cam_;
759  it->second->getCameraParameters(cam_);
760  mapOfCameraParameters[it->first] = cam_;
761  }
762 }
763 
771 unsigned int vpMbEdgeMultiTracker::getClipping(const std::string &cameraName) const
772 {
773  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
774  if (it != m_mapOfEdgeTrackers.end()) {
775  return it->second->getClipping();
776  } else {
777  std::cerr << "Cannot find camera: " << cameraName << std::endl;
778  }
779 
780  return vpMbTracker::getClipping();
781 }
782 
789 {
790  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
791  if (it != m_mapOfEdgeTrackers.end()) {
792  return it->second->getFaces();
793  }
794 
795  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl;
796  return faces;
797 }
798 
805 {
806  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
807  if (it != m_mapOfEdgeTrackers.end()) {
808  return it->second->getFaces();
809  }
810 
811  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
812  return faces;
813 }
814 
820 std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeMultiTracker::getFaces() const
821 {
822  std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > mapOfFaces;
823  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
824  it != m_mapOfEdgeTrackers.end(); ++it) {
825  mapOfFaces[it->first] = it->second->faces;
826  }
827 
828  return mapOfFaces;
829 }
830 
841 void vpMbEdgeMultiTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
842 {
843  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
844 
845  if (it_edge != m_mapOfEdgeTrackers.end()) {
846  it_edge->second->getLcircle(circlesList, level);
847  } else {
848  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
849  }
850 }
851 
863 void vpMbEdgeMultiTracker::getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
864  const unsigned int level) const
865 {
866  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
867  if (it != m_mapOfEdgeTrackers.end()) {
868  it->second->getLcircle(circlesList, level);
869  } else {
870  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
871  }
872 }
873 
884 void vpMbEdgeMultiTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList,
885  const unsigned int level) const
886 {
887  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
888 
889  if (it_edge != m_mapOfEdgeTrackers.end()) {
890  it_edge->second->getLcylinder(cylindersList, level);
891  } else {
892  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
893  }
894 }
895 
907 void vpMbEdgeMultiTracker::getLcylinder(const std::string &cameraName,
908  std::list<vpMbtDistanceCylinder *> &cylindersList,
909  const unsigned int level) const
910 {
911  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
912  if (it != m_mapOfEdgeTrackers.end()) {
913  it->second->getLcylinder(cylindersList, level);
914  } else {
915  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
916  }
917 }
918 
929 void vpMbEdgeMultiTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
930 {
931  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
932 
933  if (it_edge != m_mapOfEdgeTrackers.end()) {
934  it_edge->second->getLline(linesList, level);
935  } else {
936  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
937  }
938 }
939 
951 void vpMbEdgeMultiTracker::getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
952  const unsigned int level) const
953 {
954  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
955  if (it != m_mapOfEdgeTrackers.end()) {
956  it->second->getLline(linesList, level);
957  } else {
958  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
959  }
960 }
961 
969 {
970  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
971  if (it_edge != m_mapOfEdgeTrackers.end()) {
972  it_edge->second->getMovingEdge(p_me);
973  } else {
974  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl;
975  }
976 }
977 
984 {
985  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
986  vpMe me_tmp;
987  if (it_edge != m_mapOfEdgeTrackers.end()) {
988  it_edge->second->getMovingEdge(me_tmp);
989  } else {
990  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl;
991  }
992 
993  return me_tmp;
994 }
995 
1003 void vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName, vpMe &p_me) const
1004 {
1005  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
1006  if (it != m_mapOfEdgeTrackers.end()) {
1007  it->second->getMovingEdge(p_me);
1008  } else {
1009  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
1010  }
1011 }
1012 
1020 vpMe vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName) const
1021 {
1022  vpMe me_tmp;
1023  getMovingEdge(cameraName, me_tmp);
1024  return me_tmp;
1025 }
1026 
1039 unsigned int vpMbEdgeMultiTracker::getNbPoints(const unsigned int level) const
1040 {
1041  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1042 
1043  unsigned int nbGoodPoints = 0;
1044  if (it_edge != m_mapOfEdgeTrackers.end()) {
1045 
1046  nbGoodPoints += it_edge->second->getNbPoints(level);
1047  } else {
1048  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl;
1049  }
1050 
1051  return nbGoodPoints;
1052 }
1053 
1067 unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, const unsigned int level) const
1068 {
1069  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
1070  if (it != m_mapOfEdgeTrackers.end()) {
1071  return it->second->getNbPoints(level);
1072  } else {
1073  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
1074  }
1075 
1076  return 0;
1077 }
1078 
1085 {
1086  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1087  if (it != m_mapOfEdgeTrackers.end()) {
1088  return it->second->getNbPolygon();
1089  }
1090 
1091  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl;
1092  return 0;
1093 }
1094 
1101 unsigned int vpMbEdgeMultiTracker::getNbPolygon(const std::string &cameraName) const
1102 {
1103  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
1104  if (it != m_mapOfEdgeTrackers.end()) {
1105  return it->second->getNbPolygon();
1106  }
1107 
1108  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
1109  return 0;
1110 }
1111 
1118 std::map<std::string, unsigned int> vpMbEdgeMultiTracker::getMultiNbPolygon() const
1119 {
1120  std::map<std::string, unsigned int> mapOfNbPolygons;
1121  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1122  it != m_mapOfEdgeTrackers.end(); ++it) {
1123  mapOfNbPolygons[it->first] = it->second->getNbPolygon();
1124  }
1125 
1126  return mapOfNbPolygons;
1127 }
1128 
1136 {
1137  if (m_mapOfEdgeTrackers.size() == 2) {
1138  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1139  it->second->getPose(c1Mo);
1140  ++it;
1141 
1142  it->second->getPose(c2Mo);
1143  } else {
1144  std::cerr << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !" << std::endl;
1145  }
1146 }
1147 
1156 void vpMbEdgeMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const
1157 {
1158  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
1159  if (it != m_mapOfEdgeTrackers.end()) {
1160  it->second->getPose(cMo_);
1161  } else {
1162  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
1163  }
1164 }
1165 
1171 void vpMbEdgeMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
1172 {
1173  // Clear the map
1174  mapOfCameraPoses.clear();
1175 
1176  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1177  it != m_mapOfEdgeTrackers.end(); ++it) {
1178  vpHomogeneousMatrix cMo_;
1179  it->second->getPose(cMo_);
1180  mapOfCameraPoses[it->first] = cMo_;
1181  }
1182 }
1183 
1185 
1186 #ifdef VISP_HAVE_MODULE_GUI
1187 
1197 void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
1198  const std::string &displayFile)
1199 {
1200  if (m_mapOfEdgeTrackers.empty()) {
1201  throw vpException(vpTrackingException::initializationError, "There is no camera !");
1202  } else if (m_mapOfEdgeTrackers.size() > 1) {
1203  throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
1204  } else {
1205  // Get the vpMbEdgeTracker object for the reference camera name
1206  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1207  if (it != m_mapOfEdgeTrackers.end()) {
1208  it->second->initClick(I, points3D_list, displayFile);
1209  it->second->getPose(cMo);
1210  } else {
1211  std::stringstream ss;
1212  ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
1214  }
1215  }
1216 }
1217 
1249 void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
1250  const bool displayHelp, const vpHomogeneousMatrix &T)
1251 {
1252  if (m_mapOfEdgeTrackers.empty()) {
1253  throw vpException(vpTrackingException::initializationError, "There is no camera !");
1254  } else if (m_mapOfEdgeTrackers.size() > 1) {
1255  throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
1256  } else {
1257  // Get the vpMbEdgeTracker object for the reference camera name
1258  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1259  if (it != m_mapOfEdgeTrackers.end()) {
1260  it->second->initClick(I, initFile, displayHelp, T);
1261  it->second->getPose(cMo);
1262  } else {
1263  std::stringstream ss;
1264  ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
1266  }
1267  }
1268 }
1269 
1306  const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
1307  const bool firstCameraIsReference)
1308 {
1309  if (m_mapOfEdgeTrackers.size() == 2) {
1310  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1311  it->second->initClick(I1, initFile1, displayHelp);
1312 
1313  if (firstCameraIsReference) {
1314  // Set the reference cMo
1315  it->second->getPose(cMo);
1316 
1317  // Set the reference camera parameters
1318  it->second->getCameraParameters(this->cam);
1319  }
1320 
1321  ++it;
1322 
1323  it->second->initClick(I2, initFile2, displayHelp);
1324 
1325  if (!firstCameraIsReference) {
1326  // Set the reference cMo
1327  it->second->getPose(cMo);
1328 
1329  // Set the reference camera parameters
1330  it->second->getCameraParameters(this->cam);
1331  }
1332  } else {
1333  std::stringstream ss;
1334  ss << "Cannot init click ! Require two cameras but there are " << m_mapOfEdgeTrackers.size() << " cameras !";
1336  }
1337 }
1338 
1368 void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1369  const std::string &initFile, const bool displayHelp)
1370 {
1371  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1372  if (it_edge != m_mapOfEdgeTrackers.end()) {
1373  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1374  mapOfImages.find(m_referenceCameraName);
1375 
1376  if (it_img != mapOfImages.end()) {
1377  // Init click on reference camera
1378  it_edge->second->initClick(*it_img->second, initFile, displayHelp);
1379 
1380  // Set the reference cMo
1381  it_edge->second->getPose(cMo);
1382 
1383  // Set the pose for the others cameras
1384  for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
1385  if (it_edge->first != m_referenceCameraName) {
1386  it_img = mapOfImages.find(it_edge->first);
1387  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1388  m_mapOfCameraTransformationMatrix.find(it_edge->first);
1389 
1390  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1391  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1392  it_edge->second->setPose(*it_img->second, cCurrentMo);
1393  } else {
1394  std::stringstream ss;
1395  ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !";
1397  }
1398  }
1399  }
1400  } else {
1401  std::stringstream ss;
1402  ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !";
1404  }
1405  } else {
1406  std::stringstream ss;
1407  ss << "Cannot init click ! The reference camera: " << m_referenceCameraName << " does not exist !";
1409  }
1410 }
1411 
1441 void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1442  const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp)
1443 {
1444  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1445  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1446  mapOfImages.find(m_referenceCameraName);
1447  std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
1448 
1449  if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1450  // InitClick for the reference camera
1451  it_edge->second->initClick(*it_img->second, it_initFile->second, displayHelp);
1452 
1453  // Get reference camera pose
1454  it_edge->second->getPose(cMo);
1455  } else {
1456  throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !");
1457  }
1458 
1459  // Vector of missing pose matrices for cameras
1460  std::vector<std::string> vectorOfMissingCameraPoses;
1461 
1462  // InitClick for all the cameras that have an initFile
1463  for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
1464  if (it_edge->first != m_referenceCameraName) {
1465  it_img = mapOfImages.find(it_edge->first);
1466  it_initFile = mapOfInitFiles.find(it_edge->first);
1467 
1468  if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1469  it_edge->second->initClick(*it_img->second, it_initFile->second, displayHelp);
1470  } else {
1471  vectorOfMissingCameraPoses.push_back(it_edge->first);
1472  }
1473  }
1474  }
1475 
1476  // SetPose for cameras that do not have an initFile
1477  for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
1478  it1 != vectorOfMissingCameraPoses.end(); ++it1) {
1479  it_img = mapOfImages.find(*it1);
1480  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1482 
1483  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1484  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1485  m_mapOfEdgeTrackers[*it1]->setPose(*it_img->second, cCurrentMo);
1486  } else {
1487  std::stringstream ss;
1488  ss << "Missing image or missing camera transformation matrix ! Cannot "
1489  "set the pose for camera: "
1490  << (*it1) << " !";
1492  }
1493  }
1494 }
1495 #endif //#ifdef VISP_HAVE_MODULE_GUI
1496 
1516 void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1517 {
1518  // Monocular case only !
1519  if (m_mapOfEdgeTrackers.size() > 1) {
1521  "This method can only be used for the monocular case !");
1522  }
1523 
1524  char s[FILENAME_MAX];
1525  std::fstream finit;
1526  vpPoseVector init_pos;
1527 
1528  std::string ext = ".pos";
1529  size_t pos = initFile.rfind(ext);
1530 
1531  if (pos == initFile.size() - ext.size() && pos != 0)
1532  sprintf(s, "%s", initFile.c_str());
1533  else
1534  sprintf(s, "%s.pos", initFile.c_str());
1535 
1536  finit.open(s, std::ios::in);
1537  if (finit.fail()) {
1538  std::cerr << "cannot read " << s << std::endl;
1539  throw vpException(vpException::ioError, "cannot read init file");
1540  }
1541 
1542  for (unsigned int i = 0; i < 6; i += 1) {
1543  finit >> init_pos[i];
1544  }
1545 
1546  // Set the new pose for the reference camera
1547  cMo.buildFrom(init_pos);
1548 
1549  // Init for the reference camera
1550  std::map<std::string, vpMbEdgeTracker *>::iterator it_ref = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1551  if (it_ref == m_mapOfEdgeTrackers.end()) {
1552  throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
1553  }
1554 
1555  it_ref->second->cMo = cMo;
1556  it_ref->second->init(I);
1557 }
1558 
1566 {
1567  if (m_mapOfEdgeTrackers.size() != 1) {
1568  std::stringstream ss;
1569  ss << "This method requires exactly one camera, there are " << m_mapOfEdgeTrackers.size() << " cameras !";
1571  }
1572 
1573  this->cMo = cMo_;
1574 
1575  // Init for the reference camera
1576  std::map<std::string, vpMbEdgeTracker *>::iterator it_ref = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1577  if (it_ref == m_mapOfEdgeTrackers.end()) {
1578  throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
1579  }
1580 
1581  it_ref->second->cMo = cMo;
1582  it_ref->second->init(I);
1583 }
1584 
1592 {
1593  vpHomogeneousMatrix _cMo(cPo);
1595 }
1596 
1608  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
1609  const bool firstCameraIsReference)
1610 {
1611  // For Edge, initFromPose has the same behavior than setPose
1612  // So, for convenience we call setPose
1613  vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1614 }
1615 
1623 void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1624  const vpHomogeneousMatrix &cMo_)
1625 {
1626  // For Edge, initFromPose has the same behavior than setPose
1627  // So, for convenience we call setPose
1628  vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_);
1629 }
1630 
1637 void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1638  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
1639 {
1640  // For Edge, initFromPose has the same behavior than setPose
1641  // So, for convenience we call setPose
1642  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
1643 }
1644 
1645 void vpMbEdgeMultiTracker::initPyramid(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1646  std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid)
1647 {
1648  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it = mapOfImages.begin();
1649  it != mapOfImages.end(); ++it) {
1650  pyramid[it->first].resize(scales.size());
1651 
1652  vpMbEdgeTracker::initPyramid(*it->second, pyramid[it->first]);
1653  }
1654 }
1655 
1701 void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile)
1702 {
1703  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1704  if (it != m_mapOfEdgeTrackers.end()) {
1705  // Load ConfigFile for reference camera
1706  it->second->loadConfigFile(configFile);
1707  it->second->getCameraParameters(cam);
1708 
1709  // Set Moving Edge parameters
1710  this->me = it->second->getMovingEdge();
1711 
1712  // Set clipping
1713  this->clippingFlag = it->second->getClipping();
1714  this->angleAppears = it->second->getAngleAppear();
1715  this->angleDisappears = it->second->getAngleDisappear();
1716  } else {
1717  std::stringstream ss;
1718  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
1720  }
1721 }
1722 
1736 void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
1737  const bool firstCameraIsReference)
1738 {
1739  if (m_mapOfEdgeTrackers.size() == 2) {
1740  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1741  it->second->loadConfigFile(configFile1);
1742 
1743  if (firstCameraIsReference) {
1744  it->second->getCameraParameters(cam);
1745 
1746  // Set Moving Edge parameters
1747  this->me = it->second->getMovingEdge();
1748 
1749  // Set clipping
1750  this->clippingFlag = it->second->getClipping();
1751  this->angleAppears = it->second->getAngleAppear();
1752  this->angleDisappears = it->second->getAngleDisappear();
1753  }
1754  ++it;
1755 
1756  it->second->loadConfigFile(configFile2);
1757 
1758  if (!firstCameraIsReference) {
1759  it->second->getCameraParameters(cam);
1760 
1761  // Set Moving Edge parameters
1762  this->me = it->second->getMovingEdge();
1763 
1764  // Set clipping
1765  this->clippingFlag = it->second->getClipping();
1766  this->angleAppears = it->second->getAngleAppear();
1767  this->angleDisappears = it->second->getAngleDisappear();
1768  }
1769  } else {
1770  std::stringstream ss;
1771  ss << "Cannot loadConfigFile. Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !";
1773  }
1774 }
1775 
1786 void vpMbEdgeMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
1787 {
1788  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
1789  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
1790  std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_edge->first);
1791  if (it_config != mapOfConfigFiles.end()) {
1792  it_edge->second->loadConfigFile(it_config->second);
1793  } else {
1794  std::stringstream ss;
1795  ss << "Missing configuration file for camera: " << it_edge->first << " !";
1797  }
1798  }
1799 
1800  // Set the reference camera parameters
1801  std::map<std::string, vpMbEdgeTracker *>::iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1802  if (it != m_mapOfEdgeTrackers.end()) {
1803  it->second->getCameraParameters(cam);
1804 
1805  // Set Moving Edge parameters
1806  this->me = it->second->getMovingEdge();
1807 
1808  // Set clipping
1809  this->clippingFlag = it->second->getClipping();
1810  this->angleAppears = it->second->getAngleAppear();
1811  this->angleDisappears = it->second->getAngleDisappear();
1812  } else {
1813  std::stringstream ss;
1814  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
1816  }
1817 }
1818 
1846 void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, const bool verbose,
1847  const vpHomogeneousMatrix &T)
1848 {
1849  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1850  it != m_mapOfEdgeTrackers.end(); ++it) {
1851  it->second->loadModel(modelFile, verbose, T);
1852  }
1853 
1854  modelInitialised = true;
1855 }
1856 
1869 void vpMbEdgeMultiTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1870  const vpHomogeneousMatrix &cMo_, const bool verbose,
1871  const vpHomogeneousMatrix &T)
1872 {
1873  if (m_mapOfEdgeTrackers.size() != 1) {
1874  std::stringstream ss;
1875  ss << "This method requires exactly one camera, there are " << m_mapOfEdgeTrackers.size() << " cameras !";
1877  }
1878 
1879  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1880  if (it_edge != m_mapOfEdgeTrackers.end()) {
1881  it_edge->second->reInitModel(I, cad_name, cMo_, verbose, T);
1882 
1883  // Set reference pose
1884  it_edge->second->getPose(cMo);
1885 
1886  modelInitialised = true;
1887  }
1888 }
1889 
1904  const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
1905  const vpHomogeneousMatrix &c2Mo, const bool verbose,
1906  const bool firstCameraIsReference)
1907 {
1908  if (m_mapOfEdgeTrackers.size() == 2) {
1909  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
1910 
1911  it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose);
1912 
1913  if (firstCameraIsReference) {
1914  // Set reference pose
1915  it_edge->second->getPose(cMo);
1916  }
1917 
1918  ++it_edge;
1919 
1920  it_edge->second->reInitModel(I2, cad_name, c2Mo, verbose);
1921 
1922  if (!firstCameraIsReference) {
1923  // Set reference pose
1924  it_edge->second->getPose(cMo);
1925  }
1926  } else {
1927  throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras !");
1928  }
1929 }
1930 
1941 void vpMbEdgeMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1942  const std::string &cad_name,
1943  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
1944  const bool verbose)
1945 {
1946  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1947  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1948  mapOfImages.find(m_referenceCameraName);
1949  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
1950 
1951  if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
1952  it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
1953  modelInitialised = true;
1954 
1955  // Set reference pose
1956  it_edge->second->getPose(cMo);
1957  } else {
1958  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel for reference camera !");
1959  }
1960 
1961  std::vector<std::string> vectorOfMissingCameras;
1962  for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
1963  if (it_edge->first != m_referenceCameraName) {
1964  it_img = mapOfImages.find(it_edge->first);
1965  it_camPose = mapOfCameraPoses.find(it_edge->first);
1966 
1967  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
1968  it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
1969  } else {
1970  vectorOfMissingCameras.push_back(it_edge->first);
1971  }
1972  }
1973  }
1974 
1975  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
1976  ++it) {
1977  it_img = mapOfImages.find(*it);
1978  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1980 
1981  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1982  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1983  m_mapOfEdgeTrackers[*it]->reInitModel(*it_img->second, cad_name, cCurrentMo, verbose);
1984  }
1985  }
1986 }
1987 
1993 {
1994  this->cMo.eye();
1995 
1996  // Reset all internal trackers
1997  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1998  it != m_mapOfEdgeTrackers.end(); ++it) {
1999  it->second->resetTracker();
2000  }
2001 
2002  useScanLine = false;
2003 
2004 #ifdef VISP_HAVE_OGRE
2005  useOgre = false;
2006 #endif
2007 
2008  m_computeInteraction = true;
2009  // nline = 0; //Not used in vpMbEdgeMultiTracker class
2010  // ncylinder = 0; //Not used in vpMbEdgeMultiTracker class
2011  m_lambda = 1.0;
2012  // nbvisiblepolygone = 0; //Not used in vpMbEdgeMultiTracker class
2013  percentageGdPt = 0.4;
2014 
2015  angleAppears = vpMath::rad(89);
2018 
2020 
2021  // reinitialization of the scales.
2022  this->setScales(scales);
2023 }
2024 
2035 {
2037 
2038  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2039  it != m_mapOfEdgeTrackers.end(); ++it) {
2040  it->second->setAngleAppear(a);
2041  }
2042 }
2043 
2054 {
2056 
2057  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2058  it != m_mapOfEdgeTrackers.end(); ++it) {
2059  it->second->setAngleDisappear(a);
2060  }
2061 }
2062 
2069 {
2070  if (m_mapOfEdgeTrackers.empty()) {
2071  throw vpException(vpTrackingException::fatalError, "There is no camera !");
2072  } else if (m_mapOfEdgeTrackers.size() > 1) {
2073  throw vpException(vpTrackingException::fatalError, "There is more than one camera !");
2074  } else {
2075  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2076  if (it != m_mapOfEdgeTrackers.end()) {
2077  it->second->setCameraParameters(camera);
2078 
2079  // Set reference camera parameters
2080  this->cam = camera;
2081  } else {
2082  std::stringstream ss;
2083  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2085  }
2086  }
2087 }
2088 
2098  const bool firstCameraIsReference)
2099 {
2100  if (m_mapOfEdgeTrackers.empty()) {
2101  throw vpException(vpTrackingException::fatalError, "There is no camera !");
2102  } else if (m_mapOfEdgeTrackers.size() == 2) {
2103  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2104  it->second->setCameraParameters(camera1);
2105 
2106  ++it;
2107  it->second->setCameraParameters(camera2);
2108 
2109  if (firstCameraIsReference) {
2110  this->cam = camera1;
2111  } else {
2112  this->cam = camera2;
2113  }
2114  } else {
2115  std::stringstream ss;
2116  ss << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !";
2118  }
2119 }
2120 
2127 void vpMbEdgeMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera)
2128 {
2129  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
2130  if (it != m_mapOfEdgeTrackers.end()) {
2131  it->second->setCameraParameters(camera);
2132 
2133  if (it->first == m_referenceCameraName) {
2134  this->cam = camera;
2135  }
2136  } else {
2137  std::stringstream ss;
2138  ss << "The camera: " << cameraName << " does not exist !";
2140  }
2141 }
2142 
2148 void vpMbEdgeMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
2149 {
2150  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
2151  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2152  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_edge->first);
2153  if (it_cam != mapOfCameraParameters.end()) {
2154  it_edge->second->setCameraParameters(it_cam->second);
2155 
2156  if (it_edge->first == m_referenceCameraName) {
2157  this->cam = it_cam->second;
2158  }
2159  } else {
2160  std::stringstream ss;
2161  ss << "Missing camera parameters for camera: " << it_edge->first << " !";
2163  }
2164  }
2165 }
2166 
2175 void vpMbEdgeMultiTracker::setCameraTransformationMatrix(const std::string &cameraName,
2176  const vpHomogeneousMatrix &cameraTransformationMatrix)
2177 {
2178  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
2179  if (it != m_mapOfCameraTransformationMatrix.end()) {
2180  it->second = cameraTransformationMatrix;
2181  } else {
2182  std::stringstream ss;
2183  ss << "Cannot find camera: " << cameraName << " !";
2185  }
2186 }
2187 
2196  const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
2197 {
2198  // Check if all cameras have a transformation matrix
2199  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
2200  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2201  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2202  mapOfTransformationMatrix.find(it_edge->first);
2203 
2204  if (it_camTrans == mapOfTransformationMatrix.end()) {
2205  throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !");
2206  }
2207  }
2208 
2209  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
2210 }
2211 
2219 void vpMbEdgeMultiTracker::setClipping(const unsigned int &flags)
2220 {
2221  // Set clipping for vpMbEdgeMultiTracker class
2222  vpMbTracker::setClipping(flags);
2223 
2224  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2225  it != m_mapOfEdgeTrackers.end(); ++it) {
2226  it->second->setClipping(flags);
2227  }
2228 }
2229 
2238 void vpMbEdgeMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
2239 {
2240  // Set clipping for the given camera, do not change the clipping for
2241  // vpMbEdgeMultiTracker class
2242  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
2243  if (it != m_mapOfEdgeTrackers.end()) {
2244  it->second->setClipping(flags);
2245  } else {
2246  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2247  }
2248 }
2249 
2256 {
2258 
2259  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2260  it != m_mapOfEdgeTrackers.end(); ++it) {
2261  it->second->setCovarianceComputation(flag);
2262  }
2263 }
2264 
2282 {
2283  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2284  it != m_mapOfEdgeTrackers.end(); ++it) {
2285  it->second->setDisplayFeatures(displayF);
2286  }
2287 
2288  displayFeatures = displayF;
2289 }
2290 
2297 {
2299 
2300  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2301  it != m_mapOfEdgeTrackers.end(); ++it) {
2302  it->second->setFarClippingDistance(dist);
2303  }
2304 }
2305 
2312 void vpMbEdgeMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist)
2313 {
2314  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
2315  if (it != m_mapOfEdgeTrackers.end()) {
2316  it->second->setFarClippingDistance(dist);
2317  } else {
2318  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2319  }
2320 }
2321 
2336 {
2337  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2338  it != m_mapOfEdgeTrackers.end(); ++it) {
2339  it->second->setGoodMovingEdgesRatioThreshold(threshold);
2340  }
2341 
2342  percentageGdPt = threshold;
2343 }
2344 
2345 #ifdef VISP_HAVE_OGRE
2346 
2356 {
2357  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2358  it != m_mapOfEdgeTrackers.end(); ++it) {
2359  it->second->setGoodNbRayCastingAttemptsRatio(ratio);
2360  }
2361 }
2362 
2373 {
2374  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2375  it != m_mapOfEdgeTrackers.end(); ++it) {
2376  it->second->setNbRayCastingAttemptsForVisibility(attempts);
2377  }
2378 }
2379 #endif
2380 
2392 void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &name)
2393 {
2394  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2395  it != m_mapOfEdgeTrackers.end(); ++it) {
2396  it->second->setLod(useLod, name);
2397  }
2398 }
2399 
2412 void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name)
2413 {
2414  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);
2415 
2416  if (it_edge != m_mapOfEdgeTrackers.end()) {
2417  it_edge->second->setLod(useLod, name);
2418  } else {
2419  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
2420  }
2421 }
2422 
2432 void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
2433 {
2434  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2435  it != m_mapOfEdgeTrackers.end(); ++it) {
2436  it->second->setMinLineLengthThresh(minLineLengthThresh, name);
2437  }
2438 }
2439 
2451 void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
2452  const std::string &name)
2453 {
2454  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);
2455 
2456  if (it_edge != m_mapOfEdgeTrackers.end()) {
2457  it_edge->second->setMinLineLengthThresh(minLineLengthThresh, name);
2458  } else {
2459  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
2460  }
2461 }
2462 
2471 void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
2472 {
2473  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2474  it != m_mapOfEdgeTrackers.end(); ++it) {
2475  it->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
2476  }
2477 }
2478 
2489 void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
2490  const std::string &name)
2491 {
2492  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);
2493 
2494  if (it_edge != m_mapOfEdgeTrackers.end()) {
2495  it_edge->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
2496  } else {
2497  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
2498  }
2499 }
2500 
2507 {
2508  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2509  it != m_mapOfEdgeTrackers.end(); ++it) {
2510  it->second->setMovingEdge(me);
2511  }
2512 }
2513 
2520 void vpMbEdgeMultiTracker::setMovingEdge(const std::string &cameraName, const vpMe &me)
2521 {
2522  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
2523  if (it != m_mapOfEdgeTrackers.end()) {
2524  it->second->setMovingEdge(me);
2525  } else {
2526  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2527  }
2528 }
2529 
2536 {
2538 
2539  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2540  it != m_mapOfEdgeTrackers.end(); ++it) {
2541  it->second->setNearClippingDistance(dist);
2542  }
2543 }
2544 
2551 void vpMbEdgeMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist)
2552 {
2553  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
2554  if (it != m_mapOfEdgeTrackers.end()) {
2555  it->second->setNearClippingDistance(dist);
2556  } else {
2557  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2558  }
2559 }
2560 
2571 void vpMbEdgeMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
2572 {
2573  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2574  it != m_mapOfEdgeTrackers.end(); ++it) {
2575  it->second->setOgreShowConfigDialog(showConfigDialog);
2576  }
2577 }
2578 
2588 {
2589  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2590  it != m_mapOfEdgeTrackers.end(); ++it) {
2591  it->second->setOgreVisibilityTest(v);
2592  }
2593 
2594 #ifdef VISP_HAVE_OGRE
2595  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2596  it != m_mapOfEdgeTrackers.end(); ++it) {
2597  it->second->faces.getOgreContext()->setWindowName("Multi MBT Edge (" + it->first + ")");
2598  }
2599 #endif
2600 
2601  useOgre = v;
2602 }
2603 
2610 {
2611  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2612  it != m_mapOfEdgeTrackers.end(); ++it) {
2613  it->second->setOptimizationMethod(opt);
2614  }
2615 
2616  m_optimizationMethod = opt;
2617 }
2618 
2627 {
2628  if (m_mapOfEdgeTrackers.size() == 1) {
2629  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2630  if (it != m_mapOfEdgeTrackers.end()) {
2631  it->second->setPose(I, cMo_);
2632  this->cMo = cMo_;
2633  } else {
2634  std::stringstream ss;
2635  ss << "Cannot find the reference camera: " << m_referenceCameraName << " !";
2637  }
2638  } else {
2639  std::stringstream ss;
2640  ss << "You are trying to set the pose with only one image and cMo but "
2641  "there are multiple cameras !";
2643  }
2644 }
2645 
2654 {
2655  if (m_mapOfEdgeTrackers.size() == 1) {
2656  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2657  if (it != m_mapOfEdgeTrackers.end()) {
2658  vpImageConvert::convert(I_color, m_I);
2659  it->second->setPose(m_I, cMo_);
2660  this->cMo = cMo_;
2661  } else {
2662  std::stringstream ss;
2663  ss << "Cannot find the reference camera: " << m_referenceCameraName << " !";
2665  }
2666  } else {
2667  std::stringstream ss;
2668  ss << "You are trying to set the pose with only one image and cMo but "
2669  "there are multiple cameras !";
2671  }
2672 }
2673 
2686  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
2687  const bool firstCameraIsReference)
2688 {
2689  if (m_mapOfEdgeTrackers.size() == 2) {
2690  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2691  it->second->setPose(I1, c1Mo);
2692 
2693  ++it;
2694 
2695  it->second->setPose(I2, c2Mo);
2696 
2697  if (firstCameraIsReference) {
2698  this->cMo = c1Mo;
2699  } else {
2700  this->cMo = c2Mo;
2701  }
2702  } else {
2703  std::stringstream ss;
2704  ss << "This method requires 2 cameras but there are " << m_mapOfEdgeTrackers.size() << " cameras !";
2706  }
2707 }
2708 
2717 void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2718  const vpHomogeneousMatrix &cMo_)
2719 {
2720  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2721  if (it_edge != m_mapOfEdgeTrackers.end()) {
2722  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2723  mapOfImages.find(m_referenceCameraName);
2724 
2725  if (it_img != mapOfImages.end()) {
2726  // Set pose on reference camera
2727  it_edge->second->setPose(*it_img->second, cMo_);
2728 
2729  // Set the reference cMo
2730  cMo = cMo_;
2731 
2732  // Set the pose for the others cameras
2733  for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2734  if (it_edge->first != m_referenceCameraName) {
2735  it_img = mapOfImages.find(it_edge->first);
2736  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2737  m_mapOfCameraTransformationMatrix.find(it_edge->first);
2738 
2739  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2740  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
2741  it_edge->second->setPose(*it_img->second, cCurrentMo);
2742  } else {
2743  throw vpException(vpTrackingException::fatalError, "Missing image or camera transformation matrix !");
2744  }
2745  }
2746  }
2747  } else {
2748  std::stringstream ss;
2749  ss << "Missing image for camera: " << m_referenceCameraName << " !";
2751  }
2752  } else {
2753  std::stringstream ss;
2754  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2756  }
2757 }
2758 
2769 void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2770  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2771 {
2772  // Set the reference cMo
2773  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2774  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2775  mapOfImages.find(m_referenceCameraName);
2776  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
2777 
2778  if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2779  it_edge->second->setPose(*it_img->second, it_camPose->second);
2780  it_edge->second->getPose(cMo);
2781  } else {
2782  throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera !");
2783  }
2784 
2785  // Vector of missing pose matrices for cameras
2786  std::vector<std::string> vectorOfMissingCameraPoses;
2787 
2788  // Set pose for the specified cameras
2789  for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2790  if (it_edge->first != m_referenceCameraName) {
2791  it_img = mapOfImages.find(it_edge->first);
2792  it_camPose = mapOfCameraPoses.find(it_edge->first);
2793 
2794  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2795  // Set pose
2796  it_edge->second->setPose(*it_img->second, it_camPose->second);
2797  } else {
2798  vectorOfMissingCameraPoses.push_back(it_edge->first);
2799  }
2800  }
2801  }
2802 
2803  for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
2804  it1 != vectorOfMissingCameraPoses.end(); ++it1) {
2805  it_img = mapOfImages.find(*it1);
2806  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2808 
2809  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2810  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
2811  m_mapOfEdgeTrackers[*it1]->setPose(*it_img->second, cCurrentMo);
2812  } else {
2813  std::stringstream ss;
2814  ss << "Missing image or missing camera transformation matrix ! Cannot "
2815  "set the pose for camera: "
2816  << (*it1) << " !";
2818  }
2819  }
2820 }
2821 
2829 {
2830  // Set the flag in the current class
2832 
2833  // Set the flag for each camera
2834  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2835  it != m_mapOfEdgeTrackers.end(); ++it) {
2836  it->second->setProjectionErrorComputation(flag);
2837  }
2838 }
2839 
2845 void vpMbEdgeMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
2846 {
2847  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(referenceCameraName);
2848  if (it != m_mapOfEdgeTrackers.end()) {
2849  m_referenceCameraName = referenceCameraName;
2850  } else {
2851  std::stringstream ss;
2852  ss << "The reference camera: " << referenceCameraName << " does not exist !";
2854  }
2855 }
2856 
2878 void vpMbEdgeMultiTracker::setScales(const std::vector<bool> &scale)
2879 {
2881  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2882  it != m_mapOfEdgeTrackers.end(); ++it) {
2883  it->second->setScales(scale);
2884  }
2885 }
2886 
2893 {
2894  // Set general setScanLineVisibilityTest
2896 
2897  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2898  it != m_mapOfEdgeTrackers.end(); ++it) {
2899  it->second->setScanLineVisibilityTest(v);
2900  }
2901 }
2902 
2910 void vpMbEdgeMultiTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2911 {
2912  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2913  it != m_mapOfEdgeTrackers.end(); ++it) {
2914  it->second->setUseEdgeTracking(name, useEdgeTracking);
2915  }
2916 }
2917 
2926 {
2927  // Track only with reference camera
2928  // Get the reference camera parameters
2929  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2930 
2931  if (it != m_mapOfEdgeTrackers.end()) {
2932  it->second->track(I);
2933  it->second->getPose(cMo);
2934  } else {
2935  std::stringstream ss;
2936  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2938  }
2939 
2940  if (computeProjError) {
2941  // Use the projection error computed by the single vpMbEdgeTracker in
2942  // m_mapOfEdgeTrackers
2943  projectionError = it->second->getProjectionError();
2944  }
2945 }
2946 
2951 {
2952  std::cout << "Not supported interface, this class is deprecated." << std::endl;
2953 }
2954 
2964 {
2965  // Track with the special case of stereo cameras
2966  if (m_mapOfEdgeTrackers.size() == 2) {
2967  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2968  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2969  mapOfImages[it->first] = &I1;
2970  ++it;
2971 
2972  mapOfImages[it->first] = &I2;
2973  track(mapOfImages);
2974  } else {
2975  std::stringstream ss;
2976  ss << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !";
2978  }
2979 }
2980 
2988 void vpMbEdgeMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
2989 {
2990  // Check if there is an image for each camera
2991  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
2992  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2993  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_edge->first);
2994 
2995  if (it_img == mapOfImages.end()) {
2996  throw vpException(vpTrackingException::fatalError, "Missing images !");
2997  }
2998  }
2999 
3000  initPyramid(mapOfImages, m_mapOfPyramidalImages);
3001 
3002  unsigned int lvl = (unsigned int)scales.size();
3003  do {
3004  lvl--;
3005 
3006  projectionError = 90.0;
3007 
3008  if (scales[lvl]) {
3009  vpHomogeneousMatrix cMo_1 = cMo;
3010  try {
3011  downScale(lvl);
3012  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
3013  it1 != m_mapOfEdgeTrackers.end(); ++it1) {
3014  // Downscale for each camera
3015  it1->second->downScale(lvl);
3016 
3017  // Track moving edges
3018  try {
3019  it1->second->trackMovingEdge(*m_mapOfPyramidalImages[it1->first][lvl]);
3020  } catch (...) {
3021  vpTRACE("Error in moving edge tracking");
3022  throw;
3023  }
3024  }
3025 
3026  try {
3027  std::map<std::string, const vpImage<unsigned char> *> mapOfPyramidImages;
3028  for (std::map<std::string, std::vector<const vpImage<unsigned char> *> >::const_iterator it =
3029  m_mapOfPyramidalImages.begin();
3030  it != m_mapOfPyramidalImages.end(); ++it) {
3031  mapOfPyramidImages[it->first] = it->second[lvl];
3032  }
3033 
3034  computeVVS(mapOfPyramidImages, lvl);
3035  } catch (...) {
3036  covarianceMatrix = -1;
3037  throw; // throw the original exception
3038  }
3039 
3040  // Test tracking failed only if all testTracking failed
3041  bool isOneTestTrackingOk = false;
3042  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3043  it != m_mapOfEdgeTrackers.end(); ++it) {
3044  // Set the camera pose
3045  it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
3046 
3047  try {
3048  it->second->testTracking();
3049  isOneTestTrackingOk = true;
3050  } catch (/*vpException &e*/...) {
3051  // throw e;
3052  }
3053  }
3054 
3055  if (!isOneTestTrackingOk) {
3056  std::ostringstream oss;
3057  oss << "Not enough moving edges to track the object. Try to reduce "
3058  "the threshold="
3059  << percentageGdPt << " using setGoodMovingEdgesRatioThreshold()";
3061  }
3062 
3063  if (displayFeatures) {
3064  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3065  it != m_mapOfEdgeTrackers.end(); ++it) {
3066  it->second->m_featuresToBeDisplayedEdge = it->second->getFeaturesForDisplayEdge();
3067  }
3068  }
3069 
3070  // Looking for new visible face
3071  bool newvisibleface = false;
3072  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3073  it != m_mapOfEdgeTrackers.end(); ++it) {
3074  it->second->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
3075  }
3076 
3077  if (useScanLine) {
3078  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3079  it != m_mapOfEdgeTrackers.end(); ++it) {
3080  it->second->faces.computeClippedPolygons(it->second->cMo, it->second->cam);
3081  it->second->faces.computeScanLineRender(it->second->cam, mapOfImages[it->first]->getWidth(),
3082  mapOfImages[it->first]->getHeight());
3083  }
3084  }
3085 
3086  try {
3087  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3088  it != m_mapOfEdgeTrackers.end(); ++it) {
3089  it->second->updateMovingEdge(*mapOfImages[it->first]);
3090  }
3091  } catch (...) {
3092  throw; // throw the original exception
3093  }
3094 
3095  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3096  it != m_mapOfEdgeTrackers.end(); ++it) {
3097  it->second->initMovingEdge(*mapOfImages[it->first], it->second->cMo);
3098 
3099  // Reinit the moving edge for the lines which need it.
3100  it->second->reinitMovingEdge(*mapOfImages[it->first], it->second->cMo);
3101 
3102  if (computeProjError) {
3103  // Compute the projection error
3104  it->second->computeProjectionError(*mapOfImages[it->first]);
3105  }
3106  }
3107 
3109 
3110  upScale(lvl);
3111  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3112  it != m_mapOfEdgeTrackers.end(); ++it) {
3113  it->second->upScale(lvl);
3114  }
3115  } catch (const vpException &e) {
3116  if (lvl != 0) {
3117  cMo = cMo_1;
3118  reInitLevel(lvl);
3119  upScale(lvl);
3120 
3121  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3122  it != m_mapOfEdgeTrackers.end(); ++it) {
3123  it->second->cMo = cMo_1;
3124  it->second->reInitLevel(lvl);
3125  it->second->upScale(lvl);
3126  }
3127  } else {
3128  upScale(lvl);
3129  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
3130  it != m_mapOfEdgeTrackers.end(); ++it) {
3131  it->second->upScale(lvl);
3132  }
3133  throw(e);
3134  }
3135  }
3136  }
3137  } while (lvl != 0);
3138 
3140 }
3141 #elif !defined(VISP_BUILD_SHARED_LIBS)
3142 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeMultiTracker.cpp.o) has no symbols
3143 void dummy_vpMbEdgeMultiTracker(){}
3144 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
bool m_computeInteraction
Definition: vpMbTracker.h:185
bool computeProjError
Definition: vpMbTracker.h:133
std::string m_referenceCameraName
Name of the reference camera.
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:495
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
virtual void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
virtual void setFarClippingDistance(const double &dist)
virtual void setLod(const bool useLod, const std::string &name="")
vpColVector m_error_edge
(s - s*)
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void upScale(const unsigned int _scale)
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:252
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:476
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual void computeProjectionError()
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setDisplayFeatures(const bool displayF)
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
virtual std::map< std::string, unsigned int > getMultiNbPolygon() const
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
vpColVector m_factor
Edge VVS variables.
virtual void track(const vpImage< unsigned char > &I)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void setProjectionErrorComputation(const bool &flag)
virtual void loadConfigFile(const std::string &configFile)
vpColVector m_w_edgeMulti
Robust weights.
bool modelInitialised
Definition: vpMbTracker.h:123
virtual void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
virtual void cleanPyramid(std::map< std::string, std::vector< const vpImage< unsigned char > * > > &pyramid)
vpColVector m_w_edge
Robust weights.
error that can be emited by ViSP classes.
Definition: vpException.h:71
void reInitLevel(const unsigned int _lvl)
virtual vpMe getMovingEdge() const
virtual void setClipping(const unsigned int &flags)
Definition: vpMe.h:60
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
Make the complete tracking of an object by using its CAD model.
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpColVector m_error_edgeMulti
(s - s*)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
vpMe me
The moving edges parameters.
virtual unsigned int getNbPolygon() const
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual void setNearClippingDistance(const double &dist)
void downScale(const unsigned int _scale)
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:111
virtual void computeVVSWeights()
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
double projectionError
Definition: vpMbTracker.h:136
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)
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4660
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
virtual void initClick(const vpImage< unsigned char > &I, const std::vector< vpPoint > &points3D_list, const std::string &displayFile="")
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
vpMatrix m_L_edgeMulti
Interaction matrix.
Error that can be emited by the vpTracker class and its derivates.
void init(const vpImage< unsigned char > &I)
vp_deprecated void init()
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:419
virtual void setOgreVisibilityTest(const bool &v)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
virtual void computeVVS(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages, const unsigned int lvl)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:114
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void setCameraParameters(const vpCameraParameters &camera)
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0) const
Generic class defining intrinsic camera parameters.
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
virtual void computeVVSInteractionMatrixAndResidu()
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0) const
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:465
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getRows() const
Definition: vpArray2D.h:289
std::map< std::string, vpMbEdgeTracker * > m_mapOfEdgeTrackers
Map of Model-based edge trackers.
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
virtual void setCovarianceComputation(const bool &flag)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:108
virtual void setMovingEdge(const vpMe &me)
void insert(unsigned int i, const vpColVector &v)
virtual void setScales(const std::vector< bool > &scales)
void setScales(const std::vector< bool > &_scales)
static double deg(double rad)
Definition: vpMath.h:101
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
virtual void getCameraParameters(vpCameraParameters &camera) const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
std::map< std::string, std::vector< const vpImage< unsigned char > * > > m_mapOfPyramidalImages
Map of pyramidal images for each camera.
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)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpHomogeneousMatrix inverse() const
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0) const
virtual void computeVVSInit()
virtual std::vector< std::string > getCameraNames() const
virtual unsigned int getNbPoints(const unsigned int level=0) const
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:597
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
virtual void setClipping(const unsigned int &flags)
virtual void setAngleDisappear(const double &a)
virtual void setGoodMovingEdgesRatioThreshold(const double threshold)
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
vpMatrix m_L_edge
Interaction matrix.
virtual void setFarClippingDistance(const double &dist)
void computeVVSFirstPhase(const vpImage< unsigned char > &I, const unsigned int iter, double &count, const unsigned int lvl=0)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
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)
vpColVector m_weightedError_edgeMulti
Weighted error.
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
virtual void setAngleAppear(const double &a)
virtual void initPyramid(const std::map< std::string, const vpImage< unsigned char > * > &mapOfImages, std::map< std::string, std::vector< const vpImage< unsigned char > * > > &pyramid)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:580
virtual void setNearClippingDistance(const double &dist)