Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 #include <map>
50 
51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDisplay.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpMatrix.h>
55 #include <visp3/core/vpPoint.h>
56 #include <visp3/vision/vpPose.h>
57 #ifdef VISP_HAVE_MODULE_GUI
58 #include <visp3/gui/vpDisplayGDI.h>
59 #include <visp3/gui/vpDisplayOpenCV.h>
60 #include <visp3/gui/vpDisplayX.h>
61 #endif
62 #include <visp3/core/vpCameraParameters.h>
63 #include <visp3/core/vpColor.h>
64 #include <visp3/core/vpException.h>
65 #include <visp3/core/vpIoTools.h>
66 #include <visp3/core/vpPixelMeterConversion.h>
67 #ifdef VISP_HAVE_MODULE_IO
68 #include <visp3/io/vpImageIo.h>
69 #endif
70 #include <visp3/core/vpCPUFeatures.h>
71 #include <visp3/core/vpIoTools.h>
72 #include <visp3/core/vpMatrixException.h>
73 #include <visp3/core/vpTrackingException.h>
74 #include <visp3/mbt/vpMbTracker.h>
75 
76 #include <visp3/core/vpImageFilter.h>
77 #include <visp3/mbt/vpMbtXmlGenericParser.h>
78 
79 #ifdef VISP_HAVE_COIN3D
80 // Inventor includes
81 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
82 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
83 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
84 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
85 #include <Inventor/VRMLnodes/SoVRMLShape.h>
86 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
87 #include <Inventor/actions/SoGetMatrixAction.h>
88 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
89 #include <Inventor/actions/SoSearchAction.h>
90 #include <Inventor/actions/SoToVRML2Action.h>
91 #include <Inventor/actions/SoWriteAction.h>
92 #include <Inventor/misc/SoChildList.h>
93 #include <Inventor/nodes/SoSeparator.h>
94 #endif
95 
96 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
97 #include <emmintrin.h>
98 #define VISP_HAVE_SSE2 1
99 #endif
100 
101 #ifndef DOXYGEN_SHOULD_SKIP_THIS
102 
103 namespace
104 {
108 struct SegmentInfo {
109  SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
110 
111  std::vector<vpPoint> extremities;
112  std::string name;
113  bool useLod;
114  double minLineLengthThresh;
115 };
116 
121 struct PolygonFaceInfo {
122  PolygonFaceInfo(const double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
123  : distanceToCamera(dist), polygon(poly), faceCorners(corners)
124  {
125  }
126 
127  bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }
128 
129  double distanceToCamera;
130  vpPolygon polygon;
131  std::vector<vpPoint> faceCorners;
132 };
133 }
134 #endif // DOXYGEN_SHOULD_SKIP_THIS
135 
142  : cam(), cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
143  computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
144  displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
145  angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
146  useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
147  nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
148  minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
149  m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
150  m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(),
151  m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
152  m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
153  m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
154  m_projectionErrorCam(), m_mask(NULL)
155 {
156  oJo.eye();
157  // Map used to parse additional information in CAO model files,
158  // like name of faces or LOD setting
159  mapOfParameterNames["name"] = "string";
160  mapOfParameterNames["minPolygonAreaThreshold"] = "number";
161  mapOfParameterNames["minLineLengthThreshold"] = "number";
162  mapOfParameterNames["useLod"] = "boolean";
163 
166 }
167 
169  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
170  vpMbtDistanceLine *l = *it;
171  if (l != NULL)
172  delete l;
173  l = NULL;
174  }
175 
176  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end(); ++it) {
177  vpMbtDistanceCylinder *cy = *it;
178  if (cy != NULL)
179  delete cy;
180  cy = NULL;
181  }
182 
183  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
184  vpMbtDistanceCircle *ci = *it;
185  if (ci != NULL)
186  delete ci;
187  ci = NULL;
188  }
189 }
190 
191 #ifdef VISP_HAVE_MODULE_GUI
192 
223 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp,
224  const vpHomogeneousMatrix &T)
225 {
226  vpHomogeneousMatrix last_cMo;
227  vpPoseVector init_pos;
228  vpImagePoint ip;
230 
231  std::string ext = ".init";
232  std::string str_pose = "";
233  size_t pos = initFile.rfind(ext);
234 
235  // Load the last poses from files
236  std::fstream finitpos;
237  std::ifstream finit;
238  char s[FILENAME_MAX];
239  if (poseSavingFilename.empty()) {
240  if (pos != std::string::npos)
241  str_pose = initFile.substr(0, pos) + ".0.pos";
242  else
243  str_pose = initFile + ".0.pos";
244 
245  finitpos.open(str_pose.c_str(), std::ios::in);
246  sprintf(s, "%s", str_pose.c_str());
247  } else {
248  finitpos.open(poseSavingFilename.c_str(), std::ios::in);
249  sprintf(s, "%s", poseSavingFilename.c_str());
250  }
251  if (finitpos.fail()) {
252  std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
253  last_cMo.eye();
254  } else {
255  for (unsigned int i = 0; i < 6; i += 1) {
256  finitpos >> init_pos[i];
257  }
258 
259  finitpos.close();
260  last_cMo.buildFrom(init_pos);
261 
262  std::cout << "last_cMo : " << std::endl << last_cMo << std::endl;
263 
265  display(I, last_cMo, cam, vpColor::green, 1, true);
266  vpDisplay::displayFrame(I, last_cMo, cam, 0.05, vpColor::green);
267  vpDisplay::flush(I);
268 
269  std::cout << "No modification : left click " << std::endl;
270  std::cout << "Modify initial pose : right click " << std::endl;
271 
272  vpDisplay::displayText(I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
273 
274  vpDisplay::flush(I);
275 
276  while (!vpDisplay::getClick(I, ip, button))
277  ;
278  }
279 
280  if (!finitpos.fail() && button == vpMouseButton::button1) {
281  cMo = last_cMo;
282  } else {
283  vpDisplay *d_help = NULL;
284 
286  vpDisplay::flush(I);
287 
288  vpPose pose;
289 
290  pose.clearPoint();
291 
292  // file parser
293  // number of points
294  // X Y Z
295  // X Y Z
296  if (pos != std::string::npos)
297  sprintf(s, "%s", initFile.c_str());
298  else
299  sprintf(s, "%s.init", initFile.c_str());
300 
301  std::cout << "Load 3D points from: " << s << std::endl;
302  finit.open(s);
303  if (finit.fail()) {
304  std::cout << "cannot read " << s << std::endl;
305  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
306  }
307 
308 #ifdef VISP_HAVE_MODULE_IO
309  // Display window creation and initialisation
310  try {
311  if (displayHelp) {
312  const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"};
313  std::string dispF;
314  bool foundHelpImg = false;
315  if (pos != std::string::npos) {
316  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
317  dispF = initFile.substr(0, pos) + imgExtVec[i];
318  foundHelpImg = vpIoTools::checkFilename(dispF);
319  }
320  } else {
321  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
322  dispF = initFile + imgExtVec[i];
323  foundHelpImg = vpIoTools::checkFilename(dispF);
324  }
325  }
326 
327  if (foundHelpImg) {
328  std::cout << "Load image to help initialization: " << dispF << std::endl;
329 #if defined VISP_HAVE_X11
330  d_help = new vpDisplayX;
331 #elif defined VISP_HAVE_GDI
332  d_help = new vpDisplayGDI;
333 #elif defined VISP_HAVE_OPENCV
334  d_help = new vpDisplayOpenCV;
335 #endif
336 
337  vpImage<vpRGBa> Iref;
338  vpImageIo::read(Iref, dispF);
339 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
340  d_help->init(Iref, I.display->getWindowXPosition() + (int)I.getWidth() + 80, I.display->getWindowYPosition(),
341  "Where to initialize...");
342  vpDisplay::display(Iref);
343  vpDisplay::flush(Iref);
344 #endif
345  }
346  }
347  } catch (...) {
348  if (d_help != NULL) {
349  delete d_help;
350  d_help = NULL;
351  }
352  }
353 #else //#ifdef VISP_HAVE_MODULE_IO
354  (void)(displayHelp);
355 #endif //#ifdef VISP_HAVE_MODULE_IO
356  // skip lines starting with # as comment
357  removeComment(finit);
358 
359  unsigned int n3d;
360  finit >> n3d;
361  finit.ignore(256, '\n'); // skip the rest of the line
362  std::cout << "Number of 3D points " << n3d << std::endl;
363  if (n3d > 100000) {
364  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
365  }
366 
367  std::vector<vpPoint> P(n3d);
368  for (unsigned int i = 0; i < n3d; i++) {
369  // skip lines starting with # as comment
370  removeComment(finit);
371 
372  vpColVector pt_3d(4, 1.0);
373  finit >> pt_3d[0];
374  finit >> pt_3d[1];
375  finit >> pt_3d[2];
376  finit.ignore(256, '\n'); // skip the rest of the line
377 
378  vpColVector pt_3d_tf = T*pt_3d;
379  std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " " << pt_3d_tf[2] << std::endl;
380 
381  P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
382  }
383 
384  finit.close();
385 
386  bool isWellInit = false;
387  while (!isWellInit) {
388  std::vector<vpImagePoint> mem_ip;
389  for (unsigned int i = 0; i < n3d; i++) {
390  std::ostringstream text;
391  text << "Click on point " << i + 1;
393  vpDisplay::displayText(I, 15, 10, text.str(), vpColor::red);
394  for (unsigned int k = 0; k < mem_ip.size(); k++) {
395  vpDisplay::displayCross(I, mem_ip[k], 10, vpColor::green, 2);
396  }
397  vpDisplay::flush(I);
398 
399  std::cout << "Click on point " << i + 1 << " ";
400  double x = 0, y = 0;
401  vpDisplay::getClick(I, ip);
402  mem_ip.push_back(ip);
403  vpDisplay::flush(I);
405  P[i].set_x(x);
406  P[i].set_y(y);
407 
408  std::cout << "with 2D coordinates: " << ip << std::endl;
409 
410  pose.addPoint(P[i]); // and added to the pose computation point list
411  }
412  vpDisplay::flush(I);
414 
415  vpHomogeneousMatrix cMo1, cMo2;
416  double d1, d2;
417  d1 = d2 = std::numeric_limits<double>::max();
418  try {
419  pose.computePose(vpPose::LAGRANGE, cMo1);
420  d1 = pose.computeResidual(cMo1);
421  }
422  catch(...) {
423  // Lagrange non-planar cannot work with less than 6 points
424  }
425  try {
426  pose.computePose(vpPose::DEMENTHON, cMo2);
427  d2 = pose.computeResidual(cMo2);
428  }
429  catch(...) {
430  // Should not occur
431  }
432 
433  if (d1 < d2) {
434  cMo = cMo1;
435  } else {
436  cMo = cMo2;
437  }
439 
440  display(I, cMo, cam, vpColor::green, 1, true);
441  vpDisplay::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
442 
443  vpDisplay::flush(I);
444 
445  button = vpMouseButton::button1;
446  while (!vpDisplay::getClick(I, ip, button))
447  ;
448 
449  if (button == vpMouseButton::button1) {
450  isWellInit = true;
451  } else {
452  pose.clearPoint();
454  vpDisplay::flush(I);
455  }
456  }
458 
459  // save the pose into file
460  if (poseSavingFilename.empty())
461  savePose(str_pose);
462  else
464 
465  if (d_help != NULL) {
466  delete d_help;
467  d_help = NULL;
468  }
469  }
470 
471  std::cout << "cMo : " << std::endl << cMo << std::endl;
472 
473  init(I);
474 }
475 
486 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
487  const std::string &displayFile)
488 {
490  vpDisplay::flush(I);
491  vpDisplay *d_help = NULL;
492 
493  vpPose pose;
494  std::vector<vpPoint> P;
495  for (unsigned int i = 0; i < points3D_list.size(); i++)
496  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
497 
498 #ifdef VISP_HAVE_MODULE_IO
499  vpImage<vpRGBa> Iref;
500  // Display window creation and initialisation
501  if (vpIoTools::checkFilename(displayFile)) {
502  try {
503  std::cout << "Load image to help initialization: " << displayFile << std::endl;
504 #if defined VISP_HAVE_X11
505  d_help = new vpDisplayX;
506 #elif defined VISP_HAVE_GDI
507  d_help = new vpDisplayGDI;
508 #elif defined VISP_HAVE_OPENCV
509  d_help = new vpDisplayOpenCV;
510 #endif
511 
512  vpImageIo::read(Iref, displayFile);
513 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
514  d_help->init(Iref, I.display->getWindowXPosition() + (int)I.getWidth() + 80, I.display->getWindowYPosition(),
515  "Where to initialize...");
516  vpDisplay::display(Iref);
517  vpDisplay::flush(Iref);
518 #endif
519  } catch (...) {
520  if (d_help != NULL) {
521  delete d_help;
522  d_help = NULL;
523  }
524  }
525  }
526 #else //#ifdef VISP_HAVE_MODULE_IO
527  (void)(displayFile);
528 #endif //#ifdef VISP_HAVE_MODULE_IO
529 
530  vpImagePoint ip;
531  bool isWellInit = false;
532  while (!isWellInit) {
533  for (unsigned int i = 0; i < points3D_list.size(); i++) {
534  std::cout << "Click on point " << i + 1 << std::endl;
535  double x = 0, y = 0;
536  vpDisplay::getClick(I, ip);
538  vpDisplay::flush(I);
540  P[i].set_x(x);
541  P[i].set_y(y);
542 
543  std::cout << "Click on point " << ip << std::endl;
544 
545  vpDisplay::displayPoint(I, ip, vpColor::green); // display target point
546  pose.addPoint(P[i]); // and added to the pose computation point list
547  }
548  vpDisplay::flush(I);
549 
550  vpHomogeneousMatrix cMo1, cMo2;
551  double d1, d2;
552  d1 = d2 = std::numeric_limits<double>::max();
553  try {
554  pose.computePose(vpPose::LAGRANGE, cMo1);
555  d1 = pose.computeResidual(cMo1);
556  }
557  catch(...) {
558  // Lagrange non-planar cannot work with less than 6 points
559  }
560  try {
561  pose.computePose(vpPose::DEMENTHON, cMo2);
562  d2 = pose.computeResidual(cMo2);
563  }
564  catch(...) {
565  // Should not occur
566  }
567 
568  if (d1 < d2) {
569  cMo = cMo1;
570  } else {
571  cMo = cMo2;
572  }
574 
575  display(I, cMo, cam, vpColor::green, 1, true);
576  vpDisplay::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
577 
578  vpDisplay::flush(I);
579 
581  while (!vpDisplay::getClick(I, ip, button)) {
582  };
583 
584  if (button == vpMouseButton::button1) {
585  isWellInit = true;
586  } else {
587  pose.clearPoint();
589  vpDisplay::flush(I);
590  }
591  }
592 
594 
595  if (d_help != NULL) {
596  delete d_help;
597  d_help = NULL;
598  }
599 
600  init(I);
601 }
602 #endif //#ifdef VISP_HAVE_MODULE_GUI
603 
628 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
629 {
630  char s[FILENAME_MAX];
631  std::fstream finit;
632 
633  std::string ext = ".init";
634  size_t pos = initFile.rfind(ext);
635 
636  if (pos == initFile.size() - ext.size() && pos != 0)
637  sprintf(s, "%s", initFile.c_str());
638  else
639  sprintf(s, "%s.init", initFile.c_str());
640 
641  std::cout << "Load 2D/3D points from: " << s << std::endl;
642  finit.open(s, std::ios::in);
643  if (finit.fail()) {
644  std::cout << "cannot read " << s << std::endl;
645  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
646  }
647 
648  //********
649  // Read 3D points coordinates
650  //********
651  char c;
652  // skip lines starting with # as comment
653  finit.get(c);
654  while (!finit.fail() && (c == '#')) {
655  finit.ignore(256, '\n');
656  finit.get(c);
657  }
658  finit.unget();
659 
660  unsigned int n3d;
661  finit >> n3d;
662  finit.ignore(256, '\n'); // skip the rest of the line
663  std::cout << "Number of 3D points " << n3d << std::endl;
664  if (n3d > 100000) {
665  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
666  }
667 
668  vpPoint *P = new vpPoint[n3d];
669  for (unsigned int i = 0; i < n3d; i++) {
670  // skip lines starting with # as comment
671  finit.get(c);
672  while (!finit.fail() && (c == '#')) {
673  finit.ignore(256, '\n');
674  finit.get(c);
675  }
676  finit.unget();
677  double X, Y, Z;
678  finit >> X;
679  finit >> Y;
680  finit >> Z;
681  finit.ignore(256, '\n'); // skip the rest of the line
682 
683  std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
684  P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
685  }
686 
687  //********
688  // Read 3D points coordinates
689  //********
690  // skip lines starting with # as comment
691  finit.get(c);
692  while (!finit.fail() && (c == '#')) {
693  finit.ignore(256, '\n');
694  finit.get(c);
695  }
696  finit.unget();
697 
698  unsigned int n2d;
699  finit >> n2d;
700  finit.ignore(256, '\n'); // skip the rest of the line
701  std::cout << "Number of 2D points " << n2d << std::endl;
702  if (n2d > 100000) {
703  delete[] P;
704  throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed", s);
705  }
706 
707  if (n3d != n2d) {
708  delete[] P;
710  "In %s file, number of 2D points %d and number of 3D "
711  "points %d are not equal",
712  s, n2d, n3d);
713  }
714 
715  vpPose pose;
716  for (unsigned int i = 0; i < n2d; i++) {
717  // skip lines starting with # as comment
718  finit.get(c);
719  while (!finit.fail() && (c == '#')) {
720  finit.ignore(256, '\n');
721  finit.get(c);
722  }
723  finit.unget();
724  double u, v, x = 0, y = 0;
725  finit >> v;
726  finit >> u;
727  finit.ignore(256, '\n'); // skip the rest of the line
728 
729  vpImagePoint ip(v, u);
730  std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
732  P[i].set_x(x);
733  P[i].set_y(y);
734  pose.addPoint(P[i]);
735  }
736 
737  finit.close();
738 
739  vpHomogeneousMatrix cMo1, cMo2;
740  double d1, d2;
741  d1 = d2 = std::numeric_limits<double>::max();
742  try {
743  pose.computePose(vpPose::LAGRANGE, cMo1);
744  d1 = pose.computeResidual(cMo1);
745  }
746  catch(...) {
747  // Lagrange non-planar cannot work with less than 6 points
748  }
749  try {
750  pose.computePose(vpPose::DEMENTHON, cMo2);
751  d2 = pose.computeResidual(cMo2);
752  }
753  catch(...) {
754  // Should not occur
755  }
756 
757  if (d1 < d2)
758  cMo = cMo1;
759  else
760  cMo = cMo2;
761 
763 
764  delete[] P;
765 
766  init(I);
767 }
768 
777 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
778  const std::vector<vpPoint> &points3D_list)
779 {
780  if (points2D_list.size() != points3D_list.size())
781  vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
782  "different to the number of 3D points.");
783 
784  size_t size = points3D_list.size();
785  std::vector<vpPoint> P;
786  vpPose pose;
787 
788  for (size_t i = 0; i < size; i++) {
789  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
790  double x = 0, y = 0;
791  vpPixelMeterConversion::convertPoint(cam, points2D_list[i], x, y);
792  P[i].set_x(x);
793  P[i].set_y(y);
794  pose.addPoint(P[i]);
795  }
796 
797  vpHomogeneousMatrix cMo1, cMo2;
798  double d1, d2;
799  d1 = d2 = std::numeric_limits<double>::max();
800  try {
801  pose.computePose(vpPose::LAGRANGE, cMo1);
802  d1 = pose.computeResidual(cMo1);
803  }
804  catch(...) {
805  // Lagrange non-planar cannot work with less than 6 points
806  }
807  try {
808  pose.computePose(vpPose::DEMENTHON, cMo2);
809  d2 = pose.computeResidual(cMo2);
810  }
811  catch(...) {
812  // Should not occur
813  }
814 
815  if (d1 < d2)
816  cMo = cMo1;
817  else
818  cMo = cMo2;
819 
821 
822  init(I);
823 }
824 
842 void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
843 {
844  char s[FILENAME_MAX];
845  std::fstream finit;
846  vpPoseVector init_pos;
847 
848  std::string ext = ".pos";
849  size_t pos = initFile.rfind(ext);
850 
851  if (pos == initFile.size() - ext.size() && pos != 0)
852  sprintf(s, "%s", initFile.c_str());
853  else
854  sprintf(s, "%s.pos", initFile.c_str());
855 
856  finit.open(s, std::ios::in);
857  if (finit.fail()) {
858  std::cout << "cannot read " << s << std::endl;
859  throw vpException(vpException::ioError, "cannot read init file");
860  }
861 
862  for (unsigned int i = 0; i < 6; i += 1) {
863  finit >> init_pos[i];
864  }
865 
866  cMo.buildFrom(init_pos);
867 
868  init(I);
869 }
870 
878 {
879  this->cMo = cMo_;
880  init(I);
881 }
882 
890 {
891  vpHomogeneousMatrix _cMo(cPo);
892  initFromPose(I, _cMo);
893 }
894 
900 void vpMbTracker::savePose(const std::string &filename) const
901 {
902  vpPoseVector init_pos;
903  std::fstream finitpos;
904  char s[FILENAME_MAX];
905 
906  sprintf(s, "%s", filename.c_str());
907  finitpos.open(s, std::ios::out);
908 
909  init_pos.buildFrom(cMo);
910  finitpos << init_pos;
911  finitpos.close();
912 }
913 
914 void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, const int idFace, const std::string &polygonName,
915  const bool useLod, const double minPolygonAreaThreshold,
916  const double minLineLengthThreshold)
917 {
918  std::vector<vpPoint> corners_without_duplicates;
919  corners_without_duplicates.push_back(corners[0]);
920  for (unsigned int i = 0; i < corners.size() - 1; i++) {
921  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
922  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
923  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
924  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
925  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
926  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
927  corners_without_duplicates.push_back(corners[i + 1]);
928  }
929  }
930 
931  vpMbtPolygon polygon;
932  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
933  polygon.setIndex((int)idFace);
934  polygon.setName(polygonName);
935  polygon.setLod(useLod);
936 
937  // //if(minPolygonAreaThreshold != -1.0) {
938  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
939  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
940  // {
941  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
942  // }
943  //
944  // //if(minLineLengthThreshold != -1.0) {
945  // if(std::fabs(minLineLengthThreshold + 1.0) >
946  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
947  // {
948  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
949  // }
950 
951  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
952  polygon.setMinLineLengthThresh(minLineLengthThreshold);
953 
954  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
955  polygon.addPoint(j, corners_without_duplicates[j]);
956  }
957 
958  faces.addPolygon(&polygon);
959 
961  faces.getPolygon().back()->setClipping(clippingFlag);
962 
963  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
964  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
965 
966  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
967  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
968 }
969 
970 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
971  const int idFace, const std::string &polygonName, const bool useLod,
972  const double minPolygonAreaThreshold)
973 {
974  vpMbtPolygon polygon;
975  polygon.setNbPoint(4);
976  polygon.setName(polygonName);
977  polygon.setLod(useLod);
978 
979  // //if(minPolygonAreaThreshold != -1.0) {
980  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
981  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
982  // {
983  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
984  // }
985  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
986  // Non sense to set minLineLengthThreshold for circle
987  // but used to be coherent when applying LOD settings for all polygons
989 
990  {
991  // Create the 4 points of the circle bounding box
992  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
993 
994  // Matrice de passage entre world et circle frame
995  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
996  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
997  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
998  vpRotationMatrix wRc;
999  vpColVector x(3), y(3), z(3);
1000  // X axis is P2-P1
1001  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
1002  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
1003  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
1004  // Y axis is the normal of the plane
1005  y[0] = plane.getA() / norm_Y;
1006  y[1] = plane.getB() / norm_Y;
1007  y[2] = plane.getC() / norm_Y;
1008  // Z axis = X ^ Y
1009  z = vpColVector::crossProd(x, y);
1010  for (unsigned int i = 0; i < 3; i++) {
1011  wRc[i][0] = x[i];
1012  wRc[i][1] = y[i];
1013  wRc[i][2] = z[i];
1014  }
1015 
1016  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
1017  vpHomogeneousMatrix wMc(wtc, wRc);
1018 
1019  vpColVector c_p(4); // A point in the circle frame that is on the bbox
1020  c_p[0] = radius;
1021  c_p[1] = 0;
1022  c_p[2] = radius;
1023  c_p[3] = 1;
1024 
1025  // Matrix to rotate a point by 90 deg around Y in the circle frame
1026  for (unsigned int i = 0; i < 4; i++) {
1027  vpColVector w_p(4); // A point in the word frame
1029  w_p = wMc * cMc_90 * c_p;
1030 
1031  vpPoint w_P;
1032  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
1033 
1034  polygon.addPoint(i, w_P);
1035  }
1036  }
1037 
1038  polygon.setIndex(idFace);
1039  faces.addPolygon(&polygon);
1040 
1042  faces.getPolygon().back()->setClipping(clippingFlag);
1043 
1044  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1045  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1046 
1047  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1048  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1049 }
1050 
1051 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const int idFace, const std::string &polygonName,
1052  const bool useLod, const double minLineLengthThreshold)
1053 {
1054  // A polygon as a single line that corresponds to the revolution axis of the
1055  // cylinder
1056  vpMbtPolygon polygon;
1057  polygon.setNbPoint(2);
1058 
1059  polygon.addPoint(0, p1);
1060  polygon.addPoint(1, p2);
1061 
1062  polygon.setIndex(idFace);
1063  polygon.setName(polygonName);
1064  polygon.setLod(useLod);
1065 
1066  // //if(minLineLengthThreshold != -1.0) {
1067  // if(std::fabs(minLineLengthThreshold + 1.0) >
1068  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1069  // {
1070  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1071  // }
1072  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1073  // Non sense to set minPolygonAreaThreshold for cylinder
1074  // but used to be coherent when applying LOD settings for all polygons
1076 
1077  faces.addPolygon(&polygon);
1078 
1080  faces.getPolygon().back()->setClipping(clippingFlag);
1081 
1082  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1083  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1084 
1085  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1086  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1087 }
1088 
1089 void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace,
1090  const std::string &polygonName, const bool useLod, const double minLineLengthThreshold)
1091 {
1092  int id = idFace;
1093  for (unsigned int i = 0; i < listFaces.size(); i++) {
1094  vpMbtPolygon polygon;
1095  polygon.setNbPoint((unsigned int)listFaces[i].size());
1096  for (unsigned int j = 0; j < listFaces[i].size(); j++)
1097  polygon.addPoint(j, listFaces[i][j]);
1098 
1099  polygon.setIndex(id);
1100  polygon.setName(polygonName);
1101  polygon.setIsPolygonOriented(false);
1102  polygon.setLod(useLod);
1103  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1105 
1106  faces.addPolygon(&polygon);
1107 
1109  faces.getPolygon().back()->setClipping(clippingFlag);
1110 
1111  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1112  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1113 
1114  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1115  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1116 
1117  id++;
1118  }
1119 }
1120 
1148 void vpMbTracker::loadModel(const std::string &modelFile, const bool verbose, const vpHomogeneousMatrix &T)
1149 {
1150  std::string::const_iterator it;
1151 
1152  if (vpIoTools::checkFilename(modelFile)) {
1153  it = modelFile.end();
1154  if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
1155  (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
1156  std::vector<std::string> vectorOfModelFilename;
1157  int startIdFace = (int)faces.size();
1158  nbPoints = 0;
1159  nbLines = 0;
1160  nbPolygonLines = 0;
1161  nbPolygonPoints = 0;
1162  nbCylinders = 0;
1163  nbCircles = 0;
1164  loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, T);
1165  } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
1166  (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
1167  loadVRMLModel(modelFile);
1168  } else {
1169  throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
1170  }
1171  } else {
1172  throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
1173  }
1174 
1175  this->modelInitialised = true;
1176  this->modelFileName = modelFile;
1177 }
1178 
1209 void vpMbTracker::loadVRMLModel(const std::string &modelFile)
1210 {
1211 #ifdef VISP_HAVE_COIN3D
1212  SoDB::init(); // Call SoDB::finish() before ending the program.
1213 
1214  SoInput in;
1215  SbBool ok = in.openFile(modelFile.c_str());
1216  SoVRMLGroup *sceneGraphVRML2;
1217 
1218  if (!ok) {
1219  vpERROR_TRACE("can't open file to load model");
1220  throw vpException(vpException::fatalError, "can't open file to load model");
1221  }
1222 
1223  if (!in.isFileVRML2()) {
1224  SoSeparator *sceneGraph = SoDB::readAll(&in);
1225  if (sceneGraph == NULL) { /*return -1;*/
1226  }
1227  sceneGraph->ref();
1228 
1229  SoToVRML2Action tovrml2;
1230  tovrml2.apply(sceneGraph);
1231 
1232  sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1233  sceneGraphVRML2->ref();
1234  sceneGraph->unref();
1235  } else {
1236  sceneGraphVRML2 = SoDB::readAllVRML(&in);
1237  if (sceneGraphVRML2 == NULL) { /*return -1;*/
1238  }
1239  sceneGraphVRML2->ref();
1240  }
1241 
1242  in.closeFile();
1243 
1244  vpHomogeneousMatrix transform;
1245  int indexFace = (int)faces.size();
1246  extractGroup(sceneGraphVRML2, transform, indexFace);
1247 
1248  sceneGraphVRML2->unref();
1249 #else
1250  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1251  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
1252 #endif
1253 }
1254 
1255 void vpMbTracker::removeComment(std::ifstream &fileId)
1256 {
1257  char c;
1258 
1259  fileId.get(c);
1260  while (!fileId.fail() && (c == '#')) {
1261  fileId.ignore(256, '\n');
1262  fileId.get(c);
1263  }
1264  if (fileId.fail()) {
1265  throw(vpException(vpException::ioError, "Reached end of file"));
1266  }
1267  fileId.unget();
1268 }
1269 
1270 std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
1271 {
1272  std::map<std::string, std::string> mapOfParams;
1273 
1274  bool exit = false;
1275  while (!endLine.empty() && !exit) {
1276  exit = true;
1277 
1278  for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
1279  it != mapOfParameterNames.end(); ++it) {
1280  endLine = trim(endLine);
1281  // std::cout << "endLine=" << endLine << std::endl;
1282  std::string param(it->first + "=");
1283 
1284  // Compare with a potential parameter
1285  if (endLine.compare(0, param.size(), param) == 0) {
1286  exit = false;
1287  endLine = endLine.substr(param.size());
1288 
1289  bool parseQuote = false;
1290  if (it->second == "string") {
1291  // Check if the string is between quotes
1292  if (endLine.size() > 2 && endLine[0] == '"') {
1293  parseQuote = true;
1294  endLine = endLine.substr(1);
1295  size_t pos = endLine.find_first_of('"');
1296 
1297  if (pos != std::string::npos) {
1298  mapOfParams[it->first] = endLine.substr(0, pos);
1299  endLine = endLine.substr(pos + 1);
1300  } else {
1301  parseQuote = false;
1302  }
1303  }
1304  }
1305 
1306  if (!parseQuote) {
1307  // Deal with space or tabulation after parameter value to substring
1308  // to the next sequence
1309  size_t pos1 = endLine.find_first_of(' ');
1310  size_t pos2 = endLine.find_first_of('\t');
1311  size_t pos = pos1 < pos2 ? pos1 : pos2;
1312 
1313  mapOfParams[it->first] = endLine.substr(0, pos);
1314  endLine = endLine.substr(pos + 1);
1315  }
1316  }
1317  }
1318  }
1319 
1320  return mapOfParams;
1321 }
1322 
1372 void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
1373  int &startIdFace, const bool verbose, const bool parent,
1374  const vpHomogeneousMatrix &T)
1375 {
1376  std::ifstream fileId;
1377  fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1378  fileId.open(modelFile.c_str(), std::ifstream::in);
1379  if (fileId.fail()) {
1380  std::cout << "cannot read CAO model file: " << modelFile << std::endl;
1381  throw vpException(vpException::ioError, "cannot read CAO model file");
1382  }
1383 
1384  if (verbose) {
1385  std::cout << "Model file : " << modelFile << std::endl;
1386  }
1387  vectorOfModelFilename.push_back(modelFile);
1388 
1389  try {
1390  char c;
1391  // Extraction of the version (remove empty line and commented ones
1392  // (comment line begin with the #)).
1393  // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
1394  removeComment(fileId);
1395 
1397  int caoVersion;
1398  fileId.get(c);
1399  if (c == 'V') {
1400  fileId >> caoVersion;
1401  fileId.ignore(256, '\n'); // skip the rest of the line
1402  } else {
1403  std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
1404  "file : use V0, V1, ...";
1405  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
1406  "header file : use V0, V1, ...");
1407  }
1408 
1409  removeComment(fileId);
1410 
1412  std::string line;
1413  std::string prefix = "load";
1414 
1415  fileId.get(c);
1416  fileId.unget();
1417  bool header = false;
1418  while (c == 'l' || c == 'L') {
1419  header = true;
1420 
1421  getline(fileId, line);
1422  if (!line.compare(0, prefix.size(), prefix)) {
1423 
1424  // Get the loaded model pathname
1425  std::string headerPathRead = line.substr(6);
1426  size_t firstIndex = headerPathRead.find_first_of("\")");
1427  headerPathRead = headerPathRead.substr(0, firstIndex);
1428 
1429  std::string headerPath = headerPathRead;
1430  if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
1431  std::string parentDirectory = vpIoTools::getParent(modelFile);
1432  headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
1433  }
1434 
1435  // Normalize path
1436  headerPath = vpIoTools::path(headerPath);
1437 
1438  // Get real path
1439  headerPath = vpIoTools::getAbsolutePathname(headerPath);
1440 
1441  bool cyclic = false;
1442  for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1443  it != vectorOfModelFilename.end() && !cyclic; ++it) {
1444  if (headerPath == *it) {
1445  cyclic = true;
1446  }
1447  }
1448 
1449  if (!cyclic) {
1450  if (vpIoTools::checkFilename(headerPath)) {
1451  loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, T);
1452  } else {
1453  throw vpException(vpException::ioError, "file cannot be open");
1454  }
1455  } else {
1456  std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
1457  << std::endl;
1458  }
1459  } else {
1460  header = false;
1461  }
1462 
1463  removeComment(fileId);
1464  fileId.get(c);
1465  fileId.unget();
1466  }
1467 
1469  unsigned int caoNbrPoint;
1470  fileId >> caoNbrPoint;
1471  fileId.ignore(256, '\n'); // skip the rest of the line
1472 
1473  nbPoints += caoNbrPoint;
1474  if (verbose || vectorOfModelFilename.size() == 1) {
1475  std::cout << "> " << caoNbrPoint << " points" << std::endl;
1476  }
1477 
1478  if (caoNbrPoint > 100000) {
1479  throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
1480  }
1481 
1482  if (caoNbrPoint == 0 && !header) {
1483  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
1484  }
1485  vpPoint *caoPoints = new vpPoint[caoNbrPoint];
1486 
1487  int i; // image coordinate (used for matching)
1488  int j;
1489 
1490  for (unsigned int k = 0; k < caoNbrPoint; k++) {
1491  removeComment(fileId);
1492 
1493  vpColVector pt_3d(4, 1.0);
1494  fileId >> pt_3d[0];
1495  fileId >> pt_3d[1];
1496  fileId >> pt_3d[2];
1497 
1498  if (caoVersion == 2) {
1499  fileId >> i;
1500  fileId >> j;
1501  }
1502 
1503  fileId.ignore(256, '\n'); // skip the rest of the line
1504 
1505  vpColVector pt_3d_tf = T*pt_3d;
1506  caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1507  }
1508 
1509  removeComment(fileId);
1510 
1512  // Store in a map the potential segments to add
1513  std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1514  unsigned int caoNbrLine;
1515  fileId >> caoNbrLine;
1516  fileId.ignore(256, '\n'); // skip the rest of the line
1517 
1518  nbLines += caoNbrLine;
1519  unsigned int *caoLinePoints = NULL;
1520  if (verbose || vectorOfModelFilename.size() == 1) {
1521  std::cout << "> " << caoNbrLine << " lines" << std::endl;
1522  }
1523 
1524  if (caoNbrLine > 100000) {
1525  delete[] caoPoints;
1526  throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
1527  }
1528 
1529  if (caoNbrLine > 0)
1530  caoLinePoints = new unsigned int[2 * caoNbrLine];
1531 
1532  unsigned int index1, index2;
1533  // Initialization of idFace with startIdFace for dealing with recursive
1534  // load in header
1535  int idFace = startIdFace;
1536 
1537  for (unsigned int k = 0; k < caoNbrLine; k++) {
1538  removeComment(fileId);
1539 
1540  fileId >> index1;
1541  fileId >> index2;
1542 
1544  // Get the end of the line
1545  char buffer[256];
1546  fileId.getline(buffer, 256);
1547  std::string endLine(buffer);
1548  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1549 
1550  std::string segmentName = "";
1551  double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1552  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1553  if (mapOfParams.find("name") != mapOfParams.end()) {
1554  segmentName = mapOfParams["name"];
1555  }
1556  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1557  minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1558  }
1559  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1560  useLod = parseBoolean(mapOfParams["useLod"]);
1561  }
1562 
1563  SegmentInfo segmentInfo;
1564  segmentInfo.name = segmentName;
1565  segmentInfo.useLod = useLod;
1566  segmentInfo.minLineLengthThresh = minLineLengthThresh;
1567 
1568  caoLinePoints[2 * k] = index1;
1569  caoLinePoints[2 * k + 1] = index2;
1570 
1571  if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1572  std::vector<vpPoint> extremities;
1573  extremities.push_back(caoPoints[index1]);
1574  extremities.push_back(caoPoints[index2]);
1575  segmentInfo.extremities = extremities;
1576 
1577  std::pair<unsigned int, unsigned int> key(index1, index2);
1578 
1579  segmentTemporaryMap[key] = segmentInfo;
1580  } else {
1581  vpTRACE(" line %d has wrong coordinates.", k);
1582  }
1583  }
1584 
1585  removeComment(fileId);
1586 
1588  /* Load polygon from the lines extracted earlier (the first point of the
1589  * line is used)*/
1590  // Store in a vector the indexes of the segments added in the face segment
1591  // case
1592  std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1593  unsigned int caoNbrPolygonLine;
1594  fileId >> caoNbrPolygonLine;
1595  fileId.ignore(256, '\n'); // skip the rest of the line
1596 
1597  nbPolygonLines += caoNbrPolygonLine;
1598  if (verbose || vectorOfModelFilename.size() == 1) {
1599  std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
1600  }
1601 
1602  if (caoNbrPolygonLine > 100000) {
1603  delete[] caoPoints;
1604  delete[] caoLinePoints;
1605  throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
1606  }
1607 
1608  unsigned int index;
1609  for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1610  removeComment(fileId);
1611 
1612  unsigned int nbLinePol;
1613  fileId >> nbLinePol;
1614  std::vector<vpPoint> corners;
1615  if (nbLinePol > 100000) {
1616  throw vpException(vpException::badValue, "Exceed the max number of lines.");
1617  }
1618 
1619  for (unsigned int n = 0; n < nbLinePol; n++) {
1620  fileId >> index;
1621 
1622  if (index >= caoNbrLine) {
1623  throw vpException(vpException::badValue, "Exceed the max number of lines.");
1624  }
1625  corners.push_back(caoPoints[caoLinePoints[2 * index]]);
1626  corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
1627 
1628  std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
1629  faceSegmentKeyVector.push_back(key);
1630  }
1631 
1633  // Get the end of the line
1634  char buffer[256];
1635  fileId.getline(buffer, 256);
1636  std::string endLine(buffer);
1637  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1638 
1639  std::string polygonName = "";
1640  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1641  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
1642  if (mapOfParams.find("name") != mapOfParams.end()) {
1643  polygonName = mapOfParams["name"];
1644  }
1645  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
1646  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
1647  }
1648  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1649  useLod = parseBoolean(mapOfParams["useLod"]);
1650  }
1651 
1652  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
1653  initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
1654 
1655  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
1657  }
1658 
1659  // Add the segments which were not already added in the face segment case
1660  for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
1661  it != segmentTemporaryMap.end(); ++it) {
1662  if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
1663  faceSegmentKeyVector.end()) {
1664  addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
1665  it->second.minLineLengthThresh);
1666  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
1667 
1668  addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
1669  it->second.minLineLengthThresh);
1671  }
1672  }
1673 
1674  removeComment(fileId);
1675 
1677  /* Extract the polygon using the point coordinates (top of the file) */
1678  unsigned int caoNbrPolygonPoint;
1679  fileId >> caoNbrPolygonPoint;
1680  fileId.ignore(256, '\n'); // skip the rest of the line
1681 
1682  nbPolygonPoints += caoNbrPolygonPoint;
1683  if (verbose || vectorOfModelFilename.size() == 1) {
1684  std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
1685  }
1686 
1687  if (caoNbrPolygonPoint > 100000) {
1688  throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
1689  }
1690 
1691  for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
1692  removeComment(fileId);
1693 
1694  unsigned int nbPointPol;
1695  fileId >> nbPointPol;
1696  if (nbPointPol > 100000) {
1697  throw vpException(vpException::badValue, "Exceed the max number of points.");
1698  }
1699  std::vector<vpPoint> corners;
1700  for (unsigned int n = 0; n < nbPointPol; n++) {
1701  fileId >> index;
1702  if (index > caoNbrPoint - 1) {
1703  throw vpException(vpException::badValue, "Exceed the max number of points.");
1704  }
1705  corners.push_back(caoPoints[index]);
1706  }
1707 
1709  // Get the end of the line
1710  char buffer[256];
1711  fileId.getline(buffer, 256);
1712  std::string endLine(buffer);
1713  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1714 
1715  std::string polygonName = "";
1716  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1717  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
1718  if (mapOfParams.find("name") != mapOfParams.end()) {
1719  polygonName = mapOfParams["name"];
1720  }
1721  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
1722  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
1723  }
1724  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1725  useLod = parseBoolean(mapOfParams["useLod"]);
1726  }
1727 
1728  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
1729  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
1730 
1731  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
1733  }
1734 
1736  unsigned int caoNbCylinder;
1737  try {
1738  removeComment(fileId);
1739 
1740  if (fileId.eof()) { // check if not at the end of the file (for old
1741  // style files)
1742  delete[] caoPoints;
1743  delete[] caoLinePoints;
1744  return;
1745  }
1746 
1747  /* Extract the cylinders */
1748  fileId >> caoNbCylinder;
1749  fileId.ignore(256, '\n'); // skip the rest of the line
1750 
1751  nbCylinders += caoNbCylinder;
1752  if (verbose || vectorOfModelFilename.size() == 1) {
1753  std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
1754  }
1755 
1756  if (caoNbCylinder > 100000) {
1757  throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
1758  }
1759 
1760  for (unsigned int k = 0; k < caoNbCylinder; ++k) {
1761  removeComment(fileId);
1762 
1763  double radius;
1764  unsigned int indexP1, indexP2;
1765  fileId >> indexP1;
1766  fileId >> indexP2;
1767  fileId >> radius;
1768 
1770  // Get the end of the line
1771  char buffer[256];
1772  fileId.getline(buffer, 256);
1773  std::string endLine(buffer);
1774  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1775 
1776  std::string polygonName = "";
1777  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1778  double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1779  if (mapOfParams.find("name") != mapOfParams.end()) {
1780  polygonName = mapOfParams["name"];
1781  }
1782  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1783  minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1784  }
1785  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1786  useLod = parseBoolean(mapOfParams["useLod"]);
1787  }
1788 
1789  int idRevolutionAxis = idFace;
1790  addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
1791 
1792  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
1793 
1794  std::vector<std::vector<vpPoint> > listFaces;
1795  createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
1796  addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
1797 
1798  initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
1799 
1800  addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
1801  initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
1802 
1803  idFace += 4;
1804  }
1805 
1806  } catch (...) {
1807  std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
1808  caoNbCylinder = 0;
1809  }
1810 
1812  unsigned int caoNbCircle;
1813  try {
1814  removeComment(fileId);
1815 
1816  if (fileId.eof()) { // check if not at the end of the file (for old
1817  // style files)
1818  delete[] caoPoints;
1819  delete[] caoLinePoints;
1820  return;
1821  }
1822 
1823  /* Extract the circles */
1824  fileId >> caoNbCircle;
1825  fileId.ignore(256, '\n'); // skip the rest of the line
1826 
1827  nbCircles += caoNbCircle;
1828  if (verbose || vectorOfModelFilename.size() == 1) {
1829  std::cout << "> " << caoNbCircle << " circles" << std::endl;
1830  }
1831 
1832  if (caoNbCircle > 100000) {
1833  throw vpException(vpException::badValue, "Exceed the max number of cicles.");
1834  }
1835 
1836  for (unsigned int k = 0; k < caoNbCircle; ++k) {
1837  removeComment(fileId);
1838 
1839  double radius;
1840  unsigned int indexP1, indexP2, indexP3;
1841  fileId >> radius;
1842  fileId >> indexP1;
1843  fileId >> indexP2;
1844  fileId >> indexP3;
1845 
1847  // Get the end of the line
1848  char buffer[256];
1849  fileId.getline(buffer, 256);
1850  std::string endLine(buffer);
1851  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1852 
1853  std::string polygonName = "";
1854  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1855  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
1856  if (mapOfParams.find("name") != mapOfParams.end()) {
1857  polygonName = mapOfParams["name"];
1858  }
1859  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
1860  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
1861  }
1862  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1863  useLod = parseBoolean(mapOfParams["useLod"]);
1864  }
1865 
1866  addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
1867  minPolygonAreaThreshold);
1868 
1869  initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
1870 
1871  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
1872  minPolygonAreaThreshold);
1873  initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, polygonName);
1874  }
1875 
1876  } catch (...) {
1877  std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
1878  caoNbCircle = 0;
1879  }
1880 
1881  startIdFace = idFace;
1882 
1883  delete[] caoPoints;
1884  delete[] caoLinePoints;
1885 
1886  if (vectorOfModelFilename.size() > 1 && parent) {
1887  if (verbose) {
1888  std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
1889  std::cout << "Total nb of points : " << nbPoints << std::endl;
1890  std::cout << "Total nb of lines : " << nbLines << std::endl;
1891  std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
1892  std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
1893  std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
1894  std::cout << "Total nb of circles : " << nbCircles << std::endl;
1895  } else {
1896  std::cout << "> " << nbPoints << " points" << std::endl;
1897  std::cout << "> " << nbLines << " lines" << std::endl;
1898  std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
1899  std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
1900  std::cout << "> " << nbCylinders << " cylinders" << std::endl;
1901  std::cout << "> " << nbCircles << " circles" << std::endl;
1902  }
1903  }
1904  } catch (...) {
1905  std::cerr << "Cannot read line!" << std::endl;
1906  throw vpException(vpException::ioError, "cannot read line");
1907  }
1908 }
1909 
1910 #ifdef VISP_HAVE_COIN3D
1911 
1918 void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
1919 {
1920  vpHomogeneousMatrix transformCur;
1921  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
1922  if (sceneGraphVRML2Trasnform) {
1923  float rx, ry, rz, rw;
1924  sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
1925  vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
1926  // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
1927  // rw << std::endl;
1928 
1929  float tx, ty, tz;
1930  tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
1931  ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
1932  tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
1933  vpTranslationVector transVec(tx, ty, tz);
1934  // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
1935  // std::endl;
1936 
1937  float sx, sy, sz;
1938  sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
1939  sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
1940  sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
1941  // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
1942  // std::endl;
1943 
1944  for (unsigned int i = 0; i < 3; i++)
1945  rotMat[0][i] *= sx;
1946  for (unsigned int i = 0; i < 3; i++)
1947  rotMat[1][i] *= sy;
1948  for (unsigned int i = 0; i < 3; i++)
1949  rotMat[2][i] *= sz;
1950 
1951  transformCur = vpHomogeneousMatrix(transVec, rotMat);
1952  transform = transform * transformCur;
1953  }
1954 
1955  int nbShapes = sceneGraphVRML2->getNumChildren();
1956  // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
1957  // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
1958  // std::endl;
1959 
1960  SoNode *child;
1961 
1962  for (int i = 0; i < nbShapes; i++) {
1963  vpHomogeneousMatrix transform_recursive(transform);
1964  child = sceneGraphVRML2->getChild(i);
1965 
1966  if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
1967  extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
1968  }
1969 
1970  if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
1971  extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
1972  }
1973 
1974  if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
1975  SoChildList *child2list = child->getChildren();
1976  std::string name = child->getName().getString();
1977 
1978  for (int j = 0; j < child2list->getLength(); j++) {
1979  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
1980  SoVRMLIndexedFaceSet *face_set;
1981  face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
1982  if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
1983  extractCylinders(face_set, transform, idFace, name);
1984  } else {
1985  extractFaces(face_set, transform, idFace, name);
1986  }
1987  }
1988  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
1989  SoVRMLIndexedLineSet *line_set;
1990  line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
1991  extractLines(line_set, idFace, name);
1992  }
1993  }
1994  }
1995  }
1996 }
1997 
2007 void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2008  const std::string &polygonName)
2009 {
2010  std::vector<vpPoint> corners;
2011 
2012  // SoMFInt32 indexList = _face_set->coordIndex;
2013  // int indexListSize = indexList.getNum();
2014  int indexListSize = face_set->coordIndex.getNum();
2015 
2016  vpColVector pointTransformed(4);
2017  vpPoint pt;
2018  SoVRMLCoordinate *coord;
2019 
2020  for (int i = 0; i < indexListSize; i++) {
2021  if (face_set->coordIndex[i] == -1) {
2022  if (corners.size() > 1) {
2023  addPolygon(corners, idFace, polygonName);
2024  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2025 
2026  addProjectionErrorPolygon(corners, idFace++, polygonName);
2028  corners.resize(0);
2029  }
2030  } else {
2031  coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2032  int index = face_set->coordIndex[i];
2033  pointTransformed[0] = coord->point[index].getValue()[0];
2034  pointTransformed[1] = coord->point[index].getValue()[1];
2035  pointTransformed[2] = coord->point[index].getValue()[2];
2036  pointTransformed[3] = 1.0;
2037 
2038  pointTransformed = transform * pointTransformed;
2039 
2040  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2041  corners.push_back(pt);
2042  }
2043  }
2044 }
2045 
2060 void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2061  const std::string &polygonName)
2062 {
2063  std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
2064  // first circle and to the
2065  // second one.
2066  SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2067 
2068  unsigned int indexListSize = (unsigned int)coords->point.getNum();
2069 
2070  if (indexListSize % 2 == 1) {
2071  std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
2072  throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
2073  }
2074  corners_c1.resize(indexListSize / 2);
2075  corners_c2.resize(indexListSize / 2);
2076  vpColVector pointTransformed(4);
2077  vpPoint pt;
2078 
2079  // extract all points and fill the two sets.
2080 
2081  for (int i = 0; i < coords->point.getNum(); ++i) {
2082  pointTransformed[0] = coords->point[i].getValue()[0];
2083  pointTransformed[1] = coords->point[i].getValue()[1];
2084  pointTransformed[2] = coords->point[i].getValue()[2];
2085  pointTransformed[3] = 1.0;
2086 
2087  pointTransformed = transform * pointTransformed;
2088 
2089  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2090 
2091  if (i < (int)corners_c1.size()) {
2092  corners_c1[(unsigned int)i] = pt;
2093  } else {
2094  corners_c2[(unsigned int)i - corners_c1.size()] = pt;
2095  }
2096  }
2097 
2098  vpPoint p1 = getGravityCenter(corners_c1);
2099  vpPoint p2 = getGravityCenter(corners_c2);
2100 
2101  vpColVector dist(3);
2102  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
2103  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
2104  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
2105  double radius_c1 = sqrt(dist.sumSquare());
2106  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
2107  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
2108  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
2109  double radius_c2 = sqrt(dist.sumSquare());
2110 
2111  if (std::fabs(radius_c1 - radius_c2) >
2112  (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
2113  std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
2114  throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
2115  }
2116 
2117  // addPolygon(p1, p2, idFace, polygonName);
2118  // initCylinder(p1, p2, radius_c1, idFace++);
2119 
2120  int idRevolutionAxis = idFace;
2121  addPolygon(p1, p2, idFace, polygonName);
2122 
2123  addProjectionErrorPolygon(p1, p2, idFace++, polygonName);
2124 
2125  std::vector<std::vector<vpPoint> > listFaces;
2126  createCylinderBBox(p1, p2, radius_c1, listFaces);
2127  addPolygon(listFaces, idFace, polygonName);
2128 
2129  initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2130 
2131  addProjectionErrorPolygon(listFaces, idFace, polygonName);
2132  initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2133 
2134  idFace += 4;
2135 }
2136 
2145 void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
2146 {
2147  std::vector<vpPoint> corners;
2148  corners.resize(0);
2149 
2150  int indexListSize = line_set->coordIndex.getNum();
2151 
2152  SbVec3f point(0, 0, 0);
2153  vpPoint pt;
2154  SoVRMLCoordinate *coord;
2155 
2156  for (int i = 0; i < indexListSize; i++) {
2157  if (line_set->coordIndex[i] == -1) {
2158  if (corners.size() > 1) {
2159  addPolygon(corners, idFace, polygonName);
2160  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2161 
2162  addProjectionErrorPolygon(corners, idFace++, polygonName);
2164  corners.resize(0);
2165  }
2166  } else {
2167  coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2168  int index = line_set->coordIndex[i];
2169  point[0] = coord->point[index].getValue()[0];
2170  point[1] = coord->point[index].getValue()[1];
2171  point[2] = coord->point[index].getValue()[2];
2172 
2173  pt.setWorldCoordinates(point[0], point[1], point[2]);
2174  corners.push_back(pt);
2175  }
2176  }
2177 }
2178 
2179 #endif // VISP_HAVE_COIN3D
2180 
2190 vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
2191 {
2192  if (pts.empty()) {
2193  std::cout << "Cannot extract center of gravity of empty set." << std::endl;
2194  throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
2195  }
2196  double oX = 0;
2197  double oY = 0;
2198  double oZ = 0;
2199  vpPoint G;
2200 
2201  for (unsigned int i = 0; i < pts.size(); ++i) {
2202  oX += pts[i].get_oX();
2203  oY += pts[i].get_oY();
2204  oZ += pts[i].get_oZ();
2205  }
2206 
2207  G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
2208  return G;
2209 }
2210 
2223 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2224 vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
2225 {
2226  // Temporary variable to permit to order polygons by distance
2227  std::vector<vpPolygon> polygonsTmp;
2228  std::vector<std::vector<vpPoint> > roisPtTmp;
2229 
2230  // Pair containing the list of vpPolygon and the list of face corners
2231  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2232 
2233  for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
2234  // A face has at least 3 points
2235  if (faces.getPolygon()[i]->nbpt > 2) {
2236  if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
2237  std::vector<vpImagePoint> roiPts;
2238 
2239  if (clipPolygon) {
2240  faces.getPolygon()[i]->getRoiClipped(cam, roiPts, cMo);
2241  } else {
2242  roiPts = faces.getPolygon()[i]->getRoi(cam, cMo);
2243  }
2244 
2245  if (roiPts.size() <= 2) {
2246  continue;
2247  }
2248 
2249  polygonsTmp.push_back(vpPolygon(roiPts));
2250 
2251  std::vector<vpPoint> polyPts;
2252  if (clipPolygon) {
2253  faces.getPolygon()[i]->getPolygonClipped(polyPts);
2254  } else {
2255  for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
2256  polyPts.push_back(faces.getPolygon()[i]->p[j]);
2257  }
2258  }
2259  roisPtTmp.push_back(polyPts);
2260  }
2261  }
2262  }
2263 
2264  if (orderPolygons) {
2265  // Order polygons by distance (near to far)
2266  std::vector<PolygonFaceInfo> listOfPolygonFaces;
2267  for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
2268  double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2269  for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2270  x_centroid += roisPtTmp[i][j].get_X();
2271  y_centroid += roisPtTmp[i][j].get_Y();
2272  z_centroid += roisPtTmp[i][j].get_Z();
2273  }
2274 
2275  x_centroid /= roisPtTmp[i].size();
2276  y_centroid /= roisPtTmp[i].size();
2277  z_centroid /= roisPtTmp[i].size();
2278 
2279  double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2280  listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2281  }
2282 
2283  // Sort the list of polygon faces
2284  std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2285 
2286  polygonsTmp.resize(listOfPolygonFaces.size());
2287  roisPtTmp.resize(listOfPolygonFaces.size());
2288 
2289  size_t cpt = 0;
2290  for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2291  ++it, cpt++) {
2292  polygonsTmp[cpt] = it->polygon;
2293  roisPtTmp[cpt] = it->faceCorners;
2294  }
2295 
2296  pairOfPolygonFaces.first = polygonsTmp;
2297  pairOfPolygonFaces.second = roisPtTmp;
2298  } else {
2299  pairOfPolygonFaces.first = polygonsTmp;
2300  pairOfPolygonFaces.second = roisPtTmp;
2301  }
2302 
2303  return pairOfPolygonFaces;
2304 }
2305 
2315 {
2316  useOgre = v;
2317  if (useOgre) {
2318 #ifndef VISP_HAVE_OGRE
2319  useOgre = false;
2320  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2321  "will be used. setOgreVisibilityTest() set to false."
2322  << std::endl;
2323 #endif
2324  }
2325 }
2326 
2332 void vpMbTracker::setFarClippingDistance(const double &dist)
2333 {
2334  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2335  vpTRACE("Far clipping value cannot be inferior than near clipping value. "
2336  "Far clipping won't be considered.");
2337  else if (dist < 0)
2338  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
2339  "won't be considered.");
2340  else {
2342  distFarClip = dist;
2343  for (unsigned int i = 0; i < faces.size(); i++) {
2344  faces[i]->setFarClippingDistance(distFarClip);
2345  }
2346 #ifdef VISP_HAVE_OGRE
2348 #endif
2349  }
2350 }
2351 
2362 void vpMbTracker::setLod(const bool useLod, const std::string &name)
2363 {
2364  for (unsigned int i = 0; i < faces.size(); i++) {
2365  if (name.empty() || faces[i]->name == name) {
2366  faces[i]->setLod(useLod);
2367  }
2368  }
2369 }
2370 
2380 void vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
2381 {
2382  for (unsigned int i = 0; i < faces.size(); i++) {
2383  if (name.empty() || faces[i]->name == name) {
2384  faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2385  }
2386  }
2387 }
2388 
2397 void vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
2398 {
2399  for (unsigned int i = 0; i < faces.size(); i++) {
2400  if (name.empty() || faces[i]->name == name) {
2401  faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2402  }
2403  }
2404 }
2405 
2412 {
2413  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2414  vpTRACE("Near clipping value cannot be superior than far clipping value. "
2415  "Near clipping won't be considered.");
2416  else if (dist < 0)
2417  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
2418  "won't be considered.");
2419  else {
2421  distNearClip = dist;
2422  for (unsigned int i = 0; i < faces.size(); i++) {
2423  faces[i]->setNearClippingDistance(distNearClip);
2424  }
2425 #ifdef VISP_HAVE_OGRE
2427 #endif
2428  }
2429 }
2430 
2438 void vpMbTracker::setClipping(const unsigned int &flags)
2439 {
2440  clippingFlag = flags;
2441  for (unsigned int i = 0; i < faces.size(); i++)
2443 }
2444 
2445 void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true,
2446  const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
2447  const vpMatrix &LVJ_true, const vpColVector &error)
2448 {
2449  if (computeCovariance) {
2450  vpMatrix D;
2451  D.diag(w_true);
2452 
2453  // Note that here the covariance is computed on cMoPrev for time
2454  // computation efficiency
2455  if (isoJoIdentity_) {
2456  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
2457  } else {
2458  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
2459  }
2460  }
2461 }
2462 
2476 void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
2477 {
2478  if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
2479  throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
2480  }
2481 
2482  JTR.resize(6, false);
2483 
2484  bool checkSSE2 = vpCPUFeatures::checkSSE2();
2485 #if !VISP_HAVE_SSE2
2486  checkSSE2 = false;
2487 #endif
2488 
2489  if (checkSSE2) {
2490 #if VISP_HAVE_SSE2
2491  __m128d v_JTR_0_1 = _mm_setzero_pd();
2492  __m128d v_JTR_2_3 = _mm_setzero_pd();
2493  __m128d v_JTR_4_5 = _mm_setzero_pd();
2494 
2495  for (unsigned int i = 0; i < interaction.getRows(); i++) {
2496  const __m128d v_error = _mm_set1_pd(error[i]);
2497 
2498  __m128d v_interaction = _mm_loadu_pd(&interaction[i][0]);
2499  v_JTR_0_1 = _mm_add_pd(v_JTR_0_1, _mm_mul_pd(v_interaction, v_error));
2500 
2501  v_interaction = _mm_loadu_pd(&interaction[i][2]);
2502  v_JTR_2_3 = _mm_add_pd(v_JTR_2_3, _mm_mul_pd(v_interaction, v_error));
2503 
2504  v_interaction = _mm_loadu_pd(&interaction[i][4]);
2505  v_JTR_4_5 = _mm_add_pd(v_JTR_4_5, _mm_mul_pd(v_interaction, v_error));
2506  }
2507 
2508  _mm_storeu_pd(JTR.data, v_JTR_0_1);
2509  _mm_storeu_pd(JTR.data + 2, v_JTR_2_3);
2510  _mm_storeu_pd(JTR.data + 4, v_JTR_4_5);
2511 #endif
2512  } else {
2513  const unsigned int N = interaction.getRows();
2514 
2515  for (unsigned int i = 0; i < 6; i += 1) {
2516  double ssum = 0;
2517  for (unsigned int j = 0; j < N; j += 1) {
2518  ssum += interaction[j][i] * error[j];
2519  }
2520  JTR[i] = ssum;
2521  }
2522  }
2523 }
2524 
2526  const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
2527  double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
2528  const vpColVector *const m_w_prev)
2529 {
2531  if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
2532  mu *= 10.0;
2533 
2534  if (mu > 1.0)
2535  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
2536 
2537  cMo = cMoPrev;
2538  error = m_error_prev;
2539  if (w != NULL && m_w_prev != NULL) {
2540  *w = *m_w_prev;
2541  }
2542  reStartFromLastIncrement = true;
2543  }
2544  }
2545 }
2546 
2547 void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L,
2548  vpMatrix &LTL, vpColVector &R, const vpColVector &error,
2549  vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v,
2550  const vpColVector *const w, vpColVector *const m_w_prev)
2551 {
2552  if (isoJoIdentity_) {
2553  LTL = L.AtA();
2554  computeJTR(L, R, LTR);
2555 
2556  switch (m_optimizationMethod) {
2558  vpMatrix LMA(LTL.getRows(), LTL.getCols());
2559  LMA.eye();
2560  vpMatrix LTLmuI = LTL + (LMA * mu);
2561  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2562 
2563  if (iter != 0)
2564  mu /= 10.0;
2565 
2566  error_prev = error;
2567  if (w != NULL && m_w_prev != NULL)
2568  *m_w_prev = *w;
2569  break;
2570  }
2571 
2573  default:
2574  v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2575  break;
2576  }
2577  } else {
2579  cVo.buildFrom(cMo);
2580  vpMatrix LVJ = (L * (cVo * oJo));
2581  vpMatrix LVJTLVJ = (LVJ).AtA();
2582  vpColVector LVJTR;
2583  computeJTR(LVJ, R, LVJTR);
2584 
2585  switch (m_optimizationMethod) {
2587  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
2588  LMA.eye();
2589  vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2590  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2591  v = cVo * v;
2592 
2593  if (iter != 0)
2594  mu /= 10.0;
2595 
2596  error_prev = error;
2597  if (w != NULL && m_w_prev != NULL)
2598  *m_w_prev = *w;
2599  break;
2600  }
2602  default:
2603  v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2604  v = cVo * v;
2605  break;
2606  }
2607  }
2608 }
2609 
2611 {
2612  if (error.getRows() > 0)
2613  robust.MEstimator(vpRobust::TUKEY, error, w);
2614 }
2615 
2628 {
2629  vpColVector v(6);
2630  for (unsigned int i = 0; i < 6; i++)
2631  v[i] = oJo[i][i];
2632  return v;
2633 }
2634 
2651 {
2652  if (v.getRows() == 6) {
2653  isoJoIdentity = true;
2654  for (unsigned int i = 0; i < 6; i++) {
2655  // if(v[i] != 0){
2656  if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
2657  oJo[i][i] = 1.0;
2658  } else {
2659  oJo[i][i] = 0.0;
2660  isoJoIdentity = false;
2661  }
2662  }
2663  }
2664 }
2665 
2666 void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
2667  std::vector<std::vector<vpPoint> > &listFaces)
2668 {
2669  listFaces.clear();
2670 
2671  // std::vector<vpPoint> revolutionAxis;
2672  // revolutionAxis.push_back(p1);
2673  // revolutionAxis.push_back(p2);
2674  // listFaces.push_back(revolutionAxis);
2675 
2676  vpColVector axis(3);
2677  axis[0] = p1.get_oX() - p2.get_oX();
2678  axis[1] = p1.get_oY() - p2.get_oY();
2679  axis[2] = p1.get_oZ() - p2.get_oZ();
2680 
2681  vpColVector randomVec(3);
2682  randomVec = 0;
2683 
2684  vpColVector axisOrtho(3);
2685 
2686  randomVec[0] = 1.0;
2687  axisOrtho = vpColVector::crossProd(axis, randomVec);
2688 
2689  if (axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon()) {
2690  randomVec = 0;
2691  randomVec[1] = 1.0;
2692  axisOrtho = vpColVector::crossProd(axis, randomVec);
2693  if (axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon()) {
2694  randomVec = 0;
2695  randomVec[2] = 1.0;
2696  axisOrtho = vpColVector::crossProd(axis, randomVec);
2697  if (axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
2698  throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
2699  }
2700  }
2701 
2702  axisOrtho.normalize();
2703 
2704  vpColVector axisOrthoBis(3);
2705  axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
2706  axisOrthoBis.normalize();
2707 
2708  // First circle
2709  vpColVector p1Vec(3);
2710  p1Vec[0] = p1.get_oX();
2711  p1Vec[1] = p1.get_oY();
2712  p1Vec[2] = p1.get_oZ();
2713  vpColVector fc1 = p1Vec + axisOrtho * radius;
2714  vpColVector fc2 = p1Vec + axisOrthoBis * radius;
2715  vpColVector fc3 = p1Vec - axisOrtho * radius;
2716  vpColVector fc4 = p1Vec - axisOrthoBis * radius;
2717 
2718  vpColVector p2Vec(3);
2719  p2Vec[0] = p2.get_oX();
2720  p2Vec[1] = p2.get_oY();
2721  p2Vec[2] = p2.get_oZ();
2722  vpColVector sc1 = p2Vec + axisOrtho * radius;
2723  vpColVector sc2 = p2Vec + axisOrthoBis * radius;
2724  vpColVector sc3 = p2Vec - axisOrtho * radius;
2725  vpColVector sc4 = p2Vec - axisOrthoBis * radius;
2726 
2727  std::vector<vpPoint> pointsFace;
2728  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
2729  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
2730  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
2731  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
2732  listFaces.push_back(pointsFace);
2733 
2734  pointsFace.clear();
2735  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
2736  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
2737  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
2738  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
2739  listFaces.push_back(pointsFace);
2740 
2741  pointsFace.clear();
2742  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
2743  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
2744  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
2745  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
2746  listFaces.push_back(pointsFace);
2747 
2748  pointsFace.clear();
2749  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
2750  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
2751  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
2752  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
2753  listFaces.push_back(pointsFace);
2754 }
2755 
2765 bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
2766 {
2767  double dx = fabs(P1.get_oX() - P2.get_oX());
2768  double dy = fabs(P1.get_oY() - P2.get_oY());
2769  double dz = fabs(P1.get_oZ() - P2.get_oZ());
2770 
2771  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
2772  dz <= std::numeric_limits<double>::epsilon())
2773  return true;
2774  else
2775  return false;
2776 }
2777 
2778 void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, const int idFace, const std::string &polygonName,
2779  const bool useLod, const double minPolygonAreaThreshold,
2780  const double minLineLengthThreshold)
2781 {
2782  std::vector<vpPoint> corners_without_duplicates;
2783  corners_without_duplicates.push_back(corners[0]);
2784  for (unsigned int i = 0; i < corners.size() - 1; i++) {
2785  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
2786  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
2787  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
2788  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
2789  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
2790  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
2791  corners_without_duplicates.push_back(corners[i + 1]);
2792  }
2793  }
2794 
2795  vpMbtPolygon polygon;
2796  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
2797  polygon.setIndex((int)idFace);
2798  polygon.setName(polygonName);
2799  polygon.setLod(useLod);
2800 
2801  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
2802  polygon.setMinLineLengthThresh(minLineLengthThreshold);
2803 
2804  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
2805  polygon.addPoint(j, corners_without_duplicates[j]);
2806  }
2807 
2809 
2811  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
2812 
2813  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2814  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
2815 
2816  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2817  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
2818 }
2819 
2820 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
2821  const int idFace, const std::string &polygonName, const bool useLod,
2822  const double minPolygonAreaThreshold)
2823 {
2824  vpMbtPolygon polygon;
2825  polygon.setNbPoint(4);
2826  polygon.setName(polygonName);
2827  polygon.setLod(useLod);
2828 
2829  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
2830  // Non sense to set minLineLengthThreshold for circle
2831  // but used to be coherent when applying LOD settings for all polygons
2833 
2834  {
2835  // Create the 4 points of the circle bounding box
2836  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
2837 
2838  // Matrice de passage entre world et circle frame
2839  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
2840  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
2841  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
2842  vpRotationMatrix wRc;
2843  vpColVector x(3), y(3), z(3);
2844  // X axis is P2-P1
2845  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
2846  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
2847  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
2848  // Y axis is the normal of the plane
2849  y[0] = plane.getA() / norm_Y;
2850  y[1] = plane.getB() / norm_Y;
2851  y[2] = plane.getC() / norm_Y;
2852  // Z axis = X ^ Y
2853  z = vpColVector::crossProd(x, y);
2854  for (unsigned int i = 0; i < 3; i++) {
2855  wRc[i][0] = x[i];
2856  wRc[i][1] = y[i];
2857  wRc[i][2] = z[i];
2858  }
2859 
2860  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
2861  vpHomogeneousMatrix wMc(wtc, wRc);
2862 
2863  vpColVector c_p(4); // A point in the circle frame that is on the bbox
2864  c_p[0] = radius;
2865  c_p[1] = 0;
2866  c_p[2] = radius;
2867  c_p[3] = 1;
2868 
2869  // Matrix to rotate a point by 90 deg around Y in the circle frame
2870  for (unsigned int i = 0; i < 4; i++) {
2871  vpColVector w_p(4); // A point in the word frame
2873  w_p = wMc * cMc_90 * c_p;
2874 
2875  vpPoint w_P;
2876  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
2877 
2878  polygon.addPoint(i, w_P);
2879  }
2880  }
2881 
2882  polygon.setIndex(idFace);
2884 
2886  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
2887 
2888  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2889  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
2890 
2891  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2892  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
2893 }
2894 
2895 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const int idFace, const std::string &polygonName,
2896  const bool useLod, const double minLineLengthThreshold)
2897 {
2898  // A polygon as a single line that corresponds to the revolution axis of the
2899  // cylinder
2900  vpMbtPolygon polygon;
2901  polygon.setNbPoint(2);
2902 
2903  polygon.addPoint(0, p1);
2904  polygon.addPoint(1, p2);
2905 
2906  polygon.setIndex(idFace);
2907  polygon.setName(polygonName);
2908  polygon.setLod(useLod);
2909 
2910  polygon.setMinLineLengthThresh(minLineLengthThreshold);
2911  // Non sense to set minPolygonAreaThreshold for cylinder
2912  // but used to be coherent when applying LOD settings for all polygons
2914 
2916 
2918  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
2919 
2920  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2921  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
2922 
2923  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2924  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
2925 }
2926 
2927 void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace,
2928  const std::string &polygonName, const bool useLod, const double minLineLengthThreshold)
2929 {
2930  int id = idFace;
2931  for (unsigned int i = 0; i < listFaces.size(); i++) {
2932  vpMbtPolygon polygon;
2933  polygon.setNbPoint((unsigned int)listFaces[i].size());
2934  for (unsigned int j = 0; j < listFaces[i].size(); j++)
2935  polygon.addPoint(j, listFaces[i][j]);
2936 
2937  polygon.setIndex(id);
2938  polygon.setName(polygonName);
2939  polygon.setIsPolygonOriented(false);
2940  polygon.setLod(useLod);
2941  polygon.setMinLineLengthThresh(minLineLengthThreshold);
2943 
2945 
2947  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
2948 
2949  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2950  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
2951 
2952  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2953  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
2954 
2955  id++;
2956  }
2957 }
2958 
2959 void vpMbTracker::addProjectionErrorLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
2960 {
2961  // suppress line already in the model
2962  bool already_here = false;
2963  vpMbtDistanceLine *l;
2964 
2965  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
2966  l = *it;
2967  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
2968  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
2969  already_here = true;
2970  l->addPolygon(polygon);
2972  }
2973  }
2974 
2975  if (!already_here) {
2976  l = new vpMbtDistanceLine;
2977 
2979  l->buildFrom(P1, P2);
2980  l->addPolygon(polygon);
2983  l->useScanLine = useScanLine;
2984 
2985  l->setIndex((unsigned int)m_projectionErrorLines.size());
2986  l->setName(name);
2987 
2990 
2991  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2993 
2994  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2996 
2997  m_projectionErrorLines.push_back(l);
2998  }
2999 }
3000 
3001 void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace,
3002  const std::string &name)
3003 {
3004  bool already_here = false;
3005  vpMbtDistanceCircle *ci;
3006 
3007  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3008  ci = *it;
3009  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
3010  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
3011  already_here =
3012  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
3013  }
3014  }
3015 
3016  if (!already_here) {
3017  ci = new vpMbtDistanceCircle;
3018 
3019  ci->setCameraParameters(cam);
3020  ci->buildFrom(P1, P2, P3, r);
3022  ci->setIndex((unsigned int)m_projectionErrorCircles.size());
3023  ci->setName(name);
3024  ci->index_polygon = idFace;
3026 
3027  m_projectionErrorCircles.push_back(ci);
3028  }
3029 }
3030 
3031 void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace,
3032  const std::string &name)
3033 {
3034  bool already_here = false;
3036 
3037  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3038  ++it) {
3039  cy = *it;
3040  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
3041  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
3042  already_here =
3043  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
3044  }
3045  }
3046 
3047  if (!already_here) {
3048  cy = new vpMbtDistanceCylinder;
3049 
3050  cy->setCameraParameters(cam);
3051  cy->buildFrom(P1, P2, r);
3053  cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
3054  cy->setName(name);
3055  cy->index_polygon = idFace;
3057  m_projectionErrorCylinders.push_back(cy);
3058  }
3059 }
3060 
3061 void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
3062  const double radius, const int idFace, const std::string &name)
3063 {
3064  addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
3065 }
3066 
3067 void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, const double radius,
3068  const int idFace, const std::string &name)
3069 {
3070  addProjectionErrorCylinder(p1, p2, radius, idFace, name);
3071 }
3072 
3074 {
3075  unsigned int nbpt = polygon.getNbPoint();
3076  if (nbpt > 0) {
3077  for (unsigned int i = 0; i < nbpt - 1; i++)
3078  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3079  addProjectionErrorLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
3080  }
3081 }
3082 
3084 {
3085  unsigned int nbpt = polygon.getNbPoint();
3086  if (nbpt > 0) {
3087  for (unsigned int i = 0; i < nbpt - 1; i++)
3088  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3089  }
3090 }
3091 
3110  const vpCameraParameters &_cam)
3111 {
3112  if (!modelInitialised) {
3113  throw vpException(vpException::fatalError, "model not initialized");
3114  }
3115 
3116  unsigned int nbFeatures = 0;
3117  double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);
3118 
3119  if (nbFeatures > 0) {
3120  return vpMath::deg(totalProjectionError / (double)nbFeatures);
3121  }
3122 
3123  return 90.0;
3124 }
3125 
3127  const vpCameraParameters &_cam, unsigned int &nbFeatures)
3128 {
3129  bool update_cam = m_projectionErrorCam != _cam;
3130  if (update_cam) {
3131  m_projectionErrorCam = _cam;
3132 
3133  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3134  it != m_projectionErrorLines.end(); ++it) {
3135  vpMbtDistanceLine *l = *it;
3137  }
3138 
3139  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3140  it != m_projectionErrorCylinders.end(); ++it) {
3141  vpMbtDistanceCylinder *cy = *it;
3143  }
3144 
3145  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3146  it != m_projectionErrorCircles.end(); ++it) {
3147  vpMbtDistanceCircle *ci = *it;
3149  }
3150  }
3151 
3152 #ifdef VISP_HAVE_OGRE
3153  if (useOgre) {
3154  if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
3158  // Turn off Ogre config dialog display for the next call to this
3159  // function since settings are saved in the ogre.cfg file and used
3160  // during the next call
3162  }
3163  }
3164 #endif
3165 
3166  if (clippingFlag > 2)
3168 
3169  projectionErrorVisibleFace(I, _cMo);
3170 
3172 
3173  if (useScanLine) {
3174  if (clippingFlag <= 2)
3176 
3179  }
3180 
3182 
3183  double totalProjectionError = 0.0;
3184  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end();
3185  ++it) {
3186  vpMbtDistanceLine *l = *it;
3187  if (l->isVisible() && l->isTracked()) {
3188  for (size_t a = 0; a < l->meline.size(); a++) {
3189  if (l->meline[a] != NULL) {
3190  double lineNormGradient;
3191  unsigned int lineNbFeatures;
3192  l->meline[a]->computeProjectionError(I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
3195  totalProjectionError += lineNormGradient;
3196  nbFeatures += lineNbFeatures;
3197  }
3198  }
3199  }
3200  }
3201 
3202  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3203  it != m_projectionErrorCylinders.end(); ++it) {
3204  vpMbtDistanceCylinder *cy = *it;
3205  if (cy->isVisible() && cy->isTracked()) {
3206  if (cy->meline1 != NULL) {
3207  double cylinderNormGradient = 0;
3208  unsigned int cylinderNbFeatures = 0;
3209  cy->meline1->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3212  totalProjectionError += cylinderNormGradient;
3213  nbFeatures += cylinderNbFeatures;
3214  }
3215 
3216  if (cy->meline2 != NULL) {
3217  double cylinderNormGradient = 0;
3218  unsigned int cylinderNbFeatures = 0;
3219  cy->meline2->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3222  totalProjectionError += cylinderNormGradient;
3223  nbFeatures += cylinderNbFeatures;
3224  }
3225  }
3226  }
3227 
3228  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3229  it != m_projectionErrorCircles.end(); ++it) {
3230  vpMbtDistanceCircle *c = *it;
3231  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
3232  double circleNormGradient = 0;
3233  unsigned int circleNbFeatures = 0;
3234  c->meEllipse->computeProjectionError(I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
3237  totalProjectionError += circleNormGradient;
3238  nbFeatures += circleNbFeatures;
3239  }
3240  }
3241 
3242  return totalProjectionError;
3243 }
3244 
3246 {
3247  bool changed = false;
3248 
3249  if (!useOgre) {
3251  } else {
3252 #ifdef VISP_HAVE_OGRE
3254 #else
3256 #endif
3257  }
3258 }
3259 
3261 {
3262  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3263  for (size_t a = 0; a < (*it)->meline.size(); a++) {
3264  if ((*it)->meline[a] != NULL) {
3265  delete (*it)->meline[a];
3266  (*it)->meline[a] = NULL;
3267  }
3268  }
3269 
3270  (*it)->meline.clear();
3271  (*it)->nbFeature.clear();
3272  (*it)->nbFeatureTotal = 0;
3273  }
3274 
3275  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3276  ++it) {
3277  if ((*it)->meline1 != NULL) {
3278  delete (*it)->meline1;
3279  (*it)->meline1 = NULL;
3280  }
3281  if ((*it)->meline2 != NULL) {
3282  delete (*it)->meline2;
3283  (*it)->meline2 = NULL;
3284  }
3285 
3286  (*it)->nbFeature = 0;
3287  (*it)->nbFeaturel1 = 0;
3288  (*it)->nbFeaturel2 = 0;
3289  }
3290 
3291  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3292  if ((*it)->meEllipse != NULL) {
3293  delete (*it)->meEllipse;
3294  (*it)->meEllipse = NULL;
3295  }
3296  (*it)->nbFeature = 0;
3297  }
3298 }
3299 
3301 {
3302  const bool doNotTrack = true;
3303 
3304  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3305  it != m_projectionErrorLines.end(); ++it) {
3306  vpMbtDistanceLine *l = *it;
3307  bool isvisible = false;
3308 
3309  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
3310  ++itindex) {
3311  int index = *itindex;
3312  if (index == -1)
3313  isvisible = true;
3314  else {
3315  if (l->hiddenface->isVisible((unsigned int)index))
3316  isvisible = true;
3317  }
3318  }
3319 
3320  // Si la ligne n'appartient a aucune face elle est tout le temps visible
3321  if (l->Lindex_polygon.empty())
3322  isvisible = true; // Not sure that this can occur
3323 
3324  if (isvisible) {
3325  l->setVisible(true);
3326  l->updateTracked();
3327  if (l->meline.empty() && l->isTracked())
3328  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3329  } else {
3330  l->setVisible(false);
3331  for (size_t a = 0; a < l->meline.size(); a++) {
3332  if (l->meline[a] != NULL)
3333  delete l->meline[a];
3334  if (a < l->nbFeature.size())
3335  l->nbFeature[a] = 0;
3336  }
3337  l->nbFeatureTotal = 0;
3338  l->meline.clear();
3339  l->nbFeature.clear();
3340  }
3341  }
3342 
3343  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3344  it != m_projectionErrorCylinders.end(); ++it) {
3345  vpMbtDistanceCylinder *cy = *it;
3346 
3347  bool isvisible = false;
3348 
3349  int index = cy->index_polygon;
3350  if (index == -1)
3351  isvisible = true;
3352  else {
3353  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
3354  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
3355  isvisible = true;
3356  }
3357 
3358  if (isvisible) {
3359  cy->setVisible(true);
3360  if (cy->meline1 == NULL || cy->meline2 == NULL) {
3361  if (cy->isTracked())
3362  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3363  }
3364  } else {
3365  cy->setVisible(false);
3366  if (cy->meline1 != NULL)
3367  delete cy->meline1;
3368  if (cy->meline2 != NULL)
3369  delete cy->meline2;
3370  cy->meline1 = NULL;
3371  cy->meline2 = NULL;
3372  cy->nbFeature = 0;
3373  cy->nbFeaturel1 = 0;
3374  cy->nbFeaturel2 = 0;
3375  }
3376  }
3377 
3378  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3379  it != m_projectionErrorCircles.end(); ++it) {
3380  vpMbtDistanceCircle *ci = *it;
3381  bool isvisible = false;
3382 
3383  int index = ci->index_polygon;
3384  if (index == -1)
3385  isvisible = true;
3386  else {
3387  if (ci->hiddenface->isVisible((unsigned int)index))
3388  isvisible = true;
3389  }
3390 
3391  if (isvisible) {
3392  ci->setVisible(true);
3393  if (ci->meEllipse == NULL) {
3394  if (ci->isTracked())
3395  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3396  }
3397  } else {
3398  ci->setVisible(false);
3399  if (ci->meEllipse != NULL)
3400  delete ci->meEllipse;
3401  ci->meEllipse = NULL;
3402  ci->nbFeature = 0;
3403  }
3404  }
3405 }
3406 
3407 void vpMbTracker::loadConfigFile(const std::string &configFile)
3408 {
3409 #ifdef VISP_HAVE_XML2
3413 
3414  try {
3415  std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl;
3416  xmlp.parse(configFile);
3417  } catch (...) {
3418  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
3419  }
3420 
3421  vpMe meParser;
3422  xmlp.getProjectionErrorMe(meParser);
3423 
3424  setProjectionErrorMovingEdge(meParser);
3426 
3427 #else
3428  vpTRACE("You need the libXML2 to read the config file %s", configFile.c_str());
3429 #endif
3430 }
3431 
3438 {
3439  m_projectionErrorMe = me;
3440 
3441  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3442  vpMbtDistanceLine *l = *it;
3444  }
3445 
3446  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3447  ++it) {
3448  vpMbtDistanceCylinder *cy = *it;
3450  }
3451 
3452  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3453  vpMbtDistanceCircle *ci = *it;
3455  }
3456 }
3457 
3463 void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
3464 {
3466 
3467  m_SobelX.resize(size*2+1, size*2+1, false, false);
3469 
3470  m_SobelY.resize(size*2+1, size*2+1, false, false);
3472 }
3473 
static double getSobelKernelY(double *filter, unsigned int size)
virtual vpColVector getEstimatedDoF() const
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
vpDisplay * display
Definition: vpImage.h:134
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:136
virtual ~vpMbTracker()
bool parseBoolean(std::string &input)
Definition: vpMbTracker.h:847
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:219
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:362
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:171
virtual void setNbPoint(const unsigned int nb)
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:221
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
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:168
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
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)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int getWidth() const
Definition: vpImage.h:239
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
std::map< std::string, std::string > parseParameters(std::string &endLine)
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1582
unsigned int nbCircles
Number of circles in CAO model.
Definition: vpMbTracker.h:176
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:149
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)
std::map< std::string, std::string > mapOfParameterNames
Definition: vpMbTracker.h:188
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
double euclideanNorm() const
std::list< int > Lindex_polygon
Index of the faces which contain the line.
unsigned int nbCylinders
Number of cylinders in CAO model.
Definition: vpMbTracker.h:174
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
#define vpERROR_TRACE
Definition: vpDebug.h:393
unsigned int nbPoints
Number of points in CAO model.
Definition: vpMbTracker.h:166
virtual void loadConfigFile(const std::string &configFile)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:119
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")=0
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:211
void setProjectionErrorKernelSize(const unsigned int &size)
void setOgreShowConfigDialog(const bool showConfigDialog)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
bool modelInitialised
Definition: vpMbTracker.h:129
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:422
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
void setMinPolygonAreaThresh(const double min_polygon_area)
Definition: vpMbtPolygon.h:152
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.
static std::string path(const char *pathname)
Definition: vpIoTools.cpp:941
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
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.
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:84
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:86
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:161
unsigned int getCols() const
Definition: vpArray2D.h:146
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:126
void setIndex(const unsigned int i)
virtual void setEstimatedDoF(const vpColVector &v)
unsigned int getProjectionErrorKernelSize() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1477
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:134
static const vpColor green
Definition: vpColor.h:183
vpPoint * p1
The first extremity on the axe.
static void flush(const vpImage< unsigned char > &I)
void setFarClippingDistance(const double &dist)
Definition: vpAROgre.h:199
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
void projectionErrorVisibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo)
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:117
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)=0
bool m_projectionErrorOgreShowConfigDialog
Definition: vpMbTracker.h:209
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:157
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:675
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)=0
vpColVector & normalize()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:121
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1541
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)
std::string getName() const
Definition: vpMbtPolygon.h:108
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, const bool verbose=false, const bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
vpPoint * p2
The second extremity.
void projectionErrorResetMovingEdges()
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:693
void savePose(const std::string &filename) const
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:164
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:108
void setProjectionErrorMe(const vpMe &me)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:183
void getProjectionErrorMe(vpMe &me) const
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
VISP_EXPORT bool checkSSE2()
static void display(const vpImage< unsigned char > &I)
void clear()
Definition: vpColVector.h:113
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...
int getIndex() const
Definition: vpMbtPolygon.h:101
bool isVisible() const
void addPoint(const unsigned int n, const vpPoint &P)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:424
bool isTracked() const
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
Main methods for a model-based tracker.
Definition: vpMbTracker.h:110
std::vector< PolygonType * > & getPolygon()
void setIndex(const unsigned int i)
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, const int idFace=-1, const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:215
std::string & trim(std::string &s) const
Definition: vpMbTracker.h:872
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:193
void setIsPolygonOriented(const bool &oriented)
Definition: vpMbtPolygon.h:166
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
Definition: vpMbTracker.h:170
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:146
unsigned int getRows() const
Definition: vpArray2D.h:156
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:151
void addPolygon(PolygonType *p)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1442
static double rad(double deg)
Definition: vpMath.h:102
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
std::string poseSavingFilename
Definition: vpMbTracker.h:132
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
Definition: vpMbTracker.h:202
void setVisible(bool _isvisible)
unsigned int size() const
double sumSquare() const
void setName(const std::string &line_name)
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:227
void setCameraParameters(const vpCameraParameters &camera)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:420
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void setIndex(const unsigned int i)
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
Definition: vpMbTracker.h:172
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:185
static double deg(double rad)
Definition: vpMath.h:95
virtual void setOgreVisibilityTest(const bool &v)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
int getWindowYPosition() const
Definition: vpDisplay.h:250
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:207
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:225
bool applyLodSettingInConfig
Definition: vpMbTracker.h:181
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
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))
void setLod(const bool use_lod)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
void setMovingEdge(vpMe *Me)
void setProjectionErrorMovingEdge(const vpMe &me)
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:217
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double getB() const
Definition: vpPlane.h:104
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
bool isVisible(const unsigned int i)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
error that can be emited by the vpMatrix class and its derivates
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:153
double getA() const
Definition: vpPlane.h:102
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void addPolygon(const int &index)
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
unsigned int getHeight() const
Definition: vpImage.h:178
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
static std::string getAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1499
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
static double getSobelKernelX(double *filter, unsigned int size)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932
virtual void setClipping(const unsigned int &flags)
double getC() const
Definition: vpPlane.h:106
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double radius
The radius of the cylinder.
void addPolygon(const std::vector< vpPoint > &corners, const int idFace=-1, const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
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:159
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:213
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:137
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
Definition: vpMbTracker.h:206
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:155
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:178
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
std::vector< unsigned int > nbFeature
The number of moving edges.
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
Class that consider the case of a translation vector.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void eye()
Definition: vpMatrix.cpp:360
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:123
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:223
vpPoint * p1
The center of the circle.
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(const bool orderPolygons=true, const bool useVisibility=true, const bool clipPolygon=false)
void removeComment(std::ifstream &fileId)
bool useScanLine
Use scanline rendering.
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:324
void buildFrom(vpPoint &_p1, vpPoint &_p2)
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:208
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
Definition: vpMbTracker.h:204
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")=0
void setMinLineLengthThresh(const double min_line_length)
Definition: vpMbtPolygon.h:141
int getWindowXPosition() const
Definition: vpDisplay.h:245
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
virtual void setIndex(const int i)
Definition: vpMbtPolygon.h:124
void clearPoint()
Definition: vpPose.cpp:122
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)