Visual Servoing Platform  version 3.4.0 under development (2021-04-14)
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) {
432  vpDisplay::display(*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  }
437  vpDisplay::flush(*I);
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) {
450  vpDisplay::getClick(*I, ip);
451  mem_ip.push_back(ip);
452  vpDisplay::flush(*I);
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 
1326  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1327  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1328 
1329  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
1407  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1408  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1409 
1410  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
1445  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1446  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1447 
1448  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
1474  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1475  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1476 
1477  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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
2314 
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 {
2737  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
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 {
2816  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
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 
3178  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3179  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3180 
3181  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
3253  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3254  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3255 
3256  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
3285  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3286  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3287 
3288  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
3314  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3315  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3316 
3317  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
3356  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3358 
3359  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
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 
static double getSobelKernelY(double *filter, unsigned int size)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
vpDisplay * display
Definition: vpImage.h:144
void addPoint(unsigned int n, const vpPoint &P)
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
virtual ~vpMbTracker()
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:449
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:177
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
unsigned int nbLines
Number of lines in CAO model.
Definition: vpMbTracker.h:162
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
bool isVisible(unsigned int i)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double frobeniusNorm() const
void setCameraParameters(const vpCameraParameters &camera)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
int getIndex() const
Definition: vpMbtPolygon.h:101
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int size() const
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setIndex(unsigned int i)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::map< std::string, std::string > parseParameters(std::string &endLine)
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1487
unsigned int nbCircles
Number of circles in CAO model.
Definition: vpMbTracker.h:170
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
void setNearClippingDistance(const double &dist)
Definition: vpAROgre.h:210
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
std::map< std::string, std::string > mapOfParameterNames
Definition: vpMbTracker.h:182
std::string getName() const
Definition: vpMbtPolygon.h:108
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isTracked() const
unsigned int nbCylinders
Number of cylinders in CAO model.
Definition: vpMbTracker.h:168
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
#define vpERROR_TRACE
Definition: vpDebug.h:393
unsigned int nbPoints
Number of points in CAO model.
Definition: vpMbTracker.h:160
virtual void setNbPoint(unsigned int nb)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpPoint * p1
The first extremity.
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
Definition: vpMbTracker.h:205
void setProjectionErrorKernelSize(const unsigned int &size)
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
bool modelInitialised
Definition: vpMbTracker.h:123
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Definition: vpArray2D.h:289
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition: vpMe.h:60
Manage the line of a polygon used in the model-based tracker.
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int nbFeature
The number of moving edges.
void setName(const std::string &face_name)
Definition: vpMbtPolygon.h:159
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:94
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
std::string modelFileName
Definition: vpMbTracker.h:120
virtual void setEstimatedDoF(const vpColVector &v)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1382
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
static const vpColor green
Definition: vpColor.h:220
vpPoint * p1
The first extremity on the axe.
static void flush(const vpImage< unsigned char > &I)
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:447
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void setLod(bool useLod, const std::string &name="")
void setFarClippingDistance(const double &dist)
Definition: vpAROgre.h:199
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:841
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:497
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)=0
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:499
unsigned int getCols() const
Definition: vpArray2D.h:279
bool m_projectionErrorOgreShowConfigDialog
Definition: vpMbTracker.h:203
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
static bool parseBoolean(std::string input)
Definition: vpIoTools.cpp:1928
vpColVector & normalize()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1446
Error that can be emited by the vpTracker class and its derivates.
void setProjectionErrorKernelSize(const unsigned int &size)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setName(const std::string &circle_name)
vpPoint * p2
The second extremity.
void projectionErrorResetMovingEdges()
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:887
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:116
void setProjectionErrorMe(const vpMe &me)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
static void display(const vpImage< unsigned char > &I)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
void clear()
Definition: vpColVector.h:175
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
double getB() const
Definition: vpPlane.h:104
void setMinPolygonAreaThresh(double min_polygon_area)
Definition: vpMbtPolygon.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
Generic class defining intrinsic camera parameters.
void setOgreShowConfigDialog(bool showConfigDialog)
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Main methods for a model-based tracker.
Definition: vpMbTracker.h:104
std::vector< PolygonType * > & getPolygon()
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:451
virtual void loadVRMLModel(const std::string &modelFile)
Implementation of a rotation vector as quaternion angle minimal representation.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
void setIsPolygonOriented(const bool &oriented)
Definition: vpMbtPolygon.h:166
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1676
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
Definition: vpMbTracker.h:164
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
void addPolygon(PolygonType *p)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1347
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
bool m_sodb_init_called
Flag that indicates that SoDB::init(); was called.
Definition: vpMbTracker.h:225
static double rad(double deg)
Definition: vpMath.h:110
std::string poseSavingFilename
Definition: vpMbTracker.h:126
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
Definition: vpMbTracker.h:196
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
void setVisible(bool _isvisible)
int getWindowXPosition() const
Definition: vpDisplay.h:251
double getA() const
Definition: vpPlane.h:102
void setName(const std::string &line_name)
int getWindowYPosition() const
Definition: vpDisplay.h:256
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
void setCameraParameters(const vpCameraParameters &camera)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
Definition: vpMbTracker.h:166
bool isVisible() const
static std::string trim(std::string s)
Definition: vpIoTools.cpp:1942
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:103
void setMinLineLengthThresh(double min_line_length)
Definition: vpMbtPolygon.h:141
virtual void setOgreVisibilityTest(const bool &v)
unsigned int getHeight() const
Definition: vpImage.h:188
void setIndex(unsigned int i)
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:244
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
Definition: vpMbTracker.h:219
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
void setMovingEdge(vpMe *Me)
void setLod(bool use_lod)
void setProjectionErrorMovingEdge(const vpMe &me)
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double sumSquare() const
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
error that can be emited by the vpMatrix class and its derivates
Tukey influence function.
Definition: vpRobust.h:93
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual vpColVector getEstimatedDoF() const
double getC() const
Definition: vpPlane.h:106
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void addPolygon(const int &index)
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:640
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
static std::string getAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1404
static double getSobelKernelX(double *filter, unsigned int size)
virtual void setClipping(const unsigned int &flags)
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
Definition: vpMbTracker.h:227
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
double radius
The radius of the cylinder.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
std::vector< vpMbtMeLine * > meline
The moving edge container.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
unsigned int getProjectionErrorKernelSize() const
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
unsigned int nbFeaturel2
The number of moving edges on line 2.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
Definition: vpMbTracker.h:207
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...
Definition: vpPose.cpp:336
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void savePose(const std::string &filename) const
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
Definition: vpMbTracker.h:200
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:246
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< unsigned int > nbFeature
The number of moving edges.
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Class that consider the case of a translation vector.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void eye()
Definition: vpMatrix.cpp:449
Implementation of a rotation vector as axis-angle minimal representation.
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
virtual void setIndex(int i)
Definition: vpMbtPolygon.h:124
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
vpPoint * p1
The center of the circle.
void removeComment(std::ifstream &fileId)
bool useScanLine
Use scanline rendering.
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:202
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setIndex(unsigned int i)
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
Definition: vpMbTracker.h:198
void getProjectionErrorMe(vpMe &me) const
void clearPoint()
Definition: vpPose.cpp:134
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)