Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMbTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Generic model based tracker
33  *
34 *****************************************************************************/
35 
41 #include <algorithm>
42 #include <iostream>
43 #include <limits>
44 #include <sstream>
45 
46 #include <visp3/core/vpConfig.h>
47 #if defined(VISP_HAVE_SIMDLIB)
48 #include <Simd/SimdLib.h>
49 #endif
50 
51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDisplay.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpMatrix.h>
55 #include <visp3/core/vpPoint.h>
56 #include <visp3/vision/vpPose.h>
57 #ifdef VISP_HAVE_MODULE_GUI
58 #include <visp3/gui/vpDisplayGDI.h>
59 #include <visp3/gui/vpDisplayOpenCV.h>
60 #include <visp3/gui/vpDisplayX.h>
61 #endif
62 #include <visp3/core/vpCameraParameters.h>
63 #include <visp3/core/vpColor.h>
64 #include <visp3/core/vpException.h>
65 #include <visp3/core/vpIoTools.h>
66 #include <visp3/core/vpPixelMeterConversion.h>
67 #ifdef VISP_HAVE_MODULE_IO
68 #include <visp3/io/vpImageIo.h>
69 #endif
70 #include <visp3/core/vpCPUFeatures.h>
71 #include <visp3/core/vpIoTools.h>
72 #include <visp3/core/vpMatrixException.h>
73 #include <visp3/core/vpTrackingException.h>
74 #include <visp3/mbt/vpMbTracker.h>
75 
76 #include <visp3/core/vpImageFilter.h>
77 #include <visp3/mbt/vpMbtXmlGenericParser.h>
78 
79 #ifdef VISP_HAVE_COIN3D
80 // Inventor includes
81 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
82 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
83 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
84 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
85 #include <Inventor/VRMLnodes/SoVRMLShape.h>
86 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
87 #include <Inventor/actions/SoGetMatrixAction.h>
88 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
89 #include <Inventor/actions/SoSearchAction.h>
90 #include <Inventor/actions/SoToVRML2Action.h>
91 #include <Inventor/actions/SoWriteAction.h>
92 #include <Inventor/misc/SoChildList.h>
93 #include <Inventor/nodes/SoSeparator.h>
94 #endif
95 
96 #if defined(VISP_HAVE_THREADS)
97 #include <mutex>
98 #endif
99 
100 #ifndef DOXYGEN_SHOULD_SKIP_THIS
101 
102 namespace
103 {
104 #if defined(VISP_HAVE_THREADS)
105 std::mutex g_mutex_cout;
106 #endif
110 struct SegmentInfo
111 {
112  SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) { }
113 
114  std::vector<vpPoint> extremities;
115  std::string name;
116  bool useLod;
117  double minLineLengthThresh;
118 };
119 
124 struct PolygonFaceInfo
125 {
126  PolygonFaceInfo(double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
127  : distanceToCamera(dist), polygon(poly), faceCorners(corners)
128  { }
129 
130  bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }
131 
132  double distanceToCamera;
133  vpPolygon polygon;
134  std::vector<vpPoint> faceCorners;
135 };
136 
144 std::istream &safeGetline(std::istream &is, std::string &t)
145 {
146  t.clear();
147 
148  // The characters in the stream are read one-by-one using a std::streambuf.
149  // That is faster than reading them one-by-one using the std::istream.
150  // Code that uses streambuf this way must be guarded by a sentry object.
151  // The sentry object performs various tasks,
152  // such as thread synchronization and updating the stream state.
153 
154  std::istream::sentry se(is, true);
155  std::streambuf *sb = is.rdbuf();
156 
157  for (;;) {
158  int c = sb->sbumpc();
159  if (c == '\n') {
160  return is;
161  }
162  else if (c == '\r') {
163  if (sb->sgetc() == '\n')
164  sb->sbumpc();
165  return is;
166  }
167  else if (c == std::streambuf::traits_type::eof()) {
168  // Also handle the case when the last line has no line ending
169  if (t.empty())
170  is.setstate(std::ios::eofbit);
171  return is;
172  }
173  else { // default case
174  t += (char)c;
175  }
176  }
177 }
178 } // namespace
179 #endif // DOXYGEN_SHOULD_SKIP_THIS
180 
187  : m_cam(), m_cMo(), oJo(6, 6), m_isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
188  computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
189  displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
190  angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
191  useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
192  nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
193  minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
194  m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
195  m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
196  m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
197  m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
198  m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(nullptr), m_I(), m_sodb_init_called(false),
199  m_rand()
200 {
201  oJo.eye();
202  // Map used to parse additional information in CAO model files,
203  // like name of faces or LOD setting
204  mapOfParameterNames["name"] = "string";
205  mapOfParameterNames["minPolygonAreaThreshold"] = "number";
206  mapOfParameterNames["minLineLengthThreshold"] = "number";
207  mapOfParameterNames["useLod"] = "boolean";
208 
211 }
212 
214 {
215  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
216  it != m_projectionErrorLines.end(); ++it) {
217  vpMbtDistanceLine *l = *it;
218  if (l != nullptr)
219  delete l;
220  l = nullptr;
221  }
222 
223  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
224  it != m_projectionErrorCylinders.end(); ++it) {
225  vpMbtDistanceCylinder *cy = *it;
226  if (cy != nullptr)
227  delete cy;
228  cy = nullptr;
229  }
230 
231  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
232  it != m_projectionErrorCircles.end(); ++it) {
233  vpMbtDistanceCircle *ci = *it;
234  if (ci != nullptr)
235  delete ci;
236  ci = nullptr;
237  }
238 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
239  if (m_sodb_init_called) {
240  // Cleanup memory allocated by Coin library used to load a vrml model
241  SoDB::finish();
242  }
243 #endif
244 }
245 
246 #ifdef VISP_HAVE_MODULE_GUI
247 void vpMbTracker::initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
248  const std::string &initFile, bool displayHelp, const vpHomogeneousMatrix &T)
249 {
250  vpHomogeneousMatrix last_cMo;
251  vpPoseVector init_pos;
252  vpImagePoint ip;
254 
255  std::string ext = ".init";
256  std::string str_pose = "";
257  size_t pos = initFile.rfind(ext);
258 
259  // Load the last poses from files
260  std::fstream finitpos;
261  std::ifstream finit;
262  std::stringstream ss;
263  if (poseSavingFilename.empty()) {
264  if (pos != std::string::npos)
265  str_pose = initFile.substr(0, pos) + ".0.pos";
266  else
267  str_pose = initFile + ".0.pos";
268 
269  finitpos.open(str_pose.c_str(), std::ios::in);
270  ss << str_pose;
271  }
272  else {
273  finitpos.open(poseSavingFilename.c_str(), std::ios::in);
274  ss << poseSavingFilename;
275  }
276  if (finitpos.fail()) {
277  std::cout << "Cannot read " << ss.str() << std::endl << "cMo set to identity" << std::endl;
278  last_cMo.eye();
279  }
280  else {
281  for (unsigned int i = 0; i < 6; i += 1) {
282  finitpos >> init_pos[i];
283  }
284 
285  finitpos.close();
286  last_cMo.buildFrom(init_pos);
287 
288  std::cout << "Tracker initial pose read from " << ss.str() << ": " << std::endl << last_cMo << std::endl;
289 
290  if (I) {
291  vpDisplay::display(*I);
292  display(*I, last_cMo, m_cam, vpColor::green, 1, true);
293  vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green);
294  vpDisplay::flush(*I);
295  }
296  else {
297  vpDisplay::display(*I_color);
298  display(*I_color, last_cMo, m_cam, vpColor::green, 1, true);
299  vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green);
300  vpDisplay::flush(*I_color);
301  }
302 
303  std::cout << "No modification : left click " << std::endl;
304  std::cout << "Modify initial pose : right click " << std::endl;
305 
306  if (I) {
307  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
308 
309  vpDisplay::flush(*I);
310 
311  while (!vpDisplay::getClick(*I, ip, button)) {
312  }
313  }
314  else {
315  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose",
316  vpColor::red);
317 
318  vpDisplay::flush(*I_color);
319 
320  while (!vpDisplay::getClick(*I_color, ip, button)) {
321  }
322  }
323  }
324 
325  if (!finitpos.fail() && button == vpMouseButton::button1) {
326  m_cMo = last_cMo;
327  }
328  else {
329  vpDisplay *d_help = nullptr;
330 
331  if (I) {
332  vpDisplay::display(*I);
333  vpDisplay::flush(*I);
334  }
335  else {
336  vpDisplay::display(*I_color);
337  vpDisplay::flush(*I_color);
338  }
339 
340  vpPose pose;
341 
342  pose.clearPoint();
343 
344  // Clear string stream that previously contained the path to the "object.0.pos" file.
345  ss.str(std::string());
346 
347  // file parser
348  // number of points
349  // X Y Z
350  // X Y Z
351  if (pos != std::string::npos) {
352  ss << initFile;
353  }
354  else {
355  ss << initFile;
356  ss << ".init";
357  }
358 
359  std::cout << "Load 3D points from: " << ss.str() << std::endl;
360 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
361  finit.open(ss.str());
362 #else
363  finit.open(ss.str().c_str());
364 #endif
365  if (finit.fail()) {
366  std::cout << "Cannot read " << ss.str() << std::endl;
367  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str());
368  }
369 
370 #ifdef VISP_HAVE_MODULE_IO
371  // Display window creation and initialisation
372  try {
373  if (displayHelp) {
374  const std::string imgExtVec[] = { ".ppm", ".pgm", ".jpg", ".jpeg", ".png" };
375  std::string dispF;
376  bool foundHelpImg = false;
377  if (pos != std::string::npos) {
378  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
379  dispF = initFile.substr(0, pos) + imgExtVec[i];
380  foundHelpImg = vpIoTools::checkFilename(dispF);
381  }
382  }
383  else {
384  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
385  dispF = initFile + imgExtVec[i];
386  foundHelpImg = vpIoTools::checkFilename(dispF);
387  }
388  }
389 
390  if (foundHelpImg) {
391  std::cout << "Load image to help initialization: " << dispF << std::endl;
392 #if defined(VISP_HAVE_X11)
393  d_help = new vpDisplayX;
394 #elif defined(VISP_HAVE_GDI)
395  d_help = new vpDisplayGDI;
396 #elif defined(HAVE_OPENCV_HIGHGUI)
397  d_help = new vpDisplayOpenCV;
398 #endif
399 
400  vpImage<vpRGBa> Iref;
401  vpImageIo::read(Iref, dispF);
402 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
403  const int winXPos = I != nullptr ? I->display->getWindowXPosition() : I_color->display->getWindowXPosition();
404  const int winYPos = I != nullptr ? I->display->getWindowYPosition() : I_color->display->getWindowYPosition();
405  unsigned int width = I != nullptr ? I->getWidth() : I_color->getWidth();
406  d_help->init(Iref, winXPos + (int)width + 80, winYPos, "Where to initialize...");
407  vpDisplay::display(Iref);
408  vpDisplay::flush(Iref);
409 #endif
410  }
411  }
412  }
413  catch (...) {
414  if (d_help != nullptr) {
415  delete d_help;
416  d_help = nullptr;
417  }
418  }
419 #else //#ifdef VISP_HAVE_MODULE_IO
420  (void)(displayHelp);
421 #endif //#ifdef VISP_HAVE_MODULE_IO
422  // skip lines starting with # as comment
423  removeComment(finit);
424 
425  unsigned int n3d;
426  finit >> n3d;
427  finit.ignore(256, '\n'); // skip the rest of the line
428  std::cout << "Number of 3D points " << n3d << std::endl;
429  if (n3d > 100000) {
430  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed",
431  ss.str().c_str());
432  }
433 
434  std::vector<vpPoint> P(n3d);
435  for (unsigned int i = 0; i < n3d; i++) {
436  // skip lines starting with # as comment
438 
439  vpColVector pt_3d(4, 1.0);
440  finit >> pt_3d[0];
441  finit >> pt_3d[1];
442  finit >> pt_3d[2];
443  finit.ignore(256, '\n'); // skip the rest of the line
444 
445  vpColVector pt_3d_tf = T * pt_3d;
446  std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " "
447  << pt_3d_tf[2] << std::endl;
448 
449  P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
450  }
451 
452  finit.close();
453 
454  bool isWellInit = false;
455  while (!isWellInit) {
456  std::vector<vpImagePoint> mem_ip;
457  for (unsigned int i = 0; i < n3d; i++) {
458  std::ostringstream text;
459  text << "Click on point " << i + 1;
460  if (I) {
461  vpDisplay::display(*I);
462  vpDisplay::displayText(*I, 15, 10, text.str(), vpColor::red);
463  for (unsigned int k = 0; k < mem_ip.size(); k++) {
464  vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2);
465  }
466  vpDisplay::flush(*I);
467  }
468  else {
469  vpDisplay::display(*I_color);
470  vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red);
471  for (unsigned int k = 0; k < mem_ip.size(); k++) {
472  vpDisplay::displayCross(*I_color, mem_ip[k], 10, vpColor::green, 2);
473  }
474  vpDisplay::flush(*I_color);
475  }
476 
477  std::cout << "Click on point " << i + 1 << " ";
478  double x = 0, y = 0;
479  if (I) {
480  vpDisplay::getClick(*I, ip);
481  mem_ip.push_back(ip);
482  vpDisplay::flush(*I);
483  }
484  else {
485  vpDisplay::getClick(*I_color, ip);
486  mem_ip.push_back(ip);
487  vpDisplay::flush(*I_color);
488  }
490  P[i].set_x(x);
491  P[i].set_y(y);
492 
493  std::cout << "with 2D coordinates: " << ip << std::endl;
494 
495  pose.addPoint(P[i]); // and added to the pose computation point list
496  }
497  if (I) {
498  vpDisplay::flush(*I);
499  vpDisplay::display(*I);
500  }
501  else {
502  vpDisplay::flush(*I_color);
503  vpDisplay::display(*I_color);
504  }
505 
507 
508  if (I) {
509  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
510  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
511 
512  vpDisplay::flush(*I);
513 
514  button = vpMouseButton::button1;
515  while (!vpDisplay::getClick(*I, ip, button)) {
516  }
517 
518  if (button == vpMouseButton::button1) {
519  isWellInit = true;
520  }
521  else {
522  pose.clearPoint();
523  vpDisplay::display(*I);
524  vpDisplay::flush(*I);
525  }
526  }
527  else {
528  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
529  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object",
530  vpColor::red);
531 
532  vpDisplay::flush(*I_color);
533 
534  button = vpMouseButton::button1;
535  while (!vpDisplay::getClick(*I_color, ip, button)) {
536  }
537 
538  if (button == vpMouseButton::button1) {
539  isWellInit = true;
540  }
541  else {
542  pose.clearPoint();
543  vpDisplay::display(*I_color);
544  vpDisplay::flush(*I_color);
545  }
546  }
547  }
548  if (I)
550  else
551  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
552 
553  // save the pose into file
554  if (poseSavingFilename.empty())
555  savePose(str_pose);
556  else
558 
559  if (d_help != nullptr) {
560  delete d_help;
561  d_help = nullptr;
562  }
563  }
564 
565  std::cout << "cMo : " << std::endl << m_cMo << std::endl;
566 
567  if (I)
568  init(*I);
569  else {
570  vpImageConvert::convert(*I_color, m_I);
571  init(m_I);
572  }
573 }
574 
606 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp,
607  const vpHomogeneousMatrix &T)
608 {
609  initClick(&I, nullptr, initFile, displayHelp, T);
610 }
611 
643 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, bool displayHelp,
644  const vpHomogeneousMatrix &T)
645 {
646  initClick(nullptr, &I_color, initFile, displayHelp, T);
647 }
648 
649 void vpMbTracker::initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
650  const std::vector<vpPoint> &points3D_list, const std::string &displayFile)
651 {
652  if (I) {
653  vpDisplay::display(*I);
654  vpDisplay::flush(*I);
655  }
656  else {
657  vpDisplay::display(*I_color);
658  vpDisplay::flush(*I_color);
659  }
660 
661  vpDisplay *d_help = nullptr;
662 
663  vpPose pose;
664  std::vector<vpPoint> P;
665  for (unsigned int i = 0; i < points3D_list.size(); i++)
666  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
667 
668 #ifdef VISP_HAVE_MODULE_IO
669  vpImage<vpRGBa> Iref;
670  // Display window creation and initialisation
671  if (vpIoTools::checkFilename(displayFile)) {
672  try {
673  std::cout << "Load image to help initialization: " << displayFile << std::endl;
674 #if defined(VISP_HAVE_X11)
675  d_help = new vpDisplayX;
676 #elif defined(VISP_HAVE_GDI)
677  d_help = new vpDisplayGDI;
678 #elif defined VISP_HAVE_OPENCV
679  d_help = new vpDisplayOpenCV;
680 #endif
681 
682  vpImageIo::read(Iref, displayFile);
683 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
684  if (I) {
685  d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(),
686  "Where to initialize...");
687  }
688  else {
689  d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80,
690  I_color->display->getWindowYPosition(), "Where to initialize...");
691  }
692  vpDisplay::display(Iref);
693  vpDisplay::flush(Iref);
694 #endif
695  }
696  catch (...) {
697  if (d_help != nullptr) {
698  delete d_help;
699  d_help = nullptr;
700  }
701  }
702  }
703 #else //#ifdef VISP_HAVE_MODULE_IO
704  (void)(displayFile);
705 #endif //#ifdef VISP_HAVE_MODULE_IO
706 
707  vpImagePoint ip;
708  bool isWellInit = false;
709  while (!isWellInit) {
710  for (unsigned int i = 0; i < points3D_list.size(); i++) {
711  std::cout << "Click on point " << i + 1 << std::endl;
712  double x = 0, y = 0;
713  if (I) {
714  vpDisplay::getClick(*I, ip);
716  vpDisplay::flush(*I);
717  }
718  else {
719  vpDisplay::getClick(*I_color, ip);
720  vpDisplay::displayCross(*I_color, ip, 5, vpColor::green);
721  vpDisplay::flush(*I_color);
722  }
724  P[i].set_x(x);
725  P[i].set_y(y);
726 
727  std::cout << "Click on point " << ip << std::endl;
728 
729  if (I) {
730  vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point
731  }
732  else {
733  vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point
734  }
735  pose.addPoint(P[i]); // and added to the pose computation point list
736  }
737  if (I) {
738  vpDisplay::flush(*I);
739  }
740  else {
741  vpDisplay::flush(*I_color);
742  }
743 
745 
746  if (I) {
747  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
748  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
749 
750  vpDisplay::flush(*I);
751 
753  while (!vpDisplay::getClick(*I, ip, button)) {
754  };
755 
756  if (button == vpMouseButton::button1) {
757  isWellInit = true;
758  }
759  else {
760  pose.clearPoint();
761  vpDisplay::display(*I);
762  vpDisplay::flush(*I);
763  }
764  }
765  else {
766  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
767  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object",
768  vpColor::red);
769 
770  vpDisplay::flush(*I_color);
771 
773  while (!vpDisplay::getClick(*I_color, ip, button)) {
774  };
775 
776  if (button == vpMouseButton::button1) {
777  isWellInit = true;
778  }
779  else {
780  pose.clearPoint();
781  vpDisplay::display(*I_color);
782  vpDisplay::flush(*I_color);
783  }
784  }
785  }
786 
787  if (I) {
789  }
790  else {
791  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
792  }
793 
794  if (d_help != nullptr) {
795  delete d_help;
796  d_help = nullptr;
797  }
798 
799  if (I)
800  init(*I);
801  else {
802  vpImageConvert::convert(*I_color, m_I);
803  init(m_I);
804  }
805 }
806 
818 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
819  const std::string &displayFile)
820 {
821  initClick(&I, nullptr, points3D_list, displayFile);
822 }
823 
835 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::vector<vpPoint> &points3D_list,
836  const std::string &displayFile)
837 {
838  initClick(nullptr, &I_color, points3D_list, displayFile);
839 }
840 #endif //#ifdef VISP_HAVE_MODULE_GUI
841 
842 void vpMbTracker::initFromPoints(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
843  const std::string &initFile)
844 {
845  std::stringstream ss;
846  std::fstream finit;
847 
848  std::string ext = ".init";
849  size_t pos = initFile.rfind(ext);
850 
851  if (pos == initFile.size() - ext.size() && pos != 0) {
852  ss << initFile;
853  }
854  else {
855  ss << initFile;
856  ss << ".init";
857  }
858 
859  std::cout << "Load 2D/3D points from: " << ss.str() << std::endl;
860  finit.open(ss.str().c_str(), std::ios::in);
861  if (finit.fail()) {
862  std::cout << "cannot read " << ss.str() << std::endl;
863  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str());
864  }
865 
866  //********
867  // Read 3D points coordinates
868  //********
869  char c;
870  // skip lines starting with # as comment
871  finit.get(c);
872  while (!finit.fail() && (c == '#')) {
873  finit.ignore(256, '\n');
874  finit.get(c);
875  }
876  finit.unget();
877 
878  unsigned int n3d;
879  finit >> n3d;
880  finit.ignore(256, '\n'); // skip the rest of the line
881  std::cout << "Number of 3D points " << n3d << std::endl;
882  if (n3d > 100000) {
883  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed",
884  ss.str().c_str());
885  }
886 
887  vpPoint *P = new vpPoint[n3d];
888  for (unsigned int i = 0; i < n3d; i++) {
889  // skip lines starting with # as comment
890  finit.get(c);
891  while (!finit.fail() && (c == '#')) {
892  finit.ignore(256, '\n');
893  finit.get(c);
894  }
895  finit.unget();
896  double X, Y, Z;
897  finit >> X;
898  finit >> Y;
899  finit >> Z;
900  finit.ignore(256, '\n'); // skip the rest of the line
901 
902  std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
903  P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
904  }
905 
906  //********
907  // Read 3D points coordinates
908  //********
909  // skip lines starting with # as comment
910  finit.get(c);
911  while (!finit.fail() && (c == '#')) {
912  finit.ignore(256, '\n');
913  finit.get(c);
914  }
915  finit.unget();
916 
917  unsigned int n2d;
918  finit >> n2d;
919  finit.ignore(256, '\n'); // skip the rest of the line
920  std::cout << "Number of 2D points " << n2d << std::endl;
921  if (n2d > 100000) {
922  delete[] P;
923  throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed",
924  ss.str().c_str());
925  }
926 
927  if (n3d != n2d) {
928  delete[] P;
930  "In %s file, number of 2D points %d and number of 3D "
931  "points %d are not equal",
932  ss.str().c_str(), n2d, n3d);
933  }
934 
935  vpPose pose;
936  for (unsigned int i = 0; i < n2d; i++) {
937  // skip lines starting with # as comment
938  finit.get(c);
939  while (!finit.fail() && (c == '#')) {
940  finit.ignore(256, '\n');
941  finit.get(c);
942  }
943  finit.unget();
944  double u, v, x = 0, y = 0;
945  finit >> v;
946  finit >> u;
947  finit.ignore(256, '\n'); // skip the rest of the line
948 
949  vpImagePoint ip(v, u);
950  std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
952  P[i].set_x(x);
953  P[i].set_y(y);
954  pose.addPoint(P[i]);
955  }
956 
957  finit.close();
958 
960 
961  delete[] P;
962 
963  if (I) {
964  init(*I);
965  }
966  else {
967  vpImageConvert::convert(*I_color, m_I);
968  init(m_I);
969  }
970 }
971 
996 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
997 {
998  initFromPoints(&I, nullptr, initFile);
999 }
1000 
1025 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1026 {
1027  initFromPoints(nullptr, &I_color, initFile);
1028 }
1029 
1030 void vpMbTracker::initFromPoints(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
1031  const std::vector<vpImagePoint> &points2D_list,
1032  const std::vector<vpPoint> &points3D_list)
1033 {
1034  if (points2D_list.size() != points3D_list.size())
1035  vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
1036  "different to the number of 3D points.");
1037 
1038  size_t size = points3D_list.size();
1039  std::vector<vpPoint> P;
1040  vpPose pose;
1041 
1042  for (size_t i = 0; i < size; i++) {
1043  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1044  double x = 0, y = 0;
1045  vpPixelMeterConversion::convertPoint(m_cam, points2D_list[i], x, y);
1046  P[i].set_x(x);
1047  P[i].set_y(y);
1048  pose.addPoint(P[i]);
1049  }
1050 
1052 
1053  if (I) {
1054  init(*I);
1055  }
1056  else {
1057  vpImageConvert::convert(*I_color, m_I);
1058  init(m_I);
1059  }
1060 }
1061 
1070 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
1071  const std::vector<vpPoint> &points3D_list)
1072 {
1073  initFromPoints(&I, nullptr, points2D_list, points3D_list);
1074 }
1075 
1084 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::vector<vpImagePoint> &points2D_list,
1085  const std::vector<vpPoint> &points3D_list)
1086 {
1087  initFromPoints(nullptr, &I_color, points2D_list, points3D_list);
1088 }
1089 
1090 void vpMbTracker::initFromPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
1091  const std::string &initFile)
1092 {
1093  std::stringstream ss;
1094  std::fstream finit;
1095  vpPoseVector init_pos;
1096 
1097  std::string ext = ".pos";
1098  size_t pos = initFile.rfind(ext);
1099 
1100  if (pos == initFile.size() - ext.size() && pos != 0) {
1101  ss << initFile;
1102  }
1103  else {
1104  ss << initFile;
1105  ss << ".pos";
1106  }
1107 
1108  finit.open(ss.str().c_str(), std::ios::in);
1109  if (finit.fail()) {
1110  std::cout << "Cannot read " << ss.str() << std::endl;
1111  throw vpException(vpException::ioError, "cannot read init file");
1112  }
1113 
1114  for (unsigned int i = 0; i < 6; i += 1) {
1115  finit >> init_pos[i];
1116  }
1117 
1118  m_cMo.buildFrom(init_pos);
1119 
1120  if (I) {
1121  init(*I);
1122  }
1123  else {
1124  vpImageConvert::convert(*I_color, m_I);
1125  init(m_I);
1126  }
1127 }
1128 
1147 void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1148 {
1149  initFromPose(&I, nullptr, initFile);
1150 }
1151 
1170 void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1171 {
1172  initFromPose(nullptr, &I_color, initFile);
1173 }
1174 
1182 {
1183  m_cMo = cMo;
1184  init(I);
1185 }
1186 
1194 {
1195  m_cMo = cMo;
1196  vpImageConvert::convert(I_color, m_I);
1197  init(m_I);
1198 }
1199 
1207 {
1208  vpHomogeneousMatrix _cMo(cPo);
1209  initFromPose(I, _cMo);
1210 }
1211 
1219 {
1220  vpHomogeneousMatrix _cMo(cPo);
1221  vpImageConvert::convert(I_color, m_I);
1222  initFromPose(m_I, _cMo);
1223 }
1224 
1230 void vpMbTracker::savePose(const std::string &filename) const
1231 {
1232  vpPoseVector init_pos;
1233  std::fstream finitpos;
1234  finitpos.open(filename.c_str(), std::ios::out);
1235 
1236  init_pos.buildFrom(m_cMo);
1237  finitpos << init_pos;
1238  finitpos.close();
1239 }
1240 
1241 void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
1242  bool useLod, double minPolygonAreaThreshold, double minLineLengthThreshold)
1243 {
1244  std::vector<vpPoint> corners_without_duplicates;
1245  corners_without_duplicates.push_back(corners[0]);
1246  for (unsigned int i = 0; i < corners.size() - 1; i++) {
1247  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1248  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1249  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1250  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1251  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1252  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1253  corners_without_duplicates.push_back(corners[i + 1]);
1254  }
1255  }
1256 
1257  vpMbtPolygon polygon;
1258  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
1259  polygon.setIndex((int)idFace);
1260  polygon.setName(polygonName);
1261  polygon.setLod(useLod);
1262 
1263  // //if(minPolygonAreaThreshold != -1.0) {
1264  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1265  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1266  // {
1267  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1268  // }
1269  //
1270  // //if(minLineLengthThreshold != -1.0) {
1271  // if(std::fabs(minLineLengthThreshold + 1.0) >
1272  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1273  // {
1274  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1275  // }
1276 
1277  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1278  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1279 
1280  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1281  polygon.addPoint(j, corners_without_duplicates[j]);
1282  }
1283 
1284  faces.addPolygon(&polygon);
1285 
1287  faces.getPolygon().back()->setClipping(clippingFlag);
1288 
1290  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1291 
1293  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1294 }
1295 
1296 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1297  const std::string &polygonName, bool useLod, double minPolygonAreaThreshold)
1298 {
1299  vpMbtPolygon polygon;
1300  polygon.setNbPoint(4);
1301  polygon.setName(polygonName);
1302  polygon.setLod(useLod);
1303 
1304  // //if(minPolygonAreaThreshold != -1.0) {
1305  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1306  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1307  // {
1308  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1309  // }
1310  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1311  // Non sense to set minLineLengthThreshold for circle
1312  // but used to be coherent when applying LOD settings for all polygons
1314 
1315  {
1316  // Create the 4 points of the circle bounding box
1317  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
1318 
1319  // Matrice de passage entre world et circle frame
1320  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
1321  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
1322  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
1323  vpRotationMatrix wRc;
1324  vpColVector x(3), y(3), z(3);
1325  // X axis is P2-P1
1326  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
1327  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
1328  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
1329  // Y axis is the normal of the plane
1330  y[0] = plane.getA() / norm_Y;
1331  y[1] = plane.getB() / norm_Y;
1332  y[2] = plane.getC() / norm_Y;
1333  // Z axis = X ^ Y
1334  z = vpColVector::crossProd(x, y);
1335  for (unsigned int i = 0; i < 3; i++) {
1336  wRc[i][0] = x[i];
1337  wRc[i][1] = y[i];
1338  wRc[i][2] = z[i];
1339  }
1340 
1341  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
1342  vpHomogeneousMatrix wMc(wtc, wRc);
1343 
1344  vpColVector c_p(4); // A point in the circle frame that is on the bbox
1345  c_p[0] = radius;
1346  c_p[1] = 0;
1347  c_p[2] = radius;
1348  c_p[3] = 1;
1349 
1350  // Matrix to rotate a point by 90 deg around Y in the circle frame
1351  for (unsigned int i = 0; i < 4; i++) {
1352  vpColVector w_p(4); // A point in the word frame
1354  w_p = wMc * cMc_90 * c_p;
1355 
1356  vpPoint w_P;
1357  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
1358 
1359  polygon.addPoint(i, w_P);
1360  }
1361  }
1362 
1363  polygon.setIndex(idFace);
1364  faces.addPolygon(&polygon);
1365 
1367  faces.getPolygon().back()->setClipping(clippingFlag);
1368 
1370  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1371 
1373  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1374 }
1375 
1376 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
1377  bool useLod, double minLineLengthThreshold)
1378 {
1379  // A polygon as a single line that corresponds to the revolution axis of the
1380  // cylinder
1381  vpMbtPolygon polygon;
1382  polygon.setNbPoint(2);
1383 
1384  polygon.addPoint(0, p1);
1385  polygon.addPoint(1, p2);
1386 
1387  polygon.setIndex(idFace);
1388  polygon.setName(polygonName);
1389  polygon.setLod(useLod);
1390 
1391  // //if(minLineLengthThreshold != -1.0) {
1392  // if(std::fabs(minLineLengthThreshold + 1.0) >
1393  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1394  // {
1395  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1396  // }
1397  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1398  // Non sense to set minPolygonAreaThreshold for cylinder
1399  // but used to be coherent when applying LOD settings for all polygons
1401 
1402  faces.addPolygon(&polygon);
1403 
1405  faces.getPolygon().back()->setClipping(clippingFlag);
1406 
1408  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1409 
1411  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1412 }
1413 
1414 void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
1415  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
1416 {
1417  int id = idFace;
1418  for (unsigned int i = 0; i < listFaces.size(); i++) {
1419  vpMbtPolygon polygon;
1420  polygon.setNbPoint((unsigned int)listFaces[i].size());
1421  for (unsigned int j = 0; j < listFaces[i].size(); j++)
1422  polygon.addPoint(j, listFaces[i][j]);
1423 
1424  polygon.setIndex(id);
1425  polygon.setName(polygonName);
1426  polygon.setIsPolygonOriented(false);
1427  polygon.setLod(useLod);
1428  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1430 
1431  faces.addPolygon(&polygon);
1432 
1434  faces.getPolygon().back()->setClipping(clippingFlag);
1435 
1437  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1438 
1440  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1441 
1442  id++;
1443  }
1444 }
1445 
1461 void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &odTo)
1462 {
1463  std::string::const_iterator it;
1464 
1465  if (vpIoTools::checkFilename(modelFile)) {
1466  it = modelFile.end();
1467  if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
1468  (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
1469  std::vector<std::string> vectorOfModelFilename;
1470  int startIdFace = (int)faces.size();
1471  nbPoints = 0;
1472  nbLines = 0;
1473  nbPolygonLines = 0;
1474  nbPolygonPoints = 0;
1475  nbCylinders = 0;
1476  nbCircles = 0;
1477  loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo);
1478  }
1479  else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
1480  (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
1481  loadVRMLModel(modelFile);
1482  }
1483  else {
1484  throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
1485  }
1486  }
1487  else {
1488  throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
1489  }
1490 
1491  this->modelInitialised = true;
1492  this->modelFileName = modelFile;
1493 }
1494 
1513 void vpMbTracker::loadVRMLModel(const std::string &modelFile)
1514 {
1515 #ifdef VISP_HAVE_COIN3D
1516  m_sodb_init_called = true;
1517  SoDB::init(); // Call SoDB::finish() before ending the program.
1518 
1519  SoInput in;
1520  SbBool ok = in.openFile(modelFile.c_str());
1521  SoVRMLGroup *sceneGraphVRML2;
1522 
1523  if (!ok) {
1524  vpERROR_TRACE("can't open file to load model");
1525  throw vpException(vpException::fatalError, "can't open file to load model");
1526  }
1527 
1528  if (!in.isFileVRML2()) {
1529  SoSeparator *sceneGraph = SoDB::readAll(&in);
1530  if (sceneGraph == nullptr) { /*return -1;*/
1531  }
1532  sceneGraph->ref();
1533 
1534  SoToVRML2Action tovrml2;
1535  tovrml2.apply(sceneGraph);
1536 
1537  sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1538  sceneGraphVRML2->ref();
1539  sceneGraph->unref();
1540  }
1541  else {
1542  sceneGraphVRML2 = SoDB::readAllVRML(&in);
1543  if (sceneGraphVRML2 == nullptr) { /*return -1;*/
1544  }
1545  sceneGraphVRML2->ref();
1546  }
1547 
1548  in.closeFile();
1549 
1550  vpHomogeneousMatrix transform;
1551  int indexFace = (int)faces.size();
1552  extractGroup(sceneGraphVRML2, transform, indexFace);
1553 
1554  sceneGraphVRML2->unref();
1555 #else
1556  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1557  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
1558 #endif
1559 }
1560 
1561 void vpMbTracker::removeComment(std::ifstream &fileId)
1562 {
1563  char c;
1564 
1565  fileId.get(c);
1566  while (!fileId.fail() && (c == '#')) {
1567  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));
1568  fileId.get(c);
1569  }
1570  if (fileId.fail()) {
1571  throw(vpException(vpException::ioError, "Reached end of file"));
1572  }
1573  fileId.unget();
1574 }
1575 
1576 std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
1577 {
1578  std::map<std::string, std::string> mapOfParams;
1579 
1580  bool exit = false;
1581  while (!endLine.empty() && !exit) {
1582  exit = true;
1583 
1584  for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
1585  it != mapOfParameterNames.end(); ++it) {
1586  endLine = vpIoTools::trim(endLine);
1587  std::string param(it->first + "=");
1588 
1589  // Compare with a potential parameter
1590  if (endLine.compare(0, param.size(), param) == 0) {
1591  exit = false;
1592  endLine = endLine.substr(param.size());
1593 
1594  bool parseQuote = false;
1595  if (it->second == "string") {
1596  // Check if the string is between quotes
1597  if (endLine.size() > 2 && endLine[0] == '"') {
1598  parseQuote = true;
1599  endLine = endLine.substr(1);
1600  size_t pos = endLine.find_first_of('"');
1601 
1602  if (pos != std::string::npos) {
1603  mapOfParams[it->first] = endLine.substr(0, pos);
1604  endLine = endLine.substr(pos + 1);
1605  }
1606  else {
1607  parseQuote = false;
1608  }
1609  }
1610  }
1611 
1612  if (!parseQuote) {
1613  // Deal with space or tabulation after parameter value to substring
1614  // to the next sequence
1615  size_t pos1 = endLine.find_first_of(' ');
1616  size_t pos2 = endLine.find_first_of('\t');
1617  size_t pos = pos1 < pos2 ? pos1 : pos2;
1618 
1619  mapOfParams[it->first] = endLine.substr(0, pos);
1620  endLine = endLine.substr(pos + 1);
1621  }
1622  }
1623  }
1624  }
1625 
1626  return mapOfParams;
1627 }
1628 
1678 void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
1679  int &startIdFace, bool verbose, bool parent, const vpHomogeneousMatrix &odTo)
1680 {
1681  std::ifstream fileId;
1682  fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1683  fileId.open(modelFile.c_str(), std::ifstream::in);
1684  if (fileId.fail()) {
1685  std::cout << "cannot read CAO model file: " << modelFile << std::endl;
1686  throw vpException(vpException::ioError, "cannot read CAO model file");
1687  }
1688 
1689  if (verbose) {
1690  std::cout << "Model file : " << modelFile << std::endl;
1691  }
1692  vectorOfModelFilename.push_back(modelFile);
1693 
1694  try {
1695  char c;
1696  // Extraction of the version (remove empty line and commented ones
1697  // (comment line begin with the #)).
1698  // while ((fileId.get(c) != nullptr) && (c == '#')) fileId.ignore(256, '\n');
1699  removeComment(fileId);
1700 
1702  int caoVersion;
1703  fileId.get(c);
1704  if (c == 'V') {
1705  fileId >> caoVersion;
1706  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1707  }
1708  else {
1709  std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
1710  "file : use V0, V1, ...";
1711  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
1712  "header file : use V0, V1, ...");
1713  }
1714 
1715  removeComment(fileId);
1716 
1718  std::string line;
1719  const std::string prefix_load = "load";
1720 
1721  fileId.get(c);
1722  fileId.unget();
1723  bool header = false;
1724  while (c == 'l' || c == 'L') {
1725  getline(fileId, line);
1726 
1727  if (!line.compare(0, prefix_load.size(), prefix_load)) {
1728  // remove "load("
1729  std::string paramsStr = line.substr(5);
1730  // get parameters inside load()
1731  paramsStr = paramsStr.substr(0, paramsStr.find_first_of(")"));
1732  // split by comma
1733  std::vector<std::string> params = vpIoTools::splitChain(paramsStr, ",");
1734  // remove whitespaces
1735  for (size_t i = 0; i < params.size(); i++) {
1736  params[i] = vpIoTools::trim(params[i]);
1737  }
1738 
1739  if (!params.empty()) {
1740  // Get the loaded model pathname
1741  std::string headerPathRead = params[0];
1742  headerPathRead = headerPathRead.substr(1);
1743  headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of("\""));
1744 
1745  std::string headerPath = headerPathRead;
1746  if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
1747  std::string parentDirectory = vpIoTools::getParent(modelFile);
1748  headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
1749  }
1750 
1751  // Normalize path
1752  headerPath = vpIoTools::path(headerPath);
1753 
1754  // Get real path
1755  headerPath = vpIoTools::getAbsolutePathname(headerPath);
1756 
1757  vpHomogeneousMatrix oTo_local;
1759  vpThetaUVector tu;
1760  for (size_t i = 1; i < params.size(); i++) {
1761  std::string param = params[i];
1762  {
1763  const std::string prefix = "t=[";
1764  if (!param.compare(0, prefix.size(), prefix)) {
1765  param = param.substr(prefix.size());
1766  param = param.substr(0, param.find_first_of("]"));
1767 
1768  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1769  if (values.size() == 3) {
1770  t[0] = atof(values[0].c_str());
1771  t[1] = atof(values[1].c_str());
1772  t[2] = atof(values[2].c_str());
1773  }
1774  }
1775  }
1776  {
1777  const std::string prefix = "tu=[";
1778  if (!param.compare(0, prefix.size(), prefix)) {
1779  param = param.substr(prefix.size());
1780  param = param.substr(0, param.find_first_of("]"));
1781 
1782  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1783  if (values.size() == 3) {
1784  for (size_t j = 0; j < values.size(); j++) {
1785  std::string value = values[j];
1786  bool radian = true;
1787  size_t unitPos = value.find("deg");
1788  if (unitPos != std::string::npos) {
1789  value = value.substr(0, unitPos);
1790  radian = false;
1791  }
1792 
1793  unitPos = value.find("rad");
1794  if (unitPos != std::string::npos) {
1795  value = value.substr(0, unitPos);
1796  }
1797  tu[static_cast<unsigned int>(j)] = !radian ? vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1798  }
1799  }
1800  }
1801  }
1802  }
1803  oTo_local.buildFrom(t, tu);
1804 
1805  bool cyclic = false;
1806  for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1807  it != vectorOfModelFilename.end() && !cyclic; ++it) {
1808  if (headerPath == *it) {
1809  cyclic = true;
1810  }
1811  }
1812 
1813  if (!cyclic) {
1814  if (vpIoTools::checkFilename(headerPath)) {
1815  header = true;
1816  loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, odTo * oTo_local);
1817  }
1818  else {
1819  throw vpException(vpException::ioError, "file cannot be open");
1820  }
1821  }
1822  else {
1823  std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
1824  << std::endl;
1825  }
1826  }
1827  }
1828 
1829  removeComment(fileId);
1830  fileId.get(c);
1831  fileId.unget();
1832  }
1833 
1835  unsigned int caoNbrPoint;
1836  fileId >> caoNbrPoint;
1837  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1838 
1839  nbPoints += caoNbrPoint;
1840  if (verbose || (parent && !header)) {
1841 #if defined(VISP_HAVE_THREADS)
1842  std::lock_guard<std::mutex> lock(g_mutex_cout);
1843 #endif
1844  std::cout << "> " << caoNbrPoint << " points" << std::endl;
1845  }
1846 
1847  if (caoNbrPoint > 100000) {
1848  throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
1849  }
1850 
1851  if (caoNbrPoint == 0 && !header) {
1852  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
1853  }
1854  vpPoint *caoPoints = new vpPoint[caoNbrPoint];
1855 
1856  int i; // image coordinate (used for matching)
1857  int j;
1858 
1859  for (unsigned int k = 0; k < caoNbrPoint; k++) {
1860  removeComment(fileId);
1861 
1862  vpColVector pt_3d(4, 1.0);
1863  fileId >> pt_3d[0];
1864  fileId >> pt_3d[1];
1865  fileId >> pt_3d[2];
1866 
1867  if (caoVersion == 2) {
1868  fileId >> i;
1869  fileId >> j;
1870  }
1871 
1872  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1873 
1874  vpColVector pt_3d_tf = odTo * pt_3d;
1875  caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1876  }
1877 
1878  removeComment(fileId);
1879 
1881  // Store in a map the potential segments to add
1882  std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1883  unsigned int caoNbrLine;
1884  fileId >> caoNbrLine;
1885  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1886 
1887  nbLines += caoNbrLine;
1888  unsigned int *caoLinePoints = nullptr;
1889  if (verbose || (parent && !header)) {
1890 #if defined(VISP_HAVE_THREADS)
1891  std::lock_guard<std::mutex> lock(g_mutex_cout);
1892 #endif
1893  std::cout << "> " << caoNbrLine << " lines" << std::endl;
1894  }
1895 
1896  if (caoNbrLine > 100000) {
1897  delete[] caoPoints;
1898  throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
1899  }
1900 
1901  if (caoNbrLine > 0)
1902  caoLinePoints = new unsigned int[2 * caoNbrLine];
1903 
1904  unsigned int index1, index2;
1905  // Initialization of idFace with startIdFace for dealing with recursive
1906  // load in header
1907  int idFace = startIdFace;
1908 
1909  for (unsigned int k = 0; k < caoNbrLine; k++) {
1910  removeComment(fileId);
1911 
1912  fileId >> index1;
1913  fileId >> index2;
1914 
1916  // Get the end of the line
1917  std::string endLine = "";
1918  if (safeGetline(fileId, endLine).good()) {
1919  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1920 
1921  std::string segmentName = "";
1922  double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1923  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1924  if (mapOfParams.find("name") != mapOfParams.end()) {
1925  segmentName = mapOfParams["name"];
1926  }
1927  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1928  minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1929  }
1930  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1931  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
1932  }
1933 
1934  SegmentInfo segmentInfo;
1935  segmentInfo.name = segmentName;
1936  segmentInfo.useLod = useLod;
1937  segmentInfo.minLineLengthThresh = minLineLengthThresh;
1938 
1939  caoLinePoints[2 * k] = index1;
1940  caoLinePoints[2 * k + 1] = index2;
1941 
1942  if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1943  std::vector<vpPoint> extremities;
1944  extremities.push_back(caoPoints[index1]);
1945  extremities.push_back(caoPoints[index2]);
1946  segmentInfo.extremities = extremities;
1947 
1948  std::pair<unsigned int, unsigned int> key(index1, index2);
1949 
1950  segmentTemporaryMap[key] = segmentInfo;
1951  }
1952  else {
1953  vpTRACE(" line %d has wrong coordinates.", k);
1954  }
1955  }
1956  }
1957 
1958  removeComment(fileId);
1959 
1961  /* Load polygon from the lines extracted earlier (the first point of the
1962  * line is used)*/
1963  // Store in a vector the indexes of the segments added in the face segment
1964  // case
1965  std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1966  unsigned int caoNbrPolygonLine;
1967  fileId >> caoNbrPolygonLine;
1968  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1969 
1970  nbPolygonLines += caoNbrPolygonLine;
1971  if (verbose || (parent && !header)) {
1972 #if defined(VISP_HAVE_THREADS)
1973  std::lock_guard<std::mutex> lock(g_mutex_cout);
1974 #endif
1975  std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
1976  }
1977 
1978  if (caoNbrPolygonLine > 100000) {
1979  delete[] caoPoints;
1980  delete[] caoLinePoints;
1981  throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
1982  }
1983 
1984  unsigned int index;
1985  for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1986  removeComment(fileId);
1987 
1988  unsigned int nbLinePol;
1989  fileId >> nbLinePol;
1990  std::vector<vpPoint> corners;
1991  if (nbLinePol > 100000) {
1992  throw vpException(vpException::badValue, "Exceed the max number of lines.");
1993  }
1994 
1995  for (unsigned int n = 0; n < nbLinePol; n++) {
1996  fileId >> index;
1997 
1998  if (index >= caoNbrLine) {
1999  throw vpException(vpException::badValue, "Exceed the max number of lines.");
2000  }
2001  corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2002  corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2003 
2004  std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2005  faceSegmentKeyVector.push_back(key);
2006  }
2007 
2009  // Get the end of the line
2010  std::string endLine = "";
2011  if (safeGetline(fileId, endLine).good()) {
2012  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2013 
2014  std::string polygonName = "";
2015  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2016  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2017  if (mapOfParams.find("name") != mapOfParams.end()) {
2018  polygonName = mapOfParams["name"];
2019  }
2020  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2021  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2022  }
2023  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2024  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2025  }
2026 
2027  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2028  initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
2029 
2030  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold,
2033  }
2034  }
2035 
2036  // Add the segments which were not already added in the face segment case
2037  for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2038  it != segmentTemporaryMap.end(); ++it) {
2039  if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2040  faceSegmentKeyVector.end()) {
2041  addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
2042  it->second.minLineLengthThresh);
2043  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2044 
2045  addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod,
2046  minPolygonAreaThresholdGeneral, it->second.minLineLengthThresh);
2048  }
2049  }
2050 
2051  removeComment(fileId);
2052 
2054  /* Extract the polygon using the point coordinates (top of the file) */
2055  unsigned int caoNbrPolygonPoint;
2056  fileId >> caoNbrPolygonPoint;
2057  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2058 
2059  nbPolygonPoints += caoNbrPolygonPoint;
2060  if (verbose || (parent && !header)) {
2061 #if defined(VISP_HAVE_THREADS)
2062  std::lock_guard<std::mutex> lock(g_mutex_cout);
2063 #endif
2064  std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
2065  }
2066 
2067  if (caoNbrPolygonPoint > 100000) {
2068  throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
2069  }
2070 
2071  for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2072  removeComment(fileId);
2073 
2074  unsigned int nbPointPol;
2075  fileId >> nbPointPol;
2076  if (nbPointPol > 100000) {
2077  throw vpException(vpException::badValue, "Exceed the max number of points.");
2078  }
2079  std::vector<vpPoint> corners;
2080  for (unsigned int n = 0; n < nbPointPol; n++) {
2081  fileId >> index;
2082  if (index > caoNbrPoint - 1) {
2083  throw vpException(vpException::badValue, "Exceed the max number of points.");
2084  }
2085  corners.push_back(caoPoints[index]);
2086  }
2087 
2089  // Get the end of the line
2090  std::string endLine = "";
2091  if (safeGetline(fileId, endLine).good()) {
2092  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2093 
2094  std::string polygonName = "";
2095  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2096  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2097  if (mapOfParams.find("name") != mapOfParams.end()) {
2098  polygonName = mapOfParams["name"];
2099  }
2100  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2101  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2102  }
2103  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2104  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2105  }
2106 
2107  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2108  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2109 
2110  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold,
2113  }
2114  }
2115 
2117  unsigned int caoNbCylinder;
2118  try {
2119  removeComment(fileId);
2120 
2121  if (fileId.eof()) { // check if not at the end of the file (for old
2122  // style files)
2123  delete[] caoPoints;
2124  delete[] caoLinePoints;
2125  return;
2126  }
2127 
2128  /* Extract the cylinders */
2129  fileId >> caoNbCylinder;
2130  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2131 
2132  nbCylinders += caoNbCylinder;
2133  if (verbose || (parent && !header)) {
2134 #if defined(VISP_HAVE_THREADS)
2135  std::lock_guard<std::mutex> lock(g_mutex_cout);
2136 #endif
2137  std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
2138  }
2139 
2140  if (caoNbCylinder > 100000) {
2141  throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
2142  }
2143 
2144  for (unsigned int k = 0; k < caoNbCylinder; ++k) {
2145  removeComment(fileId);
2146 
2147  double radius;
2148  unsigned int indexP1, indexP2;
2149  fileId >> indexP1;
2150  fileId >> indexP2;
2151  fileId >> radius;
2152 
2154  // Get the end of the line
2155  std::string endLine = "";
2156  if (safeGetline(fileId, endLine).good()) {
2157  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2158 
2159  std::string polygonName = "";
2160  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2161  double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
2162  if (mapOfParams.find("name") != mapOfParams.end()) {
2163  polygonName = mapOfParams["name"];
2164  }
2165  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
2166  minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
2167  }
2168  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2169  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2170  }
2171 
2172  int idRevolutionAxis = idFace;
2173  addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2174 
2175  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod,
2176  minLineLengthThreshold);
2177 
2178  std::vector<std::vector<vpPoint> > listFaces;
2179  createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
2180  addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2181 
2182  initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2183 
2184  addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2185  initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2186 
2187  idFace += 4;
2188  }
2189  }
2190 
2191  }
2192  catch (const std::exception &e) {
2193  std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2194  std::cerr << "Exception: " << e.what() << std::endl;
2195  caoNbCylinder = 0;
2196  }
2197 
2199  unsigned int caoNbCircle;
2200  try {
2201  removeComment(fileId);
2202 
2203  if (fileId.eof()) { // check if not at the end of the file (for old
2204  // style files)
2205  delete[] caoPoints;
2206  delete[] caoLinePoints;
2207  return;
2208  }
2209 
2210  /* Extract the circles */
2211  fileId >> caoNbCircle;
2212  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2213 
2214  nbCircles += caoNbCircle;
2215  if (verbose || (parent && !header)) {
2216 #if defined(VISP_HAVE_THREADS)
2217  std::lock_guard<std::mutex> lock(g_mutex_cout);
2218 #endif
2219  std::cout << "> " << caoNbCircle << " circles" << std::endl;
2220  }
2221 
2222  if (caoNbCircle > 100000) {
2223  throw vpException(vpException::badValue, "Exceed the max number of cicles.");
2224  }
2225 
2226  for (unsigned int k = 0; k < caoNbCircle; ++k) {
2227  removeComment(fileId);
2228 
2229  double radius;
2230  unsigned int indexP1, indexP2, indexP3;
2231  fileId >> radius;
2232  fileId >> indexP1;
2233  fileId >> indexP2;
2234  fileId >> indexP3;
2235 
2237  // Get the end of the line
2238  std::string endLine = "";
2239  if (safeGetline(fileId, endLine).good()) {
2240  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2241 
2242  std::string polygonName = "";
2243  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2244  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2245  if (mapOfParams.find("name") != mapOfParams.end()) {
2246  polygonName = mapOfParams["name"];
2247  }
2248  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2249  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2250  }
2251  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2252  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2253  }
2254 
2255  addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2256  minPolygonAreaThreshold);
2257 
2258  initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2259 
2260  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace,
2261  polygonName, useLod, minPolygonAreaThreshold);
2262  initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++,
2263  polygonName);
2264  }
2265  }
2266 
2267  }
2268  catch (const std::exception &e) {
2269  std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
2270  std::cerr << "Exception: " << e.what() << std::endl;
2271  caoNbCircle = 0;
2272  }
2273 
2274  startIdFace = idFace;
2275 
2276  delete[] caoPoints;
2277  delete[] caoLinePoints;
2278 
2279  if (header && parent) {
2280  if (verbose) {
2281 #if defined(VISP_HAVE_THREADS)
2282  std::lock_guard<std::mutex> lock(g_mutex_cout);
2283 #endif
2284  std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
2285  std::cout << "Total nb of points : " << nbPoints << std::endl;
2286  std::cout << "Total nb of lines : " << nbLines << std::endl;
2287  std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
2288  std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
2289  std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
2290  std::cout << "Total nb of circles : " << nbCircles << std::endl;
2291  }
2292  else {
2293 #if defined(VISP_HAVE_THREADS)
2294  std::lock_guard<std::mutex> lock(g_mutex_cout);
2295 #endif
2296  std::cout << "> " << nbPoints << " points" << std::endl;
2297  std::cout << "> " << nbLines << " lines" << std::endl;
2298  std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
2299  std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
2300  std::cout << "> " << nbCylinders << " cylinders" << std::endl;
2301  std::cout << "> " << nbCircles << " circles" << std::endl;
2302  }
2303  }
2304 
2305  // Go up: remove current model
2306  vectorOfModelFilename.pop_back();
2307  }
2308  catch (const std::exception &e) {
2309  std::cerr << "Cannot read line!" << std::endl;
2310  std::cerr << "Exception: " << e.what() << std::endl;
2311  throw vpException(vpException::ioError, "cannot read line");
2312  }
2313 }
2314 
2315 #ifdef VISP_HAVE_COIN3D
2323 void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
2324 {
2325  vpHomogeneousMatrix transformCur;
2326  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
2327  if (sceneGraphVRML2Trasnform) {
2328  float rx, ry, rz, rw;
2329  sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2330  vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
2331  // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
2332  // rw << std::endl;
2333 
2334  float tx, ty, tz;
2335  tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2336  ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2337  tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2338  vpTranslationVector transVec(tx, ty, tz);
2339  // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
2340  // std::endl;
2341 
2342  float sx, sy, sz;
2343  sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2344  sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2345  sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2346  // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
2347  // std::endl;
2348 
2349  for (unsigned int i = 0; i < 3; i++)
2350  rotMat[0][i] *= sx;
2351  for (unsigned int i = 0; i < 3; i++)
2352  rotMat[1][i] *= sy;
2353  for (unsigned int i = 0; i < 3; i++)
2354  rotMat[2][i] *= sz;
2355 
2356  transformCur = vpHomogeneousMatrix(transVec, rotMat);
2357  transform = transform * transformCur;
2358  }
2359 
2360  int nbShapes = sceneGraphVRML2->getNumChildren();
2361  // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
2362  // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
2363  // std::endl;
2364 
2365  SoNode *child;
2366 
2367  for (int i = 0; i < nbShapes; i++) {
2368  vpHomogeneousMatrix transform_recursive(transform);
2369  child = sceneGraphVRML2->getChild(i);
2370 
2371  if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2372  extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2373  }
2374 
2375  if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2376  extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2377  }
2378 
2379  if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2380  SoChildList *child2list = child->getChildren();
2381  std::string name = child->getName().getString();
2382 
2383  for (int j = 0; j < child2list->getLength(); j++) {
2384  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2385  SoVRMLIndexedFaceSet *face_set;
2386  face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2387  if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
2388  extractCylinders(face_set, transform, idFace, name);
2389  }
2390  else {
2391  extractFaces(face_set, transform, idFace, name);
2392  }
2393  }
2394  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2395  SoVRMLIndexedLineSet *line_set;
2396  line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2397  extractLines(line_set, idFace, name);
2398  }
2399  }
2400  }
2401  }
2402 }
2403 
2413 void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2414  const std::string &polygonName)
2415 {
2416  std::vector<vpPoint> corners;
2417 
2418  // SoMFInt32 indexList = _face_set->coordIndex;
2419  // int indexListSize = indexList.getNum();
2420  int indexListSize = face_set->coordIndex.getNum();
2421 
2422  vpColVector pointTransformed(4);
2423  vpPoint pt;
2424  SoVRMLCoordinate *coord;
2425 
2426  for (int i = 0; i < indexListSize; i++) {
2427  if (face_set->coordIndex[i] == -1) {
2428  if (corners.size() > 1) {
2429  addPolygon(corners, idFace, polygonName);
2430  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2431 
2432  addProjectionErrorPolygon(corners, idFace++, polygonName);
2434  corners.resize(0);
2435  }
2436  }
2437  else {
2438  coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2439  int index = face_set->coordIndex[i];
2440  pointTransformed[0] = coord->point[index].getValue()[0];
2441  pointTransformed[1] = coord->point[index].getValue()[1];
2442  pointTransformed[2] = coord->point[index].getValue()[2];
2443  pointTransformed[3] = 1.0;
2444 
2445  pointTransformed = transform * pointTransformed;
2446 
2447  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2448  corners.push_back(pt);
2449  }
2450  }
2451 }
2452 
2467 void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2468  const std::string &polygonName)
2469 {
2470  std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
2471  // first circle and to the
2472  // second one.
2473  SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2474 
2475  unsigned int indexListSize = (unsigned int)coords->point.getNum();
2476 
2477  if (indexListSize % 2 == 1) {
2478  std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
2479  throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
2480  }
2481  corners_c1.resize(indexListSize / 2);
2482  corners_c2.resize(indexListSize / 2);
2483  vpColVector pointTransformed(4);
2484  vpPoint pt;
2485 
2486  // extract all points and fill the two sets.
2487 
2488  for (int i = 0; i < coords->point.getNum(); ++i) {
2489  pointTransformed[0] = coords->point[i].getValue()[0];
2490  pointTransformed[1] = coords->point[i].getValue()[1];
2491  pointTransformed[2] = coords->point[i].getValue()[2];
2492  pointTransformed[3] = 1.0;
2493 
2494  pointTransformed = transform * pointTransformed;
2495 
2496  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2497 
2498  if (i < (int)corners_c1.size()) {
2499  corners_c1[(unsigned int)i] = pt;
2500  }
2501  else {
2502  corners_c2[(unsigned int)i - corners_c1.size()] = pt;
2503  }
2504  }
2505 
2506  vpPoint p1 = getGravityCenter(corners_c1);
2507  vpPoint p2 = getGravityCenter(corners_c2);
2508 
2509  vpColVector dist(3);
2510  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
2511  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
2512  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
2513  double radius_c1 = sqrt(dist.sumSquare());
2514  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
2515  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
2516  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
2517  double radius_c2 = sqrt(dist.sumSquare());
2518 
2519  if (std::fabs(radius_c1 - radius_c2) >
2520  (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
2521  std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
2522  throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
2523  }
2524 
2525  // addPolygon(p1, p2, idFace, polygonName);
2526  // initCylinder(p1, p2, radius_c1, idFace++);
2527 
2528  int idRevolutionAxis = idFace;
2529  addPolygon(p1, p2, idFace, polygonName);
2530 
2531  addProjectionErrorPolygon(p1, p2, idFace++, polygonName);
2532 
2533  std::vector<std::vector<vpPoint> > listFaces;
2534  createCylinderBBox(p1, p2, radius_c1, listFaces);
2535  addPolygon(listFaces, idFace, polygonName);
2536 
2537  initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2538 
2539  addProjectionErrorPolygon(listFaces, idFace, polygonName);
2540  initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2541 
2542  idFace += 4;
2543 }
2544 
2553 void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
2554 {
2555  std::vector<vpPoint> corners;
2556  corners.resize(0);
2557 
2558  int indexListSize = line_set->coordIndex.getNum();
2559 
2560  SbVec3f point(0, 0, 0);
2561  vpPoint pt;
2562  SoVRMLCoordinate *coord;
2563 
2564  for (int i = 0; i < indexListSize; i++) {
2565  if (line_set->coordIndex[i] == -1) {
2566  if (corners.size() > 1) {
2567  addPolygon(corners, idFace, polygonName);
2568  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2569 
2570  addProjectionErrorPolygon(corners, idFace++, polygonName);
2572  corners.resize(0);
2573  }
2574  }
2575  else {
2576  coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2577  int index = line_set->coordIndex[i];
2578  point[0] = coord->point[index].getValue()[0];
2579  point[1] = coord->point[index].getValue()[1];
2580  point[2] = coord->point[index].getValue()[2];
2581 
2582  pt.setWorldCoordinates(point[0], point[1], point[2]);
2583  corners.push_back(pt);
2584  }
2585  }
2586 }
2587 
2588 #endif // VISP_HAVE_COIN3D
2589 
2599 vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
2600 {
2601  if (pts.empty()) {
2602  std::cout << "Cannot extract center of gravity of empty set." << std::endl;
2603  throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
2604  }
2605  double oX = 0;
2606  double oY = 0;
2607  double oZ = 0;
2608  vpPoint G;
2609 
2610  for (unsigned int i = 0; i < pts.size(); ++i) {
2611  oX += pts[i].get_oX();
2612  oY += pts[i].get_oY();
2613  oZ += pts[i].get_oZ();
2614  }
2615 
2616  G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
2617  return G;
2618 }
2619 
2632 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2633 vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
2634 {
2635  // Temporary variable to permit to order polygons by distance
2636  std::vector<vpPolygon> polygonsTmp;
2637  std::vector<std::vector<vpPoint> > roisPtTmp;
2638 
2639  // Pair containing the list of vpPolygon and the list of face corners
2640  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2641 
2642  for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
2643  // A face has at least 3 points
2644  if (faces.getPolygon()[i]->nbpt > 2) {
2645  if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
2646  std::vector<vpImagePoint> roiPts;
2647 
2648  if (clipPolygon) {
2649  faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo);
2650  }
2651  else {
2652  roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo);
2653  }
2654 
2655  if (roiPts.size() <= 2) {
2656  continue;
2657  }
2658 
2659  polygonsTmp.push_back(vpPolygon(roiPts));
2660 
2661  std::vector<vpPoint> polyPts;
2662  if (clipPolygon) {
2663  faces.getPolygon()[i]->getPolygonClipped(polyPts);
2664  }
2665  else {
2666  for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
2667  polyPts.push_back(faces.getPolygon()[i]->p[j]);
2668  }
2669  }
2670  roisPtTmp.push_back(polyPts);
2671  }
2672  }
2673  }
2674 
2675  if (orderPolygons) {
2676  // Order polygons by distance (near to far)
2677  std::vector<PolygonFaceInfo> listOfPolygonFaces;
2678  for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
2679  double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2680  for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2681  x_centroid += roisPtTmp[i][j].get_X();
2682  y_centroid += roisPtTmp[i][j].get_Y();
2683  z_centroid += roisPtTmp[i][j].get_Z();
2684  }
2685 
2686  x_centroid /= roisPtTmp[i].size();
2687  y_centroid /= roisPtTmp[i].size();
2688  z_centroid /= roisPtTmp[i].size();
2689 
2690  double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2691  listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2692  }
2693 
2694  // Sort the list of polygon faces
2695  std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2696 
2697  polygonsTmp.resize(listOfPolygonFaces.size());
2698  roisPtTmp.resize(listOfPolygonFaces.size());
2699 
2700  size_t cpt = 0;
2701  for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2702  ++it, cpt++) {
2703  polygonsTmp[cpt] = it->polygon;
2704  roisPtTmp[cpt] = it->faceCorners;
2705  }
2706 
2707  pairOfPolygonFaces.first = polygonsTmp;
2708  pairOfPolygonFaces.second = roisPtTmp;
2709  }
2710  else {
2711  pairOfPolygonFaces.first = polygonsTmp;
2712  pairOfPolygonFaces.second = roisPtTmp;
2713  }
2714 
2715  return pairOfPolygonFaces;
2716 }
2717 
2727 {
2728  useOgre = v;
2729  if (useOgre) {
2730 #ifndef VISP_HAVE_OGRE
2731  useOgre = false;
2732  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2733  "will be used. setOgreVisibilityTest() set to false."
2734  << std::endl;
2735 #endif
2736  }
2737 }
2738 
2744 void vpMbTracker::setFarClippingDistance(const double &dist)
2745 {
2747  vpTRACE("Far clipping value cannot be inferior than near clipping value. "
2748  "Far clipping won't be considered.");
2749  else if (dist < 0)
2750  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
2751  "won't be considered.");
2752  else {
2754  distFarClip = dist;
2755  for (unsigned int i = 0; i < faces.size(); i++) {
2756  faces[i]->setFarClippingDistance(distFarClip);
2757  }
2758 #ifdef VISP_HAVE_OGRE
2760 #endif
2761  }
2762 }
2763 
2774 void vpMbTracker::setLod(bool useLod, const std::string &name)
2775 {
2776  for (unsigned int i = 0; i < faces.size(); i++) {
2777  if (name.empty() || faces[i]->name == name) {
2778  faces[i]->setLod(useLod);
2779  }
2780  }
2781 }
2782 
2792 void vpMbTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
2793 {
2794  for (unsigned int i = 0; i < faces.size(); i++) {
2795  if (name.empty() || faces[i]->name == name) {
2796  faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2797  }
2798  }
2799 }
2800 
2809 void vpMbTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
2810 {
2811  for (unsigned int i = 0; i < faces.size(); i++) {
2812  if (name.empty() || faces[i]->name == name) {
2813  faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2814  }
2815  }
2816 }
2817 
2824 {
2826  vpTRACE("Near clipping value cannot be superior than far clipping value. "
2827  "Near clipping won't be considered.");
2828  else if (dist < 0)
2829  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
2830  "won't be considered.");
2831  else {
2833  distNearClip = dist;
2834  for (unsigned int i = 0; i < faces.size(); i++) {
2835  faces[i]->setNearClippingDistance(distNearClip);
2836  }
2837 #ifdef VISP_HAVE_OGRE
2839 #endif
2840  }
2841 }
2842 
2850 void vpMbTracker::setClipping(const unsigned int &flags)
2851 {
2852  clippingFlag = flags;
2853  for (unsigned int i = 0; i < faces.size(); i++)
2855 }
2856 
2857 void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true,
2858  const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
2859  const vpMatrix &LVJ_true, const vpColVector &error)
2860 {
2861  if (computeCovariance) {
2862  vpMatrix D;
2863  D.diag(w_true);
2864 
2865  // Note that here the covariance is computed on cMoPrev for time
2866  // computation efficiency
2867  if (isoJoIdentity) {
2868  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
2869  }
2870  else {
2871  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
2872  }
2873  }
2874 }
2875 
2888 void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
2889 {
2890  if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
2891  throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
2892  }
2893 
2894  JTR.resize(6, false);
2895 #if defined(VISP_HAVE_SIMDLIB)
2896  SimdComputeJtR(interaction.data, interaction.getRows(), error.data, JTR.data);
2897 #else
2898  const unsigned int N = interaction.getRows();
2899 
2900  for (unsigned int i = 0; i < 6; i += 1) {
2901  double ssum = 0;
2902  for (unsigned int j = 0; j < N; j += 1) {
2903  ssum += interaction[j][i] * error[j];
2904  }
2905  JTR[i] = ssum;
2906  }
2907 #endif
2908 }
2909 
2911  const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
2912  double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
2913  const vpColVector *const m_w_prev)
2914 {
2916  if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
2917  mu *= 10.0;
2918 
2919  if (mu > 1.0)
2920  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
2921 
2922  m_cMo = cMoPrev;
2923  error = m_error_prev;
2924  if (w != nullptr && m_w_prev != nullptr) {
2925  *w = *m_w_prev;
2926  }
2927  reStartFromLastIncrement = true;
2928  }
2929  }
2930 }
2931 
2932 void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix &LTL,
2933  vpColVector &R, const vpColVector &error, vpColVector &error_prev,
2934  vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w,
2935  vpColVector *const m_w_prev)
2936 {
2937  if (isoJoIdentity) {
2938  LTL = L.AtA();
2939  computeJTR(L, R, LTR);
2940 
2941  switch (m_optimizationMethod) {
2943  vpMatrix LMA(LTL.getRows(), LTL.getCols());
2944  LMA.eye();
2945  vpMatrix LTLmuI = LTL + (LMA * mu);
2946  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2947 
2948  if (iter != 0)
2949  mu /= 10.0;
2950 
2951  error_prev = error;
2952  if (w != nullptr && m_w_prev != nullptr)
2953  *m_w_prev = *w;
2954  break;
2955  }
2956 
2958  default:
2959  v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2960  break;
2961  }
2962  }
2963  else {
2965  cVo.buildFrom(m_cMo);
2966  vpMatrix LVJ = (L * (cVo * oJo));
2967  vpMatrix LVJTLVJ = (LVJ).AtA();
2968  vpColVector LVJTR;
2969  computeJTR(LVJ, R, LVJTR);
2970 
2971  switch (m_optimizationMethod) {
2973  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
2974  LMA.eye();
2975  vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2976  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2977  v = cVo * v;
2978 
2979  if (iter != 0)
2980  mu /= 10.0;
2981 
2982  error_prev = error;
2983  if (w != nullptr && m_w_prev != nullptr)
2984  *m_w_prev = *w;
2985  break;
2986  }
2988  default:
2989  v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2990  v = cVo * v;
2991  break;
2992  }
2993  }
2994 }
2995 
2997 {
2998  if (error.getRows() > 0)
2999  robust.MEstimator(vpRobust::TUKEY, error, w);
3000 }
3001 
3014 {
3015  vpColVector v(6);
3016  for (unsigned int i = 0; i < 6; i++)
3017  v[i] = oJo[i][i];
3018  return v;
3019 }
3020 
3037 {
3038  if (v.getRows() == 6) {
3039  m_isoJoIdentity = true;
3040  for (unsigned int i = 0; i < 6; i++) {
3041  // if(v[i] != 0){
3042  if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3043  oJo[i][i] = 1.0;
3044  }
3045  else {
3046  oJo[i][i] = 0.0;
3047  m_isoJoIdentity = false;
3048  }
3049  }
3050  }
3051 }
3052 
3053 void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
3054  std::vector<std::vector<vpPoint> > &listFaces)
3055 {
3056  listFaces.clear();
3057 
3058  // std::vector<vpPoint> revolutionAxis;
3059  // revolutionAxis.push_back(p1);
3060  // revolutionAxis.push_back(p2);
3061  // listFaces.push_back(revolutionAxis);
3062 
3063  vpColVector axis(3);
3064  axis[0] = p1.get_oX() - p2.get_oX();
3065  axis[1] = p1.get_oY() - p2.get_oY();
3066  axis[2] = p1.get_oZ() - p2.get_oZ();
3067 
3068  vpColVector randomVec(3);
3069  randomVec = 0;
3070 
3071  vpColVector axisOrtho(3);
3072 
3073  randomVec[0] = 1.0;
3074  axisOrtho = vpColVector::crossProd(axis, randomVec);
3075 
3076  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3077  randomVec = 0;
3078  randomVec[1] = 1.0;
3079  axisOrtho = vpColVector::crossProd(axis, randomVec);
3080  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3081  randomVec = 0;
3082  randomVec[2] = 1.0;
3083  axisOrtho = vpColVector::crossProd(axis, randomVec);
3084  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon())
3085  throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
3086  }
3087  }
3088 
3089  axisOrtho.normalize();
3090 
3091  vpColVector axisOrthoBis(3);
3092  axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
3093  axisOrthoBis.normalize();
3094 
3095  // First circle
3096  vpColVector p1Vec(3);
3097  p1Vec[0] = p1.get_oX();
3098  p1Vec[1] = p1.get_oY();
3099  p1Vec[2] = p1.get_oZ();
3100  vpColVector fc1 = p1Vec + axisOrtho * radius;
3101  vpColVector fc2 = p1Vec + axisOrthoBis * radius;
3102  vpColVector fc3 = p1Vec - axisOrtho * radius;
3103  vpColVector fc4 = p1Vec - axisOrthoBis * radius;
3104 
3105  vpColVector p2Vec(3);
3106  p2Vec[0] = p2.get_oX();
3107  p2Vec[1] = p2.get_oY();
3108  p2Vec[2] = p2.get_oZ();
3109  vpColVector sc1 = p2Vec + axisOrtho * radius;
3110  vpColVector sc2 = p2Vec + axisOrthoBis * radius;
3111  vpColVector sc3 = p2Vec - axisOrtho * radius;
3112  vpColVector sc4 = p2Vec - axisOrthoBis * radius;
3113 
3114  std::vector<vpPoint> pointsFace;
3115  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3116  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3117  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3118  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3119  listFaces.push_back(pointsFace);
3120 
3121  pointsFace.clear();
3122  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3123  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3124  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3125  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3126  listFaces.push_back(pointsFace);
3127 
3128  pointsFace.clear();
3129  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3130  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3131  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3132  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3133  listFaces.push_back(pointsFace);
3134 
3135  pointsFace.clear();
3136  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3137  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3138  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3139  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3140  listFaces.push_back(pointsFace);
3141 }
3142 
3152 bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
3153 {
3154  double dx = fabs(P1.get_oX() - P2.get_oX());
3155  double dy = fabs(P1.get_oY() - P2.get_oY());
3156  double dz = fabs(P1.get_oZ() - P2.get_oZ());
3157 
3158  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3159  dz <= std::numeric_limits<double>::epsilon())
3160  return true;
3161  else
3162  return false;
3163 }
3164 
3165 void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, int idFace,
3166  const std::string &polygonName, bool useLod, double minPolygonAreaThreshold,
3167  double minLineLengthThreshold)
3168 {
3169  std::vector<vpPoint> corners_without_duplicates;
3170  corners_without_duplicates.push_back(corners[0]);
3171  for (unsigned int i = 0; i < corners.size() - 1; i++) {
3172  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3173  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3174  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3175  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3176  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3177  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3178  corners_without_duplicates.push_back(corners[i + 1]);
3179  }
3180  }
3181 
3182  vpMbtPolygon polygon;
3183  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
3184  polygon.setIndex((int)idFace);
3185  polygon.setName(polygonName);
3186  polygon.setLod(useLod);
3187 
3188  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3189  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3190 
3191  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3192  polygon.addPoint(j, corners_without_duplicates[j]);
3193  }
3194 
3196 
3198  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3199 
3201  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3202 
3204  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3205 }
3206 
3207 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3208  int idFace, const std::string &polygonName, bool useLod,
3209  double minPolygonAreaThreshold)
3210 {
3211  vpMbtPolygon polygon;
3212  polygon.setNbPoint(4);
3213  polygon.setName(polygonName);
3214  polygon.setLod(useLod);
3215 
3216  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3217  // Non sense to set minLineLengthThreshold for circle
3218  // but used to be coherent when applying LOD settings for all polygons
3220 
3221  {
3222  // Create the 4 points of the circle bounding box
3223  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
3224 
3225  // Matrice de passage entre world et circle frame
3226  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
3227  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
3228  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
3229  vpRotationMatrix wRc;
3230  vpColVector x(3), y(3), z(3);
3231  // X axis is P2-P1
3232  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
3233  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
3234  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
3235  // Y axis is the normal of the plane
3236  y[0] = plane.getA() / norm_Y;
3237  y[1] = plane.getB() / norm_Y;
3238  y[2] = plane.getC() / norm_Y;
3239  // Z axis = X ^ Y
3240  z = vpColVector::crossProd(x, y);
3241  for (unsigned int i = 0; i < 3; i++) {
3242  wRc[i][0] = x[i];
3243  wRc[i][1] = y[i];
3244  wRc[i][2] = z[i];
3245  }
3246 
3247  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
3248  vpHomogeneousMatrix wMc(wtc, wRc);
3249 
3250  vpColVector c_p(4); // A point in the circle frame that is on the bbox
3251  c_p[0] = radius;
3252  c_p[1] = 0;
3253  c_p[2] = radius;
3254  c_p[3] = 1;
3255 
3256  // Matrix to rotate a point by 90 deg around Y in the circle frame
3257  for (unsigned int i = 0; i < 4; i++) {
3258  vpColVector w_p(4); // A point in the word frame
3260  w_p = wMc * cMc_90 * c_p;
3261 
3262  vpPoint w_P;
3263  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
3264 
3265  polygon.addPoint(i, w_P);
3266  }
3267  }
3268 
3269  polygon.setIndex(idFace);
3271 
3273  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3274 
3276  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3277 
3279  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3280 }
3281 
3282 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace,
3283  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3284 {
3285  // A polygon as a single line that corresponds to the revolution axis of the
3286  // cylinder
3287  vpMbtPolygon polygon;
3288  polygon.setNbPoint(2);
3289 
3290  polygon.addPoint(0, p1);
3291  polygon.addPoint(1, p2);
3292 
3293  polygon.setIndex(idFace);
3294  polygon.setName(polygonName);
3295  polygon.setLod(useLod);
3296 
3297  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3298  // Non sense to set minPolygonAreaThreshold for cylinder
3299  // but used to be coherent when applying LOD settings for all polygons
3301 
3303 
3305  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3306 
3308  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3309 
3311  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3312 }
3313 
3314 void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
3315  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3316 {
3317  int id = idFace;
3318  for (unsigned int i = 0; i < listFaces.size(); i++) {
3319  vpMbtPolygon polygon;
3320  polygon.setNbPoint((unsigned int)listFaces[i].size());
3321  for (unsigned int j = 0; j < listFaces[i].size(); j++)
3322  polygon.addPoint(j, listFaces[i][j]);
3323 
3324  polygon.setIndex(id);
3325  polygon.setName(polygonName);
3326  polygon.setIsPolygonOriented(false);
3327  polygon.setLod(useLod);
3328  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3330 
3332 
3334  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3335 
3337  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3338 
3340  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3341 
3342  id++;
3343  }
3344 }
3345 
3346 void vpMbTracker::addProjectionErrorLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
3347 {
3348  // suppress line already in the model
3349  bool already_here = false;
3350  vpMbtDistanceLine *l;
3351 
3352  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3353  it != m_projectionErrorLines.end(); ++it) {
3354  l = *it;
3355  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
3356  already_here = true;
3357  l->addPolygon(polygon);
3359  }
3360  }
3361 
3362  if (!already_here) {
3363  l = new vpMbtDistanceLine;
3364 
3366  l->buildFrom(P1, P2, m_rand);
3367  l->addPolygon(polygon);
3370  l->useScanLine = useScanLine;
3371 
3372  l->setIndex((unsigned int)m_projectionErrorLines.size());
3373  l->setName(name);
3374 
3377 
3380 
3383 
3384  m_projectionErrorLines.push_back(l);
3385  }
3386 }
3387 
3388 void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
3389  int idFace, const std::string &name)
3390 {
3391  bool already_here = false;
3392  vpMbtDistanceCircle *ci;
3393 
3394  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3395  it != m_projectionErrorCircles.end(); ++it) {
3396  ci = *it;
3397  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
3398  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
3399  already_here =
3400  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
3401  }
3402  }
3403 
3404  if (!already_here) {
3405  ci = new vpMbtDistanceCircle;
3406 
3408  ci->buildFrom(P1, P2, P3, r);
3410  ci->setIndex((unsigned int)m_projectionErrorCircles.size());
3411  ci->setName(name);
3412  ci->index_polygon = idFace;
3414 
3415  m_projectionErrorCircles.push_back(ci);
3416  }
3417 }
3418 
3419 void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace,
3420  const std::string &name)
3421 {
3422  bool already_here = false;
3424 
3425  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3426  it != m_projectionErrorCylinders.end(); ++it) {
3427  cy = *it;
3428  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
3429  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
3430  already_here =
3431  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
3432  }
3433  }
3434 
3435  if (!already_here) {
3436  cy = new vpMbtDistanceCylinder;
3437 
3439  cy->buildFrom(P1, P2, r);
3441  cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
3442  cy->setName(name);
3443  cy->index_polygon = idFace;
3445  m_projectionErrorCylinders.push_back(cy);
3446  }
3447 }
3448 
3449 void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3450  int idFace, const std::string &name)
3451 {
3452  addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
3453 }
3454 
3455 void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
3456  const std::string &name)
3457 {
3458  addProjectionErrorCylinder(p1, p2, radius, idFace, name);
3459 }
3460 
3462 {
3463  unsigned int nbpt = polygon.getNbPoint();
3464  if (nbpt > 0) {
3465  for (unsigned int i = 0; i < nbpt - 1; i++)
3466  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3467  addProjectionErrorLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
3468  }
3469 }
3470 
3472 {
3473  unsigned int nbpt = polygon.getNbPoint();
3474  if (nbpt > 0) {
3475  for (unsigned int i = 0; i < nbpt - 1; i++)
3476  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3477  }
3478 }
3479 
3498  const vpCameraParameters &_cam)
3499 {
3500  if (!modelInitialised) {
3501  throw vpException(vpException::fatalError, "model not initialized");
3502  }
3503 
3504  unsigned int nbFeatures = 0;
3505  double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);
3506 
3507  if (nbFeatures > 0) {
3508  return vpMath::deg(totalProjectionError / (double)nbFeatures);
3509  }
3510 
3511  return 90.0;
3512 }
3513 
3515  const vpCameraParameters &_cam, unsigned int &nbFeatures)
3516 {
3517  bool update_cam = m_projectionErrorCam != _cam;
3518  if (update_cam) {
3519  m_projectionErrorCam = _cam;
3520 
3521  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3522  it != m_projectionErrorLines.end(); ++it) {
3523  vpMbtDistanceLine *l = *it;
3525  }
3526 
3527  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3528  it != m_projectionErrorCylinders.end(); ++it) {
3529  vpMbtDistanceCylinder *cy = *it;
3531  }
3532 
3533  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3534  it != m_projectionErrorCircles.end(); ++it) {
3535  vpMbtDistanceCircle *ci = *it;
3537  }
3538  }
3539 
3540 #ifdef VISP_HAVE_OGRE
3541  if (useOgre) {
3542  if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
3546  // Turn off Ogre config dialog display for the next call to this
3547  // function since settings are saved in the ogre.cfg file and used
3548  // during the next call
3550  }
3551  }
3552 #endif
3553 
3554  if (clippingFlag > 2)
3556 
3558 
3560 
3561  if (useScanLine) {
3562  if (clippingFlag <= 2)
3564 
3567  }
3568 
3570 
3571  double totalProjectionError = 0.0;
3572  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3573  it != m_projectionErrorLines.end(); ++it) {
3574  vpMbtDistanceLine *l = *it;
3575  if (l->isVisible() && l->isTracked()) {
3576  for (size_t a = 0; a < l->meline.size(); a++) {
3577  if (l->meline[a] != nullptr) {
3578  double lineNormGradient;
3579  unsigned int lineNbFeatures;
3580  l->meline[a]->computeProjectionError(I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
3583  totalProjectionError += lineNormGradient;
3584  nbFeatures += lineNbFeatures;
3585  }
3586  }
3587  }
3588  }
3589 
3590  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3591  it != m_projectionErrorCylinders.end(); ++it) {
3592  vpMbtDistanceCylinder *cy = *it;
3593  if (cy->isVisible() && cy->isTracked()) {
3594  if (cy->meline1 != nullptr) {
3595  double cylinderNormGradient = 0;
3596  unsigned int cylinderNbFeatures = 0;
3597  cy->meline1->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3600  totalProjectionError += cylinderNormGradient;
3601  nbFeatures += cylinderNbFeatures;
3602  }
3603 
3604  if (cy->meline2 != nullptr) {
3605  double cylinderNormGradient = 0;
3606  unsigned int cylinderNbFeatures = 0;
3607  cy->meline2->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3610  totalProjectionError += cylinderNormGradient;
3611  nbFeatures += cylinderNbFeatures;
3612  }
3613  }
3614  }
3615 
3616  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3617  it != m_projectionErrorCircles.end(); ++it) {
3618  vpMbtDistanceCircle *c = *it;
3619  if (c->isVisible() && c->isTracked() && c->meEllipse != nullptr) {
3620  double circleNormGradient = 0;
3621  unsigned int circleNbFeatures = 0;
3622  c->meEllipse->computeProjectionError(I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
3625  totalProjectionError += circleNormGradient;
3626  nbFeatures += circleNbFeatures;
3627  }
3628  }
3629 
3630  return totalProjectionError;
3631 }
3632 
3633 void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
3634 {
3635  bool changed = false;
3636 
3637  if (!useOgre) {
3639  changed);
3640  }
3641  else {
3642 #ifdef VISP_HAVE_OGRE
3644  changed);
3645 #else
3647  changed);
3648 #endif
3649  }
3650 }
3651 
3653 {
3654  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3655  it != m_projectionErrorLines.end(); ++it) {
3656  for (size_t a = 0; a < (*it)->meline.size(); a++) {
3657  if ((*it)->meline[a] != nullptr) {
3658  delete (*it)->meline[a];
3659  (*it)->meline[a] = nullptr;
3660  }
3661  }
3662 
3663  (*it)->meline.clear();
3664  (*it)->nbFeature.clear();
3665  (*it)->nbFeatureTotal = 0;
3666  }
3667 
3668  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3669  it != m_projectionErrorCylinders.end(); ++it) {
3670  if ((*it)->meline1 != nullptr) {
3671  delete (*it)->meline1;
3672  (*it)->meline1 = nullptr;
3673  }
3674  if ((*it)->meline2 != nullptr) {
3675  delete (*it)->meline2;
3676  (*it)->meline2 = nullptr;
3677  }
3678 
3679  (*it)->nbFeature = 0;
3680  (*it)->nbFeaturel1 = 0;
3681  (*it)->nbFeaturel2 = 0;
3682  }
3683 
3684  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3685  it != m_projectionErrorCircles.end(); ++it) {
3686  if ((*it)->meEllipse != nullptr) {
3687  delete (*it)->meEllipse;
3688  (*it)->meEllipse = nullptr;
3689  }
3690  (*it)->nbFeature = 0;
3691  }
3692 }
3693 
3695 {
3696  const bool doNotTrack = true;
3697 
3698  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3699  it != m_projectionErrorLines.end(); ++it) {
3700  vpMbtDistanceLine *l = *it;
3701  bool isvisible = false;
3702 
3703  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
3704  ++itindex) {
3705  int index = *itindex;
3706  if (index == -1)
3707  isvisible = true;
3708  else {
3709  if (l->hiddenface->isVisible((unsigned int)index))
3710  isvisible = true;
3711  }
3712  }
3713 
3714  // Si la ligne n'appartient a aucune face elle est tout le temps visible
3715  if (l->Lindex_polygon.empty())
3716  isvisible = true; // Not sure that this can occur
3717 
3718  if (isvisible) {
3719  l->setVisible(true);
3720  l->updateTracked();
3721  if (l->meline.empty() && l->isTracked())
3722  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3723  }
3724  else {
3725  l->setVisible(false);
3726  for (size_t a = 0; a < l->meline.size(); a++) {
3727  if (l->meline[a] != nullptr)
3728  delete l->meline[a];
3729  if (a < l->nbFeature.size())
3730  l->nbFeature[a] = 0;
3731  }
3732  l->nbFeatureTotal = 0;
3733  l->meline.clear();
3734  l->nbFeature.clear();
3735  }
3736  }
3737 
3738  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3739  it != m_projectionErrorCylinders.end(); ++it) {
3740  vpMbtDistanceCylinder *cy = *it;
3741 
3742  bool isvisible = false;
3743 
3744  int index = cy->index_polygon;
3745  if (index == -1)
3746  isvisible = true;
3747  else {
3748  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
3749  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
3750  isvisible = true;
3751  }
3752 
3753  if (isvisible) {
3754  cy->setVisible(true);
3755  if (cy->meline1 == nullptr || cy->meline2 == nullptr) {
3756  if (cy->isTracked())
3757  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3758  }
3759  }
3760  else {
3761  cy->setVisible(false);
3762  if (cy->meline1 != nullptr)
3763  delete cy->meline1;
3764  if (cy->meline2 != nullptr)
3765  delete cy->meline2;
3766  cy->meline1 = nullptr;
3767  cy->meline2 = nullptr;
3768  cy->nbFeature = 0;
3769  cy->nbFeaturel1 = 0;
3770  cy->nbFeaturel2 = 0;
3771  }
3772  }
3773 
3774  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3775  it != m_projectionErrorCircles.end(); ++it) {
3776  vpMbtDistanceCircle *ci = *it;
3777  bool isvisible = false;
3778 
3779  int index = ci->index_polygon;
3780  if (index == -1)
3781  isvisible = true;
3782  else {
3783  if (ci->hiddenface->isVisible((unsigned int)index))
3784  isvisible = true;
3785  }
3786 
3787  if (isvisible) {
3788  ci->setVisible(true);
3789  if (ci->meEllipse == nullptr) {
3790  if (ci->isTracked())
3791  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3792  }
3793  }
3794  else {
3795  ci->setVisible(false);
3796  if (ci->meEllipse != nullptr)
3797  delete ci->meEllipse;
3798  ci->meEllipse = nullptr;
3799  ci->nbFeature = 0;
3800  }
3801  }
3802 }
3803 
3804 void vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose)
3805 {
3806 #if defined(VISP_HAVE_PUGIXML)
3808  xmlp.setVerbose(verbose);
3811 
3812  try {
3813  if (verbose) {
3814  std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl;
3815  }
3816  xmlp.parse(configFile);
3817  }
3818  catch (...) {
3819  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
3820  }
3821 
3822  vpMe meParser;
3823  xmlp.getProjectionErrorMe(meParser);
3824 
3825  setProjectionErrorMovingEdge(meParser);
3827 #else
3828  (void)configFile;
3829  (void)verbose;
3830  throw(vpException(vpException::ioError, "vpMbTracker::loadConfigFile() needs pugixml built-in 3rdparty"));
3831 #endif
3832 }
3833 
3840 {
3841  m_projectionErrorMe = me;
3842 
3843  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3844  it != m_projectionErrorLines.end(); ++it) {
3845  vpMbtDistanceLine *l = *it;
3847  }
3848 
3849  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3850  it != m_projectionErrorCylinders.end(); ++it) {
3851  vpMbtDistanceCylinder *cy = *it;
3853  }
3854 
3855  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3856  it != m_projectionErrorCircles.end(); ++it) {
3857  vpMbtDistanceCircle *ci = *it;
3859  }
3860 }
3861 
3867 void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
3868 {
3870 
3871  m_SobelX.resize(size * 2 + 1, size * 2 + 1, false, false);
3873 
3874  m_SobelY.resize(size * 2 + 1, size * 2 + 1, false, false);
3876 }
void setFarClippingDistance(const double &dist)
Definition: vpAROgre.h:199
void setNearClippingDistance(const double &dist)
Definition: vpAROgre.h:210
unsigned int getCols() const
Definition: vpArray2D.h:274
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int getRows() const
Definition: vpArray2D.h:284
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & normalize()
double sumSquare() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
void clear()
Definition: vpColVector.h:262
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
static const vpColor red
Definition: vpColor.h:211
static const vpColor green
Definition: vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
Class that defines generic functionalities for display.
Definition: vpDisplay.h:173
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
int getWindowXPosition() const
Definition: vpDisplay.h:247
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
int getWindowYPosition() const
Definition: vpDisplay.h:252
static void flush(const vpImage< unsigned char > &I)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static FilterType getSobelKernelX(FilterType *filter, unsigned int size)
static FilterType getSobelKernelY(FilterType *filter, unsigned int size)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:143
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:184
vpDisplay * display
Definition: vpImage.h:140
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:2377
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:1421
static std::string getAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:2105
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:1204
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:2188
static std::string trim(std::string s)
Definition: vpIoTools.cpp:2643
static bool parseBoolean(std::string input)
Definition: vpIoTools.cpp:2629
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:2147
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:2038
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1933
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:109
static double rad(double deg)
Definition: vpMath.h:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static double sqr(double x)
Definition: vpMath.h:201
static double deg(double rad)
Definition: vpMath.h:117
error that can be emitted by the vpMatrix class and its derivatives
@ incorrectMatrixSizeError
Incorrect matrix size.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void eye()
Definition: vpMatrix.cpp:442
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2286
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int size() const
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void addPolygon(PolygonType *p)
std::vector< PolygonType * > & getPolygon()
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setOgreShowConfigDialog(bool showConfigDialog)
Main methods for a model-based tracker.
Definition: vpMbTracker.h:105
std::map< std::string, std::string > parseParameters(std::string &endLine)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual vpColVector getEstimatedDoF() const
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
virtual void setEstimatedDoF(const vpColVector &v)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
Definition: vpMbTracker.h:219
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
Definition: vpMbTracker.h:166
void removeComment(std::ifstream &fileId)
bool modelInitialised
Definition: vpMbTracker.h:123
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
void projectionErrorResetMovingEdges()
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
@ LEVENBERG_MARQUARDT_OPT
Definition: vpMbTracker.h:107
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
Definition: vpMbTracker.h:198
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void init(const vpImage< unsigned char > &I)=0
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
std::map< std::string, std::string > mapOfParameterNames
Definition: vpMbTracker.h:182
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void loadVRMLModel(const std::string &modelFile)
unsigned int nbLines
Number of lines in CAO model.
Definition: vpMbTracker.h:162
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void savePose(const std::string &filename) const
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
Definition: vpMbTracker.h:227
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
unsigned int nbPoints
Number of points in CAO model.
Definition: vpMbTracker.h:160
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
std::string modelFileName
Definition: vpMbTracker.h:120
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
Definition: vpMbTracker.h:200
virtual void setOgreVisibilityTest(const bool &v)
std::string poseSavingFilename
Definition: vpMbTracker.h:126
void setProjectionErrorKernelSize(const unsigned int &size)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
Definition: vpMbTracker.h:164
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
virtual void computeVVSPoseEstimation(const bool isoJoIdentity, 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=nullptr, vpColVector *const m_w_prev=nullptr)
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual void setNearClippingDistance(const double &dist)
void setProjectionErrorMovingEdge(const vpMe &me)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
virtual ~vpMbTracker()
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
bool m_projectionErrorOgreShowConfigDialog
Definition: vpMbTracker.h:203
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
Definition: vpMbTracker.h:196
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
bool m_sodb_init_called
Flag that indicates that SoDB::init(); was called.
Definition: vpMbTracker.h:225
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
unsigned int nbCylinders
Number of cylinders in CAO model.
Definition: vpMbTracker.h:168
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
Definition: vpMbTracker.h:207
unsigned int nbCircles
Number of circles in CAO model.
Definition: vpMbTracker.h:170
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
Definition: vpMbTracker.h:205
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:202
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void setCameraParameters(const vpCameraParameters &camera)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
void setIndex(unsigned int i)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
Manage a cylinder used in the model-based tracker.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
unsigned int nbFeaturel2
The number of moving edges on line 2.
vpPoint * p2
The second extremity on the axe.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
bool isTracked() const
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
vpMbtPolygon & getPolygon()
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:58
void setMinPolygonAreaThresh(double min_polygon_area)
Definition: vpMbtPolygon.h:142
std::string getName() const
Definition: vpMbtPolygon.h:98
void setName(const std::string &face_name)
Definition: vpMbtPolygon.h:149
void setLod(bool use_lod)
virtual void setIndex(int i)
Definition: vpMbtPolygon.h:114
void setMinLineLengthThresh(double min_line_length)
Definition: vpMbtPolygon.h:131
void setIsPolygonOriented(const bool &oriented)
Definition: vpMbtPolygon.h:156
int getIndex() const
Definition: vpMbtPolygon.h:91
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setProjectionErrorMe(const vpMe &me)
unsigned int getProjectionErrorKernelSize() const
void setProjectionErrorKernelSize(const unsigned int &size)
void parse(const std::string &filename)
void getProjectionErrorMe(vpMe &me) const
Definition: vpMe.h:124
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
@ object_frame
Definition: vpPlane.h:65
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:450
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:500
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:454
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:452
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:502
Implements a 3D polygon with render functionalities like clipping.
Definition: vpPolygon3D.h:55
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:189
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:127
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:202
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:76
virtual void setNbPoint(unsigned int nb)
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:182
void addPoint(unsigned int n, const vpPoint &P)
Defines a generic 2D polygon.
Definition: vpPolygon.h:97
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:93
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:99
void clearPoint()
Definition: vpPose.cpp:86
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
Definition: vpPose.cpp:333
Implementation of a rotation vector as quaternion angle minimal representation.
Contains an M-estimator and various influence function.
Definition: vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:136
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382