Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpMbEdgeKltMultiTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Model-based edge 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/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 
887 void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
888  const bool displayHelp, const vpHomogeneousMatrix &T)
889 {
890  // Cannot use directly set pose for KLT as it is different than for the edge
891  // case It moves the KLT points instead of detecting new KLT points
892  vpMbKltMultiTracker::initClick(I, initFile, displayHelp, T);
893 
894  // Set pose for Edge (setPose or initFromPose is equivalent with
895  // vpMbEdgeTracker) And it avoids to click a second time
897 }
898 
935  const std::string &initFile1, const std::string &initFile2,
936  const bool displayHelp, const bool firstCameraIsReference)
937 {
938  vpMbKltMultiTracker::initClick(I1, I2, initFile1, initFile2, displayHelp, firstCameraIsReference);
939 
940  // Get c2Mo
941  vpHomogeneousMatrix c2Mo;
942  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
943  if (firstCameraIsReference) {
944  ++it_klt;
945  it_klt->second->getPose(c2Mo);
946  } else {
947  it_klt->second->getPose(c2Mo);
948  }
949 
950  // Set pose for Edge (setPose or initFromPose is equivalent with
951  // vpMbEdgeTracker)
952  vpMbEdgeMultiTracker::setPose(I1, I2, cMo, c2Mo, firstCameraIsReference);
953 }
954 
983 void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
984  const std::string &initFile, const bool displayHelp)
985 {
986  vpMbKltMultiTracker::initClick(mapOfImages, initFile, displayHelp);
987 
988  // Get pose for all the cameras
989  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
990  vpMbKltMultiTracker::getPose(mapOfCameraPoses);
991 
992  // Set pose for Edge for all the cameras (setPose or initFromPose is
993  // equivalent with vpMbEdgeTracker) And it avoids to click a second time
994  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
995 }
996 
1025 void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1026  const std::map<std::string, std::string> &mapOfInitFiles,
1027  const bool displayHelp)
1028 {
1029  vpMbKltMultiTracker::initClick(mapOfImages, mapOfInitFiles, displayHelp);
1030 
1031  // Get pose for all the cameras
1032  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
1033  vpMbKltMultiTracker::getPose(mapOfCameraPoses);
1034 
1035  // Set pose for Edge for all the cameras (setPose or initFromPose is
1036  // equivalent with vpMbEdgeTracker) And it avoids to click a second time
1037  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
1038 }
1039 #endif //#ifdef VISP_HAVE_MODULE_GUI
1040 
1041 void vpMbEdgeKltMultiTracker::initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int,
1042  const std::string &)
1043 {
1044  std::cerr << "The method initCircle is not used in vpMbEdgeKltMultiTracker !" << std::endl;
1045 }
1046 
1047 void vpMbEdgeKltMultiTracker::initCylinder(const vpPoint &, const vpPoint &, const double, const int,
1048  const std::string &)
1049 {
1050  std::cerr << "The method initCylinder is not used in vpMbEdgeKltMultiTracker !" << std::endl;
1051 }
1052 
1054 {
1055  std::cerr << "The method initFaceFromCorners is not used in "
1056  "vpMbEdgeKltMultiTracker !"
1057  << std::endl;
1058 }
1059 
1061 {
1062  std::cerr << "The method initFaceFromLines is not used in "
1063  "vpMbEdgeKltMultiTracker !"
1064  << std::endl;
1065 }
1066 
1085 void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1086 {
1087  // Monocular case only !
1088  if (m_mapOfKltTrackers.size() > 1) {
1089  throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !");
1090  }
1091 
1092  char s[FILENAME_MAX];
1093  std::fstream finit;
1094  vpPoseVector init_pos;
1095 
1096  std::string ext = ".pos";
1097  size_t pos = initFile.rfind(ext);
1098 
1099  if (pos == initFile.size() - ext.size() && pos != 0)
1100  sprintf(s, "%s", initFile.c_str());
1101  else
1102  sprintf(s, "%s.pos", initFile.c_str());
1103 
1104  finit.open(s, std::ios::in);
1105  if (finit.fail()) {
1106  std::cerr << "cannot read " << s << std::endl;
1107  throw vpException(vpException::ioError, "cannot read init file");
1108  }
1109 
1110  for (unsigned int i = 0; i < 6; i += 1) {
1111  finit >> init_pos[i];
1112  }
1113 
1114  cMo.buildFrom(init_pos);
1117 }
1118 
1126 {
1127  // Monocular case only !
1128  if (m_mapOfKltTrackers.size() > 1) {
1129  throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !");
1130  }
1131 
1132  this->cMo = cMo_;
1135 }
1136 
1144 {
1145  vpHomogeneousMatrix _cMo(cPo);
1146  initFromPose(I, _cMo);
1147 }
1148 
1160  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
1161  const bool firstCameraIsReference)
1162 {
1163  vpMbEdgeMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1164  vpMbKltMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
1165 }
1166 
1174 void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1175  const vpHomogeneousMatrix &cMo_)
1176 {
1177  vpMbEdgeMultiTracker::initFromPose(mapOfImages, cMo_);
1178  vpMbKltMultiTracker::initFromPose(mapOfImages, cMo_);
1179 }
1180 
1187 void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1188  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
1189 {
1190  vpMbEdgeMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
1191  vpMbKltMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
1192 }
1193 
1194 unsigned int
1195 vpMbEdgeKltMultiTracker::initMbtTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1196  unsigned int lvl)
1197 {
1198  vpMbEdgeTracker *edge = NULL;
1199  unsigned int nbrows = 0;
1200 
1201  m_factor.resize(0, false);
1202  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1203  it != m_mapOfEdgeTrackers.end(); ++it) {
1204  edge = it->second;
1205 
1206  try {
1207  edge->computeVVSInit();
1208 
1209  unsigned int nbrow = edge->m_error_edge.getRows();
1210  nbrows += nbrow;
1211 
1212  // Set the corresponding cMo for each camera
1213  // Used in computeVVSFirstPhaseFactor with computeInteractionMatrixError
1214  edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
1215 
1216  edge->computeVVSFirstPhaseFactor(*mapOfImages[it->first], lvl);
1217  m_factor.stack(edge->m_factor);
1218  } catch (...) {
1219  edge->m_L_edge.resize(0, 6, false, false);
1220  edge->m_error_edge.resize(0, false);
1221 
1222  edge->m_weightedError_edge.resize(0, false);
1223  edge->m_w_edge.resize(0, false);
1224  edge->m_factor.resize(0, false);
1225 
1226  edge->m_robustLines.resize(0);
1227  edge->m_robustCylinders.resize(0);
1228  edge->m_robustCircles.resize(0);
1229 
1230  edge->m_wLines.resize(0, false);
1231  edge->m_wCylinders.resize(0, false);
1232  edge->m_wCircles.resize(0, false);
1233 
1234  edge->m_errorLines.resize(0, false);
1235  edge->m_errorCylinders.resize(0, false);
1236  edge->m_errorCircles.resize(0, false);
1237  }
1238  }
1239 
1240  return nbrows;
1241 }
1242 
1285 void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile)
1286 {
1289 }
1290 
1304 void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
1305  const bool firstCameraIsReference)
1306 {
1307  vpMbEdgeMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference);
1308  vpMbKltMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference);
1309 }
1310 
1321 void vpMbEdgeKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
1322 {
1323  vpMbEdgeMultiTracker::loadConfigFile(mapOfConfigFiles);
1324  vpMbKltMultiTracker::loadConfigFile(mapOfConfigFiles);
1325 }
1326 
1354 void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, const bool verbose,
1355  const vpHomogeneousMatrix &T)
1356 {
1357  vpMbEdgeMultiTracker::loadModel(modelFile, verbose, T);
1358  vpMbKltMultiTracker::loadModel(modelFile, verbose, T);
1359 
1360  modelInitialised = true;
1361 }
1362 
1363 void vpMbEdgeKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
1364 {
1365  // MBT
1366  vpMbEdgeTracker *edge = NULL;
1367  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1368  it != m_mapOfEdgeTrackers.end(); ++it) {
1369  edge = it->second;
1370 
1371  edge->updateMovingEdgeWeights();
1372 
1373  if (displayFeatures) {
1375  }
1376  }
1377 
1378  // KLT
1379  vpMbKltMultiTracker::postTracking(mapOfImages);
1380 
1381  // Looking for new visible face
1382  bool newvisibleface = false;
1383  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1384  it != m_mapOfEdgeTrackers.end(); ++it) {
1385  edge = it->second;
1386  edge->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
1387  }
1388 
1389  if (useScanLine) {
1390  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1391  it != m_mapOfEdgeTrackers.end(); ++it) {
1392  edge = it->second;
1393 
1394  edge->faces.computeClippedPolygons(it->second->cMo, it->second->cam);
1395  edge->faces.computeScanLineRender(it->second->cam, mapOfImages[it->first]->getWidth(),
1396  mapOfImages[it->first]->getHeight());
1397  }
1398  }
1399 
1400  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1401  it != m_mapOfEdgeTrackers.end(); ++it) {
1402  edge = it->second;
1403  edge->updateMovingEdge(*mapOfImages[it->first]);
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->initMovingEdge(*mapOfImages[it->first], it->second->cMo);
1410 
1411  // Reinit the moving edge for the lines which need it.
1412  edge->reinitMovingEdge(*mapOfImages[it->first], it->second->cMo);
1413 
1414  if (computeProjError) {
1415  edge->computeProjectionError(*mapOfImages[it->first]);
1416  }
1417  }
1418 }
1419 
1420 void vpMbEdgeKltMultiTracker::reinit(/*const vpImage<unsigned char>& I */)
1421 {
1422  // vpMbEdgeMultiTracker::reinit();
1424 }
1425 
1438 void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1439  const vpHomogeneousMatrix &cMo_, const bool verbose,
1440  const vpHomogeneousMatrix &T)
1441 {
1442  vpMbEdgeMultiTracker::reInitModel(I, cad_name, cMo_, verbose, T);
1443  vpMbKltMultiTracker::reInitModel(I, cad_name, cMo_, verbose, T);
1444 }
1445 
1460  const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
1461  const vpHomogeneousMatrix &c2Mo, const bool verbose,
1462  const bool firstCameraIsReference)
1463 {
1464  vpMbEdgeMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference);
1465  vpMbKltMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference);
1466 }
1467 
1478 void vpMbEdgeKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
1479  const std::string &cad_name,
1480  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
1481  const bool verbose)
1482 {
1483  vpMbEdgeMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
1484  vpMbKltMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
1485 }
1486 
1492 {
1495 }
1496 
1507 {
1510 }
1511 
1522 {
1525 }
1526 
1533 {
1536 
1537  this->cam = camera;
1538 }
1539 
1549  const bool firstCameraIsReference)
1550 {
1551  vpMbEdgeMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference);
1552  vpMbKltMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference);
1553 
1554  if (firstCameraIsReference) {
1555  this->cam = camera1;
1556  } else {
1557  this->cam = camera2;
1558  }
1559 }
1560 
1567 void vpMbEdgeKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera)
1568 {
1569  vpMbEdgeMultiTracker::setCameraParameters(cameraName, camera);
1570  vpMbKltMultiTracker::setCameraParameters(cameraName, camera);
1571 
1572  if (cameraName == m_referenceCameraName) {
1573  this->cam = camera;
1574  }
1575 }
1576 
1583  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
1584 {
1585  vpMbEdgeMultiTracker::setCameraParameters(mapOfCameraParameters);
1586  vpMbKltMultiTracker::setCameraParameters(mapOfCameraParameters);
1587 
1588  for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
1589  it != mapOfCameraParameters.end(); ++it) {
1590  if (it->first == m_referenceCameraName) {
1591  this->cam = it->second;
1592  }
1593  }
1594 }
1595 
1605  const vpHomogeneousMatrix &cameraTransformationMatrix)
1606 {
1607  vpMbEdgeMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
1608  vpMbKltMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
1609 
1610  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
1611  if (it != m_mapOfCameraTransformationMatrix.end()) {
1612  it->second = cameraTransformationMatrix;
1613  } else {
1614  std::cerr << "Cannot find camera: " << cameraName << " !" << std::endl;
1615  }
1616 }
1617 
1626  const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
1627 {
1628  vpMbEdgeMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
1629  vpMbKltMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
1630 
1631  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
1632 }
1633 
1641 void vpMbEdgeKltMultiTracker::setClipping(const unsigned int &flags)
1642 {
1645 }
1646 
1655 void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
1656 {
1657  // Here, we do not change the general clipping flag
1658  vpMbEdgeMultiTracker::setClipping(cameraName, flags);
1659  vpMbKltMultiTracker::setClipping(cameraName, flags);
1660 }
1661 
1668 {
1671 }
1672 
1689 {
1692 }
1693 
1700 {
1703 }
1704 
1711 void vpMbEdgeKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist)
1712 {
1715 }
1716 
1717 #ifdef VISP_HAVE_OGRE
1718 
1728 {
1731 }
1732 
1743 {
1746 }
1747 #endif
1748 
1760 void vpMbEdgeKltMultiTracker::setLod(const bool useLod, const std::string &name)
1761 {
1762  vpMbEdgeMultiTracker::setLod(useLod, name);
1763  vpMbKltMultiTracker::setLod(useLod, name);
1764 }
1765 
1778 void vpMbEdgeKltMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name)
1779 {
1780  vpMbEdgeMultiTracker::setLod(useLod, cameraName, name);
1781  vpMbKltMultiTracker::setLod(useLod, cameraName, name);
1782 }
1783 
1793 void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
1794 {
1795  vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, name);
1796 }
1797 
1809 void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
1810  const std::string &name)
1811 {
1812  vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, cameraName, name);
1813 }
1814 
1823 void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
1824 {
1825  vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name);
1826  vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name);
1827 }
1828 
1839 void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
1840  const std::string &name)
1841 {
1842  vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
1843  vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
1844 }
1845 
1852 {
1855 }
1856 
1863 void vpMbEdgeKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist)
1864 {
1867 }
1868 
1879 void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
1880 {
1883 }
1884 
1894 {
1895  // Edge
1896  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1897  it != m_mapOfEdgeTrackers.end(); ++it) {
1898  it->second->setOgreVisibilityTest(v);
1899  }
1900 
1901 #ifdef VISP_HAVE_OGRE
1902  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
1903  it != m_mapOfEdgeTrackers.end(); ++it) {
1904  it->second->faces.getOgreContext()->setWindowName("Multi Edge MBT Hybrid (" + it->first + ")");
1905  }
1906 #endif
1907 
1908  // KLT
1909  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1910  it != m_mapOfKltTrackers.end(); ++it) {
1911  it->second->setOgreVisibilityTest(v);
1912  }
1913 
1914 #ifdef VISP_HAVE_OGRE
1915  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
1916  it != m_mapOfKltTrackers.end(); ++it) {
1917  it->second->faces.getOgreContext()->setWindowName("Multi KLT MBT Hybrid (" + it->first + ")");
1918  }
1919 #endif
1920 
1921  useOgre = v;
1922 }
1923 
1930 {
1933 }
1934 
1943 {
1944  if (m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) {
1945  std::cerr << "This method requires only 1 camera !" << std::endl;
1946  } else {
1947  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1948  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1949  if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
1950  it_edge->second->setPose(I, cMo_);
1951  it_klt->second->setPose(I, cMo_);
1952 
1953  this->cMo = cMo_;
1954  c0Mo = this->cMo;
1955  ctTc0.eye();
1956  } else {
1957  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << " !" << std::endl;
1958  }
1959  }
1960 }
1961 
1970 {
1971  if (m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) {
1972  std::cerr << "This method requires only 1 camera !" << std::endl;
1973  } else {
1974  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
1975  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
1976  if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
1977  vpImageConvert::convert(I_color, m_I);
1978  it_edge->second->setPose(m_I, cMo_);
1979  it_klt->second->setPose(m_I, cMo_);
1980 
1981  this->cMo = cMo_;
1982  c0Mo = this->cMo;
1983  ctTc0.eye();
1984  } else {
1985  std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << " !" << std::endl;
1986  }
1987  }
1988 }
1989 
2002  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
2003  const bool firstCameraIsReference)
2004 {
2005  vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
2006  vpMbKltMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
2007 }
2008 
2017 void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2018  const vpHomogeneousMatrix &cMo_)
2019 {
2020  vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_);
2021  vpMbKltMultiTracker::setPose(mapOfImages, cMo_);
2022 }
2023 
2034 void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
2035  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2036 {
2037  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
2038  vpMbKltMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
2039 }
2040 
2048 {
2049  // Set the general flag for the current class
2051 
2052  // Set the flag for each camera
2053  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2054  it != m_mapOfEdgeTrackers.end(); ++it) {
2055  it->second->setProjectionErrorComputation(flag);
2056  }
2057 }
2058 
2064 void vpMbEdgeKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
2065 {
2066  vpMbEdgeMultiTracker::setReferenceCameraName(referenceCameraName);
2067  vpMbKltMultiTracker::setReferenceCameraName(referenceCameraName);
2068  m_referenceCameraName = referenceCameraName;
2069 }
2070 
2077 {
2080 }
2081 
2088 {
2090 }
2091 
2093 {
2094  std::cerr << "The method vpMbEdgeKltMultiTracker::testTracking is not used !" << std::endl;
2095 }
2096 
2105 {
2106  // Track only with reference camera
2107  // Get the reference camera parameters
2108  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_mbt = m_mapOfEdgeTrackers.find(m_referenceCameraName);
2109  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
2110 
2111  if (it_mbt != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
2112  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2113  mapOfImages[m_referenceCameraName] = &I;
2114  track(mapOfImages);
2115  } else {
2116  std::stringstream ss;
2117  ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
2118  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2119  }
2120 
2121  // Set the projection error from the single camera
2122  if (computeProjError) {
2123  projectionError = it_mbt->second->getProjectionError();
2124  }
2125 }
2126 
2131 {
2132  std::cout << "Not supported interface, this class is deprecated." << std::endl;
2133 }
2134 
2144 {
2145  if (m_mapOfEdgeTrackers.size() == 2 && m_mapOfKltTrackers.size() == 2) {
2146  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
2147  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2148  // Assume that the first image is the first name in alphabetic order
2149  mapOfImages[it->first] = &I1;
2150  ++it;
2151 
2152  mapOfImages[it->first] = &I2;
2153  track(mapOfImages);
2154  } else {
2155  std::stringstream ss;
2156  ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
2157  throw vpException(vpTrackingException::fatalError, ss.str().c_str());
2158  }
2159 }
2160 
2168 void vpMbEdgeKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
2169 {
2170  // Reset the projectionError
2171  projectionError = 90.0;
2172 
2173  // Check if there is an image for each camera
2174  // mbt
2175  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
2176  it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
2177  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_edge->first);
2178 
2179  if (it_img == mapOfImages.end()) {
2180  throw vpException(vpTrackingException::fatalError, "Missing images for edge trackers !");
2181  }
2182  }
2183 
2184  // klt
2185  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
2186  it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
2187  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
2188 
2189  if (it_img == mapOfImages.end()) {
2190  throw vpException(vpTrackingException::fatalError, "Missing images for KLT trackers !");
2191  }
2192  }
2193 
2194  try {
2195  vpMbKltMultiTracker::preTracking(mapOfImages);
2196  } catch (...) {
2197  }
2198 
2199  // MBT: track moving edges
2200  trackMovingEdges(mapOfImages);
2201 
2202  computeVVS(mapOfImages);
2203 
2204  postTracking(mapOfImages);
2205 
2206  if (computeProjError) {
2208  }
2209 }
2210 
2211 void vpMbEdgeKltMultiTracker::trackMovingEdges(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
2212 {
2213  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
2214  it != m_mapOfEdgeTrackers.end(); ++it) {
2215  vpMbEdgeTracker *edge = it->second;
2216  // Track moving edges
2217  try {
2218  edge->trackMovingEdge(*mapOfImages[it->first]);
2219  } catch (...) {
2220  std::cerr << "Error in moving edge tracking" << std::endl;
2221  throw;
2222  }
2223  }
2224 }
2225 
2226 #elif !defined(VISP_BUILD_SHARED_LIBS)
2227 // Work arround to avoid warning:
2228 // libvisp_mbt.a(dummy_vpMbEdgeKltMultiTracker.cpp.o) has no symbols
2229 void dummy_vpMbEdgeKltMultiTracker(){}
2230 #endif // VISP_HAVE_OPENCV
2231 #elif !defined(VISP_BUILD_SHARED_LIBS)
2232 // Work arround to avoid warning:
2233 // libvisp_mbt.a(dummy_vpMbEdgeKltMultiTracker.cpp.o) has no symbols
2234 void dummy_vpMbEdgeKltMultiTracker(){}
2235 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
bool m_computeInteraction
Definition: vpMbTracker.h:185
bool computeProjError
Definition: vpMbTracker.h:133
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void trackMovingEdges(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
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:164
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 std::vector< std::vector< double > > getFeaturesForDisplayEdge()
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 void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpColVector m_weightedError_edge
Weighted error.
virtual void setCovarianceComputation(const bool &flag)
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:252
std::map< std::string, vpMbHiddenFaces< vpMbtPolygon > > getEdgeFaces() const
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual void 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 initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
virtual std::map< std::string, unsigned int > getMultiNbPolygon() const
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
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:113
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:123
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)
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.
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
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="")
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:128
void updateMovingEdge(const vpImage< unsigned char > &I)
vpMatrix m_L_hybridMulti
Interaction matrix.
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setReferenceCameraName(const std::string &referenceCameraName)
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.
virtual void postTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:111
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
virtual std::map< std::string, unsigned int > getEdgeMultiNbPolygon() const
vpHomogeneousMatrix ctTc0
virtual void setReferenceCameraName(const std::string &referenceCameraName)
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
double projectionError
Definition: vpMbTracker.h:136
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
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:4660
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
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()
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void setClipping(const unsigned int &flags)
virtual vpHomogeneousMatrix getPose() const
Definition: vpMbTracker.h:419
void displayPrimitive(const vpImage< unsigned char > &_I)
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
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 unsigned int initMbtTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages, unsigned int lvl)
static double sqr(double x)
Definition: vpMath.h:114
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)
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 computeVVSInteractionMatrixAndResidu()
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 setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void setAngleAppear(const double &a)
Model based tracker using only KLT.
unsigned int getRows() const
Definition: vpArray2D.h:289
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 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:189
vpColVector m_wCylinders
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRobust m_robustLines
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)
void insert(unsigned int i, const vpColVector &v)
vpRobust m_robustCylinders
virtual void setNearClippingDistance(const double &dist)
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:138
virtual std::map< std::string, unsigned int > getKltMultiNbPolygon() const
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
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:130
std::map< std::string, vpMbKltTracker * > m_mapOfKltTrackers
Map of Model-based klt trackers.
void stack(double d)
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:151
vpHomogeneousMatrix inverse() const
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
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)
virtual std::vector< std::string > getCameraNames() const
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
virtual void computeVVSInit()
virtual std::vector< std::string > getCameraNames() const
virtual void postTracking(std::map< std::string, const vpImage< unsigned char > * > &mapOfImages)
virtual void setScanLineVisibilityTest(const bool &v)
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*)
vpColVector m_errorLines
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
virtual void 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 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:117
Model based stereo (or more) tracker using only KLT.
vpHomogeneousMatrix c0Mo
Initial pose.
virtual std::map< std::string, unsigned int > getMultiNbPolygon() const
std::map< std::string, vpMbHiddenFaces< vpMbtPolygon > > getKltFaces() const
virtual void setAngleAppear(const double &a)
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual unsigned int getNbPolygon() const
virtual void setScanLineVisibilityTest(const bool &v)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:580
vpMatrix m_L_klt
Interaction matrix.