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