Visual Servoing Platform  version 3.1.0
vpMbEdgeKltMultiTracker.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2016 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 https://visp.inria.fr/download/ 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  * http://www.irisa.fr/lagadic
25  *
26  * If you have questions regarding the use of this file, please contact
27  * INRIA at visp@inria.fr
28  *
29  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
30  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
31  *
32  *
33  * Description:
34  * Model-based edge klt tracker with multiple cameras.
35  *
36  * Authors:
37  * Souriya Trinh
38  *
39  *****************************************************************************/
40 
46 #include <visp3/core/vpConfig.h>
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/vpMbEdgeKltMultiTracker.h>
53 
58  : vpMbEdgeMultiTracker(), vpMbKltMultiTracker(), m_factorKLT(0.65), m_factorMBT(0.35), thresholdKLT(2.),
59  thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"), m_nbrow(0),
60  m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
61 {
62  // Add default camera transformation matrix
64 
65  m_lambda = 0.8;
66  m_maxIter = 200;
67 }
68 
75  : vpMbEdgeMultiTracker(nbCameras), vpMbKltMultiTracker(nbCameras), m_factorKLT(0.65), m_factorMBT(0.35),
78 {
79 
80  if (nbCameras == 0) {
81  throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !");
82  } else if (nbCameras == 1) {
83  // Add default camera transformation matrix
85  } else if (nbCameras == 2) {
86  // Add default camera transformation matrix
89 
90  // Set by default the reference camera
91  m_referenceCameraName = "Camera1";
92  } else {
93  for (unsigned int i = 1; i <= nbCameras; i++) {
94  std::stringstream ss;
95  ss << "Camera" << i;
96 
97  // Add default camera transformation matrix
99  }
100 
101  // Set by default the reference camera to the first one
102  m_referenceCameraName = m_mapOfKltTrackers.begin()->first;
103  }
104 
105  m_lambda = 0.8;
106  m_maxIter = 200;
107 }
108 
114 vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const std::vector<std::string> &cameraNames)
115  : vpMbEdgeMultiTracker(cameraNames), vpMbKltMultiTracker(cameraNames), m_factorKLT(0.65), m_factorMBT(0.35),
118 {
119  m_lambda = 0.8;
120  m_maxIter = 200;
121 }
122 
124 
125 void vpMbEdgeKltMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
126  const unsigned int lvl)
127 {
128 
129  m_nbrow = initMbtTracking(mapOfImages, lvl);
130 
131  if (m_nbInfos < 4 && m_nbrow < 4) {
132  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
133  } else if (m_nbrow < 4) {
134  m_nbrow = 0;
135  }
136 
137  double factorMBT = m_factorMBT;
138  double factorKLT = m_factorKLT;
139  if (m_nbrow < 4) {
140  factorKLT = 1.;
141  std::cerr << "There are less than 4 KLT features, set factorKLT = 1. !" << std::endl;
142  }
143 
144  if (m_nbInfos < 4) {
145  factorMBT = 1.;
146  std::cerr << "There are less than 4 moving edges, set factorMBT = 1. !" << std::endl;
147  m_nbInfos = 0;
148  }
149 
150  computeVVSInit();
151 
152  vpHomogeneousMatrix cMoPrev;
153  vpHomogeneousMatrix ctTc0_Prev;
154  // Error vector for MBT + KLT for the previous iteration
155  vpColVector m_error_prev;
156  // Weighting vector for MBT + KLT for the previous iteration
157  vpColVector m_w_prev;
158  double mu = m_initialMu;
159 
160  // Create the map of VelocityTwistMatrices
161  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
162  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
163  it != m_mapOfCameraTransformationMatrix.end(); ++it) {
165  cVo.buildFrom(it->second);
166  mapOfVelocityTwist[it->first] = cVo;
167  }
168 
169  // Variables used in the minimization process
170  double residu = 0;
171  double residu_1 = -1;
172  unsigned int iter = 0;
173 
174  vpMatrix L_true;
175  vpMatrix LVJ_true;
176 
177  vpColVector v;
178 
179  vpMatrix LTL;
180  vpColVector LTR;
181 
182  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
183  computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
184 
185  bool reStartFromLastIncrement = false;
186  computeVVSCheckLevenbergMarquardt(iter, m_error_hybridMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
187  &m_w_prev);
188  if (reStartFromLastIncrement) {
189  ctTc0 = ctTc0_Prev;
190  }
191 
192  if (!reStartFromLastIncrement) {
194 
195  // Set weight for m_w with the good weighting between MBT and KLT
196  for (unsigned int cpt = 0; cpt < m_nbrow + 2 * m_nbInfos; cpt++) {
197  if (cpt < m_nbrow) {
198  m_w_hybridMulti[cpt] = (m_w_hybridMulti[cpt] * m_factor[cpt]) * factorMBT;
199  } else {
200  m_w_hybridMulti[cpt] *= factorKLT;
201  }
202  }
203 
204  if (computeCovariance) {
205  L_true = m_L_hybridMulti;
206  if (!isoJoIdentity) {
208  cVo.buildFrom(cMo);
209  LVJ_true = (m_L_hybridMulti * cVo * oJo);
210  }
211  }
212 
213  residu_1 = residu;
214  residu = 0;
215  double num = 0;
216  double den = 0;
217 
218  for (unsigned int i = 0; i < m_weightedError_hybridMulti.getRows(); i++) {
220  den += m_w_hybridMulti[i];
221 
223  if (m_computeInteraction) {
224  for (unsigned int j = 0; j < 6; j++) {
225  m_L_hybridMulti[i][j] *= m_w_hybridMulti[i];
226  }
227  }
228  }
229 
230  residu = sqrt(num / den);
231 
233  m_error_hybridMulti, m_error_prev, LTR, mu, v, &m_w_hybridMulti, &m_w_prev);
234 
235  cMoPrev = cMo;
236  ctTc0_Prev = ctTc0;
238  cMo = ctTc0 * c0Mo;
239  }
240 
241  iter++;
242  }
243 
245 }
246 
248 {
249  unsigned int totalNbRows = 2 * m_nbInfos + m_nbrow;
250 
251  m_L_hybridMulti.resize(totalNbRows, 6, false, false);
252  m_error_hybridMulti.resize(totalNbRows, false);
253 
254  m_weightedError_hybridMulti.resize(totalNbRows, false);
255  m_w_hybridMulti.resize(totalNbRows, false);
256  m_w_hybridMulti = 1;
257 
258  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
259  it != m_mapOfKltTrackers.end(); ++it) {
260  vpMbKltTracker *klt = it->second;
261  klt->computeVVSInit();
262  }
263 }
264 
266 {
267  throw vpException(vpException::fatalError, "vpMbEdgeKltMultiTracker::"
268  "computeVVSInteractionMatrixAndR"
269  "esidu() should not be called!");
270 }
271 
273  std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
274  std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
275 {
276  unsigned int startIdx = 0;
277 
278  if (m_nbrow >= 4) {
279  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
280  it != m_mapOfEdgeTrackers.end(); ++it) {
281  vpMbEdgeTracker *edge = it->second;
282 
283  // Set the corresponding cMo for the current camera
284  it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
285 
286  edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]);
287 
288  // Stack interaction matrix for MBT
289  m_L_hybridMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], startIdx, 0);
290  // Stack residual for MBT
291  m_error_hybridMulti.insert(startIdx, edge->m_error_edge);
292 
293  startIdx += edge->m_error_edge.getRows();
294  }
295  }
296 
297  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
298  it != m_mapOfKltTrackers.end(); ++it) {
299  vpMbKltTracker *klt = it->second;
300 
301  if (klt->m_nbInfos > 0) {
302 
303  // Use the ctTc0 variable instead of the formula in the monocular case
304  // to ensure that we have the same result than vpMbKltTracker
305  // as some slight differences can occur due to numerical imprecision
306  if (m_mapOfKltTrackers.size() == 1) {
307  klt->ctTc0 = ctTc0;
309  } else {
310  vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * cMo * klt->c0Mo.inverse();
311  klt->ctTc0 = c_curr_tTc_curr0;
313  }
314 
315  // Stack residual and interaction matrix
316  m_error_hybridMulti.insert(startIdx, klt->m_error_klt);
317  m_L_hybridMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], startIdx, 0);
318 
319  startIdx += 2 * klt->m_nbInfos;
320  }
321  }
322 }
323 
325 {
326  unsigned int startIdx = 0;
327 
328  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
329  it != m_mapOfEdgeTrackers.end(); ++it) {
330  vpMbEdgeTracker *edge = it->second;
331 
332  // Compute weights
333  edge->computeVVSWeights();
334 
335  m_w_hybridMulti.insert(startIdx, edge->m_w_edge);
336  startIdx += edge->m_w_edge.getRows();
337  }
338 
339  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
340  it != m_mapOfKltTrackers.end(); ++it) {
341  vpMbKltTracker *klt = it->second;
342 
343  // Compute weights
344  klt->computeVVSWeights(klt->m_robust_klt, klt->m_error_klt, klt->m_w_klt);
345 
346  m_w_hybridMulti.insert(startIdx, klt->m_w_klt);
347  startIdx += klt->m_w_klt.getRows();
348  }
349 }
350 
363  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
364  const bool displayFullModel)
365 {
366  vpMbEdgeMultiTracker::display(I, cMo_, cam_, col, thickness, displayFullModel);
367 
368  // Display only features for KLT trackers
369  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
370  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
371 
372  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
373  it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
374  vpMbtDistanceKltPoints *kltpoly = *it_pts;
375  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
376  kltpoly->displayPrimitive(I);
377  }
378  }
379 
380  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
381  it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
382  vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
383  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
384  kltPolyCylinder->displayPrimitive(I);
385  }
386  }
387 }
388 
401  const vpCameraParameters &cam_, const vpColor &color,
402  const unsigned int thickness, const bool displayFullModel)
403 {
404  vpMbEdgeMultiTracker::display(I, cMo_, cam_, color, thickness, displayFullModel);
405  // vpMbKltMultiTracker::display(I, cMo_, cam_, color, thickness,
406  // displayFullModel);
407 
408  // Display only features for KLT trackers
409  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
410  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
411 
412  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
413  it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
414  vpMbtDistanceKltPoints *kltpoly = *it_pts;
415  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
416  kltpoly->displayPrimitive(I);
417  }
418  }
419 
420  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
421  it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
422  vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
423  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
424  kltPolyCylinder->displayPrimitive(I);
425  }
426  }
427 }
428 
444  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
445  const vpCameraParameters &cam1, const vpCameraParameters &cam2,
446  const vpColor &color, const unsigned int thickness, const bool displayFullModel)
447 {
448  vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, thickness, displayFullModel);
449  // vpMbKltMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color,
450  // thickness, displayFullModel);
451 
452  // Display only features for KLT trackers
453  bool first = true;
454  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
455  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
456  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
457  it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
458  vpMbtDistanceKltPoints *kltpoly = *it_pts;
459  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
460  if (first) {
461  kltpoly->displayPrimitive(I1);
462  } else {
463  kltpoly->displayPrimitive(I2);
464  }
465  }
466  }
467 
468  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
469  it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
470  vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
471  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
472  if (first) {
473  kltPolyCylinder->displayPrimitive(I1);
474  } else {
475  kltPolyCylinder->displayPrimitive(I2);
476  }
477  }
478  }
479 
480  first = false;
481  }
482 }
483 
499  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
500  const vpCameraParameters &cam1, const vpCameraParameters &cam2,
501  const vpColor &color, const unsigned int thickness, const bool displayFullModel)
502 {
503  vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, thickness, displayFullModel);
504  // vpMbKltMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color,
505  // thickness, displayFullModel);
506 
507  // Display only features for KLT trackers (not the model)
508  bool first = true;
509  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
510  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
511  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
512  it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
513  vpMbtDistanceKltPoints *kltpoly = *it_pts;
514  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
515  if (first) {
516  kltpoly->displayPrimitive(I1);
517  } else {
518  kltpoly->displayPrimitive(I2);
519  }
520  }
521  }
522 
523  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
524  it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
525  vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
526  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
527  if (first) {
528  kltPolyCylinder->displayPrimitive(I1);
529  } else {
530  kltPolyCylinder->displayPrimitive(I2);
531  }
532  }
533  }
534 
535  first = false;
536  }
537 }
538 
550 void vpMbEdgeKltMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
551  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
552  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
553  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
554 {
555  vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, mapOfCameraParameters, col, thickness, displayFullModel);
556  // vpMbKltMultiTracker::display(mapOfImages, mapOfCameraPoses,
557  // mapOfCameraParameters, col, thickness, displayFullModel);
558 
559  // Display only features for KLT trackers (not the model)
560  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
561  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
562 
563  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
564  if (it_img != mapOfImages.end()) {
565  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
566  it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
567  vpMbtDistanceKltPoints *kltpoly = *it_pts;
568  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
569  kltpoly->displayPrimitive(*(it_img->second));
570  }
571  }
572 
573  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
574  it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
575  vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
576  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
577  kltPolyCylinder->displayPrimitive(*(it_img->second));
578  }
579  }
580  }
581 }
582 
594 void vpMbEdgeKltMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
595  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
596  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
597  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
598 {
599  vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, mapOfCameraParameters, col, thickness, displayFullModel);
600  // vpMbKltMultiTracker::display(mapOfImages, mapOfCameraPoses,
601  // mapOfCameraParameters, col, thickness, displayFullModel);
602 
603  // Display only features for KLT trackers (not the model)
604  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
605  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
606 
607  std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
608  if (it_img != mapOfImages.end()) {
609  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
610  it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
611  vpMbtDistanceKltPoints *kltpoly = *it_pts;
612  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
613  kltpoly->displayPrimitive(*(it_img->second));
614  }
615  }
616 
617  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
618  it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
619  vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
620  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
621  kltPolyCylinder->displayPrimitive(*(it_img->second));
622  }
623  }
624  }
625 }
626 
632 std::vector<std::string> vpMbEdgeKltMultiTracker::getCameraNames() const
633 {
635 }
636 
643 
651 {
652  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
654 }
655 
662 void vpMbEdgeKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const
663 {
664  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
665  vpMbEdgeMultiTracker::getCameraParameters(cameraName, camera);
666 }
667 
674  std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
675 {
676  // Clear the input map
677  mapOfCameraParameters.clear();
678 
679  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
680  vpMbEdgeMultiTracker::getCameraParameters(mapOfCameraParameters);
681 }
682 
690 unsigned int vpMbEdgeKltMultiTracker::getClipping(const std::string &cameraName) const
691 {
692  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
693  if (it != m_mapOfKltTrackers.end()) {
694  // Return the clipping flags for m_mapOfKltTrackers as it should be the
695  // same for the same camera in m_mapOfEdgeTrackers
696  return it->second->getClipping();
697  } else {
698  std::cerr << "Cannot find camera: " << cameraName << std::endl;
699  }
700 
701  return vpMbTracker::getClipping();
702 }
703 
705 {
706  std::cerr << "Return the wrong faces reference !" << std::endl;
707  std::cerr << "Use vpMbEdgeKltMultiTracker::getEdgeFaces or "
708  "vpMbEdgeKltMultiTracker::getKltFaces instead !"
709  << std::endl;
710 
711  return faces;
712 }
713 
721 {
722  return vpMbEdgeMultiTracker::getFaces(cameraName);
723 }
724 
730 std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getEdgeFaces() const
731 {
733 }
734 
742 {
743  return vpMbKltMultiTracker::getFaces(cameraName);
744 }
745 
751 std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getKltFaces() const
752 {
754 }
755 
757 {
758  std::cerr << "Use vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon or "
759  "vpMbEdgeKltMultiTracker::getKltMultiNbPolygon instead !"
760  << std::endl;
761  return 0;
762 }
763 
770 std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const
771 {
773 }
774 
781 std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const
782 {
784 }
785 
793 {
794  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
795  vpMbEdgeMultiTracker::getPose(c1Mo, c2Mo);
796 }
797 
806 void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const
807 {
808  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
809  vpMbEdgeMultiTracker::getPose(cameraName, cMo_);
810 }
811 
817 void vpMbEdgeKltMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
818 {
819  // Clear the map
820  mapOfCameraPoses.clear();
821 
822  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
823  vpMbEdgeMultiTracker::getPose(mapOfCameraPoses);
824 }
825 
827 {
828  if (!modelInitialised) {
829  throw vpException(vpTrackingException::initializationError, "model not initialized");
830  }
831 }
832 
833 #ifdef VISP_HAVE_MODULE_GUI
834 
844 void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
845  const std::string &displayFile)
846 {
847  // Cannot use directly set pose for KLT as it is different than for the edge
848  // case It moves the KLT points instead of detecting new KLT points
849  vpMbKltMultiTracker::initClick(I, points3D_list, displayFile);
850 
851  // Set pose for Edge (setPose or initFromPose is equivalent with
852  // vpMbEdgeTracker) And it avoids to click a second time
854 }
855 
884 void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
885  const bool displayHelp)
886 {
887  // Cannot use directly set pose for KLT as it is different than for the edge
888  // case It moves the KLT points instead of detecting new KLT points
889  vpMbKltMultiTracker::initClick(I, initFile, displayHelp);
890 
891  // Set pose for Edge (setPose or initFromPose is equivalent with
892  // vpMbEdgeTracker) And it avoids to click a second time
894 }
895 
932  const std::string &initFile1, const std::string &initFile2,
933  const bool displayHelp, const bool firstCameraIsReference)
934 {
935  vpMbKltMultiTracker::initClick(I1, I2, initFile1, initFile2, displayHelp, firstCameraIsReference);
936 
937  // Get c2Mo
938  vpHomogeneousMatrix c2Mo;
939  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
940  if (firstCameraIsReference) {
941  ++it_klt;
942  it_klt->second->getPose(c2Mo);
943  } else {
944  it_klt->second->getPose(c2Mo);
945  }
946 
947  // Set pose for Edge (setPose or initFromPose is equivalent with
948  // vpMbEdgeTracker)
949  vpMbEdgeMultiTracker::setPose(I1, I2, cMo, c2Mo, firstCameraIsReference);
950 }
951 
980 void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
981  const std::string &initFile, const bool displayHelp)
982 {
983  vpMbKltMultiTracker::initClick(mapOfImages, initFile, displayHelp);
984 
985  // Get pose for all the cameras
986  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
987  vpMbKltMultiTracker::getPose(mapOfCameraPoses);
988 
989  // Set pose for Edge for all the cameras (setPose or initFromPose is
990  // equivalent with vpMbEdgeTracker) And it avoids to click a second time
991  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
992 }
993 
1022 void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1023  const std::map<std::string, std::string> &mapOfInitFiles,
1024  const bool displayHelp)
1025 {
1026  vpMbKltMultiTracker::initClick(mapOfImages, mapOfInitFiles, displayHelp);
1027 
1028  // Get pose for all the cameras
1029  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
1030  vpMbKltMultiTracker::getPose(mapOfCameraPoses);
1031 
1032  // Set pose for Edge for all the cameras (setPose or initFromPose is
1033  // equivalent with vpMbEdgeTracker) And it avoids to click a second time
1034  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
1035 }
1036 #endif //#ifdef VISP_HAVE_MODULE_GUI
1037 
1038 void vpMbEdgeKltMultiTracker::initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int,
1039  const std::string &)
1040 {
1041  std::cerr << "The method initCircle is not used in vpMbEdgeKltMultiTracker !" << std::endl;
1042 }
1043 
1044 void vpMbEdgeKltMultiTracker::initCylinder(const vpPoint &, const vpPoint &, const double, const int,
1045  const std::string &)
1046 {
1047  std::cerr << "The method initCylinder is not used in vpMbEdgeKltMultiTracker !" << std::endl;
1048 }
1049 
1051 {
1052  std::cerr << "The method initFaceFromCorners is not used in "
1053  "vpMbEdgeKltMultiTracker !"
1054  << std::endl;
1055 }
1056 
1058 {
1059  std::cerr << "The method initFaceFromLines is not used in "
1060  "vpMbEdgeKltMultiTracker !"
1061  << std::endl;
1062 }
1063 
1082 void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1083 {
1084  // Monocular case only !
1085  if (m_mapOfKltTrackers.size() > 1) {
1086  throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !");
1087  }
1088 
1089  char s[FILENAME_MAX];
1090  std::fstream finit;
1091  vpPoseVector init_pos;
1092 
1093  std::string ext = ".pos";
1094  size_t pos = initFile.rfind(ext);
1095 
1096  if (pos == initFile.size() - ext.size() && pos != 0)
1097  sprintf(s, "%s", initFile.c_str());
1098  else
1099  sprintf(s, "%s.pos", initFile.c_str());
1100 
1101  finit.open(s, std::ios::in);
1102  if (finit.fail()) {
1103  std::cerr << "cannot read " << s << std::endl;
1104  throw vpException(vpException::ioError, "cannot read init file");
1105  }
1106 
1107  for (unsigned int i = 0; i < 6; i += 1) {
1108  finit >> init_pos[i];
1109  }
1110 
1111  cMo.buildFrom(init_pos);
1114 }
1115 
1123 {
1124  // Monocular case only !
1125  if (m_mapOfKltTrackers.size() > 1) {
1126  throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !");
1127  }
1128 
1129  this->cMo = cMo_;
1132 }
1133 
1141 {
1142  vpHomogeneousMatrix _cMo(cPo);
1143  initFromPose(I, _cMo);
1144 }
1145 
1157  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
1158  const bool firstCameraIsReference)
1159 {
1160  vpMbEdgeMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1161  vpMbKltMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1162 }
1163 
1171 void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1172  const vpHomogeneousMatrix &cMo_)
1173 {
1174  vpMbEdgeMultiTracker::initFromPose(mapOfImages, cMo_);
1175  vpMbKltMultiTracker::initFromPose(mapOfImages, cMo_);
1176 }
1177 
1184 void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1185  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
1186 {
1187  vpMbEdgeMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
1188  vpMbKltMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
1189 }
1190 
1191 unsigned int
1192 vpMbEdgeKltMultiTracker::initMbtTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1193  unsigned int lvl)
1194 {
1195  vpMbEdgeTracker *edge = NULL;
1196  unsigned int nbrows = 0;
1197 
1198  m_factor.resize(0, false);
1199  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1200  it != m_mapOfEdgeTrackers.end(); ++it) {
1201  edge = it->second;
1202 
1203  try {
1204  edge->computeVVSInit();
1205 
1206  unsigned int nbrow = edge->m_error_edge.getRows();
1207  nbrows += nbrow;
1208 
1209  // Set the corresponding cMo for each camera
1210  // Used in computeVVSFirstPhaseFactor with computeInteractionMatrixError
1211  edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
1212 
1213  edge->computeVVSFirstPhaseFactor(*mapOfImages[it->first], lvl);
1214  m_factor.stack(edge->m_factor);
1215  } catch (...) {
1216  edge->m_L_edge.resize(0, 6, false, false);
1217  edge->m_error_edge.resize(0, false);
1218 
1219  edge->m_weightedError_edge.resize(0, false);
1220  edge->m_w_edge.resize(0, false);
1221  edge->m_factor.resize(0, false);
1222 
1223  edge->m_robustLines.resize(0);
1224  edge->m_robustCylinders.resize(0);
1225  edge->m_robustCircles.resize(0);
1226 
1227  edge->m_wLines.resize(0, false);
1228  edge->m_wCylinders.resize(0, false);
1229  edge->m_wCircles.resize(0, false);
1230 
1231  edge->m_errorLines.resize(0, false);
1232  edge->m_errorCylinders.resize(0, false);
1233  edge->m_errorCircles.resize(0, false);
1234  }
1235  }
1236 
1237  return nbrows;
1238 }
1239 
1287 void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile)
1288 {
1291 }
1292 
1309 void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
1310  const bool firstCameraIsReference)
1311 {
1312  vpMbEdgeMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference);
1313  vpMbKltMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference);
1314 }
1315 
1329 void vpMbEdgeKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
1330 {
1331  vpMbEdgeMultiTracker::loadConfigFile(mapOfConfigFiles);
1332  vpMbKltMultiTracker::loadConfigFile(mapOfConfigFiles);
1333 }
1334 
1360 void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, const bool verbose)
1361 {
1362  vpMbEdgeMultiTracker::loadModel(modelFile, verbose);
1363  vpMbKltMultiTracker::loadModel(modelFile, verbose);
1364 
1365  modelInitialised = true;
1366 }
1367 
1368 void vpMbEdgeKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1369  const unsigned int lvl)
1370 {
1371  // MBT
1372  vpMbEdgeTracker *edge = NULL;
1373  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1374  it != m_mapOfEdgeTrackers.end(); ++it) {
1375  edge = it->second;
1376 
1377  edge->updateMovingEdgeWeights();
1378 
1379  if (displayFeatures) {
1380  edge->displayFeaturesOnImage(*mapOfImages[it->first], lvl);
1381  }
1382  }
1383 
1384  // KLT
1385  vpMbKltMultiTracker::postTracking(mapOfImages);
1386 
1387  // Looking for new visible face
1388  bool newvisibleface = false;
1389  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1390  it != m_mapOfEdgeTrackers.end(); ++it) {
1391  edge = it->second;
1392  edge->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
1393  }
1394 
1395  if (useScanLine) {
1396  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1397  it != m_mapOfEdgeTrackers.end(); ++it) {
1398  edge = it->second;
1399 
1400  edge->faces.computeClippedPolygons(it->second->cMo, it->second->cam);
1401  edge->faces.computeScanLineRender(it->second->cam, mapOfImages[it->first]->getWidth(),
1402  mapOfImages[it->first]->getHeight());
1403  }
1404  }
1405 
1406  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1407  it != m_mapOfEdgeTrackers.end(); ++it) {
1408  edge = it->second;
1409  edge->updateMovingEdge(*mapOfImages[it->first]);
1410  }
1411 
1412  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1413  it != m_mapOfEdgeTrackers.end(); ++it) {
1414  edge = it->second;
1415  edge->initMovingEdge(*mapOfImages[it->first], it->second->cMo);
1416 
1417  // Reinit the moving edge for the lines which need it.
1418  edge->reinitMovingEdge(*mapOfImages[it->first], it->second->cMo);
1419 
1420  if (computeProjError) {
1421  edge->computeProjectionError(*mapOfImages[it->first]);
1422  }
1423  }
1424 }
1425 
1426 void vpMbEdgeKltMultiTracker::reinit(/*const vpImage<unsigned char>& I */)
1427 {
1428  // vpMbEdgeMultiTracker::reinit();
1430 }
1431 
1441 void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1442  const vpHomogeneousMatrix &cMo_, const bool verbose)
1443 {
1444  vpMbEdgeMultiTracker::reInitModel(I, cad_name, cMo_, verbose);
1445  vpMbKltMultiTracker::reInitModel(I, cad_name, cMo_, verbose);
1446 }
1447 
1462  const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
1463  const vpHomogeneousMatrix &c2Mo, const bool verbose,
1464  const bool firstCameraIsReference)
1465 {
1466  vpMbEdgeMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference);
1467  vpMbKltMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference);
1468 }
1469 
1480 void vpMbEdgeKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1481  const std::string &cad_name,
1482  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
1483  const bool verbose)
1484 {
1485  vpMbEdgeMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
1486  vpMbKltMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
1487 }
1488 
1494 {
1497 }
1498 
1509 {
1512 }
1513 
1524 {
1527 }
1528 
1535 {
1538 
1539  this->cam = camera;
1540 }
1541 
1551  const bool firstCameraIsReference)
1552 {
1553  vpMbEdgeMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference);
1554  vpMbKltMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference);
1555 
1556  if (firstCameraIsReference) {
1557  this->cam = camera1;
1558  } else {
1559  this->cam = camera2;
1560  }
1561 }
1562 
1569 void vpMbEdgeKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera)
1570 {
1571  vpMbEdgeMultiTracker::setCameraParameters(cameraName, camera);
1572  vpMbKltMultiTracker::setCameraParameters(cameraName, camera);
1573 
1574  if (cameraName == m_referenceCameraName) {
1575  this->cam = camera;
1576  }
1577 }
1578 
1585  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
1586 {
1587  vpMbEdgeMultiTracker::setCameraParameters(mapOfCameraParameters);
1588  vpMbKltMultiTracker::setCameraParameters(mapOfCameraParameters);
1589 
1590  for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
1591  it != mapOfCameraParameters.end(); ++it) {
1592  if (it->first == m_referenceCameraName) {
1593  this->cam = it->second;
1594  }
1595  }
1596 }
1597 
1607  const vpHomogeneousMatrix &cameraTransformationMatrix)
1608 {
1609  vpMbEdgeMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
1610  vpMbKltMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
1611 
1612  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
1613  if (it != m_mapOfCameraTransformationMatrix.end()) {
1614  it->second = cameraTransformationMatrix;
1615  } else {
1616  std::cerr << "Cannot find camera: " << cameraName << " !" << std::endl;
1617  }
1618 }
1619 
1628  const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
1629 {
1630  vpMbEdgeMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
1631  vpMbKltMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
1632 
1633  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
1634 }
1635 
1643 void vpMbEdgeKltMultiTracker::setClipping(const unsigned int &flags)
1644 {
1647 }
1648 
1657 void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
1658 {
1659  // Here, we do not change the general clipping flag
1660  vpMbEdgeMultiTracker::setClipping(cameraName, flags);
1661  vpMbKltMultiTracker::setClipping(cameraName, flags);
1662 }
1663 
1670 {
1673 }
1674 
1691 {
1694 }
1695 
1702 {
1705 }
1706 
1713 void vpMbEdgeKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist)
1714 {
1717 }
1718 
1719 #ifdef VISP_HAVE_OGRE
1720 
1730 {
1733 }
1734 
1745 {
1748 }
1749 #endif
1750 
1762 void vpMbEdgeKltMultiTracker::setLod(const bool useLod, const std::string &name)
1763 {
1764  vpMbEdgeMultiTracker::setLod(useLod, name);
1765  vpMbKltMultiTracker::setLod(useLod, name);
1766 }
1767 
1780 void vpMbEdgeKltMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name)
1781 {
1782  vpMbEdgeMultiTracker::setLod(useLod, cameraName, name);
1783  vpMbKltMultiTracker::setLod(useLod, cameraName, name);
1784 }
1785 
1795 void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
1796 {
1797  vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, name);
1798 }
1799 
1811 void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
1812  const std::string &name)
1813 {
1814  vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, cameraName, name);
1815 }
1816 
1825 void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
1826 {
1827  vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name);
1828  vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name);
1829 }
1830 
1841 void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
1842  const std::string &name)
1843 {
1844  vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
1845  vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
1846 }
1847 
1854 {
1857 }
1858 
1865 void vpMbEdgeKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist)
1866 {
1869 }
1870 
1881 void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
1882 {
1885 }
1886 
1896 {
1897  // Edge
1898  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1899  it != m_mapOfEdgeTrackers.end(); ++it) {
1900  it->second->setOgreVisibilityTest(v);
1901  }
1902 
1903 #ifdef VISP_HAVE_OGRE
1904  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1905  it != m_mapOfEdgeTrackers.end(); ++it) {
1906  it->second->faces.getOgreContext()->setWindowName("Multi Edge MBT Hybrid (" + it->first + ")");
1907  }
1908 #endif
1909 
1910  // KLT
1911  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1912  it != m_mapOfKltTrackers.end(); ++it) {
1913  it->second->setOgreVisibilityTest(v);
1914  }
1915 
1916 #ifdef VISP_HAVE_OGRE
1917  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1918  it != m_mapOfKltTrackers.end(); ++it) {
1919  it->second->faces.getOgreContext()->setWindowName("Multi KLT MBT Hybrid (" + it->first + ")");
1920  }
1921 #endif
1922 
1923  useOgre = v;
1924 }
1925 
1932 {
1935 }
1936 
1945 {
1946  if (m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) {
1947  std::cerr << "This method requires only 1 camera !" << std::endl;
1948  } else {
1949  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1950  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1951  if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
1952  it_edge->second->setPose(I, cMo_);
1953  it_klt->second->setPose(I, cMo_);
1954 
1955  this->cMo = cMo_;
1956  c0Mo = this->cMo;
1957  ctTc0.eye();
1958  } else {
1959  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << " !" << std::endl;
1960  }
1961  }
1962 }
1963 
1976  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
1977  const bool firstCameraIsReference)
1978 {
1979  vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1980  vpMbKltMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1981 }
1982 
1991 void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1992  const vpHomogeneousMatrix &cMo_)
1993 {
1994  vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_);
1995  vpMbKltMultiTracker::setPose(mapOfImages, cMo_);
1996 }
1997 
2008 void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2009  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2010 {
2011  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
2012  vpMbKltMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
2013 }
2014 
2022 {
2023  // Set the general flag for the current class
2025 
2026  // Set the flag for each camera
2027  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2028  it != m_mapOfEdgeTrackers.end(); ++it) {
2029  it->second->setProjectionErrorComputation(flag);
2030  }
2031 }
2032 
2038 void vpMbEdgeKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
2039 {
2040  vpMbEdgeMultiTracker::setReferenceCameraName(referenceCameraName);
2041  vpMbKltMultiTracker::setReferenceCameraName(referenceCameraName);
2042  m_referenceCameraName = referenceCameraName;
2043 }
2044 
2051 {
2054 }
2055 
2062 {
2064 }
2065 
2067 {
2068  std::cerr << "The method vpMbEdgeKltMultiTracker::testTracking is not used !" << std::endl;
2069 }
2070 
2079 {
2080  // Track only with reference camera
2081  // Get the reference camera parameters
2082  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_mbt = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2083  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
2084 
2085  if (it_mbt != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
2086  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2087  mapOfImages[m_referenceCameraName] = &I;
2088  track(mapOfImages);
2089  } else {
2090  std::stringstream ss;
2091  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2092  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2093  }
2094 
2095  // Set the projection error from the single camera
2096  if (computeProjError) {
2097  projectionError = it_mbt->second->getProjectionError();
2098  }
2099 }
2100 
2110 {
2111  if (m_mapOfEdgeTrackers.size() == 2 && m_mapOfKltTrackers.size() == 2) {
2112  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2113  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2114  // Assume that the first image is the first name in alphabetic order
2115  mapOfImages[it->first] = &I1;
2116  ++it;
2117 
2118  mapOfImages[it->first] = &I2;
2119  track(mapOfImages);
2120  } else {
2121  std::stringstream ss;
2122  ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
2123  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2124  }
2125 }
2126 
2134 void vpMbEdgeKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
2135 {
2136  // Reset the projectionError
2137  projectionError = 90.0;
2138 
2139  // Check if there is an image for each camera
2140  // mbt
2141  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
2142  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2143  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_edge->first);
2144 
2145  if (it_img == mapOfImages.end()) {
2146  throw vpException(vpTrackingException::fatalError, "Missing images for edge trackers !");
2147  }
2148  }
2149 
2150  // klt
2151  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
2152  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2153  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
2154 
2155  if (it_img == mapOfImages.end()) {
2156  throw vpException(vpTrackingException::fatalError, "Missing images for KLT trackers !");
2157  }
2158  }
2159 
2160  try {
2161  vpMbKltMultiTracker::preTracking(mapOfImages);
2162  } catch (...) {
2163  }
2164 
2165  // MBT: track moving edges
2166  trackMovingEdges(mapOfImages);
2167 
2168  computeVVS(mapOfImages);
2169 
2170  postTracking(mapOfImages, 0);
2171 
2172  if (computeProjError) {
2174  }
2175 }
2176 
2177 void vpMbEdgeKltMultiTracker::trackMovingEdges(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
2178 {
2179  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2180  it != m_mapOfEdgeTrackers.end(); ++it) {
2181  vpMbEdgeTracker *edge = it->second;
2182  // Track moving edges
2183  try {
2184  edge->trackMovingEdge(*mapOfImages[it->first]);
2185  } catch (...) {
2186  std::cerr << "Error in moving edge tracking" << std::endl;
2187  throw;
2188  }
2189  }
2190 }
2191 
2192 #elif !defined(VISP_BUILD_SHARED_LIBS)
2193 // Work arround to avoid warning:
2194 // libvisp_mbt.a(dummy_vpMbEdgeKltMultiTracker.cpp.o) has no symbols
2195 void dummy_vpMbEdgeKltMultiTracker(){};
2196 #endif // VISP_HAVE_OPENCV
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:223
bool m_computeInteraction
Definition: vpMbTracker.h:187
bool computeProjError
Definition: vpMbTracker.h:135
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void initClick(const vpImage< unsigned char > &I, const std::vector< vpPoint > &points3D_list, const std::string &displayFile="")
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
virtual void loadModel(const std::string &modelFile, const bool verbose=false)
void displayPrimitive(const vpImage< unsigned char > &_I)
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
virtual void setFarClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual void setNearClippingDistance(const double &dist)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void setLod(const bool useLod, const std::string &name="")
double thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setOgreVisibilityTest(const bool &v)
vpColVector m_error_edge
(s - s*)
virtual std::map< std::string, unsigned int > getEdgeMultiNbPolygon() const
vpColVector m_weightedError_edge
Weighted error.
virtual void setCovarianceComputation(const bool &flag)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void stack(const double &d)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:145
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void computeProjectionError()
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setAngleDisappear(const double &a)
std::map< std::string, vpHomogeneousMatrix > m_mapOfCameraTransformationMatrix
virtual void setDisplayFeatures(const bool displayF)
virtual void loadConfigFile(const std::string &configFile)
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void trackMovingEdges(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages)
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:171
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
virtual void setLod(const bool useLod, const std::string &name="")
vpColVector m_factor
Edge VVS variables.
virtual void setFarClippingDistance(const double &dist)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:115
double m_factorMBT
Factor for edge trackers.
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void setDisplayFeatures(const bool displayF)
virtual void loadConfigFile(const std::string &configFile)
virtual void setDisplayFeatures(const bool displayF)
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
unsigned int m_nbrow
Number of features.
bool modelInitialised
Definition: vpMbTracker.h:125
vpColVector m_w_edge
Robust weights.
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
vpColVector m_wCircles
error that can be emited by ViSP classes.
Definition: vpException.h:71
virtual void setThresholdAcceptation(const double th)
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
virtual void setCovarianceComputation(const bool &flag)
virtual void setClipping(const unsigned int &flags)
unsigned int getRows() const
Definition: vpArray2D.h:156
vpHomogeneousMatrix inverse() const
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
virtual void init(const vpImage< unsigned char > &I)
virtual void setAngleAppear(const double &a)
virtual void setAngleDisappear(const double &a)
Make the complete tracking of an object by using its CAD model.
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:157
virtual void setLod(const bool useLod, const std::string &name="")
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
void displayFeaturesOnImage(const vpImage< unsigned char > &I, const unsigned int lvl)
virtual void loadConfigFile(const std::string &configFile)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:130
void updateMovingEdge(const vpImage< unsigned char > &I)
vpMatrix m_L_hybridMulti
Interaction matrix.
virtual void loadModel(const std::string &modelFile, const bool verbose=false)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual std::map< std::string, unsigned int > getMultiNbPolygon() const
virtual void setNearClippingDistance(const double &dist)
vpColVector m_weightedError_hybridMulti
Weighted error.
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void computeVVSInteractionMatrixAndResidu()
Class that defines what is a point.
Definition: vpPoint.h:58
double thresholdKLT
The threshold used in the robust estimation of KLT.
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:113
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
vpHomogeneousMatrix ctTc0
virtual void setReferenceCameraName(const std::string &referenceCameraName)
double projectionError
Definition: vpMbTracker.h:138
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void postTracking(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages, const unsigned int lvl)
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:390
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpRobust m_robust_klt
Robust.
virtual void track(const vpImage< unsigned char > &I)
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4512
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:117
Error that can be emited by the vpTracker class and its derivates.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void setClipping(const unsigned int &flags)
void displayPrimitive(const vpImage< unsigned char > &_I)
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:160
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
Definition: vpMath.h:108
vpColVector m_error_hybridMulti
(s - s*)
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
Generic class defining intrinsic camera parameters.
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void preTracking(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages)
virtual unsigned int initMbtTracking(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages, unsigned int lvl)
virtual void computeVVSInteractionMatrixAndResidu()
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:195
virtual std::map< std::string, unsigned int > getKltMultiNbPolygon() const
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void setAngleAppear(const double &a)
Model based tracker using only KLT.
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)
std::map< std::string, vpMbEdgeTracker * > m_mapOfEdgeTrackers
Map of Model-based edge trackers.
vpColVector m_errorCylinders
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
virtual void setCovarianceComputation(const bool &flag)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpColVector m_errorCircles
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:191
vpColVector m_wCylinders
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRobust m_robustLines
virtual void postTracking(std::map< std::string, const vpImage< unsigned char > *> &mapOfImages)
virtual void setThresholdAcceptation(const double th)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void computeProjectionError(const vpImage< unsigned char > &_I)
virtual void getCameraParameters(vpCameraParameters &camera) const
void insert(unsigned int i, const vpColVector &v)
vpRobust m_robustCylinders
virtual void setNearClippingDistance(const double &dist)
std::map< std::string, vpMbHiddenFaces< vpMbtPolygon > > getEdgeFaces() const
vpRobust m_robustCircles
virtual void setCameraParameters(const vpCameraParameters &camera)
vpColVector m_w_klt
Robust weights.
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:140
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
std::map< std::string, vpMbKltTracker * > m_mapOfKltTrackers
Map of Model-based klt trackers.
virtual std::map< std::string, unsigned int > getMultiNbPolygon() const
virtual void loadModel(const std::string &modelFile, const bool verbose=false)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
virtual void setCameraParameters(const vpCameraParameters &camera)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void computeVVSInit()
std::string m_referenceCameraName
Name of the reference camera.
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
virtual void computeVVSInit()
virtual void setScanLineVisibilityTest(const bool &v)
virtual std::vector< std::string > getCameraNames() const
vpColVector m_wLines
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setAngleDisappear(const double &a)
virtual void initClick(const vpImage< unsigned char > &I, const std::vector< vpPoint > &points3D_list, const std::string &displayFile="")
unsigned int m_nbInfos
virtual void setClipping(const unsigned int &flags)
vpMatrix m_L_edge
Interaction matrix.
void trackMovingEdge(const vpImage< unsigned char > &I)
double m_factorKLT
Factor for KLT trackers.
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
vpColVector m_error_klt
(s - s*)
std::map< std::string, vpMbHiddenFaces< vpMbtPolygon > > getKltFaces() const
vpColVector m_errorLines
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual unsigned int getNbPolygon() const
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
Make the complete stereo (or more) tracking of an object by using its CAD model.
virtual void setProjectionErrorComputation(const bool &flag)
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpColVector m_w_hybridMulti
Robust weights.
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:119
Model based stereo (or more) tracker using only KLT.
vpHomogeneousMatrix c0Mo
Initial pose.
virtual std::vector< std::string > getCameraNames() const
virtual void setAngleAppear(const double &a)
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:241
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:531
vpMatrix m_L_klt
Interaction matrix.