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