Visual Servoing Platform  version 3.2.1 under development (2019-08-08)
vpMbKltMultiTracker.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 klt tracker with multiple cameras.
33  *
34  * Authors:
35  * Souriya Trinh
36  *
37  *****************************************************************************/
38 
44 #include <visp3/core/vpConfig.h>
45 
46 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
47 
48 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
49 
50 #include <visp3/core/vpTrackingException.h>
51 #include <visp3/core/vpVelocityTwistMatrix.h>
52 #include <visp3/mbt/vpMbKltMultiTracker.h>
53 
58  : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(),
59  m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti()
60 {
61  m_mapOfKltTrackers["Camera"] = new vpMbKltTracker;
62 
63  // Add default camera transformation matrix
65 }
66 
72 vpMbKltMultiTracker::vpMbKltMultiTracker(const unsigned int nbCameras)
75 {
76 
77  if (nbCameras == 0) {
78  throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !");
79  } else if (nbCameras == 1) {
80  m_mapOfKltTrackers["Camera"] = new vpMbKltTracker;
81 
82  // Add default camera transformation matrix
84  } else if (nbCameras == 2) {
85  m_mapOfKltTrackers["Camera1"] = new vpMbKltTracker;
86  m_mapOfKltTrackers["Camera2"] = new vpMbKltTracker;
87 
88  // Add default camera transformation matrix
91 
92  // Set by default the reference camera
93  m_referenceCameraName = "Camera1";
94  } else {
95  for (unsigned int i = 1; i <= nbCameras; i++) {
96  std::stringstream ss;
97  ss << "Camera" << i;
98  m_mapOfKltTrackers[ss.str()] = new vpMbKltTracker;
99 
100  // Add default camera transformation matrix
102  }
103 
104  // Set by default the reference camera to the first one
105  m_referenceCameraName = m_mapOfKltTrackers.begin()->first;
106  }
107 }
108 
114 vpMbKltMultiTracker::vpMbKltMultiTracker(const std::vector<std::string> &cameraNames)
117 {
118  if (cameraNames.empty()) {
119  throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbKltMultiTracker with no camera !");
120  }
121 
122  for (std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
124  }
125 
126  // Set by default the reference camera
127  m_referenceCameraName = cameraNames.front();
128 }
129 
134 {
135  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
136  it != m_mapOfKltTrackers.end(); ++it) {
137  delete it->second;
138  }
139 }
140 
149 void vpMbKltMultiTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
150  const std::string &name)
151 {
152  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
153  it != m_mapOfKltTrackers.end(); ++it) {
154  it->second->addCircle(P1, P2, P3, r, name);
155  }
156 }
157 
159 {
160  vpMatrix L_true;
161  vpMatrix LVJ_true;
162  vpColVector v;
163 
164  vpMatrix LTL;
165  vpColVector LTR;
166  vpHomogeneousMatrix cMoPrev;
167  vpHomogeneousMatrix ctTc0_Prev;
168  vpColVector error_prev;
169  double mu = m_initialMu;
170 
171  double normRes = 0;
172  double normRes_1 = -1;
173  unsigned int iter = 0;
174 
175  computeVVSInit();
176  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
177  it != m_mapOfKltTrackers.end(); ++it) {
178  vpMbKltTracker *klt = it->second;
179  klt->computeVVSInit();
180  }
181 
182  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
183  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
184  it != m_mapOfCameraTransformationMatrix.end(); ++it) {
186  cVo.buildFrom(it->second);
187  mapOfVelocityTwist[it->first] = cVo;
188  }
189 
190  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
191  computeVVSInteractionMatrixAndResidu(mapOfVelocityTwist);
192 
193  bool reStartFromLastIncrement = false;
194  computeVVSCheckLevenbergMarquardt(iter, m_error_kltMulti, error_prev, cMoPrev, mu, reStartFromLastIncrement);
195  if (reStartFromLastIncrement) {
196  ctTc0 = ctTc0_Prev;
197  }
198 
199  if (!reStartFromLastIncrement) {
201 
202  if (computeCovariance) {
203  L_true = m_L_kltMulti;
204  if (!isoJoIdentity) {
206  cVo.buildFrom(cMo);
207  LVJ_true = (m_L_kltMulti * cVo * oJo);
208  }
209  }
210 
211  normRes_1 = normRes;
212  normRes = 0.0;
213 
214  for (unsigned int i = 0; i < m_weightedError_kltMulti.getRows(); i++) {
216  normRes += m_weightedError_kltMulti[i];
217  }
218 
219  if ((iter == 0) || m_computeInteraction) {
220  for (unsigned int i = 0; i < m_L_kltMulti.getRows(); i++) {
221  for (unsigned int j = 0; j < 6; j++) {
222  m_L_kltMulti[i][j] *= m_w_kltMulti[i];
223  }
224  }
225  }
226 
228  error_prev, LTR, mu, v);
229 
230  cMoPrev = cMo;
231  ctTc0_Prev = ctTc0;
233  cMo = ctTc0 * c0Mo;
234  } // endif(!reStartFromLastIncrement)
235 
236  iter++;
237  }
238 
240 }
241 
243 {
244  unsigned int nbFeatures = 2 * m_nbInfos;
245 
246  m_L_kltMulti.resize(nbFeatures, 6, false, false);
247  m_w_kltMulti.resize(nbFeatures, false);
248  m_error_kltMulti.resize(nbFeatures, false);
249  m_weightedError_kltMulti.resize(nbFeatures, false);
250 }
251 
253 {
254  throw vpException(vpException::fatalError, "vpMbKltMultiTracker::"
255  "computeVVSInteractionMatrixAndR"
256  "esidu() should not be called!");
257 }
258 
260  std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
261 {
262  unsigned int startIdx = 0;
263 
264  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
265  it != m_mapOfKltTrackers.end(); ++it) {
266  vpMbKltTracker *klt = it->second;
267 
268  // Use the ctTc0 variable instead of the formula in the monocular case
269  // to ensure that we have the same result than vpMbKltTracker
270  // as some slight differences can occur due to numerical imprecision
271  if (m_mapOfKltTrackers.size() == 1) {
272  klt->ctTc0 = ctTc0;
274  } else {
275  vpHomogeneousMatrix c_curr_tTc_curr0 =
276  m_mapOfCameraTransformationMatrix[it->first] * cMo * it->second->c0Mo.inverse();
277  klt->ctTc0 = c_curr_tTc_curr0;
279  }
280 
281  m_error_kltMulti.insert(startIdx, klt->m_error_klt);
282  m_L_kltMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], startIdx, 0);
283 
284  startIdx += klt->m_error_klt.getRows();
285  }
286 }
287 
289 {
290  unsigned int startIdx = 0;
291  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
292  it != m_mapOfKltTrackers.end(); ++it) {
293  vpMbKltTracker *klt = it->second;
294  klt->computeVVSWeights(klt->m_robust_klt, klt->m_error_klt, klt->m_w_klt);
295 
296  m_w_kltMulti.insert(startIdx, klt->m_w_klt);
297  startIdx += klt->m_w_klt.getRows();
298  }
299 }
300 
313  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
314  const bool displayFullModel)
315 {
316  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
317  if (it != m_mapOfKltTrackers.end()) {
318  it->second->display(I, cMo_, cam_, col, thickness, displayFullModel);
319  } else {
320  std::cerr << "Cannot find reference camera:" << m_referenceCameraName << " !" << std::endl;
321  }
322 }
323 
336  const vpCameraParameters &cam_, const vpColor &color, const unsigned int thickness,
337  const bool displayFullModel)
338 {
339  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
340  if (it != m_mapOfKltTrackers.end()) {
341  it->second->display(I, cMo_, cam_, color, thickness, displayFullModel);
342  } else {
343  std::cerr << "Cannot find reference camera:" << m_referenceCameraName << " !" << std::endl;
344  }
345 }
346 
362  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
363  const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &color,
364  const unsigned int thickness, const bool displayFullModel)
365 {
366  if (m_mapOfKltTrackers.size() == 2) {
367  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
368  it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
369  ++it;
370 
371  it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
372  } else {
373  std::cerr << "This display is only for the stereo case ! There are " << m_mapOfKltTrackers.size() << " cameras !"
374  << std::endl;
375  }
376 }
377 
393  const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1,
394  const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness,
395  const bool displayFullModel)
396 {
397  if (m_mapOfKltTrackers.size() == 2) {
398  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
399  it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
400  ++it;
401 
402  it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
403  } else {
404  std::cerr << "This display is only for the stereo case ! There are " << m_mapOfKltTrackers.size() << " cameras !"
405  << std::endl;
406  }
407 }
408 
420 void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
421  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
422  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
423  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
424 {
425 
426  // Display only for the given images
427  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
428  it_img != mapOfImages.end(); ++it_img) {
429  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(it_img->first);
430  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
431  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
432 
433  if (it_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
434  it_cam != mapOfCameraParameters.end()) {
435  it_klt->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
436  } else {
437  std::cerr << "Missing elements ! " << std::endl;
438  }
439  }
440 }
441 
453 void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
454  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
455  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
456  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
457 {
458 
459  // Display only for the given images
460  for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
461  it_img != mapOfImages.end(); ++it_img) {
462  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(it_img->first);
463  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
464  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
465 
466  if (it_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
467  it_cam != mapOfCameraParameters.end()) {
468  it_klt->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
469  } else {
470  std::cerr << "Missing elements ! " << std::endl;
471  }
472  }
473 }
474 
480 std::vector<std::string> vpMbKltMultiTracker::getCameraNames() const
481 {
482  std::vector<std::string> cameraNames;
483 
484  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
485  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
486  cameraNames.push_back(it_klt->first);
487  }
488 
489  return cameraNames;
490 }
491 
498 {
499  // Get the reference camera parameters
500  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
501  if (it != m_mapOfKltTrackers.end()) {
502  it->second->getCameraParameters(camera);
503  } else {
504  std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl;
505  }
506 }
507 
515 {
516  if (m_mapOfKltTrackers.size() == 2) {
517  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
518  it->second->getCameraParameters(cam1);
519  ++it;
520 
521  it->second->getCameraParameters(cam2);
522  } else {
523  std::cerr << "Problem with the number of cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !"
524  << std::endl;
525  }
526 }
527 
534 void vpMbKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const
535 {
536  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
537  if (it != m_mapOfKltTrackers.end()) {
538  it->second->getCameraParameters(camera);
539  } else {
540  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
541  }
542 }
543 
549 void vpMbKltMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
550 {
551  // Clear the input map
552  mapOfCameraParameters.clear();
553 
554  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
555  it != m_mapOfKltTrackers.end(); ++it) {
556  vpCameraParameters cam_;
557  it->second->getCameraParameters(cam_);
558  mapOfCameraParameters[it->first] = cam_;
559  }
560 }
561 
569 unsigned int vpMbKltMultiTracker::getClipping(const std::string &cameraName) const
570 {
571  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
572  if (it != m_mapOfKltTrackers.end()) {
573  return it->second->getClipping();
574  } else {
575  std::cerr << "Cannot find camera: " << cameraName << std::endl;
576  }
577 
578  return vpMbTracker::getClipping();
579 }
580 
587 {
588  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
589  if (it != m_mapOfKltTrackers.end()) {
590  return it->second->getFaces();
591  }
592 
593  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl;
594  return faces;
595 }
596 
603 {
604  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
605  if (it != m_mapOfKltTrackers.end()) {
606  return it->second->getFaces();
607  }
608 
609  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
610  return faces;
611 }
612 
618 std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbKltMultiTracker::getFaces() const
619 {
620  std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > mapOfFaces;
621  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
622  it != m_mapOfKltTrackers.end(); ++it) {
623  mapOfFaces[it->first] = it->second->faces;
624  }
625 
626  return mapOfFaces;
627 }
628 
634 std::list<vpMbtDistanceCircle *> &vpMbKltMultiTracker::getFeaturesCircle()
635 {
636  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
637 
638  if (it_klt != m_mapOfKltTrackers.end()) {
639  return it_klt->second->getFeaturesCircle();
640  } else {
641  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
642  }
643 
644  return circles_disp;
645 }
646 
653 std::list<vpMbtDistanceCircle *> &vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName)
654 {
655  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
656 
657  if (it != m_mapOfKltTrackers.end()) {
658  return it->second->getFeaturesCircle();
659  } else {
660  std::cerr << "The camera: " << cameraName << " does not exist !";
661  }
662 
663  return circles_disp;
664 }
665 
672 std::list<vpMbtDistanceKltPoints *> &vpMbKltMultiTracker::getFeaturesKlt()
673 {
674  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
675 
676  if (it_klt != m_mapOfKltTrackers.end()) {
677  return it_klt->second->getFeaturesKlt();
678  } else {
679  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
680  }
681 
682  return kltPolygons;
683 }
684 
692 std::list<vpMbtDistanceKltPoints *> &vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName)
693 {
694  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
695 
696  if (it != m_mapOfKltTrackers.end()) {
697  return it->second->getFeaturesKlt();
698  } else {
699  std::cerr << "The camera: " << cameraName << " does not exist !";
700  }
701 
702  return kltPolygons;
703 }
704 
710 std::list<vpMbtDistanceKltCylinder *> &vpMbKltMultiTracker::getFeaturesKltCylinder()
711 {
712  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
713 
714  if (it_klt != m_mapOfKltTrackers.end()) {
715  return it_klt->second->getFeaturesKltCylinder();
716  } else {
717  std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
718  }
719 
720  return kltCylinders;
721 }
722 
729 std::list<vpMbtDistanceKltCylinder *> &vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName)
730 {
731  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
732 
733  if (it != m_mapOfKltTrackers.end()) {
734  return it->second->getFeaturesKltCylinder();
735  } else {
736  std::cerr << "The camera: " << cameraName << " does not exist !";
737  }
738 
739  return kltCylinders;
740 }
741 
750 std::map<std::string, std::vector<vpImagePoint> > vpMbKltMultiTracker::getKltImagePoints() const
751 {
752  std::map<std::string, std::vector<vpImagePoint> > mapOfFeatures;
753 
754  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
755  it != m_mapOfKltTrackers.end(); ++it) {
756  mapOfFeatures[it->first] = it->second->getKltImagePoints();
757  }
758 
759  return mapOfFeatures;
760 }
761 
771 std::map<std::string, std::map<int, vpImagePoint> > vpMbKltMultiTracker::getKltImagePointsWithId() const
772 {
773  std::map<std::string, std::map<int, vpImagePoint> > mapOfFeatures;
774 
775  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
776  it != m_mapOfKltTrackers.end(); ++it) {
777  mapOfFeatures[it->first] = it->second->getKltImagePointsWithId();
778  }
779 
780  return mapOfFeatures;
781 }
782 
788 std::map<std::string, vpKltOpencv> vpMbKltMultiTracker::getKltOpencv() const
789 {
790  std::map<std::string, vpKltOpencv> mapOfKltOpenCVTracker;
791 
792  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
793  it != m_mapOfKltTrackers.end(); ++it) {
794  mapOfKltOpenCVTracker[it->first] = it->second->getKltOpencv();
795  }
796 
797  return mapOfKltOpenCVTracker;
798 }
799 
805 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
806 std::map<std::string, std::vector<cv::Point2f> > vpMbKltMultiTracker::getKltPoints() const
807 {
808  std::map<std::string, std::vector<cv::Point2f> > mapOfFeatures;
809 
810  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
811  it != m_mapOfKltTrackers.end(); ++it) {
812  mapOfFeatures[it->first] = it->second->getKltPoints();
813  }
814 
815  return mapOfFeatures;
816 }
817 #else
818 std::map<std::string, CvPoint2D32f *> vpMbKltMultiTracker::getKltPoints()
819 {
820  std::map<std::string, CvPoint2D32f *> mapOfFeatures;
821 
822  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
823  it != m_mapOfKltTrackers.end(); ++it) {
824  mapOfFeatures[it->first] = it->second->getKltPoints();
825  }
826 
827  return mapOfFeatures;
828 }
829 #endif
830 
836 std::map<std::string, int> vpMbKltMultiTracker::getKltNbPoints() const
837 {
838  std::map<std::string, int> mapOfFeatures;
839 
840  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
841  it != m_mapOfKltTrackers.end(); ++it) {
842  mapOfFeatures[it->first] = it->second->getKltNbPoints();
843  }
844 
845  return mapOfFeatures;
846 }
847 
854 {
855  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
856  if (it != m_mapOfKltTrackers.end()) {
857  return it->second->getNbPolygon();
858  }
859 
860  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl;
861  return 0;
862 }
863 
870 std::map<std::string, unsigned int> vpMbKltMultiTracker::getMultiNbPolygon() const
871 {
872  std::map<std::string, unsigned int> mapOfNbPolygons;
873  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
874  it != m_mapOfKltTrackers.end(); ++it) {
875  mapOfNbPolygons[it->first] = it->second->getNbPolygon();
876  }
877 
878  return mapOfNbPolygons;
879 }
880 
888 {
889  if (m_mapOfKltTrackers.size() == 2) {
890  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
891  it->second->getPose(c1Mo);
892  ++it;
893 
894  it->second->getPose(c2Mo);
895  } else {
896  std::cerr << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !" << std::endl;
897  }
898 }
899 
908 void vpMbKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const
909 {
910  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
911  if (it != m_mapOfKltTrackers.end()) {
912  it->second->getPose(cMo_);
913  } else {
914  std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
915  }
916 }
917 
923 void vpMbKltMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
924 {
925  // Clear the map
926  mapOfCameraPoses.clear();
927 
928  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
929  it != m_mapOfKltTrackers.end(); ++it) {
930  vpHomogeneousMatrix cMo_;
931  it->second->getPose(cMo_);
932  mapOfCameraPoses[it->first] = cMo_;
933  }
934 }
935 
937 
938 #ifdef VISP_HAVE_MODULE_GUI
939 
949 void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
950  const std::string &displayFile)
951 {
952  if (m_mapOfKltTrackers.empty()) {
953  throw vpException(vpTrackingException::initializationError, "There is no camera !");
954  } else if (m_mapOfKltTrackers.size() > 1) {
955  throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
956  } else {
957  // Get the vpMbKltTracker object for the reference camera name
958  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
959  if (it != m_mapOfKltTrackers.end()) {
960  it->second->initClick(I, points3D_list, displayFile);
961  it->second->getPose(cMo);
962 
963  // Init c0Mo
964  c0Mo = cMo;
965  ctTc0.eye();
966  } else {
967  std::stringstream ss;
968  ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
969  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
970  }
971  }
972 }
973 
1005 void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
1006  const bool displayHelp, const vpHomogeneousMatrix &T)
1007 {
1008  if (m_mapOfKltTrackers.empty()) {
1009  throw vpException(vpTrackingException::initializationError, "There is no camera !");
1010  } else if (m_mapOfKltTrackers.size() > 1) {
1011  throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
1012  } else {
1013  // Get the vpMbKltTracker object for the reference camera name
1014  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
1015  if (it != m_mapOfKltTrackers.end()) {
1016  it->second->initClick(I, initFile, displayHelp, T);
1017  it->second->getPose(cMo);
1018 
1019  // Init c0Mo
1020  c0Mo = cMo;
1021  ctTc0.eye();
1022  } else {
1023  std::stringstream ss;
1024  ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
1025  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1026  }
1027  }
1028 }
1029 
1068  const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
1069  const bool firstCameraIsReference)
1070 {
1071  if (m_mapOfKltTrackers.size() == 2) {
1072  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1073  it->second->initClick(I1, initFile1, displayHelp);
1074 
1075  if (firstCameraIsReference) {
1076  // Set the reference cMo
1077  it->second->getPose(cMo);
1078 
1079  // Set the reference camera parameters
1080  it->second->getCameraParameters(this->cam);
1081 
1082  // Init c0Mo and ctTc0
1083  c0Mo = cMo;
1084  ctTc0.eye();
1085  }
1086 
1087  ++it;
1088 
1089  it->second->initClick(I2, initFile2, displayHelp);
1090 
1091  if (!firstCameraIsReference) {
1092  // Set the reference cMo
1093  it->second->getPose(cMo);
1094 
1095  // Set the reference camera parameters
1096  it->second->getCameraParameters(this->cam);
1097 
1098  // Init c0Mo and ctTc0
1099  c0Mo = cMo;
1100  ctTc0.eye();
1101  }
1102  } else {
1103  std::stringstream ss;
1104  ss << "Cannot init click ! Require two cameras but there are " << m_mapOfKltTrackers.size() << " cameras !";
1105  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1106  }
1107 }
1108 
1138 void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1139  const std::string &initFile, const bool displayHelp)
1140 {
1141  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1142  if (it_klt != m_mapOfKltTrackers.end()) {
1143  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1144  mapOfImages.find(m_referenceCameraName);
1145 
1146  if (it_img != mapOfImages.end()) {
1147  it_klt->second->initClick(*it_img->second, initFile, displayHelp);
1148 
1149  // Set the reference cMo
1150  it_klt->second->getPose(cMo);
1151 
1152  // Init c0Mo
1153  c0Mo = cMo;
1154  ctTc0.eye();
1155 
1156  // Set the pose for the others cameras
1157  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
1158  if (it_klt->first != m_referenceCameraName) {
1159  it_img = mapOfImages.find(it_klt->first);
1160  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1161  m_mapOfCameraTransformationMatrix.find(it_klt->first);
1162 
1163  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1164  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1165  it_klt->second->cMo = cCurrentMo;
1166  it_klt->second->init(*it_img->second);
1167  } else {
1168  std::stringstream ss;
1169  ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !";
1170  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1171  }
1172  }
1173  }
1174  } else {
1175  std::stringstream ss;
1176  ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !";
1177  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1178  }
1179  } else {
1180  std::stringstream ss;
1181  ss << "Cannot init click ! The reference camera: " << m_referenceCameraName << " does not exist !";
1182  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1183  }
1184 }
1185 
1215 void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1216  const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp)
1217 {
1218  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1219  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1220  mapOfImages.find(m_referenceCameraName);
1221  std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
1222 
1223  if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1224  it_klt->second->initClick(*it_img->second, it_initFile->second, displayHelp);
1225  it_klt->second->getPose(cMo);
1226 
1227  c0Mo = this->cMo;
1228  ctTc0.eye();
1229  } else {
1230  throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !");
1231  }
1232 
1233  // Vector of missing initFile for cameras
1234  std::vector<std::string> vectorOfMissingCameraPoses;
1235 
1236  // Set pose for the specified cameras
1237  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
1238  if (it_klt->first != m_referenceCameraName) {
1239  it_img = mapOfImages.find(it_klt->first);
1240  it_initFile = mapOfInitFiles.find(it_klt->first);
1241 
1242  if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
1243  // InitClick for the current camera
1244  it_klt->second->initClick(*it_img->second, it_initFile->second, displayHelp);
1245  } else {
1246  vectorOfMissingCameraPoses.push_back(it_klt->first);
1247  }
1248  }
1249  }
1250 
1251  // Set pose for cameras that do not have an initFile
1252  for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
1253  it1 != vectorOfMissingCameraPoses.end(); ++it1) {
1254  it_img = mapOfImages.find(*it1);
1255  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1257 
1258  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1259  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1260  m_mapOfKltTrackers[*it1]->cMo = cCurrentMo;
1261  m_mapOfKltTrackers[*it1]->init(*it_img->second);
1262  } else {
1263  std::stringstream ss;
1264  ss << "Missing image or missing camera transformation matrix ! Cannot "
1265  "set the pose for camera: "
1266  << (*it1) << " !";
1267  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1268  }
1269  }
1270 }
1271 #endif //#ifdef VISP_HAVE_MODULE_GUI
1272 
1291 void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1292 {
1293  // Monocular case only !
1294  if (m_mapOfKltTrackers.size() > 1) {
1295  throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !");
1296  }
1297 
1298  char s[FILENAME_MAX];
1299  std::fstream finit;
1300  vpPoseVector init_pos;
1301 
1302  std::string ext = ".pos";
1303  size_t pos = initFile.rfind(ext);
1304 
1305  if (pos == initFile.size() - ext.size() && pos != 0)
1306  sprintf(s, "%s", initFile.c_str());
1307  else
1308  sprintf(s, "%s.pos", initFile.c_str());
1309 
1310  finit.open(s, std::ios::in);
1311  if (finit.fail()) {
1312  std::cerr << "cannot read " << s << std::endl;
1313  throw vpException(vpException::ioError, "cannot read init file");
1314  }
1315 
1316  for (unsigned int i = 0; i < 6; i += 1) {
1317  finit >> init_pos[i];
1318  }
1319 
1320  // Set the new pose for the reference camera
1321  cMo.buildFrom(init_pos);
1322  c0Mo = cMo;
1323  ctTc0.eye();
1324 
1325  // Init for the reference camera
1326  std::map<std::string, vpMbKltTracker *>::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName);
1327  if (it_ref == m_mapOfKltTrackers.end()) {
1328  throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
1329  }
1330 
1331  it_ref->second->initFromPose(I, cMo);
1332 }
1333 
1341 {
1342  this->cMo = cMo_;
1343  c0Mo = cMo;
1344  ctTc0.eye();
1345 
1346  // Init for the reference camera
1347  std::map<std::string, vpMbKltTracker *>::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName);
1348  if (it_ref == m_mapOfKltTrackers.end()) {
1349  throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
1350  }
1351 
1352  it_ref->second->initFromPose(I, cMo);
1353 }
1354 
1362 {
1363  vpHomogeneousMatrix _cMo(cPo);
1364  initFromPose(I, _cMo);
1365 }
1366 
1378  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
1379  const bool firstCameraIsReference)
1380 {
1381  if (m_mapOfKltTrackers.size() == 2) {
1382  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1383  it->second->initFromPose(I1, c1Mo);
1384 
1385  ++it;
1386 
1387  it->second->initFromPose(I2, c2Mo);
1388 
1389  if (firstCameraIsReference) {
1390  this->cMo = c1Mo;
1391  } else {
1392  this->cMo = c2Mo;
1393  }
1394 
1395  c0Mo = this->cMo;
1396  ctTc0.eye();
1397  } else {
1398  std::stringstream ss;
1399  ss << "This method requires 2 cameras but there are " << m_mapOfKltTrackers.size() << " cameras !";
1401  }
1402 }
1403 
1411 void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1412  const vpHomogeneousMatrix &cMo_)
1413 {
1414  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1415 
1416  if (it_klt != m_mapOfKltTrackers.end()) {
1417  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
1418 
1419  if (it_img != mapOfImages.end()) {
1420  it_klt->second->initFromPose(*it_img->second, cMo_);
1421 
1422  cMo = cMo_;
1423  c0Mo = this->cMo;
1424  ctTc0.eye();
1425 
1426  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
1427  if (it_klt->first != m_referenceCameraName) {
1428  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1429  m_mapOfCameraTransformationMatrix.find(it_klt->first);
1430  it_img = mapOfImages.find(it_klt->first);
1431 
1432  if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && it_img != mapOfImages.end()) {
1433  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1434  it_klt->second->initFromPose(*it_img->second, cCurrentMo);
1435  } else {
1437  "Cannot find camera transformation matrix or image !");
1438  }
1439  }
1440  }
1441  } else {
1442  throw vpException(vpTrackingException::initializationError, "Cannot find image for reference camera !");
1443  }
1444  } else {
1445  std::stringstream ss;
1446  ss << "Cannot find reference camera: " << m_referenceCameraName << std::endl;
1448  }
1449 }
1450 
1457 void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1458  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
1459 {
1460  // Set the reference cMo
1461  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1462  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1463  mapOfImages.find(m_referenceCameraName);
1464  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
1465 
1466  if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
1467  it_klt->second->initFromPose(*it_img->second, it_camPose->second);
1468  it_klt->second->getPose(cMo);
1469 
1470  c0Mo = this->cMo;
1471  ctTc0.eye();
1472  } else {
1473  throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera !");
1474  }
1475 
1476  // Vector of missing pose matrices for cameras
1477  std::vector<std::string> vectorOfMissingCameraPoses;
1478 
1479  // Set pose for the specified cameras
1480  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
1481  it_img = mapOfImages.find(it_klt->first);
1482  it_camPose = mapOfCameraPoses.find(it_klt->first);
1483 
1484  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
1485  // Set pose
1486  it_klt->second->initFromPose(*it_img->second, it_camPose->second);
1487  } else {
1488  vectorOfMissingCameraPoses.push_back(it_klt->first);
1489  }
1490  }
1491 
1492  for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
1493  it1 != vectorOfMissingCameraPoses.end(); ++it1) {
1494  it_img = mapOfImages.find(*it1);
1495  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1497 
1498  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1499  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1500  m_mapOfKltTrackers[*it1]->initFromPose(*it_img->second, cCurrentMo);
1501  } else {
1502  std::stringstream ss;
1503  ss << "Missing image or missing camera transformation matrix ! Cannot "
1504  "set the pose for camera: "
1505  << (*it1) << " !";
1506  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1507  }
1508  }
1509 }
1510 
1553 void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile)
1554 {
1555  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
1556  if (it != m_mapOfKltTrackers.end()) {
1557  // Load ConfigFile for reference camera
1558  it->second->loadConfigFile(configFile);
1559  it->second->getCameraParameters(cam);
1560 
1561  // Set clipping
1562  this->clippingFlag = it->second->getClipping();
1563  this->angleAppears = it->second->getAngleAppear();
1564  this->angleDisappears = it->second->getAngleDisappear();
1565  } else {
1566  std::stringstream ss;
1567  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
1568  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1569  }
1570 }
1571 
1585 void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
1586  const bool firstCameraIsReference)
1587 {
1588  if (m_mapOfKltTrackers.size() == 2) {
1589  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1590  it->second->loadConfigFile(configFile1);
1591 
1592  if (firstCameraIsReference) {
1593  it->second->getCameraParameters(cam);
1594 
1595  // Set clipping
1596  this->clippingFlag = it->second->getClipping();
1597  this->angleAppears = it->second->getAngleAppear();
1598  this->angleDisappears = it->second->getAngleDisappear();
1599  }
1600  ++it;
1601 
1602  it->second->loadConfigFile(configFile2);
1603 
1604  if (!firstCameraIsReference) {
1605  it->second->getCameraParameters(cam);
1606 
1607  // Set clipping
1608  this->clippingFlag = it->second->getClipping();
1609  this->angleAppears = it->second->getAngleAppear();
1610  this->angleDisappears = it->second->getAngleDisappear();
1611  }
1612  } else {
1613  std::stringstream ss;
1614  ss << "Cannot loadConfigFile. Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
1615  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1616  }
1617 }
1618 
1629 void vpMbKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
1630 {
1631  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
1632  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
1633  std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_klt->first);
1634  if (it_config != mapOfConfigFiles.end()) {
1635  it_klt->second->loadConfigFile(it_config->second);
1636  } else {
1637  std::stringstream ss;
1638  ss << "Missing configuration file for camera: " << it_klt->first << " !";
1639  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1640  }
1641  }
1642 
1643  // Set the reference camera parameters
1644  std::map<std::string, vpMbKltTracker *>::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
1645  if (it != m_mapOfKltTrackers.end()) {
1646  it->second->getCameraParameters(cam);
1647 
1648  // Set clipping
1649  this->clippingFlag = it->second->getClipping();
1650  this->angleAppears = it->second->getAngleAppear();
1651  this->angleDisappears = it->second->getAngleDisappear();
1652  } else {
1653  std::stringstream ss;
1654  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
1655  throw vpException(vpTrackingException::initializationError, ss.str().c_str());
1656  }
1657 }
1658 
1686 void vpMbKltMultiTracker::loadModel(const std::string &modelFile, const bool verbose,
1687  const vpHomogeneousMatrix &T)
1688 {
1689  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1690  it != m_mapOfKltTrackers.end(); ++it) {
1691  it->second->loadModel(modelFile, verbose, T);
1692  }
1693 
1694  modelInitialised = true;
1695 }
1696 
1697 void vpMbKltMultiTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
1698 {
1699  m_nbInfos = 0;
1700  m_nbFaceUsed = 0;
1701 
1702  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1703  it != m_mapOfKltTrackers.end(); ++it) {
1704  vpMbKltTracker *klt = it->second;
1705 
1706  try {
1707  klt->preTracking(*mapOfImages[it->first]);
1708  m_nbInfos += klt->m_nbInfos;
1709  m_nbFaceUsed += klt->m_nbFaceUsed;
1710  } catch (...) {
1711  }
1712  }
1713 }
1714 
1715 void vpMbKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
1716 {
1717  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1718  it != m_mapOfKltTrackers.end(); ++it) {
1719  vpMbKltTracker *klt = it->second;
1720 
1721  // Set the camera pose
1722  it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
1723 
1724  if (klt->m_nbInfos > 0 && klt->postTracking(*mapOfImages[it->first], klt->m_w_klt)) {
1725  klt->reinit(*mapOfImages[it->first]);
1726 
1727  // set ctTc0 to identity
1728  if (it->first == m_referenceCameraName) {
1729  reinit(/*mapOfImages[it->first]*/);
1730  }
1731  }
1732 
1733  if (displayFeatures) {
1735  }
1736  }
1737 }
1738 
1742 void vpMbKltMultiTracker::reinit(/* const vpImage<unsigned char>& I*/)
1743 {
1744  c0Mo = cMo;
1745  ctTc0.eye();
1746 }
1747 
1760 void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1761  const vpHomogeneousMatrix &cMo_, const bool verbose,
1762  const vpHomogeneousMatrix &T)
1763 {
1764  if (m_mapOfKltTrackers.size() != 1) {
1765  std::stringstream ss;
1766  ss << "This method requires exactly one camera, there are " << m_mapOfKltTrackers.size() << " cameras !";
1767  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
1768  }
1769 
1770  firstInitialisation = true;
1771  modelInitialised = true;
1772 
1773  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1774  if (it_klt != m_mapOfKltTrackers.end()) {
1775  it_klt->second->reInitModel(I, cad_name, cMo_, verbose, T);
1776 
1777  // Set reference pose
1778  it_klt->second->getPose(cMo);
1779 
1780  c0Mo = cMo;
1781  ctTc0.eye();
1782  } else {
1783  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel the reference camera !");
1784  }
1785 }
1786 
1801  const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
1802  const vpHomogeneousMatrix &c2Mo, const bool verbose,
1803  const bool firstCameraIsReference)
1804 {
1805  if (m_mapOfKltTrackers.size() == 2) {
1806  std::map<std::string, vpMbKltTracker *>::const_iterator it_edge = m_mapOfKltTrackers.begin();
1807 
1808  it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose);
1809 
1810  if (firstCameraIsReference) {
1811  // Set reference pose
1812  it_edge->second->getPose(cMo);
1813  }
1814 
1815  ++it_edge;
1816 
1817  it_edge->second->reInitModel(I2, cad_name, c2Mo, verbose);
1818 
1819  if (!firstCameraIsReference) {
1820  // Set reference pose
1821  it_edge->second->getPose(cMo);
1822  }
1823 
1824  c0Mo = cMo;
1825  ctTc0.eye();
1826  } else {
1827  throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras !");
1828  }
1829 }
1830 
1841 void vpMbKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1842  const std::string &cad_name,
1843  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
1844  const bool verbose)
1845 {
1846  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1847  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
1848  mapOfImages.find(m_referenceCameraName);
1849  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
1850 
1851  if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
1852  it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
1853  modelInitialised = true;
1854 
1855  // Set reference pose
1856  it_klt->second->getPose(cMo);
1857 
1858  c0Mo = cMo;
1859  ctTc0.eye();
1860  } else {
1861  throw vpException(vpTrackingException::fatalError, "Cannot reInitModel for reference camera !");
1862  }
1863 
1864  std::vector<std::string> vectorOfMissingCameras;
1865  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
1866  if (it_klt->first != m_referenceCameraName) {
1867  it_img = mapOfImages.find(it_klt->first);
1868  it_camPose = mapOfCameraPoses.find(it_klt->first);
1869 
1870  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
1871  it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
1872  } else {
1873  vectorOfMissingCameras.push_back(it_klt->first);
1874  }
1875  }
1876  }
1877 
1878  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
1879  ++it) {
1880  it_img = mapOfImages.find(*it);
1881  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
1883 
1884  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
1885  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
1886  m_mapOfKltTrackers[*it]->reInitModel(*it_img->second, cad_name, cCurrentMo, verbose);
1887  }
1888  }
1889 }
1890 
1896 {
1897  cMo.eye();
1898  ctTc0.eye();
1899 
1900  // Call resetTracker for all the cameras
1901  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1902  it != m_mapOfKltTrackers.end(); ++it) {
1903  it->second->resetTracker();
1904  }
1905 
1906  m_computeInteraction = true;
1907  firstInitialisation = true;
1908  computeCovariance = false;
1909 
1910  angleAppears = vpMath::rad(89);
1912 
1914 
1915  maskBorder = 5;
1916  threshold_outlier = 0.5;
1917  percentGood = 0.7;
1918 
1919  m_lambda = 0.8;
1920  m_maxIter = 200;
1921 
1923 
1924  useScanLine = false;
1925 
1926 #ifdef VISP_HAVE_OGRE
1927  useOgre = false;
1928 #endif
1929 }
1930 
1941 {
1943 
1944  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1945  it != m_mapOfKltTrackers.end(); ++it) {
1946  it->second->setAngleAppear(a);
1947  }
1948 }
1949 
1960 {
1962 
1963  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1964  it != m_mapOfKltTrackers.end(); ++it) {
1965  it->second->setAngleDisappear(a);
1966  }
1967 }
1968 
1975 {
1976  if (m_mapOfKltTrackers.empty()) {
1977  throw vpException(vpTrackingException::fatalError, "There is no camera !");
1978  } else if (m_mapOfKltTrackers.size() > 1) {
1979  throw vpException(vpTrackingException::fatalError, "There is more than one camera !");
1980  } else {
1981  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
1982  if (it != m_mapOfKltTrackers.end()) {
1983  it->second->setCameraParameters(camera);
1984 
1985  // Set reference camera parameters
1986  this->cam = camera;
1987  } else {
1988  std::stringstream ss;
1989  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
1991  }
1992  }
1993 }
1994 
2004  const bool firstCameraIsReference)
2005 {
2006  if (m_mapOfKltTrackers.empty()) {
2007  throw vpException(vpTrackingException::fatalError, "There is no camera !");
2008  } else if (m_mapOfKltTrackers.size() == 2) {
2009  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2010  it->second->setCameraParameters(camera1);
2011 
2012  ++it;
2013  it->second->setCameraParameters(camera2);
2014 
2015  if (firstCameraIsReference) {
2016  this->cam = camera1;
2017  } else {
2018  this->cam = camera2;
2019  }
2020  } else {
2021  std::stringstream ss;
2022  ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
2024  }
2025 }
2026 
2033 void vpMbKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera)
2034 {
2035  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
2036  if (it != m_mapOfKltTrackers.end()) {
2037  it->second->setCameraParameters(camera);
2038 
2039  if (it->first == m_referenceCameraName) {
2040  this->cam = camera;
2041  }
2042  } else {
2043  std::stringstream ss;
2044  ss << "The camera: " << cameraName << " does not exist !";
2046  }
2047 }
2048 
2054 void vpMbKltMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
2055 {
2056  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
2057  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2058  std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_klt->first);
2059  if (it_cam != mapOfCameraParameters.end()) {
2060  it_klt->second->setCameraParameters(it_cam->second);
2061 
2062  if (it_klt->first == m_referenceCameraName) {
2063  this->cam = it_cam->second;
2064  }
2065  } else {
2066  std::stringstream ss;
2067  ss << "Missing camera parameters for camera: " << it_klt->first << " !";
2069  }
2070  }
2071 }
2072 
2081 void vpMbKltMultiTracker::setCameraTransformationMatrix(const std::string &cameraName,
2082  const vpHomogeneousMatrix &cameraTransformationMatrix)
2083 {
2084  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
2085  if (it != m_mapOfCameraTransformationMatrix.end()) {
2086  it->second = cameraTransformationMatrix;
2087  } else {
2088  std::stringstream ss;
2089  ss << "Cannot find camera: " << cameraName << " !";
2091  }
2092 }
2093 
2102  const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
2103 {
2104  // Check if all cameras have a transformation matrix
2105  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
2106  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2107  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2108  mapOfTransformationMatrix.find(it_klt->first);
2109 
2110  if (it_camTrans == mapOfTransformationMatrix.end()) {
2111  throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !");
2112  }
2113  }
2114 
2115  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
2116 }
2117 
2125 void vpMbKltMultiTracker::setClipping(const unsigned int &flags)
2126 {
2127  vpMbTracker::setClipping(flags);
2128 
2129  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2130  it != m_mapOfKltTrackers.end(); ++it) {
2131  it->second->setClipping(flags);
2132  }
2133 }
2134 
2143 void vpMbKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
2144 {
2145  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
2146  if (it != m_mapOfKltTrackers.end()) {
2147  it->second->setClipping(flags);
2148  } else {
2149  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2150  }
2151 }
2152 
2159 {
2161 
2162  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2163  it != m_mapOfKltTrackers.end(); ++it) {
2164  it->second->setCovarianceComputation(flag);
2165  }
2166 }
2167 
2174 {
2175  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2176  it != m_mapOfKltTrackers.end(); ++it) {
2177  it->second->setDisplayFeatures(displayF);
2178  }
2179 
2180  displayFeatures = displayF;
2181 }
2182 
2189 {
2191 
2192  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2193  it != m_mapOfKltTrackers.end(); ++it) {
2194  it->second->setFarClippingDistance(dist);
2195  }
2196 }
2197 
2204 void vpMbKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist)
2205 {
2206  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
2207  if (it != m_mapOfKltTrackers.end()) {
2208  it->second->setFarClippingDistance(dist);
2209  } else {
2210  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2211  }
2212 }
2213 
2214 #ifdef VISP_HAVE_OGRE
2215 
2225 {
2226  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2227  it != m_mapOfKltTrackers.end(); ++it) {
2228  it->second->setGoodNbRayCastingAttemptsRatio(ratio);
2229  }
2230 }
2231 
2242 {
2243  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2244  it != m_mapOfKltTrackers.end(); ++it) {
2245  it->second->setNbRayCastingAttemptsForVisibility(attempts);
2246  }
2247 }
2248 #endif
2249 
2256 {
2257  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
2258  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2259  it_klt->second->setKltOpencv(t);
2260  }
2261 }
2262 
2268 void vpMbKltMultiTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfOpenCVTrackers)
2269 {
2270  for (std::map<std::string, vpKltOpencv>::const_iterator it_kltOpenCV = mapOfOpenCVTrackers.begin();
2271  it_kltOpenCV != mapOfOpenCVTrackers.end(); ++it_kltOpenCV) {
2272  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(it_kltOpenCV->first);
2273  if (it_klt != m_mapOfKltTrackers.end()) {
2274  it_klt->second->setKltOpencv(it_kltOpenCV->second);
2275  } else {
2276  std::cerr << "The camera: " << it_kltOpenCV->first << " does not exist !" << std::endl;
2277  }
2278  }
2279 }
2280 
2292 void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &name)
2293 {
2294  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2295  it != m_mapOfKltTrackers.end(); ++it) {
2296  it->second->setLod(useLod, name);
2297  }
2298 }
2299 
2312 void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name)
2313 {
2314  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName);
2315 
2316  if (it_klt != m_mapOfKltTrackers.end()) {
2317  it_klt->second->setLod(useLod, name);
2318  } else {
2319  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
2320  }
2321 }
2322 
2328 void vpMbKltMultiTracker::setKltMaskBorder(const unsigned int &e)
2329 {
2330  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2331  it != m_mapOfKltTrackers.end(); ++it) {
2332  it->second->setKltMaskBorder(e);
2333  }
2334 
2335  maskBorder = e;
2336 }
2337 
2341 void vpMbKltMultiTracker::setMinLineLengthThresh(const double /*minLineLengthThresh*/, const std::string & /*name*/)
2342 {
2343  std::cerr << "Useless for KLT tracker !" << std::endl;
2344 }
2345 
2354 void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
2355 {
2356  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2357  it != m_mapOfKltTrackers.end(); ++it) {
2358  it->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
2359  }
2360 }
2361 
2372 void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
2373  const std::string &name)
2374 {
2375  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName);
2376 
2377  if (it_klt != m_mapOfKltTrackers.end()) {
2378  it_klt->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
2379  } else {
2380  std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
2381  }
2382 }
2383 
2390 {
2392 
2393  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2394  it != m_mapOfKltTrackers.end(); ++it) {
2395  it->second->setNearClippingDistance(dist);
2396  }
2397 }
2398 
2409 void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
2410 {
2411  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2412  it != m_mapOfKltTrackers.end(); ++it) {
2413  it->second->setOgreShowConfigDialog(showConfigDialog);
2414  }
2415 }
2416 
2426 {
2427  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2428  it != m_mapOfKltTrackers.end(); ++it) {
2429  it->second->setOgreVisibilityTest(v);
2430  }
2431 
2432 #ifdef VISP_HAVE_OGRE
2433  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2434  it != m_mapOfKltTrackers.end(); ++it) {
2435  it->second->faces.getOgreContext()->setWindowName("Multi MBT Klt (" + it->first + ")");
2436  }
2437 #endif
2438 
2439  useOgre = v;
2440 }
2441 
2448 void vpMbKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist)
2449 {
2450  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
2451  if (it != m_mapOfKltTrackers.end()) {
2452  it->second->setNearClippingDistance(dist);
2453  } else {
2454  std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
2455  }
2456 }
2457 
2464 {
2465  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2466  it != m_mapOfKltTrackers.end(); ++it) {
2467  it->second->setOptimizationMethod(opt);
2468  }
2469 
2470  m_optimizationMethod = opt;
2471 }
2472 
2481 {
2482  if (m_mapOfKltTrackers.size() == 1) {
2483  std::map<std::string, vpMbKltTracker *>::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
2484  if (it != m_mapOfKltTrackers.end()) {
2485  it->second->setPose(I, cMo_);
2486  this->cMo = cMo_;
2487 
2488  c0Mo = this->cMo;
2489  ctTc0.eye();
2490  } else {
2491  std::stringstream ss;
2492  ss << "Cannot find the reference camera: " << m_referenceCameraName << " !";
2494  }
2495  } else {
2496  std::stringstream ss;
2497  ss << "You are trying to set the pose with only one image and cMo "
2498  "but there are multiple cameras !";
2500  }
2501 }
2502 
2511 {
2512  if (m_mapOfKltTrackers.size() == 1) {
2513  std::map<std::string, vpMbKltTracker *>::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
2514  if (it != m_mapOfKltTrackers.end()) {
2515  vpImageConvert::convert(I_color, m_I);
2516  it->second->setPose(m_I, cMo_);
2517  this->cMo = cMo_;
2518 
2519  c0Mo = this->cMo;
2520  ctTc0.eye();
2521  } else {
2522  std::stringstream ss;
2523  ss << "Cannot find the reference camera: " << m_referenceCameraName << " !";
2525  }
2526  } else {
2527  std::stringstream ss;
2528  ss << "You are trying to set the pose with only one image and cMo "
2529  "but there are multiple cameras !";
2531  }
2532 }
2533 
2546  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
2547  const bool firstCameraIsReference)
2548 {
2549  if (m_mapOfKltTrackers.size() == 2) {
2550  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2551  it->second->setPose(I1, c1Mo);
2552 
2553  ++it;
2554 
2555  it->second->setPose(I2, c2Mo);
2556 
2557  if (firstCameraIsReference) {
2558  this->cMo = c1Mo;
2559  } else {
2560  this->cMo = c2Mo;
2561  }
2562 
2563  c0Mo = this->cMo;
2564  ctTc0.eye();
2565  } else {
2566  std::stringstream ss;
2567  ss << "This method requires 2 cameras but there are " << m_mapOfKltTrackers.size() << " cameras !";
2569  }
2570 }
2571 
2580 void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2581  const vpHomogeneousMatrix &cMo_)
2582 {
2583  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
2584  if (it_klt != m_mapOfKltTrackers.end()) {
2585  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2586  mapOfImages.find(m_referenceCameraName);
2587 
2588  if (it_img != mapOfImages.end()) {
2589  // Set pose on reference camera
2590  it_klt->second->setPose(*it_img->second, cMo_);
2591 
2592  // Set the reference cMo
2593  cMo = cMo_;
2594 
2595  c0Mo = this->cMo;
2596  ctTc0.eye();
2597 
2598  // Set the pose for the others cameras
2599  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2600  if (it_klt->first != m_referenceCameraName) {
2601  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2602  m_mapOfCameraTransformationMatrix.find(it_klt->first);
2603  it_img = mapOfImages.find(it_klt->first);
2604 
2605  if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && it_img != mapOfImages.end()) {
2606  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
2607  it_klt->second->setPose(*it_img->second, cCurrentMo);
2608  } else {
2609  throw vpException(vpTrackingException::fatalError, "Cannot find camera transformation matrix or image !");
2610  }
2611  }
2612  }
2613  } else {
2614  std::stringstream ss;
2615  ss << "Missing image for reference camera: " << m_referenceCameraName << " !";
2616  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2617  }
2618  } else {
2619  std::stringstream ss;
2620  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2621  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2622  }
2623 }
2624 
2635 void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2636  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2637 {
2638  // Set the reference cMo
2639  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
2640  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
2641  mapOfImages.find(m_referenceCameraName);
2642  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
2643 
2644  if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2645  it_klt->second->setPose(*it_img->second, it_camPose->second);
2646  it_klt->second->getPose(cMo);
2647 
2648  c0Mo = this->cMo;
2649  ctTc0.eye();
2650  } else {
2651  throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera !");
2652  }
2653 
2654  // Vector of missing pose matrices for cameras
2655  std::vector<std::string> vectorOfMissingCameraPoses;
2656 
2657  // Set pose for the specified cameras
2658  for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2659  if (it_klt->first != m_referenceCameraName) {
2660  it_img = mapOfImages.find(it_klt->first);
2661  it_camPose = mapOfCameraPoses.find(it_klt->first);
2662 
2663  if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
2664  // Set pose
2665  it_klt->second->setPose(*it_img->second, it_camPose->second);
2666  } else {
2667  vectorOfMissingCameraPoses.push_back(it_klt->first);
2668  }
2669  }
2670  }
2671 
2672  for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
2673  it1 != vectorOfMissingCameraPoses.end(); ++it1) {
2674  it_img = mapOfImages.find(*it1);
2675  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
2677 
2678  if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
2679  vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
2680  m_mapOfKltTrackers[*it1]->setPose(*it_img->second, cCurrentMo);
2681  } else {
2682  std::stringstream ss;
2683  ss << "Missing image or missing camera transformation matrix ! Cannot "
2684  "set the pose for camera: "
2685  << (*it1) << " !";
2686  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2687  }
2688  }
2689 }
2690 
2696 void vpMbKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
2697 {
2698  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(referenceCameraName);
2699  if (it != m_mapOfKltTrackers.end()) {
2700  m_referenceCameraName = referenceCameraName;
2701  } else {
2702  std::stringstream ss;
2703  ss << "The reference camera: " << referenceCameraName << " does not exist !";
2704  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2705  }
2706 }
2707 
2714 {
2715  // Set general setScanLineVisibilityTest
2717 
2718  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2719  it != m_mapOfKltTrackers.end(); ++it) {
2720  it->second->setScanLineVisibilityTest(v);
2721  }
2722 }
2723 
2730 {
2731  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2732  it != m_mapOfKltTrackers.end(); ++it) {
2733  it->second->setKltThresholdAcceptation(th);
2734  }
2735 
2736  threshold_outlier = th;
2737 }
2738 
2746 void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
2747 {
2748  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2749  it != m_mapOfKltTrackers.end(); ++it) {
2750  it->second->setUseKltTracking(name, useKltTracking);
2751  }
2752 }
2753 
2762 {
2763  // Track only with reference camera
2764  // Get the reference camera parameters
2765  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
2766 
2767  if (it != m_mapOfKltTrackers.end()) {
2768  it->second->track(I);
2769  it->second->getPose(cMo);
2770  } else {
2771  std::stringstream ss;
2772  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2773  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2774  }
2775 }
2776 
2781 {
2782  std::cout << "Not supported interface, this class is deprecated." << std::endl;
2783 }
2784 
2794 {
2795  if (m_mapOfKltTrackers.size() == 2) {
2796  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2797  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2798  mapOfImages[it->first] = &I1;
2799  ++it;
2800 
2801  mapOfImages[it->first] = &I2;
2802  track(mapOfImages);
2803  } else {
2804  std::stringstream ss;
2805  ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
2806  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2807  }
2808 }
2809 
2817 void vpMbKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
2818 {
2819  // Check if there is an image for each camera
2820  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
2821  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2822  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
2823 
2824  if (it_img == mapOfImages.end()) {
2825  throw vpException(vpTrackingException::fatalError, "Missing images !");
2826  }
2827  }
2828 
2829  preTracking(mapOfImages);
2830 
2831  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
2832  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
2833  }
2834 
2835  computeVVS();
2836 
2837  postTracking(mapOfImages);
2838 }
2839 
2841 
2847 std::map<std::string, int> vpMbKltMultiTracker::getNbKltPoints() const { return getKltNbPoints(); }
2848 
2855 void vpMbKltMultiTracker::setMaskBorder(const unsigned int &e) { setKltMaskBorder(e); }
2856 
2863 
2864 #elif !defined(VISP_BUILD_SHARED_LIBS)
2865 // Work arround to avoid warning:
2866 // libvisp_mbt.a(dummy_vpMbKltMultiTracker.cpp.o) has no symbols
2867 void dummy_vpMbKltMultiTracker(){}
2868 #endif // VISP_HAVE_OPENCV
2869 #elif !defined(VISP_BUILD_SHARED_LIBS)
2870 // Work arround to avoid warning: libvisp_mbt.a(dummy_vpMbKltMultiTracker.cpp.o) has no symbols
2871 void dummy_vpMbKltMultiTracker(){}
2872 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
bool m_computeInteraction
Definition: vpMbTracker.h:185
virtual unsigned int getNbPolygon() const
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:495
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
virtual std::vector< std::string > getCameraNames() const
virtual void getCameraParameters(vpCameraParameters &camera) const
vpMatrix m_L_kltMulti
Interaction matrix.
virtual std::list< vpMbtDistanceKltCylinder * > & getFeaturesKltCylinder()
virtual void setFarClippingDistance(const double &dist)
virtual std::map< std::string, int > getKltNbPoints() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void track(const vpImage< unsigned char > &I)
virtual void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="")
virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void setCovarianceComputation(const bool &flag)
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:252
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
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 std::map< std::string, int > getNbKltPoints() const
virtual void setAngleDisappear(const double &a)
virtual void loadConfigFile(const std::string &configFile)
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual std::list< vpMbtDistanceKltPoints * > & getFeaturesKlt()
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
std::list< vpMbtDistanceKltCylinder * > kltCylinders
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void setKltThresholdAcceptation(const double th)
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
virtual void setDisplayFeatures(const bool displayF)
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
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)
bool modelInitialised
Definition: vpMbTracker.h:123
error that can be emited by ViSP classes.
Definition: vpException.h:71
virtual void setThresholdAcceptation(const double th)
virtual void computeVVSInteractionMatrixAndResidu()
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
virtual void setLod(const bool useLod, const std::string &name="")
virtual void init(const vpImage< unsigned char > &I)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
virtual void reinit(const vpImage< unsigned char > &I)
virtual void computeVVSInteractionMatrixAndResidu()
Class that defines what is a point.
Definition: vpPoint.h:58
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:111
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
vpHomogeneousMatrix ctTc0
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual std::map< std::string, vpKltOpencv > getKltOpencv() const
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual std::map< std::string, std::vector< vpImagePoint > > getKltImagePoints() const
virtual void setKltMaskBorder(const unsigned int &e)
vpRobust m_robust_klt
Robust.
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4653
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
Error that can be emited by the vpTracker class and its derivates.
vp_deprecated void init()
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
virtual void setClipping(const unsigned int &flags)
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:419
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
virtual std::list< vpMbtDistanceCircle * > & getFeaturesCircle()
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual std::map< std::string, std::vector< cv::Point2f > > getKltPoints() const
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
vpColVector m_weightedError_kltMulti
Weighted error.
unsigned int maskBorder
Erosion of the mask.
Generic class defining intrinsic camera parameters.
unsigned int m_nbFaceUsed
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual void preTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:465
virtual void setAngleAppear(const double &a)
Model based tracker using only KLT.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getRows() const
Definition: vpArray2D.h:289
std::string m_referenceCameraName
Name of the reference camera.
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
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
double threshold_outlier
vpColVector m_w_kltMulti
Robust weights.
virtual void setKltOpencv(const vpKltOpencv &t)
virtual std::map< std::string, std::map< int, vpImagePoint > > getKltImagePointsWithId() const
void insert(unsigned int i, const vpColVector &v)
virtual void setNearClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void setOgreVisibilityTest(const bool &v)
vpColVector m_w_klt
Robust weights.
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
void preTracking(const vpImage< unsigned char > &I)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
std::map< std::string, vpMbKltTracker * > m_mapOfKltTrackers
Map of Model-based klt trackers.
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
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
vpHomogeneousMatrix inverse() const
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
virtual void computeVVSInit()
virtual void postTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:597
virtual void setClipping(const unsigned int &flags)
virtual void initClick(const vpImage< unsigned char > &I, const std::vector< vpPoint > &points3D_list, const std::string &displayFile="")
unsigned int m_nbInfos
virtual void computeVVSWeights()
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
vpColVector m_error_klt
(s - s*)
vpColVector m_error_kltMulti
(s - s*)
virtual void setFarClippingDistance(const double &dist)
virtual void setMaskBorder(const unsigned int &e)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
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)
std::list< vpMbtDistanceKltPoints * > kltPolygons
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
vpHomogeneousMatrix c0Mo
Initial pose.
virtual std::map< std::string, unsigned int > getMultiNbPolygon() const
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void setScanLineVisibilityTest(const bool &v)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setNearClippingDistance(const double &dist)
vpMatrix m_L_klt
Interaction matrix.