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