Visual Servoing Platform  version 3.3.1 under development (2020-10-25)
vpMbTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Generic model based tracker
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  * Eric Marchand
38  *
39  *****************************************************************************/
40 
46 #include <algorithm>
47 #include <iostream>
48 #include <limits>
49 
50 #include <visp3/core/vpColVector.h>
51 #include <visp3/core/vpDisplay.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpMatrix.h>
54 #include <visp3/core/vpPoint.h>
55 #include <visp3/vision/vpPose.h>
56 #ifdef VISP_HAVE_MODULE_GUI
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayOpenCV.h>
59 #include <visp3/gui/vpDisplayX.h>
60 #endif
61 #include <visp3/core/vpCameraParameters.h>
62 #include <visp3/core/vpColor.h>
63 #include <visp3/core/vpException.h>
64 #include <visp3/core/vpIoTools.h>
65 #include <visp3/core/vpPixelMeterConversion.h>
66 #ifdef VISP_HAVE_MODULE_IO
67 #include <visp3/io/vpImageIo.h>
68 #endif
69 #include <visp3/core/vpCPUFeatures.h>
70 #include <visp3/core/vpIoTools.h>
71 #include <visp3/core/vpMatrixException.h>
72 #include <visp3/core/vpTrackingException.h>
73 #include <visp3/mbt/vpMbTracker.h>
74 
75 #include <visp3/core/vpImageFilter.h>
76 #include <visp3/mbt/vpMbtXmlGenericParser.h>
77 
78 #ifdef VISP_HAVE_COIN3D
79 // Inventor includes
80 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
81 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
82 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
83 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
84 #include <Inventor/VRMLnodes/SoVRMLShape.h>
85 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
86 #include <Inventor/actions/SoGetMatrixAction.h>
87 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
88 #include <Inventor/actions/SoSearchAction.h>
89 #include <Inventor/actions/SoToVRML2Action.h>
90 #include <Inventor/actions/SoWriteAction.h>
91 #include <Inventor/misc/SoChildList.h>
92 #include <Inventor/nodes/SoSeparator.h>
93 #endif
94 
95 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
96 #include <emmintrin.h>
97 #define VISP_HAVE_SSE2 1
98 #endif
99 
100 #ifndef DOXYGEN_SHOULD_SKIP_THIS
101 
102 namespace
103 {
107 struct SegmentInfo {
108  SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
109 
110  std::vector<vpPoint> extremities;
111  std::string name;
112  bool useLod;
113  double minLineLengthThresh;
114 };
115 
120 struct PolygonFaceInfo {
121  PolygonFaceInfo(double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
122  : distanceToCamera(dist), polygon(poly), faceCorners(corners)
123  {
124  }
125 
126  bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }
127 
128  double distanceToCamera;
129  vpPolygon polygon;
130  std::vector<vpPoint> faceCorners;
131 };
132 
140 std::istream& safeGetline(std::istream& is, std::string& t)
141 {
142  t.clear();
143 
144  // The characters in the stream are read one-by-one using a std::streambuf.
145  // That is faster than reading them one-by-one using the std::istream.
146  // Code that uses streambuf this way must be guarded by a sentry object.
147  // The sentry object performs various tasks,
148  // such as thread synchronization and updating the stream state.
149 
150  std::istream::sentry se(is, true);
151  std::streambuf* sb = is.rdbuf();
152 
153  for(;;) {
154  int c = sb->sbumpc();
155  if (c == '\n') {
156  return is;
157  }
158  else if (c == '\r') {
159  if(sb->sgetc() == '\n')
160  sb->sbumpc();
161  return is;
162  }
163  else if (c == std::streambuf::traits_type::eof()) {
164  // Also handle the case when the last line has no line ending
165  if(t.empty())
166  is.setstate(std::ios::eofbit);
167  return is;
168  }
169  else { // default case
170  t += (char)c;
171  }
172  }
173 }
174 }
175 #endif // DOXYGEN_SHOULD_SKIP_THIS
176 
183  : m_cam(), m_cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
184  computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
185  displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
186  angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
187  useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
188  nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
189  minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
190  m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
191  m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(),
192  m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
193  m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
194  m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
195  m_projectionErrorCam(), m_mask(NULL), m_I()
196 {
197  oJo.eye();
198  // Map used to parse additional information in CAO model files,
199  // like name of faces or LOD setting
200  mapOfParameterNames["name"] = "string";
201  mapOfParameterNames["minPolygonAreaThreshold"] = "number";
202  mapOfParameterNames["minLineLengthThreshold"] = "number";
203  mapOfParameterNames["useLod"] = "boolean";
204 
207 }
208 
210  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
211  vpMbtDistanceLine *l = *it;
212  if (l != NULL)
213  delete l;
214  l = NULL;
215  }
216 
217  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end(); ++it) {
218  vpMbtDistanceCylinder *cy = *it;
219  if (cy != NULL)
220  delete cy;
221  cy = NULL;
222  }
223 
224  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
225  vpMbtDistanceCircle *ci = *it;
226  if (ci != NULL)
227  delete ci;
228  ci = NULL;
229  }
230 }
231 
232 #ifdef VISP_HAVE_MODULE_GUI
233 void vpMbTracker::initClick(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
234  const std::string &initFile, bool displayHelp, const vpHomogeneousMatrix &T)
235 {
236  vpHomogeneousMatrix last_cMo;
237  vpPoseVector init_pos;
238  vpImagePoint ip;
240 
241  std::string ext = ".init";
242  std::string str_pose = "";
243  size_t pos = initFile.rfind(ext);
244 
245  // Load the last poses from files
246  std::fstream finitpos;
247  std::ifstream finit;
248  char s[FILENAME_MAX];
249  if (poseSavingFilename.empty()) {
250  if (pos != std::string::npos)
251  str_pose = initFile.substr(0, pos) + ".0.pos";
252  else
253  str_pose = initFile + ".0.pos";
254 
255  finitpos.open(str_pose.c_str(), std::ios::in);
256  sprintf(s, "%s", str_pose.c_str());
257  } else {
258  finitpos.open(poseSavingFilename.c_str(), std::ios::in);
259  sprintf(s, "%s", poseSavingFilename.c_str());
260  }
261  if (finitpos.fail()) {
262  std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
263  last_cMo.eye();
264  } else {
265  for (unsigned int i = 0; i < 6; i += 1) {
266  finitpos >> init_pos[i];
267  }
268 
269  finitpos.close();
270  last_cMo.buildFrom(init_pos);
271 
272  std::cout << "last_cMo : " << std::endl << last_cMo << std::endl;
273 
274  if (I) {
275  vpDisplay::display(*I);
276  display(*I, last_cMo, m_cam, vpColor::green, 1, true);
277  vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green);
278  vpDisplay::flush(*I);
279  } else {
280  vpDisplay::display(*I_color);
281  display(*I_color, last_cMo, m_cam, vpColor::green, 1, true);
282  vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green);
283  vpDisplay::flush(*I_color);
284  }
285 
286  std::cout << "No modification : left click " << std::endl;
287  std::cout << "Modify initial pose : right click " << std::endl;
288 
289  if (I) {
290  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
291 
292  vpDisplay::flush(*I );
293 
294  while (!vpDisplay::getClick(*I, ip, button))
295  ;
296  } else {
297  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
298 
299  vpDisplay::flush(*I_color);
300 
301  while (!vpDisplay::getClick(*I_color, ip, button))
302  ;
303  }
304 
305  }
306 
307  if (!finitpos.fail() && button == vpMouseButton::button1) {
308  m_cMo = last_cMo;
309  } else {
310  vpDisplay *d_help = NULL;
311 
312  if (I) {
313  vpDisplay::display(*I);
314  vpDisplay::flush(*I);
315  }
316  else {
317  vpDisplay::display(*I_color);
318  vpDisplay::flush(*I_color);
319  }
320 
321  vpPose pose;
322 
323  pose.clearPoint();
324 
325  // file parser
326  // number of points
327  // X Y Z
328  // X Y Z
329  if (pos != std::string::npos)
330  sprintf(s, "%s", initFile.c_str());
331  else
332  sprintf(s, "%s.init", initFile.c_str());
333 
334  std::cout << "Load 3D points from: " << s << std::endl;
335  finit.open(s);
336  if (finit.fail()) {
337  std::cout << "cannot read " << s << std::endl;
338  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
339  }
340 
341 #ifdef VISP_HAVE_MODULE_IO
342  // Display window creation and initialisation
343  try {
344  if (displayHelp) {
345  const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"};
346  std::string dispF;
347  bool foundHelpImg = false;
348  if (pos != std::string::npos) {
349  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
350  dispF = initFile.substr(0, pos) + imgExtVec[i];
351  foundHelpImg = vpIoTools::checkFilename(dispF);
352  }
353  } else {
354  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
355  dispF = initFile + imgExtVec[i];
356  foundHelpImg = vpIoTools::checkFilename(dispF);
357  }
358  }
359 
360  if (foundHelpImg) {
361  std::cout << "Load image to help initialization: " << dispF << std::endl;
362 #if defined VISP_HAVE_X11
363  d_help = new vpDisplayX;
364 #elif defined VISP_HAVE_GDI
365  d_help = new vpDisplayGDI;
366 #elif defined VISP_HAVE_OPENCV
367  d_help = new vpDisplayOpenCV;
368 #endif
369 
370  vpImage<vpRGBa> Iref;
371  vpImageIo::read(Iref, dispF);
372 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
373  const int winXPos = I != NULL ? I->display->getWindowXPosition() : I_color->display->getWindowXPosition();
374  const int winYPos = I != NULL ? I->display->getWindowYPosition() : I_color->display->getWindowYPosition();
375  unsigned int width = I != NULL ? I->getWidth() : I_color->getWidth();
376  d_help->init(Iref, winXPos + (int)width + 80, winYPos,
377  "Where to initialize...");
378  vpDisplay::display(Iref);
379  vpDisplay::flush(Iref);
380 #endif
381  }
382  }
383  } catch (...) {
384  if (d_help != NULL) {
385  delete d_help;
386  d_help = NULL;
387  }
388  }
389 #else //#ifdef VISP_HAVE_MODULE_IO
390  (void)(displayHelp);
391 #endif //#ifdef VISP_HAVE_MODULE_IO
392  // skip lines starting with # as comment
393  removeComment(finit);
394 
395  unsigned int n3d;
396  finit >> n3d;
397  finit.ignore(256, '\n'); // skip the rest of the line
398  std::cout << "Number of 3D points " << n3d << std::endl;
399  if (n3d > 100000) {
400  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
401  }
402 
403  std::vector<vpPoint> P(n3d);
404  for (unsigned int i = 0; i < n3d; i++) {
405  // skip lines starting with # as comment
406  removeComment(finit);
407 
408  vpColVector pt_3d(4, 1.0);
409  finit >> pt_3d[0];
410  finit >> pt_3d[1];
411  finit >> pt_3d[2];
412  finit.ignore(256, '\n'); // skip the rest of the line
413 
414  vpColVector pt_3d_tf = T*pt_3d;
415  std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " " << pt_3d_tf[2] << std::endl;
416 
417  P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
418  }
419 
420  finit.close();
421 
422  bool isWellInit = false;
423  while (!isWellInit) {
424  std::vector<vpImagePoint> mem_ip;
425  for (unsigned int i = 0; i < n3d; i++) {
426  std::ostringstream text;
427  text << "Click on point " << i + 1;
428  if (I) {
429  vpDisplay::display(*I);
430  vpDisplay::displayText(*I, 15, 10, text.str(), vpColor::red);
431  for (unsigned int k = 0; k < mem_ip.size(); k++) {
432  vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2);
433  }
434  vpDisplay::flush(*I);
435  } else {
436  vpDisplay::display(*I_color);
437  vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red);
438  for (unsigned int k = 0; k < mem_ip.size(); k++) {
439  vpDisplay::displayCross(*I_color, mem_ip[k], 10, vpColor::green, 2);
440  }
441  vpDisplay::flush(*I_color);
442  }
443 
444  std::cout << "Click on point " << i + 1 << " ";
445  double x = 0, y = 0;
446  if (I) {
447  vpDisplay::getClick(*I, ip);
448  mem_ip.push_back(ip);
449  vpDisplay::flush(*I);
450  } else {
451  vpDisplay::getClick(*I_color, ip);
452  mem_ip.push_back(ip);
453  vpDisplay::flush(*I_color);
454  }
456  P[i].set_x(x);
457  P[i].set_y(y);
458 
459  std::cout << "with 2D coordinates: " << ip << std::endl;
460 
461  pose.addPoint(P[i]); // and added to the pose computation point list
462  }
463  if (I) {
464  vpDisplay::flush(*I);
465  vpDisplay::display(*I);
466  } else {
467  vpDisplay::flush(*I_color);
468  vpDisplay::display(*I_color);
469  }
470 
471  vpHomogeneousMatrix cMo1, cMo2;
472  double d1, d2;
473  d1 = d2 = std::numeric_limits<double>::max();
474  try {
475  pose.computePose(vpPose::LAGRANGE, cMo1);
476  d1 = pose.computeResidual(cMo1);
477  }
478  catch(...) {
479  // Lagrange non-planar cannot work with less than 6 points
480  }
481  try {
482  pose.computePose(vpPose::DEMENTHON, cMo2);
483  d2 = pose.computeResidual(cMo2);
484  }
485  catch(...) {
486  // Should not occur
487  }
488 
489  if (d1 < d2) {
490  m_cMo = cMo1;
491  } else {
492  m_cMo = cMo2;
493  }
495 
496  if (I) {
497  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
498  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
499 
500  vpDisplay::flush(*I);
501 
502  button = vpMouseButton::button1;
503  while (!vpDisplay::getClick(*I, ip, button))
504  ;
505 
506  if (button == vpMouseButton::button1) {
507  isWellInit = true;
508  } else {
509  pose.clearPoint();
510  vpDisplay::display(*I);
511  vpDisplay::flush(*I);
512  }
513  } else {
514  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
515  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
516 
517  vpDisplay::flush(*I_color);
518 
519  button = vpMouseButton::button1;
520  while (!vpDisplay::getClick(*I_color, ip, button))
521  ;
522 
523  if (button == vpMouseButton::button1) {
524  isWellInit = true;
525  } else {
526  pose.clearPoint();
527  vpDisplay::display(*I_color);
528  vpDisplay::flush(*I_color);
529  }
530  }
531  }
532  if (I)
534  else
535  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
536 
537  // save the pose into file
538  if (poseSavingFilename.empty())
539  savePose(str_pose);
540  else
542 
543  if (d_help != NULL) {
544  delete d_help;
545  d_help = NULL;
546  }
547  }
548 
549  std::cout << "cMo : " << std::endl << m_cMo << std::endl;
550 
551  if (I)
552  init(*I);
553  else {
554  vpImageConvert::convert(*I_color, m_I);
555  init(m_I);
556  }
557 }
558 
590 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp,
591  const vpHomogeneousMatrix &T)
592 {
593  initClick(&I, NULL, initFile, displayHelp, T);
594 }
595 
627 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, bool displayHelp,
628  const vpHomogeneousMatrix &T)
629 {
630  initClick(NULL, &I_color, initFile, displayHelp, T);
631 }
632 
633 void vpMbTracker::initClick(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
634  const std::vector<vpPoint> &points3D_list, const std::string &displayFile)
635 {
636  if (I) {
637  vpDisplay::display(*I);
638  vpDisplay::flush(*I);
639  } else {
640  vpDisplay::display(*I_color);
641  vpDisplay::flush(*I_color);
642  }
643 
644  vpDisplay *d_help = NULL;
645 
646  vpPose pose;
647  std::vector<vpPoint> P;
648  for (unsigned int i = 0; i < points3D_list.size(); i++)
649  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
650 
651 #ifdef VISP_HAVE_MODULE_IO
652  vpImage<vpRGBa> Iref;
653  // Display window creation and initialisation
654  if (vpIoTools::checkFilename(displayFile)) {
655  try {
656  std::cout << "Load image to help initialization: " << displayFile << std::endl;
657 #if defined VISP_HAVE_X11
658  d_help = new vpDisplayX;
659 #elif defined VISP_HAVE_GDI
660  d_help = new vpDisplayGDI;
661 #elif defined VISP_HAVE_OPENCV
662  d_help = new vpDisplayOpenCV;
663 #endif
664 
665  vpImageIo::read(Iref, displayFile);
666 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
667  if (I) {
668  d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(),
669  "Where to initialize...");
670  } else {
671  d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80, I_color->display->getWindowYPosition(),
672  "Where to initialize...");
673  }
674  vpDisplay::display(Iref);
675  vpDisplay::flush(Iref);
676 #endif
677  } catch (...) {
678  if (d_help != NULL) {
679  delete d_help;
680  d_help = NULL;
681  }
682  }
683  }
684 #else //#ifdef VISP_HAVE_MODULE_IO
685  (void)(displayFile);
686 #endif //#ifdef VISP_HAVE_MODULE_IO
687 
688  vpImagePoint ip;
689  bool isWellInit = false;
690  while (!isWellInit) {
691  for (unsigned int i = 0; i < points3D_list.size(); i++) {
692  std::cout << "Click on point " << i + 1 << std::endl;
693  double x = 0, y = 0;
694  if (I) {
695  vpDisplay::getClick(*I, ip);
697  vpDisplay::flush(*I);
698  } else {
699  vpDisplay::getClick(*I_color, ip);
700  vpDisplay::displayCross(*I_color, ip, 5, vpColor::green);
701  vpDisplay::flush(*I_color);
702  }
704  P[i].set_x(x);
705  P[i].set_y(y);
706 
707  std::cout << "Click on point " << ip << std::endl;
708 
709  if (I) {
710  vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point
711  } else {
712  vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point
713  }
714  pose.addPoint(P[i]); // and added to the pose computation point list
715  }
716  if (I) {
717  vpDisplay::flush(*I);
718  } else {
719  vpDisplay::flush(*I_color);
720  }
721 
722  vpHomogeneousMatrix cMo1, cMo2;
723  double d1, d2;
724  d1 = d2 = std::numeric_limits<double>::max();
725  try {
726  pose.computePose(vpPose::LAGRANGE, cMo1);
727  d1 = pose.computeResidual(cMo1);
728  }
729  catch(...) {
730  // Lagrange non-planar cannot work with less than 6 points
731  }
732  try {
733  pose.computePose(vpPose::DEMENTHON, cMo2);
734  d2 = pose.computeResidual(cMo2);
735  }
736  catch(...) {
737  // Should not occur
738  }
739 
740  if (d1 < d2) {
741  m_cMo = cMo1;
742  } else {
743  m_cMo = cMo2;
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  } else {
760  pose.clearPoint();
761  vpDisplay::display(*I);
762  vpDisplay::flush(*I);
763  }
764  } else {
765  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
766  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
767 
768  vpDisplay::flush(*I_color);
769 
771  while (!vpDisplay::getClick(*I_color, ip, button)) {
772  };
773 
774  if (button == vpMouseButton::button1) {
775  isWellInit = true;
776  } else {
777  pose.clearPoint();
778  vpDisplay::display(*I_color);
779  vpDisplay::flush(*I_color);
780  }
781  }
782  }
783 
784  if (I) {
786  } else {
787  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
788  }
789 
790  if (d_help != NULL) {
791  delete d_help;
792  d_help = NULL;
793  }
794 
795  if (I)
796  init(*I);
797  else {
798  vpImageConvert::convert(*I_color, m_I);
799  init(m_I);
800  }
801 }
802 
814 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
815  const std::string &displayFile)
816 {
817  initClick(&I, NULL, points3D_list, displayFile);
818 }
819 
831 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::vector<vpPoint> &points3D_list,
832  const std::string &displayFile)
833 {
834  initClick(NULL, &I_color, points3D_list, displayFile);
835 }
836 #endif //#ifdef VISP_HAVE_MODULE_GUI
837 
838 void vpMbTracker::initFromPoints(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
839  const std::string &initFile)
840 {
841  char s[FILENAME_MAX];
842  std::fstream finit;
843 
844  std::string ext = ".init";
845  size_t pos = initFile.rfind(ext);
846 
847  if (pos == initFile.size() - ext.size() && pos != 0)
848  sprintf(s, "%s", initFile.c_str());
849  else
850  sprintf(s, "%s.init", initFile.c_str());
851 
852  std::cout << "Load 2D/3D points from: " << s << std::endl;
853  finit.open(s, std::ios::in);
854  if (finit.fail()) {
855  std::cout << "cannot read " << s << std::endl;
856  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
857  }
858 
859  //********
860  // Read 3D points coordinates
861  //********
862  char c;
863  // skip lines starting with # as comment
864  finit.get(c);
865  while (!finit.fail() && (c == '#')) {
866  finit.ignore(256, '\n');
867  finit.get(c);
868  }
869  finit.unget();
870 
871  unsigned int n3d;
872  finit >> n3d;
873  finit.ignore(256, '\n'); // skip the rest of the line
874  std::cout << "Number of 3D points " << n3d << std::endl;
875  if (n3d > 100000) {
876  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
877  }
878 
879  vpPoint *P = new vpPoint[n3d];
880  for (unsigned int i = 0; i < n3d; i++) {
881  // skip lines starting with # as comment
882  finit.get(c);
883  while (!finit.fail() && (c == '#')) {
884  finit.ignore(256, '\n');
885  finit.get(c);
886  }
887  finit.unget();
888  double X, Y, Z;
889  finit >> X;
890  finit >> Y;
891  finit >> Z;
892  finit.ignore(256, '\n'); // skip the rest of the line
893 
894  std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
895  P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
896  }
897 
898  //********
899  // Read 3D points coordinates
900  //********
901  // skip lines starting with # as comment
902  finit.get(c);
903  while (!finit.fail() && (c == '#')) {
904  finit.ignore(256, '\n');
905  finit.get(c);
906  }
907  finit.unget();
908 
909  unsigned int n2d;
910  finit >> n2d;
911  finit.ignore(256, '\n'); // skip the rest of the line
912  std::cout << "Number of 2D points " << n2d << std::endl;
913  if (n2d > 100000) {
914  delete[] P;
915  throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed", s);
916  }
917 
918  if (n3d != n2d) {
919  delete[] P;
921  "In %s file, number of 2D points %d and number of 3D "
922  "points %d are not equal",
923  s, n2d, n3d);
924  }
925 
926  vpPose pose;
927  for (unsigned int i = 0; i < n2d; i++) {
928  // skip lines starting with # as comment
929  finit.get(c);
930  while (!finit.fail() && (c == '#')) {
931  finit.ignore(256, '\n');
932  finit.get(c);
933  }
934  finit.unget();
935  double u, v, x = 0, y = 0;
936  finit >> v;
937  finit >> u;
938  finit.ignore(256, '\n'); // skip the rest of the line
939 
940  vpImagePoint ip(v, u);
941  std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
943  P[i].set_x(x);
944  P[i].set_y(y);
945  pose.addPoint(P[i]);
946  }
947 
948  finit.close();
949 
950  vpHomogeneousMatrix cMo1, cMo2;
951  double d1, d2;
952  d1 = d2 = std::numeric_limits<double>::max();
953  try {
954  pose.computePose(vpPose::LAGRANGE, cMo1);
955  d1 = pose.computeResidual(cMo1);
956  }
957  catch(...) {
958  // Lagrange non-planar cannot work with less than 6 points
959  }
960  try {
961  pose.computePose(vpPose::DEMENTHON, cMo2);
962  d2 = pose.computeResidual(cMo2);
963  }
964  catch(...) {
965  // Should not occur
966  }
967 
968  if (d1 < d2)
969  m_cMo = cMo1;
970  else
971  m_cMo = cMo2;
972 
974 
975  delete[] P;
976 
977  if (I) {
978  init(*I);
979  } else {
980  vpImageConvert::convert(*I_color, m_I);
981  init(m_I);
982  }
983 }
984 
1009 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
1010 {
1011  initFromPoints(&I, NULL, initFile);
1012 }
1013 
1038 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1039 {
1040  initFromPoints(NULL, &I_color, initFile);
1041 }
1042 
1043 void vpMbTracker::initFromPoints(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
1044  const std::vector<vpImagePoint> &points2D_list, const std::vector<vpPoint> &points3D_list)
1045 {
1046  if (points2D_list.size() != points3D_list.size())
1047  vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
1048  "different to the number of 3D points.");
1049 
1050  size_t size = points3D_list.size();
1051  std::vector<vpPoint> P;
1052  vpPose pose;
1053 
1054  for (size_t i = 0; i < size; i++) {
1055  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1056  double x = 0, y = 0;
1057  vpPixelMeterConversion::convertPoint(m_cam, points2D_list[i], x, y);
1058  P[i].set_x(x);
1059  P[i].set_y(y);
1060  pose.addPoint(P[i]);
1061  }
1062 
1063  vpHomogeneousMatrix cMo1, cMo2;
1064  double d1, d2;
1065  d1 = d2 = std::numeric_limits<double>::max();
1066  try {
1067  pose.computePose(vpPose::LAGRANGE, cMo1);
1068  d1 = pose.computeResidual(cMo1);
1069  }
1070  catch(...) {
1071  // Lagrange non-planar cannot work with less than 6 points
1072  }
1073  try {
1074  pose.computePose(vpPose::DEMENTHON, cMo2);
1075  d2 = pose.computeResidual(cMo2);
1076  }
1077  catch(...) {
1078  // Should not occur
1079  }
1080 
1081  if (d1 < d2)
1082  m_cMo = cMo1;
1083  else
1084  m_cMo = cMo2;
1085 
1087 
1088  if (I) {
1089  init(*I);
1090  } else {
1091  vpImageConvert::convert(*I_color, m_I);
1092  init(m_I);
1093  }
1094 }
1095 
1104 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
1105  const std::vector<vpPoint> &points3D_list)
1106 {
1107  initFromPoints(&I, NULL, points2D_list, points3D_list);
1108 }
1109 
1118 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::vector<vpImagePoint> &points2D_list,
1119  const std::vector<vpPoint> &points3D_list)
1120 {
1121  initFromPoints(NULL, &I_color, points2D_list, points3D_list);
1122 }
1123 
1124 void vpMbTracker::initFromPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
1125  const std::string &initFile)
1126 {
1127  char s[FILENAME_MAX];
1128  std::fstream finit;
1129  vpPoseVector init_pos;
1130 
1131  std::string ext = ".pos";
1132  size_t pos = initFile.rfind(ext);
1133 
1134  if (pos == initFile.size() - ext.size() && pos != 0)
1135  sprintf(s, "%s", initFile.c_str());
1136  else
1137  sprintf(s, "%s.pos", initFile.c_str());
1138 
1139  finit.open(s, std::ios::in);
1140  if (finit.fail()) {
1141  std::cout << "cannot read " << s << std::endl;
1142  throw vpException(vpException::ioError, "cannot read init file");
1143  }
1144 
1145  for (unsigned int i = 0; i < 6; i += 1) {
1146  finit >> init_pos[i];
1147  }
1148 
1149  m_cMo.buildFrom(init_pos);
1150 
1151  if (I) {
1152  init(*I);
1153  } else {
1154  vpImageConvert::convert(*I_color, m_I);
1155  init(m_I);
1156  }
1157 }
1158 
1177 void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1178 {
1179  initFromPose(&I, NULL, initFile);
1180 }
1181 
1200 void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1201 {
1202  initFromPose(NULL, &I_color, initFile);
1203 }
1204 
1212 {
1213  m_cMo = cMo;
1214  init(I);
1215 }
1216 
1224 {
1225  m_cMo = cMo;
1226  vpImageConvert::convert(I_color, m_I);
1227  init(m_I);
1228 }
1229 
1237 {
1238  vpHomogeneousMatrix _cMo(cPo);
1239  initFromPose(I, _cMo);
1240 }
1241 
1249 {
1250  vpHomogeneousMatrix _cMo(cPo);
1251  vpImageConvert::convert(I_color, m_I);
1252  initFromPose(m_I, _cMo);
1253 }
1254 
1260 void vpMbTracker::savePose(const std::string &filename) const
1261 {
1262  vpPoseVector init_pos;
1263  std::fstream finitpos;
1264  char s[FILENAME_MAX];
1265 
1266  sprintf(s, "%s", filename.c_str());
1267  finitpos.open(s, std::ios::out);
1268 
1269  init_pos.buildFrom(m_cMo);
1270  finitpos << init_pos;
1271  finitpos.close();
1272 }
1273 
1274 void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
1275  bool useLod, double minPolygonAreaThreshold,
1276  double minLineLengthThreshold)
1277 {
1278  std::vector<vpPoint> corners_without_duplicates;
1279  corners_without_duplicates.push_back(corners[0]);
1280  for (unsigned int i = 0; i < corners.size() - 1; i++) {
1281  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1282  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1283  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1284  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1285  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1286  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1287  corners_without_duplicates.push_back(corners[i + 1]);
1288  }
1289  }
1290 
1291  vpMbtPolygon polygon;
1292  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
1293  polygon.setIndex((int)idFace);
1294  polygon.setName(polygonName);
1295  polygon.setLod(useLod);
1296 
1297  // //if(minPolygonAreaThreshold != -1.0) {
1298  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1299  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1300  // {
1301  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1302  // }
1303  //
1304  // //if(minLineLengthThreshold != -1.0) {
1305  // if(std::fabs(minLineLengthThreshold + 1.0) >
1306  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1307  // {
1308  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1309  // }
1310 
1311  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1312  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1313 
1314  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1315  polygon.addPoint(j, corners_without_duplicates[j]);
1316  }
1317 
1318  faces.addPolygon(&polygon);
1319 
1321  faces.getPolygon().back()->setClipping(clippingFlag);
1322 
1323  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1324  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1325 
1326  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1327  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1328 }
1329 
1330 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1331  int idFace, const std::string &polygonName, bool useLod,
1332  double minPolygonAreaThreshold)
1333 {
1334  vpMbtPolygon polygon;
1335  polygon.setNbPoint(4);
1336  polygon.setName(polygonName);
1337  polygon.setLod(useLod);
1338 
1339  // //if(minPolygonAreaThreshold != -1.0) {
1340  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1341  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1342  // {
1343  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1344  // }
1345  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1346  // Non sense to set minLineLengthThreshold for circle
1347  // but used to be coherent when applying LOD settings for all polygons
1349 
1350  {
1351  // Create the 4 points of the circle bounding box
1352  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
1353 
1354  // Matrice de passage entre world et circle frame
1355  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
1356  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
1357  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
1358  vpRotationMatrix wRc;
1359  vpColVector x(3), y(3), z(3);
1360  // X axis is P2-P1
1361  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
1362  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
1363  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
1364  // Y axis is the normal of the plane
1365  y[0] = plane.getA() / norm_Y;
1366  y[1] = plane.getB() / norm_Y;
1367  y[2] = plane.getC() / norm_Y;
1368  // Z axis = X ^ Y
1369  z = vpColVector::crossProd(x, y);
1370  for (unsigned int i = 0; i < 3; i++) {
1371  wRc[i][0] = x[i];
1372  wRc[i][1] = y[i];
1373  wRc[i][2] = z[i];
1374  }
1375 
1376  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
1377  vpHomogeneousMatrix wMc(wtc, wRc);
1378 
1379  vpColVector c_p(4); // A point in the circle frame that is on the bbox
1380  c_p[0] = radius;
1381  c_p[1] = 0;
1382  c_p[2] = radius;
1383  c_p[3] = 1;
1384 
1385  // Matrix to rotate a point by 90 deg around Y in the circle frame
1386  for (unsigned int i = 0; i < 4; i++) {
1387  vpColVector w_p(4); // A point in the word frame
1389  w_p = wMc * cMc_90 * c_p;
1390 
1391  vpPoint w_P;
1392  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
1393 
1394  polygon.addPoint(i, w_P);
1395  }
1396  }
1397 
1398  polygon.setIndex(idFace);
1399  faces.addPolygon(&polygon);
1400 
1402  faces.getPolygon().back()->setClipping(clippingFlag);
1403 
1404  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1405  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1406 
1407  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1408  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1409 }
1410 
1411 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
1412  bool useLod, double minLineLengthThreshold)
1413 {
1414  // A polygon as a single line that corresponds to the revolution axis of the
1415  // cylinder
1416  vpMbtPolygon polygon;
1417  polygon.setNbPoint(2);
1418 
1419  polygon.addPoint(0, p1);
1420  polygon.addPoint(1, p2);
1421 
1422  polygon.setIndex(idFace);
1423  polygon.setName(polygonName);
1424  polygon.setLod(useLod);
1425 
1426  // //if(minLineLengthThreshold != -1.0) {
1427  // if(std::fabs(minLineLengthThreshold + 1.0) >
1428  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1429  // {
1430  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1431  // }
1432  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1433  // Non sense to set minPolygonAreaThreshold for cylinder
1434  // but used to be coherent when applying LOD settings for all polygons
1436 
1437  faces.addPolygon(&polygon);
1438 
1440  faces.getPolygon().back()->setClipping(clippingFlag);
1441 
1442  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1443  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1444 
1445  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1446  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1447 }
1448 
1449 void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
1450  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
1451 {
1452  int id = idFace;
1453  for (unsigned int i = 0; i < listFaces.size(); i++) {
1454  vpMbtPolygon polygon;
1455  polygon.setNbPoint((unsigned int)listFaces[i].size());
1456  for (unsigned int j = 0; j < listFaces[i].size(); j++)
1457  polygon.addPoint(j, listFaces[i][j]);
1458 
1459  polygon.setIndex(id);
1460  polygon.setName(polygonName);
1461  polygon.setIsPolygonOriented(false);
1462  polygon.setLod(useLod);
1463  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1465 
1466  faces.addPolygon(&polygon);
1467 
1469  faces.getPolygon().back()->setClipping(clippingFlag);
1470 
1471  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1472  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1473 
1474  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1475  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1476 
1477  id++;
1478  }
1479 }
1480 
1508 void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &odTo)
1509 {
1510  std::string::const_iterator it;
1511 
1512  if (vpIoTools::checkFilename(modelFile)) {
1513  it = modelFile.end();
1514  if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
1515  (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
1516  std::vector<std::string> vectorOfModelFilename;
1517  int startIdFace = (int)faces.size();
1518  nbPoints = 0;
1519  nbLines = 0;
1520  nbPolygonLines = 0;
1521  nbPolygonPoints = 0;
1522  nbCylinders = 0;
1523  nbCircles = 0;
1524  loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo);
1525  } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
1526  (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
1527  loadVRMLModel(modelFile);
1528  } else {
1529  throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
1530  }
1531  } else {
1532  throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
1533  }
1534 
1535  this->modelInitialised = true;
1536  this->modelFileName = modelFile;
1537 }
1538 
1569 void vpMbTracker::loadVRMLModel(const std::string &modelFile)
1570 {
1571 #ifdef VISP_HAVE_COIN3D
1572  SoDB::init(); // Call SoDB::finish() before ending the program.
1573 
1574  SoInput in;
1575  SbBool ok = in.openFile(modelFile.c_str());
1576  SoVRMLGroup *sceneGraphVRML2;
1577 
1578  if (!ok) {
1579  vpERROR_TRACE("can't open file to load model");
1580  throw vpException(vpException::fatalError, "can't open file to load model");
1581  }
1582 
1583  if (!in.isFileVRML2()) {
1584  SoSeparator *sceneGraph = SoDB::readAll(&in);
1585  if (sceneGraph == NULL) { /*return -1;*/
1586  }
1587  sceneGraph->ref();
1588 
1589  SoToVRML2Action tovrml2;
1590  tovrml2.apply(sceneGraph);
1591 
1592  sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1593  sceneGraphVRML2->ref();
1594  sceneGraph->unref();
1595  } else {
1596  sceneGraphVRML2 = SoDB::readAllVRML(&in);
1597  if (sceneGraphVRML2 == NULL) { /*return -1;*/
1598  }
1599  sceneGraphVRML2->ref();
1600  }
1601 
1602  in.closeFile();
1603 
1604  vpHomogeneousMatrix transform;
1605  int indexFace = (int)faces.size();
1606  extractGroup(sceneGraphVRML2, transform, indexFace);
1607 
1608  sceneGraphVRML2->unref();
1609 #else
1610  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1611  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
1612 #endif
1613 }
1614 
1615 void vpMbTracker::removeComment(std::ifstream &fileId)
1616 {
1617  char c;
1618 
1619  fileId.get(c);
1620  while (!fileId.fail() && (c == '#')) {
1621  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));
1622  fileId.get(c);
1623  }
1624  if (fileId.fail()) {
1625  throw(vpException(vpException::ioError, "Reached end of file"));
1626  }
1627  fileId.unget();
1628 }
1629 
1630 std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
1631 {
1632  std::map<std::string, std::string> mapOfParams;
1633 
1634  bool exit = false;
1635  while (!endLine.empty() && !exit) {
1636  exit = true;
1637 
1638  for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
1639  it != mapOfParameterNames.end(); ++it) {
1640  endLine = vpIoTools::trim(endLine);
1641  std::string param(it->first + "=");
1642 
1643  // Compare with a potential parameter
1644  if (endLine.compare(0, param.size(), param) == 0) {
1645  exit = false;
1646  endLine = endLine.substr(param.size());
1647 
1648  bool parseQuote = false;
1649  if (it->second == "string") {
1650  // Check if the string is between quotes
1651  if (endLine.size() > 2 && endLine[0] == '"') {
1652  parseQuote = true;
1653  endLine = endLine.substr(1);
1654  size_t pos = endLine.find_first_of('"');
1655 
1656  if (pos != std::string::npos) {
1657  mapOfParams[it->first] = endLine.substr(0, pos);
1658  endLine = endLine.substr(pos + 1);
1659  } else {
1660  parseQuote = false;
1661  }
1662  }
1663  }
1664 
1665  if (!parseQuote) {
1666  // Deal with space or tabulation after parameter value to substring
1667  // to the next sequence
1668  size_t pos1 = endLine.find_first_of(' ');
1669  size_t pos2 = endLine.find_first_of('\t');
1670  size_t pos = pos1 < pos2 ? pos1 : pos2;
1671 
1672  mapOfParams[it->first] = endLine.substr(0, pos);
1673  endLine = endLine.substr(pos + 1);
1674  }
1675  }
1676  }
1677  }
1678 
1679  return mapOfParams;
1680 }
1681 
1731 void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
1732  int &startIdFace, bool verbose, bool parent,
1733  const vpHomogeneousMatrix &odTo)
1734 {
1735  std::ifstream fileId;
1736  fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1737  fileId.open(modelFile.c_str(), std::ifstream::in);
1738  if (fileId.fail()) {
1739  std::cout << "cannot read CAO model file: " << modelFile << std::endl;
1740  throw vpException(vpException::ioError, "cannot read CAO model file");
1741  }
1742 
1743  if (verbose) {
1744  std::cout << "Model file : " << modelFile << std::endl;
1745  }
1746  vectorOfModelFilename.push_back(modelFile);
1747 
1748  try {
1749  char c;
1750  // Extraction of the version (remove empty line and commented ones
1751  // (comment line begin with the #)).
1752  // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
1753  removeComment(fileId);
1754 
1756  int caoVersion;
1757  fileId.get(c);
1758  if (c == 'V') {
1759  fileId >> caoVersion;
1760  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1761  } else {
1762  std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
1763  "file : use V0, V1, ...";
1764  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
1765  "header file : use V0, V1, ...");
1766  }
1767 
1768  removeComment(fileId);
1769 
1771  std::string line;
1772  const std::string prefix_load = "load";
1773 
1774  fileId.get(c);
1775  fileId.unget();
1776  bool header = false;
1777  while (c == 'l' || c == 'L') {
1778  getline(fileId, line);
1779 
1780  if (!line.compare(0, prefix_load.size(), prefix_load)) {
1781  //remove "load("
1782  std::string paramsStr = line.substr(5);
1783  //get parameters inside load()
1784  paramsStr = paramsStr.substr(0, paramsStr.find_first_of(")"));
1785  //split by comma
1786  std::vector<std::string> params = vpIoTools::splitChain(paramsStr, ",");
1787  //remove whitespaces
1788  for (size_t i = 0; i < params.size(); i++) {
1789  params[i] = vpIoTools::trim(params[i]);
1790  }
1791 
1792  if (!params.empty()) {
1793  // Get the loaded model pathname
1794  std::string headerPathRead = params[0];
1795  headerPathRead = headerPathRead.substr(1);
1796  headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of("\""));
1797 
1798  std::string headerPath = headerPathRead;
1799  if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
1800  std::string parentDirectory = vpIoTools::getParent(modelFile);
1801  headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
1802  }
1803 
1804  // Normalize path
1805  headerPath = vpIoTools::path(headerPath);
1806 
1807  // Get real path
1808  headerPath = vpIoTools::getAbsolutePathname(headerPath);
1809 
1810  vpHomogeneousMatrix oTo_local;
1812  vpThetaUVector tu;
1813  for (size_t i = 1; i < params.size(); i++) {
1814  std::string param = params[i];
1815  {
1816  const std::string prefix = "t=[";
1817  if (!param.compare(0, prefix.size(), prefix)) {
1818  param = param.substr(prefix.size());
1819  param = param.substr(0, param.find_first_of("]"));
1820 
1821  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1822  if (values.size() == 3) {
1823  t[0] = atof(values[0].c_str());
1824  t[1] = atof(values[1].c_str());
1825  t[2] = atof(values[2].c_str());
1826  }
1827  }
1828  }
1829  {
1830  const std::string prefix = "tu=[";
1831  if (!param.compare(0, prefix.size(), prefix)) {
1832  param = param.substr(prefix.size());
1833  param = param.substr(0, param.find_first_of("]"));
1834 
1835  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1836  if (values.size() == 3) {
1837  for (size_t j = 0; j < values.size(); j++) {
1838  std::string value = values[j];
1839  bool radian = true;
1840  size_t unitPos = value.find("deg");
1841  if (unitPos != std::string::npos) {
1842  value = value.substr(0, unitPos);
1843  radian = false;
1844  }
1845 
1846  unitPos = value.find("rad");
1847  if (unitPos != std::string::npos) {
1848  value = value.substr(0, unitPos);
1849  }
1850  tu[static_cast<unsigned int>(j)] = !radian ? vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1851  }
1852  }
1853  }
1854  }
1855  }
1856  oTo_local.buildFrom(t, tu);
1857 
1858  bool cyclic = false;
1859  for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1860  it != vectorOfModelFilename.end() && !cyclic; ++it) {
1861  if (headerPath == *it) {
1862  cyclic = true;
1863  }
1864  }
1865 
1866  if (!cyclic) {
1867  if (vpIoTools::checkFilename(headerPath)) {
1868  header = true;
1869  loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, odTo*oTo_local);
1870  } else {
1871  throw vpException(vpException::ioError, "file cannot be open");
1872  }
1873  } else {
1874  std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
1875  << std::endl;
1876  }
1877  }
1878  }
1879 
1880  removeComment(fileId);
1881  fileId.get(c);
1882  fileId.unget();
1883  }
1884 
1886  unsigned int caoNbrPoint;
1887  fileId >> caoNbrPoint;
1888  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1889 
1890  nbPoints += caoNbrPoint;
1891  if (verbose || (parent && !header)) {
1892  std::cout << "> " << caoNbrPoint << " points" << std::endl;
1893  }
1894 
1895  if (caoNbrPoint > 100000) {
1896  throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
1897  }
1898 
1899  if (caoNbrPoint == 0 && !header) {
1900  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
1901  }
1902  vpPoint *caoPoints = new vpPoint[caoNbrPoint];
1903 
1904  int i; // image coordinate (used for matching)
1905  int j;
1906 
1907  for (unsigned int k = 0; k < caoNbrPoint; k++) {
1908  removeComment(fileId);
1909 
1910  vpColVector pt_3d(4, 1.0);
1911  fileId >> pt_3d[0];
1912  fileId >> pt_3d[1];
1913  fileId >> pt_3d[2];
1914 
1915  if (caoVersion == 2) {
1916  fileId >> i;
1917  fileId >> j;
1918  }
1919 
1920  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1921 
1922  vpColVector pt_3d_tf = odTo*pt_3d;
1923  caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1924  }
1925 
1926  removeComment(fileId);
1927 
1929  // Store in a map the potential segments to add
1930  std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1931  unsigned int caoNbrLine;
1932  fileId >> caoNbrLine;
1933  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1934 
1935  nbLines += caoNbrLine;
1936  unsigned int *caoLinePoints = NULL;
1937  if (verbose || (parent && !header)) {
1938  std::cout << "> " << caoNbrLine << " lines" << std::endl;
1939  }
1940 
1941  if (caoNbrLine > 100000) {
1942  delete[] caoPoints;
1943  throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
1944  }
1945 
1946  if (caoNbrLine > 0)
1947  caoLinePoints = new unsigned int[2 * caoNbrLine];
1948 
1949  unsigned int index1, index2;
1950  // Initialization of idFace with startIdFace for dealing with recursive
1951  // load in header
1952  int idFace = startIdFace;
1953 
1954  for (unsigned int k = 0; k < caoNbrLine; k++) {
1955  removeComment(fileId);
1956 
1957  fileId >> index1;
1958  fileId >> index2;
1959 
1961  // Get the end of the line
1962  std::string endLine = "";
1963  if (safeGetline(fileId, endLine).good()) {
1964  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1965 
1966  std::string segmentName = "";
1967  double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1968  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1969  if (mapOfParams.find("name") != mapOfParams.end()) {
1970  segmentName = mapOfParams["name"];
1971  }
1972  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1973  minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1974  }
1975  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1976  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
1977  }
1978 
1979  SegmentInfo segmentInfo;
1980  segmentInfo.name = segmentName;
1981  segmentInfo.useLod = useLod;
1982  segmentInfo.minLineLengthThresh = minLineLengthThresh;
1983 
1984  caoLinePoints[2 * k] = index1;
1985  caoLinePoints[2 * k + 1] = index2;
1986 
1987  if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1988  std::vector<vpPoint> extremities;
1989  extremities.push_back(caoPoints[index1]);
1990  extremities.push_back(caoPoints[index2]);
1991  segmentInfo.extremities = extremities;
1992 
1993  std::pair<unsigned int, unsigned int> key(index1, index2);
1994 
1995  segmentTemporaryMap[key] = segmentInfo;
1996  } else {
1997  vpTRACE(" line %d has wrong coordinates.", k);
1998  }
1999  }
2000  }
2001 
2002  removeComment(fileId);
2003 
2005  /* Load polygon from the lines extracted earlier (the first point of the
2006  * line is used)*/
2007  // Store in a vector the indexes of the segments added in the face segment
2008  // case
2009  std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
2010  unsigned int caoNbrPolygonLine;
2011  fileId >> caoNbrPolygonLine;
2012  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2013 
2014  nbPolygonLines += caoNbrPolygonLine;
2015  if (verbose || (parent && !header)) {
2016  std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
2017  }
2018 
2019  if (caoNbrPolygonLine > 100000) {
2020  delete[] caoPoints;
2021  delete[] caoLinePoints;
2022  throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
2023  }
2024 
2025  unsigned int index;
2026  for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
2027  removeComment(fileId);
2028 
2029  unsigned int nbLinePol;
2030  fileId >> nbLinePol;
2031  std::vector<vpPoint> corners;
2032  if (nbLinePol > 100000) {
2033  throw vpException(vpException::badValue, "Exceed the max number of lines.");
2034  }
2035 
2036  for (unsigned int n = 0; n < nbLinePol; n++) {
2037  fileId >> index;
2038 
2039  if (index >= caoNbrLine) {
2040  throw vpException(vpException::badValue, "Exceed the max number of lines.");
2041  }
2042  corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2043  corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2044 
2045  std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2046  faceSegmentKeyVector.push_back(key);
2047  }
2048 
2050  // Get the end of the line
2051  std::string endLine = "";
2052  if (safeGetline(fileId, endLine).good()) {
2053  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2054 
2055  std::string polygonName = "";
2056  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2057  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2058  if (mapOfParams.find("name") != mapOfParams.end()) {
2059  polygonName = mapOfParams["name"];
2060  }
2061  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2062  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2063  }
2064  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2065  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2066  }
2067 
2068  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2069  initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
2070 
2071  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2073  }
2074  }
2075 
2076  // Add the segments which were not already added in the face segment case
2077  for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2078  it != segmentTemporaryMap.end(); ++it) {
2079  if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2080  faceSegmentKeyVector.end()) {
2081  addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
2082  it->second.minLineLengthThresh);
2083  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2084 
2085  addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
2086  it->second.minLineLengthThresh);
2088  }
2089  }
2090 
2091  removeComment(fileId);
2092 
2094  /* Extract the polygon using the point coordinates (top of the file) */
2095  unsigned int caoNbrPolygonPoint;
2096  fileId >> caoNbrPolygonPoint;
2097  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2098 
2099  nbPolygonPoints += caoNbrPolygonPoint;
2100  if (verbose || (parent && !header)) {
2101  std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
2102  }
2103 
2104  if (caoNbrPolygonPoint > 100000) {
2105  throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
2106  }
2107 
2108  for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2109  removeComment(fileId);
2110 
2111  unsigned int nbPointPol;
2112  fileId >> nbPointPol;
2113  if (nbPointPol > 100000) {
2114  throw vpException(vpException::badValue, "Exceed the max number of points.");
2115  }
2116  std::vector<vpPoint> corners;
2117  for (unsigned int n = 0; n < nbPointPol; n++) {
2118  fileId >> index;
2119  if (index > caoNbrPoint - 1) {
2120  throw vpException(vpException::badValue, "Exceed the max number of points.");
2121  }
2122  corners.push_back(caoPoints[index]);
2123  }
2124 
2126  // Get the end of the line
2127  std::string endLine = "";
2128  if (safeGetline(fileId, endLine).good()) {
2129  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2130 
2131  std::string polygonName = "";
2132  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2133  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2134  if (mapOfParams.find("name") != mapOfParams.end()) {
2135  polygonName = mapOfParams["name"];
2136  }
2137  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2138  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2139  }
2140  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2141  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2142  }
2143 
2144  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2145  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2146 
2147  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2149  }
2150  }
2151 
2153  unsigned int caoNbCylinder;
2154  try {
2155  removeComment(fileId);
2156 
2157  if (fileId.eof()) { // check if not at the end of the file (for old
2158  // style files)
2159  delete[] caoPoints;
2160  delete[] caoLinePoints;
2161  return;
2162  }
2163 
2164  /* Extract the cylinders */
2165  fileId >> caoNbCylinder;
2166  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2167 
2168  nbCylinders += caoNbCylinder;
2169  if (verbose || (parent && !header)) {
2170  std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
2171  }
2172 
2173  if (caoNbCylinder > 100000) {
2174  throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
2175  }
2176 
2177  for (unsigned int k = 0; k < caoNbCylinder; ++k) {
2178  removeComment(fileId);
2179 
2180  double radius;
2181  unsigned int indexP1, indexP2;
2182  fileId >> indexP1;
2183  fileId >> indexP2;
2184  fileId >> radius;
2185 
2187  // Get the end of the line
2188  std::string endLine = "";
2189  if (safeGetline(fileId, endLine).good()) {
2190  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2191 
2192  std::string polygonName = "";
2193  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2194  double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
2195  if (mapOfParams.find("name") != mapOfParams.end()) {
2196  polygonName = mapOfParams["name"];
2197  }
2198  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
2199  minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
2200  }
2201  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2202  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2203  }
2204 
2205  int idRevolutionAxis = idFace;
2206  addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2207 
2208  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
2209 
2210  std::vector<std::vector<vpPoint> > listFaces;
2211  createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
2212  addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2213 
2214  initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2215 
2216  addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2217  initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2218 
2219  idFace += 4;
2220  }
2221  }
2222 
2223  } catch (const std::exception& e) {
2224  std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2225  std::cerr << "Exception: " << e.what() << std::endl;
2226  caoNbCylinder = 0;
2227  }
2228 
2230  unsigned int caoNbCircle;
2231  try {
2232  removeComment(fileId);
2233 
2234  if (fileId.eof()) { // check if not at the end of the file (for old
2235  // style files)
2236  delete[] caoPoints;
2237  delete[] caoLinePoints;
2238  return;
2239  }
2240 
2241  /* Extract the circles */
2242  fileId >> caoNbCircle;
2243  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2244 
2245  nbCircles += caoNbCircle;
2246  if (verbose || (parent && !header)) {
2247  std::cout << "> " << caoNbCircle << " circles" << std::endl;
2248  }
2249 
2250  if (caoNbCircle > 100000) {
2251  throw vpException(vpException::badValue, "Exceed the max number of cicles.");
2252  }
2253 
2254  for (unsigned int k = 0; k < caoNbCircle; ++k) {
2255  removeComment(fileId);
2256 
2257  double radius;
2258  unsigned int indexP1, indexP2, indexP3;
2259  fileId >> radius;
2260  fileId >> indexP1;
2261  fileId >> indexP2;
2262  fileId >> indexP3;
2263 
2265  // Get the end of the line
2266  std::string endLine = "";
2267  if (safeGetline(fileId, endLine).good()) {
2268  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2269 
2270  std::string polygonName = "";
2271  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2272  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2273  if (mapOfParams.find("name") != mapOfParams.end()) {
2274  polygonName = mapOfParams["name"];
2275  }
2276  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2277  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2278  }
2279  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2280  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2281  }
2282 
2283  addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2284  minPolygonAreaThreshold);
2285 
2286  initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2287 
2288  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2289  minPolygonAreaThreshold);
2290  initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, polygonName);
2291  }
2292  }
2293 
2294  } catch (const std::exception& e) {
2295  std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
2296  std::cerr << "Exception: " << e.what() << std::endl;
2297  caoNbCircle = 0;
2298  }
2299 
2300  startIdFace = idFace;
2301 
2302  delete[] caoPoints;
2303  delete[] caoLinePoints;
2304 
2305  if (header && parent) {
2306  if (verbose) {
2307  std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
2308  std::cout << "Total nb of points : " << nbPoints << std::endl;
2309  std::cout << "Total nb of lines : " << nbLines << std::endl;
2310  std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
2311  std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
2312  std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
2313  std::cout << "Total nb of circles : " << nbCircles << std::endl;
2314  } else {
2315  std::cout << "> " << nbPoints << " points" << std::endl;
2316  std::cout << "> " << nbLines << " lines" << std::endl;
2317  std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
2318  std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
2319  std::cout << "> " << nbCylinders << " cylinders" << std::endl;
2320  std::cout << "> " << nbCircles << " circles" << std::endl;
2321  }
2322  }
2323 
2324  //Go up: remove current model
2325  vectorOfModelFilename.pop_back();
2326  } catch (const std::exception& e) {
2327  std::cerr << "Cannot read line!" << std::endl;
2328  std::cerr << "Exception: " << e.what() << std::endl;
2329  throw vpException(vpException::ioError, "cannot read line");
2330  }
2331 }
2332 
2333 #ifdef VISP_HAVE_COIN3D
2334 
2341 void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
2342 {
2343  vpHomogeneousMatrix transformCur;
2344  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
2345  if (sceneGraphVRML2Trasnform) {
2346  float rx, ry, rz, rw;
2347  sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2348  vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
2349  // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
2350  // rw << std::endl;
2351 
2352  float tx, ty, tz;
2353  tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2354  ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2355  tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2356  vpTranslationVector transVec(tx, ty, tz);
2357  // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
2358  // std::endl;
2359 
2360  float sx, sy, sz;
2361  sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2362  sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2363  sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2364  // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
2365  // std::endl;
2366 
2367  for (unsigned int i = 0; i < 3; i++)
2368  rotMat[0][i] *= sx;
2369  for (unsigned int i = 0; i < 3; i++)
2370  rotMat[1][i] *= sy;
2371  for (unsigned int i = 0; i < 3; i++)
2372  rotMat[2][i] *= sz;
2373 
2374  transformCur = vpHomogeneousMatrix(transVec, rotMat);
2375  transform = transform * transformCur;
2376  }
2377 
2378  int nbShapes = sceneGraphVRML2->getNumChildren();
2379  // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
2380  // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
2381  // std::endl;
2382 
2383  SoNode *child;
2384 
2385  for (int i = 0; i < nbShapes; i++) {
2386  vpHomogeneousMatrix transform_recursive(transform);
2387  child = sceneGraphVRML2->getChild(i);
2388 
2389  if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2390  extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2391  }
2392 
2393  if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2394  extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2395  }
2396 
2397  if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2398  SoChildList *child2list = child->getChildren();
2399  std::string name = child->getName().getString();
2400 
2401  for (int j = 0; j < child2list->getLength(); j++) {
2402  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2403  SoVRMLIndexedFaceSet *face_set;
2404  face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2405  if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
2406  extractCylinders(face_set, transform, idFace, name);
2407  } else {
2408  extractFaces(face_set, transform, idFace, name);
2409  }
2410  }
2411  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2412  SoVRMLIndexedLineSet *line_set;
2413  line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2414  extractLines(line_set, idFace, name);
2415  }
2416  }
2417  }
2418  }
2419 }
2420 
2430 void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2431  const std::string &polygonName)
2432 {
2433  std::vector<vpPoint> corners;
2434 
2435  // SoMFInt32 indexList = _face_set->coordIndex;
2436  // int indexListSize = indexList.getNum();
2437  int indexListSize = face_set->coordIndex.getNum();
2438 
2439  vpColVector pointTransformed(4);
2440  vpPoint pt;
2441  SoVRMLCoordinate *coord;
2442 
2443  for (int i = 0; i < indexListSize; i++) {
2444  if (face_set->coordIndex[i] == -1) {
2445  if (corners.size() > 1) {
2446  addPolygon(corners, idFace, polygonName);
2447  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2448 
2449  addProjectionErrorPolygon(corners, idFace++, polygonName);
2451  corners.resize(0);
2452  }
2453  } else {
2454  coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2455  int index = face_set->coordIndex[i];
2456  pointTransformed[0] = coord->point[index].getValue()[0];
2457  pointTransformed[1] = coord->point[index].getValue()[1];
2458  pointTransformed[2] = coord->point[index].getValue()[2];
2459  pointTransformed[3] = 1.0;
2460 
2461  pointTransformed = transform * pointTransformed;
2462 
2463  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2464  corners.push_back(pt);
2465  }
2466  }
2467 }
2468 
2483 void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2484  const std::string &polygonName)
2485 {
2486  std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
2487  // first circle and to the
2488  // second one.
2489  SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2490 
2491  unsigned int indexListSize = (unsigned int)coords->point.getNum();
2492 
2493  if (indexListSize % 2 == 1) {
2494  std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
2495  throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
2496  }
2497  corners_c1.resize(indexListSize / 2);
2498  corners_c2.resize(indexListSize / 2);
2499  vpColVector pointTransformed(4);
2500  vpPoint pt;
2501 
2502  // extract all points and fill the two sets.
2503 
2504  for (int i = 0; i < coords->point.getNum(); ++i) {
2505  pointTransformed[0] = coords->point[i].getValue()[0];
2506  pointTransformed[1] = coords->point[i].getValue()[1];
2507  pointTransformed[2] = coords->point[i].getValue()[2];
2508  pointTransformed[3] = 1.0;
2509 
2510  pointTransformed = transform * pointTransformed;
2511 
2512  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2513 
2514  if (i < (int)corners_c1.size()) {
2515  corners_c1[(unsigned int)i] = pt;
2516  } else {
2517  corners_c2[(unsigned int)i - corners_c1.size()] = pt;
2518  }
2519  }
2520 
2521  vpPoint p1 = getGravityCenter(corners_c1);
2522  vpPoint p2 = getGravityCenter(corners_c2);
2523 
2524  vpColVector dist(3);
2525  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
2526  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
2527  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
2528  double radius_c1 = sqrt(dist.sumSquare());
2529  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
2530  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
2531  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
2532  double radius_c2 = sqrt(dist.sumSquare());
2533 
2534  if (std::fabs(radius_c1 - radius_c2) >
2535  (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
2536  std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
2537  throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
2538  }
2539 
2540  // addPolygon(p1, p2, idFace, polygonName);
2541  // initCylinder(p1, p2, radius_c1, idFace++);
2542 
2543  int idRevolutionAxis = idFace;
2544  addPolygon(p1, p2, idFace, polygonName);
2545 
2546  addProjectionErrorPolygon(p1, p2, idFace++, polygonName);
2547 
2548  std::vector<std::vector<vpPoint> > listFaces;
2549  createCylinderBBox(p1, p2, radius_c1, listFaces);
2550  addPolygon(listFaces, idFace, polygonName);
2551 
2552  initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2553 
2554  addProjectionErrorPolygon(listFaces, idFace, polygonName);
2555  initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2556 
2557  idFace += 4;
2558 }
2559 
2568 void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
2569 {
2570  std::vector<vpPoint> corners;
2571  corners.resize(0);
2572 
2573  int indexListSize = line_set->coordIndex.getNum();
2574 
2575  SbVec3f point(0, 0, 0);
2576  vpPoint pt;
2577  SoVRMLCoordinate *coord;
2578 
2579  for (int i = 0; i < indexListSize; i++) {
2580  if (line_set->coordIndex[i] == -1) {
2581  if (corners.size() > 1) {
2582  addPolygon(corners, idFace, polygonName);
2583  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2584 
2585  addProjectionErrorPolygon(corners, idFace++, polygonName);
2587  corners.resize(0);
2588  }
2589  } else {
2590  coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2591  int index = line_set->coordIndex[i];
2592  point[0] = coord->point[index].getValue()[0];
2593  point[1] = coord->point[index].getValue()[1];
2594  point[2] = coord->point[index].getValue()[2];
2595 
2596  pt.setWorldCoordinates(point[0], point[1], point[2]);
2597  corners.push_back(pt);
2598  }
2599  }
2600 }
2601 
2602 #endif // VISP_HAVE_COIN3D
2603 
2613 vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
2614 {
2615  if (pts.empty()) {
2616  std::cout << "Cannot extract center of gravity of empty set." << std::endl;
2617  throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
2618  }
2619  double oX = 0;
2620  double oY = 0;
2621  double oZ = 0;
2622  vpPoint G;
2623 
2624  for (unsigned int i = 0; i < pts.size(); ++i) {
2625  oX += pts[i].get_oX();
2626  oY += pts[i].get_oY();
2627  oZ += pts[i].get_oZ();
2628  }
2629 
2630  G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
2631  return G;
2632 }
2633 
2646 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2647 vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
2648 {
2649  // Temporary variable to permit to order polygons by distance
2650  std::vector<vpPolygon> polygonsTmp;
2651  std::vector<std::vector<vpPoint> > roisPtTmp;
2652 
2653  // Pair containing the list of vpPolygon and the list of face corners
2654  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2655 
2656  for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
2657  // A face has at least 3 points
2658  if (faces.getPolygon()[i]->nbpt > 2) {
2659  if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
2660  std::vector<vpImagePoint> roiPts;
2661 
2662  if (clipPolygon) {
2663  faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo);
2664  } else {
2665  roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo);
2666  }
2667 
2668  if (roiPts.size() <= 2) {
2669  continue;
2670  }
2671 
2672  polygonsTmp.push_back(vpPolygon(roiPts));
2673 
2674  std::vector<vpPoint> polyPts;
2675  if (clipPolygon) {
2676  faces.getPolygon()[i]->getPolygonClipped(polyPts);
2677  } else {
2678  for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
2679  polyPts.push_back(faces.getPolygon()[i]->p[j]);
2680  }
2681  }
2682  roisPtTmp.push_back(polyPts);
2683  }
2684  }
2685  }
2686 
2687  if (orderPolygons) {
2688  // Order polygons by distance (near to far)
2689  std::vector<PolygonFaceInfo> listOfPolygonFaces;
2690  for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
2691  double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2692  for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2693  x_centroid += roisPtTmp[i][j].get_X();
2694  y_centroid += roisPtTmp[i][j].get_Y();
2695  z_centroid += roisPtTmp[i][j].get_Z();
2696  }
2697 
2698  x_centroid /= roisPtTmp[i].size();
2699  y_centroid /= roisPtTmp[i].size();
2700  z_centroid /= roisPtTmp[i].size();
2701 
2702  double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2703  listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2704  }
2705 
2706  // Sort the list of polygon faces
2707  std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2708 
2709  polygonsTmp.resize(listOfPolygonFaces.size());
2710  roisPtTmp.resize(listOfPolygonFaces.size());
2711 
2712  size_t cpt = 0;
2713  for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2714  ++it, cpt++) {
2715  polygonsTmp[cpt] = it->polygon;
2716  roisPtTmp[cpt] = it->faceCorners;
2717  }
2718 
2719  pairOfPolygonFaces.first = polygonsTmp;
2720  pairOfPolygonFaces.second = roisPtTmp;
2721  } else {
2722  pairOfPolygonFaces.first = polygonsTmp;
2723  pairOfPolygonFaces.second = roisPtTmp;
2724  }
2725 
2726  return pairOfPolygonFaces;
2727 }
2728 
2738 {
2739  useOgre = v;
2740  if (useOgre) {
2741 #ifndef VISP_HAVE_OGRE
2742  useOgre = false;
2743  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2744  "will be used. setOgreVisibilityTest() set to false."
2745  << std::endl;
2746 #endif
2747  }
2748 }
2749 
2755 void vpMbTracker::setFarClippingDistance(const double &dist)
2756 {
2757  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2758  vpTRACE("Far clipping value cannot be inferior than near clipping value. "
2759  "Far clipping won't be considered.");
2760  else if (dist < 0)
2761  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
2762  "won't be considered.");
2763  else {
2765  distFarClip = dist;
2766  for (unsigned int i = 0; i < faces.size(); i++) {
2767  faces[i]->setFarClippingDistance(distFarClip);
2768  }
2769 #ifdef VISP_HAVE_OGRE
2771 #endif
2772  }
2773 }
2774 
2785 void vpMbTracker::setLod(bool useLod, const std::string &name)
2786 {
2787  for (unsigned int i = 0; i < faces.size(); i++) {
2788  if (name.empty() || faces[i]->name == name) {
2789  faces[i]->setLod(useLod);
2790  }
2791  }
2792 }
2793 
2803 void vpMbTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
2804 {
2805  for (unsigned int i = 0; i < faces.size(); i++) {
2806  if (name.empty() || faces[i]->name == name) {
2807  faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2808  }
2809  }
2810 }
2811 
2820 void vpMbTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
2821 {
2822  for (unsigned int i = 0; i < faces.size(); i++) {
2823  if (name.empty() || faces[i]->name == name) {
2824  faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2825  }
2826  }
2827 }
2828 
2835 {
2836  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2837  vpTRACE("Near clipping value cannot be superior than far clipping value. "
2838  "Near clipping won't be considered.");
2839  else if (dist < 0)
2840  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
2841  "won't be considered.");
2842  else {
2844  distNearClip = dist;
2845  for (unsigned int i = 0; i < faces.size(); i++) {
2846  faces[i]->setNearClippingDistance(distNearClip);
2847  }
2848 #ifdef VISP_HAVE_OGRE
2850 #endif
2851  }
2852 }
2853 
2861 void vpMbTracker::setClipping(const unsigned int &flags)
2862 {
2863  clippingFlag = flags;
2864  for (unsigned int i = 0; i < faces.size(); i++)
2866 }
2867 
2868 void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true,
2869  const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
2870  const vpMatrix &LVJ_true, const vpColVector &error)
2871 {
2872  if (computeCovariance) {
2873  vpMatrix D;
2874  D.diag(w_true);
2875 
2876  // Note that here the covariance is computed on cMoPrev for time
2877  // computation efficiency
2878  if (isoJoIdentity_) {
2879  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
2880  } else {
2881  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
2882  }
2883  }
2884 }
2885 
2899 void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
2900 {
2901  if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
2902  throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
2903  }
2904 
2905  JTR.resize(6, false);
2906 
2907  bool checkSSE2 = vpCPUFeatures::checkSSE2();
2908 #if !VISP_HAVE_SSE2
2909  checkSSE2 = false;
2910 #endif
2911 
2912  if (checkSSE2) {
2913 #if VISP_HAVE_SSE2
2914  __m128d v_JTR_0_1 = _mm_setzero_pd();
2915  __m128d v_JTR_2_3 = _mm_setzero_pd();
2916  __m128d v_JTR_4_5 = _mm_setzero_pd();
2917 
2918  for (unsigned int i = 0; i < interaction.getRows(); i++) {
2919  const __m128d v_error = _mm_set1_pd(error[i]);
2920 
2921  __m128d v_interaction = _mm_loadu_pd(&interaction[i][0]);
2922  v_JTR_0_1 = _mm_add_pd(v_JTR_0_1, _mm_mul_pd(v_interaction, v_error));
2923 
2924  v_interaction = _mm_loadu_pd(&interaction[i][2]);
2925  v_JTR_2_3 = _mm_add_pd(v_JTR_2_3, _mm_mul_pd(v_interaction, v_error));
2926 
2927  v_interaction = _mm_loadu_pd(&interaction[i][4]);
2928  v_JTR_4_5 = _mm_add_pd(v_JTR_4_5, _mm_mul_pd(v_interaction, v_error));
2929  }
2930 
2931  _mm_storeu_pd(JTR.data, v_JTR_0_1);
2932  _mm_storeu_pd(JTR.data + 2, v_JTR_2_3);
2933  _mm_storeu_pd(JTR.data + 4, v_JTR_4_5);
2934 #endif
2935  } else {
2936  const unsigned int N = interaction.getRows();
2937 
2938  for (unsigned int i = 0; i < 6; i += 1) {
2939  double ssum = 0;
2940  for (unsigned int j = 0; j < N; j += 1) {
2941  ssum += interaction[j][i] * error[j];
2942  }
2943  JTR[i] = ssum;
2944  }
2945  }
2946 }
2947 
2949  const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
2950  double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
2951  const vpColVector *const m_w_prev)
2952 {
2954  if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
2955  mu *= 10.0;
2956 
2957  if (mu > 1.0)
2958  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
2959 
2960  m_cMo = cMoPrev;
2961  error = m_error_prev;
2962  if (w != NULL && m_w_prev != NULL) {
2963  *w = *m_w_prev;
2964  }
2965  reStartFromLastIncrement = true;
2966  }
2967  }
2968 }
2969 
2970 void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L,
2971  vpMatrix &LTL, vpColVector &R, const vpColVector &error,
2972  vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v,
2973  const vpColVector *const w, vpColVector *const m_w_prev)
2974 {
2975  if (isoJoIdentity_) {
2976  LTL = L.AtA();
2977  computeJTR(L, R, LTR);
2978 
2979  switch (m_optimizationMethod) {
2981  vpMatrix LMA(LTL.getRows(), LTL.getCols());
2982  LMA.eye();
2983  vpMatrix LTLmuI = LTL + (LMA * mu);
2984  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2985 
2986  if (iter != 0)
2987  mu /= 10.0;
2988 
2989  error_prev = error;
2990  if (w != NULL && m_w_prev != NULL)
2991  *m_w_prev = *w;
2992  break;
2993  }
2994 
2996  default:
2997  v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2998  break;
2999  }
3000  } else {
3002  cVo.buildFrom(m_cMo);
3003  vpMatrix LVJ = (L * (cVo * oJo));
3004  vpMatrix LVJTLVJ = (LVJ).AtA();
3005  vpColVector LVJTR;
3006  computeJTR(LVJ, R, LVJTR);
3007 
3008  switch (m_optimizationMethod) {
3010  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
3011  LMA.eye();
3012  vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
3013  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
3014  v = cVo * v;
3015 
3016  if (iter != 0)
3017  mu /= 10.0;
3018 
3019  error_prev = error;
3020  if (w != NULL && m_w_prev != NULL)
3021  *m_w_prev = *w;
3022  break;
3023  }
3025  default:
3026  v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
3027  v = cVo * v;
3028  break;
3029  }
3030  }
3031 }
3032 
3034 {
3035  if (error.getRows() > 0)
3036  robust.MEstimator(vpRobust::TUKEY, error, w);
3037 }
3038 
3051 {
3052  vpColVector v(6);
3053  for (unsigned int i = 0; i < 6; i++)
3054  v[i] = oJo[i][i];
3055  return v;
3056 }
3057 
3074 {
3075  if (v.getRows() == 6) {
3076  isoJoIdentity = true;
3077  for (unsigned int i = 0; i < 6; i++) {
3078  // if(v[i] != 0){
3079  if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3080  oJo[i][i] = 1.0;
3081  } else {
3082  oJo[i][i] = 0.0;
3083  isoJoIdentity = false;
3084  }
3085  }
3086  }
3087 }
3088 
3089 void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
3090  std::vector<std::vector<vpPoint> > &listFaces)
3091 {
3092  listFaces.clear();
3093 
3094  // std::vector<vpPoint> revolutionAxis;
3095  // revolutionAxis.push_back(p1);
3096  // revolutionAxis.push_back(p2);
3097  // listFaces.push_back(revolutionAxis);
3098 
3099  vpColVector axis(3);
3100  axis[0] = p1.get_oX() - p2.get_oX();
3101  axis[1] = p1.get_oY() - p2.get_oY();
3102  axis[2] = p1.get_oZ() - p2.get_oZ();
3103 
3104  vpColVector randomVec(3);
3105  randomVec = 0;
3106 
3107  vpColVector axisOrtho(3);
3108 
3109  randomVec[0] = 1.0;
3110  axisOrtho = vpColVector::crossProd(axis, randomVec);
3111 
3112  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3113  randomVec = 0;
3114  randomVec[1] = 1.0;
3115  axisOrtho = vpColVector::crossProd(axis, randomVec);
3116  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3117  randomVec = 0;
3118  randomVec[2] = 1.0;
3119  axisOrtho = vpColVector::crossProd(axis, randomVec);
3120  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon())
3121  throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
3122  }
3123  }
3124 
3125  axisOrtho.normalize();
3126 
3127  vpColVector axisOrthoBis(3);
3128  axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
3129  axisOrthoBis.normalize();
3130 
3131  // First circle
3132  vpColVector p1Vec(3);
3133  p1Vec[0] = p1.get_oX();
3134  p1Vec[1] = p1.get_oY();
3135  p1Vec[2] = p1.get_oZ();
3136  vpColVector fc1 = p1Vec + axisOrtho * radius;
3137  vpColVector fc2 = p1Vec + axisOrthoBis * radius;
3138  vpColVector fc3 = p1Vec - axisOrtho * radius;
3139  vpColVector fc4 = p1Vec - axisOrthoBis * radius;
3140 
3141  vpColVector p2Vec(3);
3142  p2Vec[0] = p2.get_oX();
3143  p2Vec[1] = p2.get_oY();
3144  p2Vec[2] = p2.get_oZ();
3145  vpColVector sc1 = p2Vec + axisOrtho * radius;
3146  vpColVector sc2 = p2Vec + axisOrthoBis * radius;
3147  vpColVector sc3 = p2Vec - axisOrtho * radius;
3148  vpColVector sc4 = p2Vec - axisOrthoBis * radius;
3149 
3150  std::vector<vpPoint> pointsFace;
3151  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3152  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3153  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3154  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3155  listFaces.push_back(pointsFace);
3156 
3157  pointsFace.clear();
3158  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3159  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3160  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3161  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3162  listFaces.push_back(pointsFace);
3163 
3164  pointsFace.clear();
3165  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3166  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3167  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3168  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3169  listFaces.push_back(pointsFace);
3170 
3171  pointsFace.clear();
3172  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3173  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3174  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3175  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3176  listFaces.push_back(pointsFace);
3177 }
3178 
3188 bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
3189 {
3190  double dx = fabs(P1.get_oX() - P2.get_oX());
3191  double dy = fabs(P1.get_oY() - P2.get_oY());
3192  double dz = fabs(P1.get_oZ() - P2.get_oZ());
3193 
3194  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3195  dz <= std::numeric_limits<double>::epsilon())
3196  return true;
3197  else
3198  return false;
3199 }
3200 
3201 void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
3202  bool useLod, double minPolygonAreaThreshold,
3203  double minLineLengthThreshold)
3204 {
3205  std::vector<vpPoint> corners_without_duplicates;
3206  corners_without_duplicates.push_back(corners[0]);
3207  for (unsigned int i = 0; i < corners.size() - 1; i++) {
3208  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3209  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3210  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3211  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3212  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3213  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3214  corners_without_duplicates.push_back(corners[i + 1]);
3215  }
3216  }
3217 
3218  vpMbtPolygon polygon;
3219  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
3220  polygon.setIndex((int)idFace);
3221  polygon.setName(polygonName);
3222  polygon.setLod(useLod);
3223 
3224  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3225  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3226 
3227  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3228  polygon.addPoint(j, corners_without_duplicates[j]);
3229  }
3230 
3232 
3234  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3235 
3236  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3237  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3238 
3239  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3240  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3241 }
3242 
3243 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3244  int idFace, const std::string &polygonName, bool useLod,
3245  double minPolygonAreaThreshold)
3246 {
3247  vpMbtPolygon polygon;
3248  polygon.setNbPoint(4);
3249  polygon.setName(polygonName);
3250  polygon.setLod(useLod);
3251 
3252  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3253  // Non sense to set minLineLengthThreshold for circle
3254  // but used to be coherent when applying LOD settings for all polygons
3256 
3257  {
3258  // Create the 4 points of the circle bounding box
3259  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
3260 
3261  // Matrice de passage entre world et circle frame
3262  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
3263  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
3264  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
3265  vpRotationMatrix wRc;
3266  vpColVector x(3), y(3), z(3);
3267  // X axis is P2-P1
3268  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
3269  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
3270  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
3271  // Y axis is the normal of the plane
3272  y[0] = plane.getA() / norm_Y;
3273  y[1] = plane.getB() / norm_Y;
3274  y[2] = plane.getC() / norm_Y;
3275  // Z axis = X ^ Y
3276  z = vpColVector::crossProd(x, y);
3277  for (unsigned int i = 0; i < 3; i++) {
3278  wRc[i][0] = x[i];
3279  wRc[i][1] = y[i];
3280  wRc[i][2] = z[i];
3281  }
3282 
3283  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
3284  vpHomogeneousMatrix wMc(wtc, wRc);
3285 
3286  vpColVector c_p(4); // A point in the circle frame that is on the bbox
3287  c_p[0] = radius;
3288  c_p[1] = 0;
3289  c_p[2] = radius;
3290  c_p[3] = 1;
3291 
3292  // Matrix to rotate a point by 90 deg around Y in the circle frame
3293  for (unsigned int i = 0; i < 4; i++) {
3294  vpColVector w_p(4); // A point in the word frame
3296  w_p = wMc * cMc_90 * c_p;
3297 
3298  vpPoint w_P;
3299  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
3300 
3301  polygon.addPoint(i, w_P);
3302  }
3303  }
3304 
3305  polygon.setIndex(idFace);
3307 
3309  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3310 
3311  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3312  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3313 
3314  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3315  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3316 }
3317 
3318 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
3319  bool useLod, double minLineLengthThreshold)
3320 {
3321  // A polygon as a single line that corresponds to the revolution axis of the
3322  // cylinder
3323  vpMbtPolygon polygon;
3324  polygon.setNbPoint(2);
3325 
3326  polygon.addPoint(0, p1);
3327  polygon.addPoint(1, p2);
3328 
3329  polygon.setIndex(idFace);
3330  polygon.setName(polygonName);
3331  polygon.setLod(useLod);
3332 
3333  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3334  // Non sense to set minPolygonAreaThreshold for cylinder
3335  // but used to be coherent when applying LOD settings for all polygons
3337 
3339 
3341  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3342 
3343  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3344  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3345 
3346  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3347  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3348 }
3349 
3350 void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
3351  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3352 {
3353  int id = idFace;
3354  for (unsigned int i = 0; i < listFaces.size(); i++) {
3355  vpMbtPolygon polygon;
3356  polygon.setNbPoint((unsigned int)listFaces[i].size());
3357  for (unsigned int j = 0; j < listFaces[i].size(); j++)
3358  polygon.addPoint(j, listFaces[i][j]);
3359 
3360  polygon.setIndex(id);
3361  polygon.setName(polygonName);
3362  polygon.setIsPolygonOriented(false);
3363  polygon.setLod(useLod);
3364  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3366 
3368 
3370  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3371 
3372  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3373  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3374 
3375  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3376  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3377 
3378  id++;
3379  }
3380 }
3381 
3382 void vpMbTracker::addProjectionErrorLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
3383 {
3384  // suppress line already in the model
3385  bool already_here = false;
3386  vpMbtDistanceLine *l;
3387 
3388  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3389  l = *it;
3390  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
3391  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
3392  already_here = true;
3393  l->addPolygon(polygon);
3395  }
3396  }
3397 
3398  if (!already_here) {
3399  l = new vpMbtDistanceLine;
3400 
3402  l->buildFrom(P1, P2);
3403  l->addPolygon(polygon);
3406  l->useScanLine = useScanLine;
3407 
3408  l->setIndex((unsigned int)m_projectionErrorLines.size());
3409  l->setName(name);
3410 
3413 
3414  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3416 
3417  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3419 
3420  m_projectionErrorLines.push_back(l);
3421  }
3422 }
3423 
3424 void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
3425  const std::string &name)
3426 {
3427  bool already_here = false;
3428  vpMbtDistanceCircle *ci;
3429 
3430  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3431  ci = *it;
3432  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
3433  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
3434  already_here =
3435  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
3436  }
3437  }
3438 
3439  if (!already_here) {
3440  ci = new vpMbtDistanceCircle;
3441 
3443  ci->buildFrom(P1, P2, P3, r);
3445  ci->setIndex((unsigned int)m_projectionErrorCircles.size());
3446  ci->setName(name);
3447  ci->index_polygon = idFace;
3449 
3450  m_projectionErrorCircles.push_back(ci);
3451  }
3452 }
3453 
3454 void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace,
3455  const std::string &name)
3456 {
3457  bool already_here = false;
3459 
3460  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3461  ++it) {
3462  cy = *it;
3463  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
3464  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
3465  already_here =
3466  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
3467  }
3468  }
3469 
3470  if (!already_here) {
3471  cy = new vpMbtDistanceCylinder;
3472 
3474  cy->buildFrom(P1, P2, r);
3476  cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
3477  cy->setName(name);
3478  cy->index_polygon = idFace;
3480  m_projectionErrorCylinders.push_back(cy);
3481  }
3482 }
3483 
3484 void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
3485  double radius, int idFace, const std::string &name)
3486 {
3487  addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
3488 }
3489 
3490 void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius,
3491  int idFace, const std::string &name)
3492 {
3493  addProjectionErrorCylinder(p1, p2, radius, idFace, name);
3494 }
3495 
3497 {
3498  unsigned int nbpt = polygon.getNbPoint();
3499  if (nbpt > 0) {
3500  for (unsigned int i = 0; i < nbpt - 1; i++)
3501  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3502  addProjectionErrorLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
3503  }
3504 }
3505 
3507 {
3508  unsigned int nbpt = polygon.getNbPoint();
3509  if (nbpt > 0) {
3510  for (unsigned int i = 0; i < nbpt - 1; i++)
3511  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3512  }
3513 }
3514 
3533  const vpCameraParameters &_cam)
3534 {
3535  if (!modelInitialised) {
3536  throw vpException(vpException::fatalError, "model not initialized");
3537  }
3538 
3539  unsigned int nbFeatures = 0;
3540  double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);
3541 
3542  if (nbFeatures > 0) {
3543  return vpMath::deg(totalProjectionError / (double)nbFeatures);
3544  }
3545 
3546  return 90.0;
3547 }
3548 
3550  const vpCameraParameters &_cam, unsigned int &nbFeatures)
3551 {
3552  bool update_cam = m_projectionErrorCam != _cam;
3553  if (update_cam) {
3554  m_projectionErrorCam = _cam;
3555 
3556  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3557  it != m_projectionErrorLines.end(); ++it) {
3558  vpMbtDistanceLine *l = *it;
3560  }
3561 
3562  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3563  it != m_projectionErrorCylinders.end(); ++it) {
3564  vpMbtDistanceCylinder *cy = *it;
3566  }
3567 
3568  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3569  it != m_projectionErrorCircles.end(); ++it) {
3570  vpMbtDistanceCircle *ci = *it;
3572  }
3573  }
3574 
3575 #ifdef VISP_HAVE_OGRE
3576  if (useOgre) {
3577  if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
3581  // Turn off Ogre config dialog display for the next call to this
3582  // function since settings are saved in the ogre.cfg file and used
3583  // during the next call
3585  }
3586  }
3587 #endif
3588 
3589  if (clippingFlag > 2)
3591 
3593 
3595 
3596  if (useScanLine) {
3597  if (clippingFlag <= 2)
3599 
3602  }
3603 
3605 
3606  double totalProjectionError = 0.0;
3607  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end();
3608  ++it) {
3609  vpMbtDistanceLine *l = *it;
3610  if (l->isVisible() && l->isTracked()) {
3611  for (size_t a = 0; a < l->meline.size(); a++) {
3612  if (l->meline[a] != NULL) {
3613  double lineNormGradient;
3614  unsigned int lineNbFeatures;
3615  l->meline[a]->computeProjectionError(I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
3618  totalProjectionError += lineNormGradient;
3619  nbFeatures += lineNbFeatures;
3620  }
3621  }
3622  }
3623  }
3624 
3625  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3626  it != m_projectionErrorCylinders.end(); ++it) {
3627  vpMbtDistanceCylinder *cy = *it;
3628  if (cy->isVisible() && cy->isTracked()) {
3629  if (cy->meline1 != NULL) {
3630  double cylinderNormGradient = 0;
3631  unsigned int cylinderNbFeatures = 0;
3632  cy->meline1->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3635  totalProjectionError += cylinderNormGradient;
3636  nbFeatures += cylinderNbFeatures;
3637  }
3638 
3639  if (cy->meline2 != NULL) {
3640  double cylinderNormGradient = 0;
3641  unsigned int cylinderNbFeatures = 0;
3642  cy->meline2->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3645  totalProjectionError += cylinderNormGradient;
3646  nbFeatures += cylinderNbFeatures;
3647  }
3648  }
3649  }
3650 
3651  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3652  it != m_projectionErrorCircles.end(); ++it) {
3653  vpMbtDistanceCircle *c = *it;
3654  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
3655  double circleNormGradient = 0;
3656  unsigned int circleNbFeatures = 0;
3657  c->meEllipse->computeProjectionError(I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
3660  totalProjectionError += circleNormGradient;
3661  nbFeatures += circleNbFeatures;
3662  }
3663  }
3664 
3665  return totalProjectionError;
3666 }
3667 
3668 void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
3669 {
3670  bool changed = false;
3671 
3672  if (!useOgre) {
3674  } else {
3675 #ifdef VISP_HAVE_OGRE
3677 #else
3679 #endif
3680  }
3681 }
3682 
3684 {
3685  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3686  for (size_t a = 0; a < (*it)->meline.size(); a++) {
3687  if ((*it)->meline[a] != NULL) {
3688  delete (*it)->meline[a];
3689  (*it)->meline[a] = NULL;
3690  }
3691  }
3692 
3693  (*it)->meline.clear();
3694  (*it)->nbFeature.clear();
3695  (*it)->nbFeatureTotal = 0;
3696  }
3697 
3698  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3699  ++it) {
3700  if ((*it)->meline1 != NULL) {
3701  delete (*it)->meline1;
3702  (*it)->meline1 = NULL;
3703  }
3704  if ((*it)->meline2 != NULL) {
3705  delete (*it)->meline2;
3706  (*it)->meline2 = NULL;
3707  }
3708 
3709  (*it)->nbFeature = 0;
3710  (*it)->nbFeaturel1 = 0;
3711  (*it)->nbFeaturel2 = 0;
3712  }
3713 
3714  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3715  if ((*it)->meEllipse != NULL) {
3716  delete (*it)->meEllipse;
3717  (*it)->meEllipse = NULL;
3718  }
3719  (*it)->nbFeature = 0;
3720  }
3721 }
3722 
3724 {
3725  const bool doNotTrack = true;
3726 
3727  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3728  it != m_projectionErrorLines.end(); ++it) {
3729  vpMbtDistanceLine *l = *it;
3730  bool isvisible = false;
3731 
3732  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
3733  ++itindex) {
3734  int index = *itindex;
3735  if (index == -1)
3736  isvisible = true;
3737  else {
3738  if (l->hiddenface->isVisible((unsigned int)index))
3739  isvisible = true;
3740  }
3741  }
3742 
3743  // Si la ligne n'appartient a aucune face elle est tout le temps visible
3744  if (l->Lindex_polygon.empty())
3745  isvisible = true; // Not sure that this can occur
3746 
3747  if (isvisible) {
3748  l->setVisible(true);
3749  l->updateTracked();
3750  if (l->meline.empty() && l->isTracked())
3751  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3752  } else {
3753  l->setVisible(false);
3754  for (size_t a = 0; a < l->meline.size(); a++) {
3755  if (l->meline[a] != NULL)
3756  delete l->meline[a];
3757  if (a < l->nbFeature.size())
3758  l->nbFeature[a] = 0;
3759  }
3760  l->nbFeatureTotal = 0;
3761  l->meline.clear();
3762  l->nbFeature.clear();
3763  }
3764  }
3765 
3766  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3767  it != m_projectionErrorCylinders.end(); ++it) {
3768  vpMbtDistanceCylinder *cy = *it;
3769 
3770  bool isvisible = false;
3771 
3772  int index = cy->index_polygon;
3773  if (index == -1)
3774  isvisible = true;
3775  else {
3776  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
3777  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
3778  isvisible = true;
3779  }
3780 
3781  if (isvisible) {
3782  cy->setVisible(true);
3783  if (cy->meline1 == NULL || cy->meline2 == NULL) {
3784  if (cy->isTracked())
3785  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3786  }
3787  } else {
3788  cy->setVisible(false);
3789  if (cy->meline1 != NULL)
3790  delete cy->meline1;
3791  if (cy->meline2 != NULL)
3792  delete cy->meline2;
3793  cy->meline1 = NULL;
3794  cy->meline2 = NULL;
3795  cy->nbFeature = 0;
3796  cy->nbFeaturel1 = 0;
3797  cy->nbFeaturel2 = 0;
3798  }
3799  }
3800 
3801  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3802  it != m_projectionErrorCircles.end(); ++it) {
3803  vpMbtDistanceCircle *ci = *it;
3804  bool isvisible = false;
3805 
3806  int index = ci->index_polygon;
3807  if (index == -1)
3808  isvisible = true;
3809  else {
3810  if (ci->hiddenface->isVisible((unsigned int)index))
3811  isvisible = true;
3812  }
3813 
3814  if (isvisible) {
3815  ci->setVisible(true);
3816  if (ci->meEllipse == NULL) {
3817  if (ci->isTracked())
3818  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3819  }
3820  } else {
3821  ci->setVisible(false);
3822  if (ci->meEllipse != NULL)
3823  delete ci->meEllipse;
3824  ci->meEllipse = NULL;
3825  ci->nbFeature = 0;
3826  }
3827  }
3828 }
3829 
3830 void vpMbTracker::loadConfigFile(const std::string &configFile)
3831 {
3832 #ifdef VISP_HAVE_PUGIXML
3836 
3837  try {
3838  std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl;
3839  xmlp.parse(configFile);
3840  } catch (...) {
3841  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
3842  }
3843 
3844  vpMe meParser;
3845  xmlp.getProjectionErrorMe(meParser);
3846 
3847  setProjectionErrorMovingEdge(meParser);
3849 
3850 #else
3851  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
3852 #endif
3853 }
3854 
3861 {
3862  m_projectionErrorMe = me;
3863 
3864  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3865  vpMbtDistanceLine *l = *it;
3867  }
3868 
3869  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3870  ++it) {
3871  vpMbtDistanceCylinder *cy = *it;
3873  }
3874 
3875  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3876  vpMbtDistanceCircle *ci = *it;
3878  }
3879 }
3880 
3886 void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
3887 {
3889 
3890  m_SobelX.resize(size*2+1, size*2+1, false, false);
3892 
3893  m_SobelY.resize(size*2+1, size*2+1, false, false);
3895 }
3896 
static double getSobelKernelY(double *filter, unsigned int size)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
vpDisplay * display
Definition: vpImage.h:144
void addPoint(unsigned int n, const vpPoint &P)
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2449
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
virtual ~vpMbTracker()
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:449
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:177
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
unsigned int nbLines
Number of lines in CAO model.
Definition: vpMbTracker.h:162
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
bool isVisible(unsigned int i)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double frobeniusNorm() const
void setCameraParameters(const vpCameraParameters &camera)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
int getIndex() const
Definition: vpMbtPolygon.h:101
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int size() const
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setIndex(unsigned int i)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::map< std::string, std::string > parseParameters(std::string &endLine)
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1487
unsigned int nbCircles
Number of circles in CAO model.
Definition: vpMbTracker.h:170
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
void setNearClippingDistance(const double &dist)
Definition: vpAROgre.h:210
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
std::map< std::string, std::string > mapOfParameterNames
Definition: vpMbTracker.h:182
std::string getName() const
Definition: vpMbtPolygon.h:108
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isTracked() const
unsigned int nbCylinders
Number of cylinders in CAO model.
Definition: vpMbTracker.h:168
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
vpMatrix AtA() const
Definition: vpMatrix.cpp:736
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
#define vpERROR_TRACE
Definition: vpDebug.h:393
unsigned int nbPoints
Number of points in CAO model.
Definition: vpMbTracker.h:160
virtual void loadConfigFile(const std::string &configFile)
virtual void setNbPoint(unsigned int nb)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpPoint * p1
The first extremity.
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
Definition: vpMbTracker.h:205
void setProjectionErrorKernelSize(const unsigned int &size)
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
bool modelInitialised
Definition: vpMbTracker.h:123
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Definition: vpArray2D.h:289
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition: vpMe.h:60
Manage the line of a polygon used in the model-based tracker.
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)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int nbFeature
The number of moving edges.
void setName(const std::string &face_name)
Definition: vpMbtPolygon.h:159
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:94
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
std::string modelFileName
Definition: vpMbTracker.h:120
virtual void setEstimatedDoF(const vpColVector &v)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1382
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
static const vpColor green
Definition: vpColor.h:220
vpPoint * p1
The first extremity on the axe.
static void flush(const vpImage< unsigned char > &I)
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:447
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void setLod(bool useLod, const std::string &name="")
void setFarClippingDistance(const double &dist)
Definition: vpAROgre.h:199
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
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:841
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:497
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)=0
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:499
unsigned int getCols() const
Definition: vpArray2D.h:279
bool m_projectionErrorOgreShowConfigDialog
Definition: vpMbTracker.h:203
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
static bool parseBoolean(std::string input)
Definition: vpIoTools.cpp:1928
vpColVector & normalize()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1446
Error that can be emited by the vpTracker class and its derivates.
void setProjectionErrorKernelSize(const unsigned int &size)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setName(const std::string &circle_name)
vpPoint * p2
The second extremity.
void projectionErrorResetMovingEdges()
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:994
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:116
void setProjectionErrorMe(const vpMe &me)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
VISP_EXPORT bool checkSSE2()
static void display(const vpImage< unsigned char > &I)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
void clear()
Definition: vpColVector.h:175
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
double getB() const
Definition: vpPlane.h:104
void setMinPolygonAreaThresh(double min_polygon_area)
Definition: vpMbtPolygon.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
Generic class defining intrinsic camera parameters.
void setOgreShowConfigDialog(bool showConfigDialog)
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Main methods for a model-based tracker.
Definition: vpMbTracker.h:104
std::vector< PolygonType * > & getPolygon()
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:451
virtual void loadVRMLModel(const std::string &modelFile)
Implementation of a rotation vector as quaternion angle minimal representation.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
void setIsPolygonOriented(const bool &oriented)
Definition: vpMbtPolygon.h:166
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1676
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
Definition: vpMbTracker.h:164
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
void addPolygon(PolygonType *p)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1347
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
static double rad(double deg)
Definition: vpMath.h:110
std::string poseSavingFilename
Definition: vpMbTracker.h:126
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
Definition: vpMbTracker.h:196
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)
void setVisible(bool _isvisible)
int getWindowXPosition() const
Definition: vpDisplay.h:251
double getA() const
Definition: vpPlane.h:102
void setName(const std::string &line_name)
int getWindowYPosition() const
Definition: vpDisplay.h:256
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
void setCameraParameters(const vpCameraParameters &camera)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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=NULL, vpColVector *const m_w_prev=NULL)
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
Definition: vpMbTracker.h:166
bool isVisible() const
static std::string trim(std::string s)
Definition: vpIoTools.cpp:1942
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:103
void setMinLineLengthThresh(double min_line_length)
Definition: vpMbtPolygon.h:141
virtual void setOgreVisibilityTest(const bool &v)
unsigned int getHeight() const
Definition: vpImage.h:188
void setIndex(unsigned int i)
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:244
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
Definition: vpMbTracker.h:219
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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))
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
void setMovingEdge(vpMe *Me)
void setLod(bool use_lod)
void setProjectionErrorMovingEdge(const vpMe &me)
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double sumSquare() const
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
error that can be emited by the vpMatrix class and its derivates
Tukey influence function.
Definition: vpRobust.h:93
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual vpColVector getEstimatedDoF() const
double getC() const
Definition: vpPlane.h:106
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void addPolygon(const int &index)
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:640
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
static std::string getAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1404
static double getSobelKernelX(double *filter, unsigned int size)
virtual void setClipping(const unsigned int &flags)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
double radius
The radius of the cylinder.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
std::vector< vpMbtMeLine * > meline
The moving edge container.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
unsigned int getProjectionErrorKernelSize() const
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
unsigned int nbFeaturel2
The number of moving edges on line 2.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
Definition: vpMbTracker.h:207
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...
Definition: vpPose.cpp:336
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void savePose(const std::string &filename) const
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
Definition: vpMbTracker.h:200
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:246
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< unsigned int > nbFeature
The number of moving edges.
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Class that consider the case of a translation vector.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void eye()
Definition: vpMatrix.cpp:511
Implementation of a rotation vector as axis-angle minimal representation.
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
virtual void setIndex(int i)
Definition: vpMbtPolygon.h:124
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
vpPoint * p1
The center of the circle.
void removeComment(std::ifstream &fileId)
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:202
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setIndex(unsigned int i)
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
Definition: vpMbTracker.h:198
void getProjectionErrorMe(vpMe &me) const
void clearPoint()
Definition: vpPose.cpp:134
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)