Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpMbTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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  *****************************************************************************/
35 
41 #include <algorithm>
42 #include <iostream>
43 #include <limits>
44 #include <sstream>
45 
46 #include <Simd/SimdLib.hpp>
47 
48 #include <visp3/core/vpColVector.h>
49 #include <visp3/core/vpDisplay.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMatrix.h>
52 #include <visp3/core/vpPoint.h>
53 #include <visp3/vision/vpPose.h>
54 #ifdef VISP_HAVE_MODULE_GUI
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayOpenCV.h>
57 #include <visp3/gui/vpDisplayX.h>
58 #endif
59 #include <visp3/core/vpCameraParameters.h>
60 #include <visp3/core/vpColor.h>
61 #include <visp3/core/vpException.h>
62 #include <visp3/core/vpIoTools.h>
63 #include <visp3/core/vpPixelMeterConversion.h>
64 #ifdef VISP_HAVE_MODULE_IO
65 #include <visp3/io/vpImageIo.h>
66 #endif
67 #include <visp3/core/vpCPUFeatures.h>
68 #include <visp3/core/vpIoTools.h>
69 #include <visp3/core/vpMatrixException.h>
70 #include <visp3/core/vpTrackingException.h>
71 #include <visp3/mbt/vpMbTracker.h>
72 
73 #include <visp3/core/vpImageFilter.h>
74 #include <visp3/mbt/vpMbtXmlGenericParser.h>
75 
76 #ifdef VISP_HAVE_COIN3D
77 // Inventor includes
78 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
79 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
80 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
81 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
82 #include <Inventor/VRMLnodes/SoVRMLShape.h>
83 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
84 #include <Inventor/actions/SoGetMatrixAction.h>
85 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
86 #include <Inventor/actions/SoSearchAction.h>
87 #include <Inventor/actions/SoToVRML2Action.h>
88 #include <Inventor/actions/SoWriteAction.h>
89 #include <Inventor/misc/SoChildList.h>
90 #include <Inventor/nodes/SoSeparator.h>
91 #endif
92 
93 #ifndef DOXYGEN_SHOULD_SKIP_THIS
94 
95 namespace
96 {
100 struct SegmentInfo {
101  SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
102 
103  std::vector<vpPoint> extremities;
104  std::string name;
105  bool useLod;
106  double minLineLengthThresh;
107 };
108 
113 struct PolygonFaceInfo {
114  PolygonFaceInfo(double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
115  : distanceToCamera(dist), polygon(poly), faceCorners(corners)
116  {
117  }
118 
119  bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }
120 
121  double distanceToCamera;
122  vpPolygon polygon;
123  std::vector<vpPoint> faceCorners;
124 };
125 
133 std::istream &safeGetline(std::istream &is, std::string &t)
134 {
135  t.clear();
136 
137  // The characters in the stream are read one-by-one using a std::streambuf.
138  // That is faster than reading them one-by-one using the std::istream.
139  // Code that uses streambuf this way must be guarded by a sentry object.
140  // The sentry object performs various tasks,
141  // such as thread synchronization and updating the stream state.
142 
143  std::istream::sentry se(is, true);
144  std::streambuf *sb = is.rdbuf();
145 
146  for (;;) {
147  int c = sb->sbumpc();
148  if (c == '\n') {
149  return is;
150  } else if (c == '\r') {
151  if (sb->sgetc() == '\n')
152  sb->sbumpc();
153  return is;
154  } else if (c == std::streambuf::traits_type::eof()) {
155  // Also handle the case when the last line has no line ending
156  if (t.empty())
157  is.setstate(std::ios::eofbit);
158  return is;
159  } else { // default case
160  t += (char)c;
161  }
162  }
163 }
164 } // namespace
165 #endif // DOXYGEN_SHOULD_SKIP_THIS
166 
173  : m_cam(), m_cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
174  computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
175  displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
176  angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
177  useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
178  nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
179  minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
180  m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
181  m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
182  m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
183  m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
184  m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false),
185  m_rand()
186 {
187  oJo.eye();
188  // Map used to parse additional information in CAO model files,
189  // like name of faces or LOD setting
190  mapOfParameterNames["name"] = "string";
191  mapOfParameterNames["minPolygonAreaThreshold"] = "number";
192  mapOfParameterNames["minLineLengthThreshold"] = "number";
193  mapOfParameterNames["useLod"] = "boolean";
194 
197 }
198 
200 {
201  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
202  it != m_projectionErrorLines.end(); ++it) {
203  vpMbtDistanceLine *l = *it;
204  if (l != NULL)
205  delete l;
206  l = NULL;
207  }
208 
209  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
210  it != m_projectionErrorCylinders.end(); ++it) {
211  vpMbtDistanceCylinder *cy = *it;
212  if (cy != NULL)
213  delete cy;
214  cy = NULL;
215  }
216 
217  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
218  it != m_projectionErrorCircles.end(); ++it) {
219  vpMbtDistanceCircle *ci = *it;
220  if (ci != NULL)
221  delete ci;
222  ci = NULL;
223  }
224 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
225  if (m_sodb_init_called) {
226  // Cleanup memory allocated by Coin library used to load a vrml model
227  SoDB::finish();
228  }
229 #endif
230 }
231 
232 #ifdef VISP_HAVE_MODULE_GUI
233 void vpMbTracker::initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
234  const std::string &initFile, bool displayHelp, const vpHomogeneousMatrix &T)
235 {
236  vpHomogeneousMatrix last_cMo;
237  vpPoseVector init_pos;
238  vpImagePoint ip;
240 
241  std::string ext = ".init";
242  std::string str_pose = "";
243  size_t pos = initFile.rfind(ext);
244 
245  // Load the last poses from files
246  std::fstream finitpos;
247  std::ifstream finit;
248  std::stringstream ss;
249  if (poseSavingFilename.empty()) {
250  if (pos != std::string::npos)
251  str_pose = initFile.substr(0, pos) + ".0.pos";
252  else
253  str_pose = initFile + ".0.pos";
254 
255  finitpos.open(str_pose.c_str(), std::ios::in);
256  ss << str_pose;
257  } else {
258  finitpos.open(poseSavingFilename.c_str(), std::ios::in);
259  ss << poseSavingFilename;
260  }
261  if (finitpos.fail()) {
262  std::cout << "Cannot read " << ss.str() << std::endl << "cMo set to identity" << std::endl;
263  last_cMo.eye();
264  } else {
265  for (unsigned int i = 0; i < 6; i += 1) {
266  finitpos >> init_pos[i];
267  }
268 
269  finitpos.close();
270  last_cMo.buildFrom(init_pos);
271 
272  std::cout << "last_cMo : " << std::endl << last_cMo << std::endl;
273 
274  if (I) {
275  vpDisplay::display(*I);
276  display(*I, last_cMo, m_cam, vpColor::green, 1, true);
277  vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green);
278  vpDisplay::flush(*I);
279  } else {
280  vpDisplay::display(*I_color);
281  display(*I_color, last_cMo, m_cam, vpColor::green, 1, true);
282  vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green);
283  vpDisplay::flush(*I_color);
284  }
285 
286  std::cout << "No modification : left click " << std::endl;
287  std::cout << "Modify initial pose : right click " << std::endl;
288 
289  if (I) {
290  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
291 
292  vpDisplay::flush(*I);
293 
294  while (!vpDisplay::getClick(*I, ip, button)) {
295  }
296  } else {
297  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose",
298  vpColor::red);
299 
300  vpDisplay::flush(*I_color);
301 
302  while (!vpDisplay::getClick(*I_color, ip, button)) {
303  }
304  }
305  }
306 
307  if (!finitpos.fail() && button == vpMouseButton::button1) {
308  m_cMo = last_cMo;
309  } else {
310  vpDisplay *d_help = NULL;
311 
312  if (I) {
313  vpDisplay::display(*I);
314  vpDisplay::flush(*I);
315  } else {
316  vpDisplay::display(*I_color);
317  vpDisplay::flush(*I_color);
318  }
319 
320  vpPose pose;
321 
322  pose.clearPoint();
323 
324  // Clear string stream that previously contained the path to the "object.0.pos" file.
325  ss.str(std::string());
326 
327  // file parser
328  // number of points
329  // X Y Z
330  // X Y Z
331  if (pos != std::string::npos) {
332  ss << initFile;
333  } else {
334  ss << initFile;
335  ss << ".init";
336  }
337 
338  std::cout << "Load 3D points from: " << ss.str() << std::endl;
339 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
340  finit.open(ss.str());
341 #else
342  finit.open(ss.str().c_str());
343 #endif
344  if (finit.fail()) {
345  std::cout << "Cannot read " << ss.str() << std::endl;
346  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str());
347  }
348 
349 #ifdef VISP_HAVE_MODULE_IO
350  // Display window creation and initialisation
351  try {
352  if (displayHelp) {
353  const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"};
354  std::string dispF;
355  bool foundHelpImg = false;
356  if (pos != std::string::npos) {
357  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
358  dispF = initFile.substr(0, pos) + imgExtVec[i];
359  foundHelpImg = vpIoTools::checkFilename(dispF);
360  }
361  } else {
362  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
363  dispF = initFile + imgExtVec[i];
364  foundHelpImg = vpIoTools::checkFilename(dispF);
365  }
366  }
367 
368  if (foundHelpImg) {
369  std::cout << "Load image to help initialization: " << dispF << std::endl;
370 #if defined VISP_HAVE_X11
371  d_help = new vpDisplayX;
372 #elif defined VISP_HAVE_GDI
373  d_help = new vpDisplayGDI;
374 #elif defined VISP_HAVE_OPENCV
375  d_help = new vpDisplayOpenCV;
376 #endif
377 
378  vpImage<vpRGBa> Iref;
379  vpImageIo::read(Iref, dispF);
380 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
381  const int winXPos = I != NULL ? I->display->getWindowXPosition() : I_color->display->getWindowXPosition();
382  const int winYPos = I != NULL ? I->display->getWindowYPosition() : I_color->display->getWindowYPosition();
383  unsigned int width = I != NULL ? I->getWidth() : I_color->getWidth();
384  d_help->init(Iref, winXPos + (int)width + 80, winYPos, "Where to initialize...");
385  vpDisplay::display(Iref);
386  vpDisplay::flush(Iref);
387 #endif
388  }
389  }
390  } catch (...) {
391  if (d_help != NULL) {
392  delete d_help;
393  d_help = NULL;
394  }
395  }
396 #else //#ifdef VISP_HAVE_MODULE_IO
397  (void)(displayHelp);
398 #endif //#ifdef VISP_HAVE_MODULE_IO
399  // skip lines starting with # as comment
400  removeComment(finit);
401 
402  unsigned int n3d;
403  finit >> n3d;
404  finit.ignore(256, '\n'); // skip the rest of the line
405  std::cout << "Number of 3D points " << n3d << std::endl;
406  if (n3d > 100000) {
407  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed",
408  ss.str().c_str());
409  }
410 
411  std::vector<vpPoint> P(n3d);
412  for (unsigned int i = 0; i < n3d; i++) {
413  // skip lines starting with # as comment
414  removeComment(finit);
415 
416  vpColVector pt_3d(4, 1.0);
417  finit >> pt_3d[0];
418  finit >> pt_3d[1];
419  finit >> pt_3d[2];
420  finit.ignore(256, '\n'); // skip the rest of the line
421 
422  vpColVector pt_3d_tf = T * pt_3d;
423  std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " "
424  << pt_3d_tf[2] << std::endl;
425 
426  P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
427  }
428 
429  finit.close();
430 
431  bool isWellInit = false;
432  while (!isWellInit) {
433  std::vector<vpImagePoint> mem_ip;
434  for (unsigned int i = 0; i < n3d; i++) {
435  std::ostringstream text;
436  text << "Click on point " << i + 1;
437  if (I) {
438  vpDisplay::display(*I);
439  vpDisplay::displayText(*I, 15, 10, text.str(), vpColor::red);
440  for (unsigned int k = 0; k < mem_ip.size(); k++) {
441  vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2);
442  }
443  vpDisplay::flush(*I);
444  } else {
445  vpDisplay::display(*I_color);
446  vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red);
447  for (unsigned int k = 0; k < mem_ip.size(); k++) {
448  vpDisplay::displayCross(*I_color, mem_ip[k], 10, vpColor::green, 2);
449  }
450  vpDisplay::flush(*I_color);
451  }
452 
453  std::cout << "Click on point " << i + 1 << " ";
454  double x = 0, y = 0;
455  if (I) {
457  mem_ip.push_back(ip);
458  vpDisplay::flush(*I);
459  } else {
460  vpDisplay::getClick(*I_color, ip);
461  mem_ip.push_back(ip);
462  vpDisplay::flush(*I_color);
463  }
465  P[i].set_x(x);
466  P[i].set_y(y);
467 
468  std::cout << "with 2D coordinates: " << ip << std::endl;
469 
470  pose.addPoint(P[i]); // and added to the pose computation point list
471  }
472  if (I) {
473  vpDisplay::flush(*I);
474  vpDisplay::display(*I);
475  } else {
476  vpDisplay::flush(*I_color);
477  vpDisplay::display(*I_color);
478  }
479 
480  vpHomogeneousMatrix cMo1, cMo2;
481  double d1, d2;
482  d1 = d2 = std::numeric_limits<double>::max();
483  try {
484  pose.computePose(vpPose::LAGRANGE, cMo1);
485  d1 = pose.computeResidual(cMo1);
486  } catch (...) {
487  // Lagrange non-planar cannot work with less than 6 points
488  }
489  try {
490  pose.computePose(vpPose::DEMENTHON, cMo2);
491  d2 = pose.computeResidual(cMo2);
492  } catch (...) {
493  // Should not occur
494  }
495 
496  if (d1 < d2) {
497  m_cMo = cMo1;
498  } else {
499  m_cMo = cMo2;
500  }
502 
503  if (I) {
504  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
505  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
506 
507  vpDisplay::flush(*I);
508 
509  button = vpMouseButton::button1;
510  while (!vpDisplay::getClick(*I, ip, button)) {
511  }
512 
513  if (button == vpMouseButton::button1) {
514  isWellInit = true;
515  } else {
516  pose.clearPoint();
517  vpDisplay::display(*I);
518  vpDisplay::flush(*I);
519  }
520  } else {
521  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
522  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object",
523  vpColor::red);
524 
525  vpDisplay::flush(*I_color);
526 
527  button = vpMouseButton::button1;
528  while (!vpDisplay::getClick(*I_color, ip, button)) {
529  }
530 
531  if (button == vpMouseButton::button1) {
532  isWellInit = true;
533  } else {
534  pose.clearPoint();
535  vpDisplay::display(*I_color);
536  vpDisplay::flush(*I_color);
537  }
538  }
539  }
540  if (I)
542  else
543  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
544 
545  // save the pose into file
546  if (poseSavingFilename.empty())
547  savePose(str_pose);
548  else
550 
551  if (d_help != NULL) {
552  delete d_help;
553  d_help = NULL;
554  }
555  }
556 
557  std::cout << "cMo : " << std::endl << m_cMo << std::endl;
558 
559  if (I)
560  init(*I);
561  else {
562  vpImageConvert::convert(*I_color, m_I);
563  init(m_I);
564  }
565 }
566 
598 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp,
599  const vpHomogeneousMatrix &T)
600 {
601  initClick(&I, NULL, initFile, displayHelp, T);
602 }
603 
635 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, bool displayHelp,
636  const vpHomogeneousMatrix &T)
637 {
638  initClick(NULL, &I_color, initFile, displayHelp, T);
639 }
640 
641 void vpMbTracker::initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
642  const std::vector<vpPoint> &points3D_list, const std::string &displayFile)
643 {
644  if (I) {
645  vpDisplay::display(*I);
646  vpDisplay::flush(*I);
647  } else {
648  vpDisplay::display(*I_color);
649  vpDisplay::flush(*I_color);
650  }
651 
652  vpDisplay *d_help = NULL;
653 
654  vpPose pose;
655  std::vector<vpPoint> P;
656  for (unsigned int i = 0; i < points3D_list.size(); i++)
657  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
658 
659 #ifdef VISP_HAVE_MODULE_IO
660  vpImage<vpRGBa> Iref;
661  // Display window creation and initialisation
662  if (vpIoTools::checkFilename(displayFile)) {
663  try {
664  std::cout << "Load image to help initialization: " << displayFile << std::endl;
665 #if defined VISP_HAVE_X11
666  d_help = new vpDisplayX;
667 #elif defined VISP_HAVE_GDI
668  d_help = new vpDisplayGDI;
669 #elif defined VISP_HAVE_OPENCV
670  d_help = new vpDisplayOpenCV;
671 #endif
672 
673  vpImageIo::read(Iref, displayFile);
674 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
675  if (I) {
676  d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(),
677  "Where to initialize...");
678  } else {
679  d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80,
680  I_color->display->getWindowYPosition(), "Where to initialize...");
681  }
682  vpDisplay::display(Iref);
683  vpDisplay::flush(Iref);
684 #endif
685  } catch (...) {
686  if (d_help != NULL) {
687  delete d_help;
688  d_help = NULL;
689  }
690  }
691  }
692 #else //#ifdef VISP_HAVE_MODULE_IO
693  (void)(displayFile);
694 #endif //#ifdef VISP_HAVE_MODULE_IO
695 
696  vpImagePoint ip;
697  bool isWellInit = false;
698  while (!isWellInit) {
699  for (unsigned int i = 0; i < points3D_list.size(); i++) {
700  std::cout << "Click on point " << i + 1 << std::endl;
701  double x = 0, y = 0;
702  if (I) {
703  vpDisplay::getClick(*I, ip);
705  vpDisplay::flush(*I);
706  } else {
707  vpDisplay::getClick(*I_color, ip);
708  vpDisplay::displayCross(*I_color, ip, 5, vpColor::green);
709  vpDisplay::flush(*I_color);
710  }
712  P[i].set_x(x);
713  P[i].set_y(y);
714 
715  std::cout << "Click on point " << ip << std::endl;
716 
717  if (I) {
718  vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point
719  } else {
720  vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point
721  }
722  pose.addPoint(P[i]); // and added to the pose computation point list
723  }
724  if (I) {
725  vpDisplay::flush(*I);
726  } else {
727  vpDisplay::flush(*I_color);
728  }
729 
730  vpHomogeneousMatrix cMo1, cMo2;
731  double d1, d2;
732  d1 = d2 = std::numeric_limits<double>::max();
733  try {
734  pose.computePose(vpPose::LAGRANGE, cMo1);
735  d1 = pose.computeResidual(cMo1);
736  } catch (...) {
737  // Lagrange non-planar cannot work with less than 6 points
738  }
739  try {
740  pose.computePose(vpPose::DEMENTHON, cMo2);
741  d2 = pose.computeResidual(cMo2);
742  } catch (...) {
743  // Should not occur
744  }
745 
746  if (d1 < d2) {
747  m_cMo = cMo1;
748  } else {
749  m_cMo = cMo2;
750  }
752 
753  if (I) {
754  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
755  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
756 
757  vpDisplay::flush(*I);
758 
760  while (!vpDisplay::getClick(*I, ip, button)) {
761  };
762 
763  if (button == vpMouseButton::button1) {
764  isWellInit = true;
765  } else {
766  pose.clearPoint();
767  vpDisplay::display(*I);
768  vpDisplay::flush(*I);
769  }
770  } else {
771  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
772  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object",
773  vpColor::red);
774 
775  vpDisplay::flush(*I_color);
776 
778  while (!vpDisplay::getClick(*I_color, ip, button)) {
779  };
780 
781  if (button == vpMouseButton::button1) {
782  isWellInit = true;
783  } else {
784  pose.clearPoint();
785  vpDisplay::display(*I_color);
786  vpDisplay::flush(*I_color);
787  }
788  }
789  }
790 
791  if (I) {
793  } else {
794  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
795  }
796 
797  if (d_help != NULL) {
798  delete d_help;
799  d_help = NULL;
800  }
801 
802  if (I)
803  init(*I);
804  else {
805  vpImageConvert::convert(*I_color, m_I);
806  init(m_I);
807  }
808 }
809 
821 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
822  const std::string &displayFile)
823 {
824  initClick(&I, NULL, points3D_list, displayFile);
825 }
826 
838 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::vector<vpPoint> &points3D_list,
839  const std::string &displayFile)
840 {
841  initClick(NULL, &I_color, points3D_list, displayFile);
842 }
843 #endif //#ifdef VISP_HAVE_MODULE_GUI
844 
845 void vpMbTracker::initFromPoints(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
846  const std::string &initFile)
847 {
848  std::stringstream ss;
849  std::fstream finit;
850 
851  std::string ext = ".init";
852  size_t pos = initFile.rfind(ext);
853 
854  if (pos == initFile.size() - ext.size() && pos != 0) {
855  ss << initFile;
856  } else {
857  ss << initFile;
858  ss << ".init";
859  }
860 
861  std::cout << "Load 2D/3D points from: " << ss.str() << std::endl;
862  finit.open(ss.str().c_str(), std::ios::in);
863  if (finit.fail()) {
864  std::cout << "cannot read " << ss.str() << std::endl;
865  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str());
866  }
867 
868  //********
869  // Read 3D points coordinates
870  //********
871  char c;
872  // skip lines starting with # as comment
873  finit.get(c);
874  while (!finit.fail() && (c == '#')) {
875  finit.ignore(256, '\n');
876  finit.get(c);
877  }
878  finit.unget();
879 
880  unsigned int n3d;
881  finit >> n3d;
882  finit.ignore(256, '\n'); // skip the rest of the line
883  std::cout << "Number of 3D points " << n3d << std::endl;
884  if (n3d > 100000) {
885  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed",
886  ss.str().c_str());
887  }
888 
889  vpPoint *P = new vpPoint[n3d];
890  for (unsigned int i = 0; i < n3d; i++) {
891  // skip lines starting with # as comment
892  finit.get(c);
893  while (!finit.fail() && (c == '#')) {
894  finit.ignore(256, '\n');
895  finit.get(c);
896  }
897  finit.unget();
898  double X, Y, Z;
899  finit >> X;
900  finit >> Y;
901  finit >> Z;
902  finit.ignore(256, '\n'); // skip the rest of the line
903 
904  std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
905  P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
906  }
907 
908  //********
909  // Read 3D points coordinates
910  //********
911  // skip lines starting with # as comment
912  finit.get(c);
913  while (!finit.fail() && (c == '#')) {
914  finit.ignore(256, '\n');
915  finit.get(c);
916  }
917  finit.unget();
918 
919  unsigned int n2d;
920  finit >> n2d;
921  finit.ignore(256, '\n'); // skip the rest of the line
922  std::cout << "Number of 2D points " << n2d << std::endl;
923  if (n2d > 100000) {
924  delete[] P;
925  throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed",
926  ss.str().c_str());
927  }
928 
929  if (n3d != n2d) {
930  delete[] P;
932  "In %s file, number of 2D points %d and number of 3D "
933  "points %d are not equal",
934  ss.str().c_str(), n2d, n3d);
935  }
936 
937  vpPose pose;
938  for (unsigned int i = 0; i < n2d; i++) {
939  // skip lines starting with # as comment
940  finit.get(c);
941  while (!finit.fail() && (c == '#')) {
942  finit.ignore(256, '\n');
943  finit.get(c);
944  }
945  finit.unget();
946  double u, v, x = 0, y = 0;
947  finit >> v;
948  finit >> u;
949  finit.ignore(256, '\n'); // skip the rest of the line
950 
951  vpImagePoint ip(v, u);
952  std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
954  P[i].set_x(x);
955  P[i].set_y(y);
956  pose.addPoint(P[i]);
957  }
958 
959  finit.close();
960 
961  vpHomogeneousMatrix cMo1, cMo2;
962  double d1, d2;
963  d1 = d2 = std::numeric_limits<double>::max();
964  try {
965  pose.computePose(vpPose::LAGRANGE, cMo1);
966  d1 = pose.computeResidual(cMo1);
967  } catch (...) {
968  // Lagrange non-planar cannot work with less than 6 points
969  }
970  try {
971  pose.computePose(vpPose::DEMENTHON, cMo2);
972  d2 = pose.computeResidual(cMo2);
973  } catch (...) {
974  // Should not occur
975  }
976 
977  if (d1 < d2)
978  m_cMo = cMo1;
979  else
980  m_cMo = cMo2;
981 
983 
984  delete[] P;
985 
986  if (I) {
987  init(*I);
988  } else {
989  vpImageConvert::convert(*I_color, m_I);
990  init(m_I);
991  }
992 }
993 
1018 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
1019 {
1020  initFromPoints(&I, NULL, initFile);
1021 }
1022 
1047 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1048 {
1049  initFromPoints(NULL, &I_color, initFile);
1050 }
1051 
1052 void vpMbTracker::initFromPoints(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
1053  const std::vector<vpImagePoint> &points2D_list,
1054  const std::vector<vpPoint> &points3D_list)
1055 {
1056  if (points2D_list.size() != points3D_list.size())
1057  vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
1058  "different to the number of 3D points.");
1059 
1060  size_t size = points3D_list.size();
1061  std::vector<vpPoint> P;
1062  vpPose pose;
1063 
1064  for (size_t i = 0; i < size; i++) {
1065  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1066  double x = 0, y = 0;
1067  vpPixelMeterConversion::convertPoint(m_cam, points2D_list[i], x, y);
1068  P[i].set_x(x);
1069  P[i].set_y(y);
1070  pose.addPoint(P[i]);
1071  }
1072 
1073  vpHomogeneousMatrix cMo1, cMo2;
1074  double d1, d2;
1075  d1 = d2 = std::numeric_limits<double>::max();
1076  try {
1077  pose.computePose(vpPose::LAGRANGE, cMo1);
1078  d1 = pose.computeResidual(cMo1);
1079  } catch (...) {
1080  // Lagrange non-planar cannot work with less than 6 points
1081  }
1082  try {
1083  pose.computePose(vpPose::DEMENTHON, cMo2);
1084  d2 = pose.computeResidual(cMo2);
1085  } catch (...) {
1086  // Should not occur
1087  }
1088 
1089  if (d1 < d2)
1090  m_cMo = cMo1;
1091  else
1092  m_cMo = cMo2;
1093 
1095 
1096  if (I) {
1097  init(*I);
1098  } else {
1099  vpImageConvert::convert(*I_color, m_I);
1100  init(m_I);
1101  }
1102 }
1103 
1112 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
1113  const std::vector<vpPoint> &points3D_list)
1114 {
1115  initFromPoints(&I, NULL, points2D_list, points3D_list);
1116 }
1117 
1126 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::vector<vpImagePoint> &points2D_list,
1127  const std::vector<vpPoint> &points3D_list)
1128 {
1129  initFromPoints(NULL, &I_color, points2D_list, points3D_list);
1130 }
1131 
1132 void vpMbTracker::initFromPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
1133  const std::string &initFile)
1134 {
1135  std::stringstream ss;
1136  std::fstream finit;
1137  vpPoseVector init_pos;
1138 
1139  std::string ext = ".pos";
1140  size_t pos = initFile.rfind(ext);
1141 
1142  if (pos == initFile.size() - ext.size() && pos != 0) {
1143  ss << initFile;
1144  } else {
1145  ss << initFile;
1146  ss << ".pos";
1147  }
1148 
1149  finit.open(ss.str().c_str(), std::ios::in);
1150  if (finit.fail()) {
1151  std::cout << "Cannot read " << ss.str() << std::endl;
1152  throw vpException(vpException::ioError, "cannot read init file");
1153  }
1154 
1155  for (unsigned int i = 0; i < 6; i += 1) {
1156  finit >> init_pos[i];
1157  }
1158 
1159  m_cMo.buildFrom(init_pos);
1160 
1161  if (I) {
1162  init(*I);
1163  } else {
1164  vpImageConvert::convert(*I_color, m_I);
1165  init(m_I);
1166  }
1167 }
1168 
1187 void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1188 {
1189  initFromPose(&I, NULL, initFile);
1190 }
1191 
1210 void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1211 {
1212  initFromPose(NULL, &I_color, initFile);
1213 }
1214 
1222 {
1223  m_cMo = cMo;
1224  init(I);
1225 }
1226 
1234 {
1235  m_cMo = cMo;
1236  vpImageConvert::convert(I_color, m_I);
1237  init(m_I);
1238 }
1239 
1247 {
1248  vpHomogeneousMatrix _cMo(cPo);
1249  initFromPose(I, _cMo);
1250 }
1251 
1259 {
1260  vpHomogeneousMatrix _cMo(cPo);
1261  vpImageConvert::convert(I_color, m_I);
1262  initFromPose(m_I, _cMo);
1263 }
1264 
1270 void vpMbTracker::savePose(const std::string &filename) const
1271 {
1272  vpPoseVector init_pos;
1273  std::fstream finitpos;
1274  finitpos.open(filename.c_str(), std::ios::out);
1275 
1276  init_pos.buildFrom(m_cMo);
1277  finitpos << init_pos;
1278  finitpos.close();
1279 }
1280 
1281 void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
1282  bool useLod, double minPolygonAreaThreshold, double minLineLengthThreshold)
1283 {
1284  std::vector<vpPoint> corners_without_duplicates;
1285  corners_without_duplicates.push_back(corners[0]);
1286  for (unsigned int i = 0; i < corners.size() - 1; i++) {
1287  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1288  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1289  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1290  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1291  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1292  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1293  corners_without_duplicates.push_back(corners[i + 1]);
1294  }
1295  }
1296 
1297  vpMbtPolygon polygon;
1298  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
1299  polygon.setIndex((int)idFace);
1300  polygon.setName(polygonName);
1301  polygon.setLod(useLod);
1302 
1303  // //if(minPolygonAreaThreshold != -1.0) {
1304  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1305  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1306  // {
1307  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1308  // }
1309  //
1310  // //if(minLineLengthThreshold != -1.0) {
1311  // if(std::fabs(minLineLengthThreshold + 1.0) >
1312  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1313  // {
1314  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1315  // }
1316 
1317  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1318  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1319 
1320  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1321  polygon.addPoint(j, corners_without_duplicates[j]);
1322  }
1323 
1324  faces.addPolygon(&polygon);
1325 
1327  faces.getPolygon().back()->setClipping(clippingFlag);
1328 
1330  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1331 
1333  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1334 }
1335 
1336 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1337  const std::string &polygonName, bool useLod, double minPolygonAreaThreshold)
1338 {
1339  vpMbtPolygon polygon;
1340  polygon.setNbPoint(4);
1341  polygon.setName(polygonName);
1342  polygon.setLod(useLod);
1343 
1344  // //if(minPolygonAreaThreshold != -1.0) {
1345  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1346  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1347  // {
1348  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1349  // }
1350  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1351  // Non sense to set minLineLengthThreshold for circle
1352  // but used to be coherent when applying LOD settings for all polygons
1354 
1355  {
1356  // Create the 4 points of the circle bounding box
1357  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
1358 
1359  // Matrice de passage entre world et circle frame
1360  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
1361  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
1362  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
1363  vpRotationMatrix wRc;
1364  vpColVector x(3), y(3), z(3);
1365  // X axis is P2-P1
1366  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
1367  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
1368  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
1369  // Y axis is the normal of the plane
1370  y[0] = plane.getA() / norm_Y;
1371  y[1] = plane.getB() / norm_Y;
1372  y[2] = plane.getC() / norm_Y;
1373  // Z axis = X ^ Y
1374  z = vpColVector::crossProd(x, y);
1375  for (unsigned int i = 0; i < 3; i++) {
1376  wRc[i][0] = x[i];
1377  wRc[i][1] = y[i];
1378  wRc[i][2] = z[i];
1379  }
1380 
1381  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
1382  vpHomogeneousMatrix wMc(wtc, wRc);
1383 
1384  vpColVector c_p(4); // A point in the circle frame that is on the bbox
1385  c_p[0] = radius;
1386  c_p[1] = 0;
1387  c_p[2] = radius;
1388  c_p[3] = 1;
1389 
1390  // Matrix to rotate a point by 90 deg around Y in the circle frame
1391  for (unsigned int i = 0; i < 4; i++) {
1392  vpColVector w_p(4); // A point in the word frame
1394  w_p = wMc * cMc_90 * c_p;
1395 
1396  vpPoint w_P;
1397  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
1398 
1399  polygon.addPoint(i, w_P);
1400  }
1401  }
1402 
1403  polygon.setIndex(idFace);
1404  faces.addPolygon(&polygon);
1405 
1407  faces.getPolygon().back()->setClipping(clippingFlag);
1408 
1410  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1411 
1413  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1414 }
1415 
1416 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
1417  bool useLod, double minLineLengthThreshold)
1418 {
1419  // A polygon as a single line that corresponds to the revolution axis of the
1420  // cylinder
1421  vpMbtPolygon polygon;
1422  polygon.setNbPoint(2);
1423 
1424  polygon.addPoint(0, p1);
1425  polygon.addPoint(1, p2);
1426 
1427  polygon.setIndex(idFace);
1428  polygon.setName(polygonName);
1429  polygon.setLod(useLod);
1430 
1431  // //if(minLineLengthThreshold != -1.0) {
1432  // if(std::fabs(minLineLengthThreshold + 1.0) >
1433  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1434  // {
1435  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1436  // }
1437  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1438  // Non sense to set minPolygonAreaThreshold for cylinder
1439  // but used to be coherent when applying LOD settings for all polygons
1441 
1442  faces.addPolygon(&polygon);
1443 
1445  faces.getPolygon().back()->setClipping(clippingFlag);
1446 
1448  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1449 
1451  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1452 }
1453 
1454 void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
1455  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
1456 {
1457  int id = idFace;
1458  for (unsigned int i = 0; i < listFaces.size(); i++) {
1459  vpMbtPolygon polygon;
1460  polygon.setNbPoint((unsigned int)listFaces[i].size());
1461  for (unsigned int j = 0; j < listFaces[i].size(); j++)
1462  polygon.addPoint(j, listFaces[i][j]);
1463 
1464  polygon.setIndex(id);
1465  polygon.setName(polygonName);
1466  polygon.setIsPolygonOriented(false);
1467  polygon.setLod(useLod);
1468  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1470 
1471  faces.addPolygon(&polygon);
1472 
1474  faces.getPolygon().back()->setClipping(clippingFlag);
1475 
1477  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1478 
1480  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1481 
1482  id++;
1483  }
1484 }
1485 
1501 void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &odTo)
1502 {
1503  std::string::const_iterator it;
1504 
1505  if (vpIoTools::checkFilename(modelFile)) {
1506  it = modelFile.end();
1507  if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
1508  (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
1509  std::vector<std::string> vectorOfModelFilename;
1510  int startIdFace = (int)faces.size();
1511  nbPoints = 0;
1512  nbLines = 0;
1513  nbPolygonLines = 0;
1514  nbPolygonPoints = 0;
1515  nbCylinders = 0;
1516  nbCircles = 0;
1517  loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo);
1518  } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
1519  (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
1520  loadVRMLModel(modelFile);
1521  } else {
1522  throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
1523  }
1524  } else {
1525  throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
1526  }
1527 
1528  this->modelInitialised = true;
1529  this->modelFileName = modelFile;
1530 }
1531 
1550 void vpMbTracker::loadVRMLModel(const std::string &modelFile)
1551 {
1552 #ifdef VISP_HAVE_COIN3D
1553  m_sodb_init_called = true;
1554  SoDB::init(); // Call SoDB::finish() before ending the program.
1555 
1556  SoInput in;
1557  SbBool ok = in.openFile(modelFile.c_str());
1558  SoVRMLGroup *sceneGraphVRML2;
1559 
1560  if (!ok) {
1561  vpERROR_TRACE("can't open file to load model");
1562  throw vpException(vpException::fatalError, "can't open file to load model");
1563  }
1564 
1565  if (!in.isFileVRML2()) {
1566  SoSeparator *sceneGraph = SoDB::readAll(&in);
1567  if (sceneGraph == NULL) { /*return -1;*/
1568  }
1569  sceneGraph->ref();
1570 
1571  SoToVRML2Action tovrml2;
1572  tovrml2.apply(sceneGraph);
1573 
1574  sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1575  sceneGraphVRML2->ref();
1576  sceneGraph->unref();
1577  } else {
1578  sceneGraphVRML2 = SoDB::readAllVRML(&in);
1579  if (sceneGraphVRML2 == NULL) { /*return -1;*/
1580  }
1581  sceneGraphVRML2->ref();
1582  }
1583 
1584  in.closeFile();
1585 
1586  vpHomogeneousMatrix transform;
1587  int indexFace = (int)faces.size();
1588  extractGroup(sceneGraphVRML2, transform, indexFace);
1589 
1590  sceneGraphVRML2->unref();
1591 #else
1592  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1593  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
1594 #endif
1595 }
1596 
1597 void vpMbTracker::removeComment(std::ifstream &fileId)
1598 {
1599  char c;
1600 
1601  fileId.get(c);
1602  while (!fileId.fail() && (c == '#')) {
1603  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));
1604  fileId.get(c);
1605  }
1606  if (fileId.fail()) {
1607  throw(vpException(vpException::ioError, "Reached end of file"));
1608  }
1609  fileId.unget();
1610 }
1611 
1612 std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
1613 {
1614  std::map<std::string, std::string> mapOfParams;
1615 
1616  bool exit = false;
1617  while (!endLine.empty() && !exit) {
1618  exit = true;
1619 
1620  for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
1621  it != mapOfParameterNames.end(); ++it) {
1622  endLine = vpIoTools::trim(endLine);
1623  std::string param(it->first + "=");
1624 
1625  // Compare with a potential parameter
1626  if (endLine.compare(0, param.size(), param) == 0) {
1627  exit = false;
1628  endLine = endLine.substr(param.size());
1629 
1630  bool parseQuote = false;
1631  if (it->second == "string") {
1632  // Check if the string is between quotes
1633  if (endLine.size() > 2 && endLine[0] == '"') {
1634  parseQuote = true;
1635  endLine = endLine.substr(1);
1636  size_t pos = endLine.find_first_of('"');
1637 
1638  if (pos != std::string::npos) {
1639  mapOfParams[it->first] = endLine.substr(0, pos);
1640  endLine = endLine.substr(pos + 1);
1641  } else {
1642  parseQuote = false;
1643  }
1644  }
1645  }
1646 
1647  if (!parseQuote) {
1648  // Deal with space or tabulation after parameter value to substring
1649  // to the next sequence
1650  size_t pos1 = endLine.find_first_of(' ');
1651  size_t pos2 = endLine.find_first_of('\t');
1652  size_t pos = pos1 < pos2 ? pos1 : pos2;
1653 
1654  mapOfParams[it->first] = endLine.substr(0, pos);
1655  endLine = endLine.substr(pos + 1);
1656  }
1657  }
1658  }
1659  }
1660 
1661  return mapOfParams;
1662 }
1663 
1713 void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
1714  int &startIdFace, bool verbose, bool parent, const vpHomogeneousMatrix &odTo)
1715 {
1716  std::ifstream fileId;
1717  fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1718  fileId.open(modelFile.c_str(), std::ifstream::in);
1719  if (fileId.fail()) {
1720  std::cout << "cannot read CAO model file: " << modelFile << std::endl;
1721  throw vpException(vpException::ioError, "cannot read CAO model file");
1722  }
1723 
1724  if (verbose) {
1725  std::cout << "Model file : " << modelFile << std::endl;
1726  }
1727  vectorOfModelFilename.push_back(modelFile);
1728 
1729  try {
1730  char c;
1731  // Extraction of the version (remove empty line and commented ones
1732  // (comment line begin with the #)).
1733  // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
1734  removeComment(fileId);
1735 
1737  int caoVersion;
1738  fileId.get(c);
1739  if (c == 'V') {
1740  fileId >> caoVersion;
1741  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1742  } else {
1743  std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
1744  "file : use V0, V1, ...";
1745  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
1746  "header file : use V0, V1, ...");
1747  }
1748 
1749  removeComment(fileId);
1750 
1752  std::string line;
1753  const std::string prefix_load = "load";
1754 
1755  fileId.get(c);
1756  fileId.unget();
1757  bool header = false;
1758  while (c == 'l' || c == 'L') {
1759  getline(fileId, line);
1760 
1761  if (!line.compare(0, prefix_load.size(), prefix_load)) {
1762  // remove "load("
1763  std::string paramsStr = line.substr(5);
1764  // get parameters inside load()
1765  paramsStr = paramsStr.substr(0, paramsStr.find_first_of(")"));
1766  // split by comma
1767  std::vector<std::string> params = vpIoTools::splitChain(paramsStr, ",");
1768  // remove whitespaces
1769  for (size_t i = 0; i < params.size(); i++) {
1770  params[i] = vpIoTools::trim(params[i]);
1771  }
1772 
1773  if (!params.empty()) {
1774  // Get the loaded model pathname
1775  std::string headerPathRead = params[0];
1776  headerPathRead = headerPathRead.substr(1);
1777  headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of("\""));
1778 
1779  std::string headerPath = headerPathRead;
1780  if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
1781  std::string parentDirectory = vpIoTools::getParent(modelFile);
1782  headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
1783  }
1784 
1785  // Normalize path
1786  headerPath = vpIoTools::path(headerPath);
1787 
1788  // Get real path
1789  headerPath = vpIoTools::getAbsolutePathname(headerPath);
1790 
1791  vpHomogeneousMatrix oTo_local;
1793  vpThetaUVector tu;
1794  for (size_t i = 1; i < params.size(); i++) {
1795  std::string param = params[i];
1796  {
1797  const std::string prefix = "t=[";
1798  if (!param.compare(0, prefix.size(), prefix)) {
1799  param = param.substr(prefix.size());
1800  param = param.substr(0, param.find_first_of("]"));
1801 
1802  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1803  if (values.size() == 3) {
1804  t[0] = atof(values[0].c_str());
1805  t[1] = atof(values[1].c_str());
1806  t[2] = atof(values[2].c_str());
1807  }
1808  }
1809  }
1810  {
1811  const std::string prefix = "tu=[";
1812  if (!param.compare(0, prefix.size(), prefix)) {
1813  param = param.substr(prefix.size());
1814  param = param.substr(0, param.find_first_of("]"));
1815 
1816  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1817  if (values.size() == 3) {
1818  for (size_t j = 0; j < values.size(); j++) {
1819  std::string value = values[j];
1820  bool radian = true;
1821  size_t unitPos = value.find("deg");
1822  if (unitPos != std::string::npos) {
1823  value = value.substr(0, unitPos);
1824  radian = false;
1825  }
1826 
1827  unitPos = value.find("rad");
1828  if (unitPos != std::string::npos) {
1829  value = value.substr(0, unitPos);
1830  }
1831  tu[static_cast<unsigned int>(j)] = !radian ? vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1832  }
1833  }
1834  }
1835  }
1836  }
1837  oTo_local.buildFrom(t, tu);
1838 
1839  bool cyclic = false;
1840  for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1841  it != vectorOfModelFilename.end() && !cyclic; ++it) {
1842  if (headerPath == *it) {
1843  cyclic = true;
1844  }
1845  }
1846 
1847  if (!cyclic) {
1848  if (vpIoTools::checkFilename(headerPath)) {
1849  header = true;
1850  loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, odTo * oTo_local);
1851  } else {
1852  throw vpException(vpException::ioError, "file cannot be open");
1853  }
1854  } else {
1855  std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
1856  << std::endl;
1857  }
1858  }
1859  }
1860 
1861  removeComment(fileId);
1862  fileId.get(c);
1863  fileId.unget();
1864  }
1865 
1867  unsigned int caoNbrPoint;
1868  fileId >> caoNbrPoint;
1869  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1870 
1871  nbPoints += caoNbrPoint;
1872  if (verbose || (parent && !header)) {
1873  std::cout << "> " << caoNbrPoint << " points" << std::endl;
1874  }
1875 
1876  if (caoNbrPoint > 100000) {
1877  throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
1878  }
1879 
1880  if (caoNbrPoint == 0 && !header) {
1881  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
1882  }
1883  vpPoint *caoPoints = new vpPoint[caoNbrPoint];
1884 
1885  int i; // image coordinate (used for matching)
1886  int j;
1887 
1888  for (unsigned int k = 0; k < caoNbrPoint; k++) {
1889  removeComment(fileId);
1890 
1891  vpColVector pt_3d(4, 1.0);
1892  fileId >> pt_3d[0];
1893  fileId >> pt_3d[1];
1894  fileId >> pt_3d[2];
1895 
1896  if (caoVersion == 2) {
1897  fileId >> i;
1898  fileId >> j;
1899  }
1900 
1901  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1902 
1903  vpColVector pt_3d_tf = odTo * pt_3d;
1904  caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1905  }
1906 
1907  removeComment(fileId);
1908 
1910  // Store in a map the potential segments to add
1911  std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1912  unsigned int caoNbrLine;
1913  fileId >> caoNbrLine;
1914  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1915 
1916  nbLines += caoNbrLine;
1917  unsigned int *caoLinePoints = NULL;
1918  if (verbose || (parent && !header)) {
1919  std::cout << "> " << caoNbrLine << " lines" << std::endl;
1920  }
1921 
1922  if (caoNbrLine > 100000) {
1923  delete[] caoPoints;
1924  throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
1925  }
1926 
1927  if (caoNbrLine > 0)
1928  caoLinePoints = new unsigned int[2 * caoNbrLine];
1929 
1930  unsigned int index1, index2;
1931  // Initialization of idFace with startIdFace for dealing with recursive
1932  // load in header
1933  int idFace = startIdFace;
1934 
1935  for (unsigned int k = 0; k < caoNbrLine; k++) {
1936  removeComment(fileId);
1937 
1938  fileId >> index1;
1939  fileId >> index2;
1940 
1942  // Get the end of the line
1943  std::string endLine = "";
1944  if (safeGetline(fileId, endLine).good()) {
1945  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1946 
1947  std::string segmentName = "";
1948  double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1949  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1950  if (mapOfParams.find("name") != mapOfParams.end()) {
1951  segmentName = mapOfParams["name"];
1952  }
1953  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1954  minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1955  }
1956  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1957  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
1958  }
1959 
1960  SegmentInfo segmentInfo;
1961  segmentInfo.name = segmentName;
1962  segmentInfo.useLod = useLod;
1963  segmentInfo.minLineLengthThresh = minLineLengthThresh;
1964 
1965  caoLinePoints[2 * k] = index1;
1966  caoLinePoints[2 * k + 1] = index2;
1967 
1968  if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1969  std::vector<vpPoint> extremities;
1970  extremities.push_back(caoPoints[index1]);
1971  extremities.push_back(caoPoints[index2]);
1972  segmentInfo.extremities = extremities;
1973 
1974  std::pair<unsigned int, unsigned int> key(index1, index2);
1975 
1976  segmentTemporaryMap[key] = segmentInfo;
1977  } else {
1978  vpTRACE(" line %d has wrong coordinates.", k);
1979  }
1980  }
1981  }
1982 
1983  removeComment(fileId);
1984 
1986  /* Load polygon from the lines extracted earlier (the first point of the
1987  * line is used)*/
1988  // Store in a vector the indexes of the segments added in the face segment
1989  // case
1990  std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1991  unsigned int caoNbrPolygonLine;
1992  fileId >> caoNbrPolygonLine;
1993  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1994 
1995  nbPolygonLines += caoNbrPolygonLine;
1996  if (verbose || (parent && !header)) {
1997  std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
1998  }
1999 
2000  if (caoNbrPolygonLine > 100000) {
2001  delete[] caoPoints;
2002  delete[] caoLinePoints;
2003  throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
2004  }
2005 
2006  unsigned int index;
2007  for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
2008  removeComment(fileId);
2009 
2010  unsigned int nbLinePol;
2011  fileId >> nbLinePol;
2012  std::vector<vpPoint> corners;
2013  if (nbLinePol > 100000) {
2014  throw vpException(vpException::badValue, "Exceed the max number of lines.");
2015  }
2016 
2017  for (unsigned int n = 0; n < nbLinePol; n++) {
2018  fileId >> index;
2019 
2020  if (index >= caoNbrLine) {
2021  throw vpException(vpException::badValue, "Exceed the max number of lines.");
2022  }
2023  corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2024  corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2025 
2026  std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2027  faceSegmentKeyVector.push_back(key);
2028  }
2029 
2031  // Get the end of the line
2032  std::string endLine = "";
2033  if (safeGetline(fileId, endLine).good()) {
2034  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2035 
2036  std::string polygonName = "";
2037  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2038  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2039  if (mapOfParams.find("name") != mapOfParams.end()) {
2040  polygonName = mapOfParams["name"];
2041  }
2042  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2043  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2044  }
2045  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2046  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2047  }
2048 
2049  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2050  initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
2051 
2052  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold,
2055  }
2056  }
2057 
2058  // Add the segments which were not already added in the face segment case
2059  for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2060  it != segmentTemporaryMap.end(); ++it) {
2061  if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2062  faceSegmentKeyVector.end()) {
2063  addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
2064  it->second.minLineLengthThresh);
2065  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2066 
2067  addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod,
2068  minPolygonAreaThresholdGeneral, it->second.minLineLengthThresh);
2070  }
2071  }
2072 
2073  removeComment(fileId);
2074 
2076  /* Extract the polygon using the point coordinates (top of the file) */
2077  unsigned int caoNbrPolygonPoint;
2078  fileId >> caoNbrPolygonPoint;
2079  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2080 
2081  nbPolygonPoints += caoNbrPolygonPoint;
2082  if (verbose || (parent && !header)) {
2083  std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
2084  }
2085 
2086  if (caoNbrPolygonPoint > 100000) {
2087  throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
2088  }
2089 
2090  for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2091  removeComment(fileId);
2092 
2093  unsigned int nbPointPol;
2094  fileId >> nbPointPol;
2095  if (nbPointPol > 100000) {
2096  throw vpException(vpException::badValue, "Exceed the max number of points.");
2097  }
2098  std::vector<vpPoint> corners;
2099  for (unsigned int n = 0; n < nbPointPol; n++) {
2100  fileId >> index;
2101  if (index > caoNbrPoint - 1) {
2102  throw vpException(vpException::badValue, "Exceed the max number of points.");
2103  }
2104  corners.push_back(caoPoints[index]);
2105  }
2106 
2108  // Get the end of the line
2109  std::string endLine = "";
2110  if (safeGetline(fileId, endLine).good()) {
2111  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2112 
2113  std::string polygonName = "";
2114  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2115  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2116  if (mapOfParams.find("name") != mapOfParams.end()) {
2117  polygonName = mapOfParams["name"];
2118  }
2119  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2120  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2121  }
2122  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2123  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2124  }
2125 
2126  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2127  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2128 
2129  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold,
2132  }
2133  }
2134 
2136  unsigned int caoNbCylinder;
2137  try {
2138  removeComment(fileId);
2139 
2140  if (fileId.eof()) { // check if not at the end of the file (for old
2141  // style files)
2142  delete[] caoPoints;
2143  delete[] caoLinePoints;
2144  return;
2145  }
2146 
2147  /* Extract the cylinders */
2148  fileId >> caoNbCylinder;
2149  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2150 
2151  nbCylinders += caoNbCylinder;
2152  if (verbose || (parent && !header)) {
2153  std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
2154  }
2155 
2156  if (caoNbCylinder > 100000) {
2157  throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
2158  }
2159 
2160  for (unsigned int k = 0; k < caoNbCylinder; ++k) {
2161  removeComment(fileId);
2162 
2163  double radius;
2164  unsigned int indexP1, indexP2;
2165  fileId >> indexP1;
2166  fileId >> indexP2;
2167  fileId >> radius;
2168 
2170  // Get the end of the line
2171  std::string endLine = "";
2172  if (safeGetline(fileId, endLine).good()) {
2173  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2174 
2175  std::string polygonName = "";
2176  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2177  double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
2178  if (mapOfParams.find("name") != mapOfParams.end()) {
2179  polygonName = mapOfParams["name"];
2180  }
2181  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
2182  minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
2183  }
2184  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2185  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2186  }
2187 
2188  int idRevolutionAxis = idFace;
2189  addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2190 
2191  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod,
2192  minLineLengthThreshold);
2193 
2194  std::vector<std::vector<vpPoint> > listFaces;
2195  createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
2196  addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2197 
2198  initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2199 
2200  addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2201  initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2202 
2203  idFace += 4;
2204  }
2205  }
2206 
2207  } catch (const std::exception &e) {
2208  std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2209  std::cerr << "Exception: " << e.what() << std::endl;
2210  caoNbCylinder = 0;
2211  }
2212 
2214  unsigned int caoNbCircle;
2215  try {
2216  removeComment(fileId);
2217 
2218  if (fileId.eof()) { // check if not at the end of the file (for old
2219  // style files)
2220  delete[] caoPoints;
2221  delete[] caoLinePoints;
2222  return;
2223  }
2224 
2225  /* Extract the circles */
2226  fileId >> caoNbCircle;
2227  fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2228 
2229  nbCircles += caoNbCircle;
2230  if (verbose || (parent && !header)) {
2231  std::cout << "> " << caoNbCircle << " circles" << std::endl;
2232  }
2233 
2234  if (caoNbCircle > 100000) {
2235  throw vpException(vpException::badValue, "Exceed the max number of cicles.");
2236  }
2237 
2238  for (unsigned int k = 0; k < caoNbCircle; ++k) {
2239  removeComment(fileId);
2240 
2241  double radius;
2242  unsigned int indexP1, indexP2, indexP3;
2243  fileId >> radius;
2244  fileId >> indexP1;
2245  fileId >> indexP2;
2246  fileId >> indexP3;
2247 
2249  // Get the end of the line
2250  std::string endLine = "";
2251  if (safeGetline(fileId, endLine).good()) {
2252  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2253 
2254  std::string polygonName = "";
2255  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2256  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2257  if (mapOfParams.find("name") != mapOfParams.end()) {
2258  polygonName = mapOfParams["name"];
2259  }
2260  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2261  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2262  }
2263  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2264  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2265  }
2266 
2267  addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2268  minPolygonAreaThreshold);
2269 
2270  initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2271 
2272  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace,
2273  polygonName, useLod, minPolygonAreaThreshold);
2274  initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++,
2275  polygonName);
2276  }
2277  }
2278 
2279  } catch (const std::exception &e) {
2280  std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
2281  std::cerr << "Exception: " << e.what() << std::endl;
2282  caoNbCircle = 0;
2283  }
2284 
2285  startIdFace = idFace;
2286 
2287  delete[] caoPoints;
2288  delete[] caoLinePoints;
2289 
2290  if (header && parent) {
2291  if (verbose) {
2292  std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
2293  std::cout << "Total nb of points : " << nbPoints << std::endl;
2294  std::cout << "Total nb of lines : " << nbLines << std::endl;
2295  std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
2296  std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
2297  std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
2298  std::cout << "Total nb of circles : " << nbCircles << std::endl;
2299  } else {
2300  std::cout << "> " << nbPoints << " points" << std::endl;
2301  std::cout << "> " << nbLines << " lines" << std::endl;
2302  std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
2303  std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
2304  std::cout << "> " << nbCylinders << " cylinders" << std::endl;
2305  std::cout << "> " << nbCircles << " circles" << std::endl;
2306  }
2307  }
2308 
2309  // Go up: remove current model
2310  vectorOfModelFilename.pop_back();
2311  } catch (const std::exception &e) {
2312  std::cerr << "Cannot read line!" << std::endl;
2313  std::cerr << "Exception: " << e.what() << std::endl;
2314  throw vpException(vpException::ioError, "cannot read line");
2315  }
2316 }
2317 
2318 #ifdef VISP_HAVE_COIN3D
2326 void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
2327 {
2328  vpHomogeneousMatrix transformCur;
2329  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
2330  if (sceneGraphVRML2Trasnform) {
2331  float rx, ry, rz, rw;
2332  sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2333  vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
2334  // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
2335  // rw << std::endl;
2336 
2337  float tx, ty, tz;
2338  tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2339  ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2340  tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2341  vpTranslationVector transVec(tx, ty, tz);
2342  // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
2343  // std::endl;
2344 
2345  float sx, sy, sz;
2346  sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2347  sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2348  sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2349  // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
2350  // std::endl;
2351 
2352  for (unsigned int i = 0; i < 3; i++)
2353  rotMat[0][i] *= sx;
2354  for (unsigned int i = 0; i < 3; i++)
2355  rotMat[1][i] *= sy;
2356  for (unsigned int i = 0; i < 3; i++)
2357  rotMat[2][i] *= sz;
2358 
2359  transformCur = vpHomogeneousMatrix(transVec, rotMat);
2360  transform = transform * transformCur;
2361  }
2362 
2363  int nbShapes = sceneGraphVRML2->getNumChildren();
2364  // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
2365  // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
2366  // std::endl;
2367 
2368  SoNode *child;
2369 
2370  for (int i = 0; i < nbShapes; i++) {
2371  vpHomogeneousMatrix transform_recursive(transform);
2372  child = sceneGraphVRML2->getChild(i);
2373 
2374  if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2375  extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2376  }
2377 
2378  if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2379  extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2380  }
2381 
2382  if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2383  SoChildList *child2list = child->getChildren();
2384  std::string name = child->getName().getString();
2385 
2386  for (int j = 0; j < child2list->getLength(); j++) {
2387  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2388  SoVRMLIndexedFaceSet *face_set;
2389  face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2390  if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
2391  extractCylinders(face_set, transform, idFace, name);
2392  } else {
2393  extractFaces(face_set, transform, idFace, name);
2394  }
2395  }
2396  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2397  SoVRMLIndexedLineSet *line_set;
2398  line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2399  extractLines(line_set, idFace, name);
2400  }
2401  }
2402  }
2403  }
2404 }
2405 
2415 void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2416  const std::string &polygonName)
2417 {
2418  std::vector<vpPoint> corners;
2419 
2420  // SoMFInt32 indexList = _face_set->coordIndex;
2421  // int indexListSize = indexList.getNum();
2422  int indexListSize = face_set->coordIndex.getNum();
2423 
2424  vpColVector pointTransformed(4);
2425  vpPoint pt;
2426  SoVRMLCoordinate *coord;
2427 
2428  for (int i = 0; i < indexListSize; i++) {
2429  if (face_set->coordIndex[i] == -1) {
2430  if (corners.size() > 1) {
2431  addPolygon(corners, idFace, polygonName);
2432  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2433 
2434  addProjectionErrorPolygon(corners, idFace++, polygonName);
2436  corners.resize(0);
2437  }
2438  } else {
2439  coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2440  int index = face_set->coordIndex[i];
2441  pointTransformed[0] = coord->point[index].getValue()[0];
2442  pointTransformed[1] = coord->point[index].getValue()[1];
2443  pointTransformed[2] = coord->point[index].getValue()[2];
2444  pointTransformed[3] = 1.0;
2445 
2446  pointTransformed = transform * pointTransformed;
2447 
2448  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2449  corners.push_back(pt);
2450  }
2451  }
2452 }
2453 
2468 void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2469  const std::string &polygonName)
2470 {
2471  std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
2472  // first circle and to the
2473  // second one.
2474  SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2475 
2476  unsigned int indexListSize = (unsigned int)coords->point.getNum();
2477 
2478  if (indexListSize % 2 == 1) {
2479  std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
2480  throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
2481  }
2482  corners_c1.resize(indexListSize / 2);
2483  corners_c2.resize(indexListSize / 2);
2484  vpColVector pointTransformed(4);
2485  vpPoint pt;
2486 
2487  // extract all points and fill the two sets.
2488 
2489  for (int i = 0; i < coords->point.getNum(); ++i) {
2490  pointTransformed[0] = coords->point[i].getValue()[0];
2491  pointTransformed[1] = coords->point[i].getValue()[1];
2492  pointTransformed[2] = coords->point[i].getValue()[2];
2493  pointTransformed[3] = 1.0;
2494 
2495  pointTransformed = transform * pointTransformed;
2496 
2497  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2498 
2499  if (i < (int)corners_c1.size()) {
2500  corners_c1[(unsigned int)i] = pt;
2501  } else {
2502  corners_c2[(unsigned int)i - corners_c1.size()] = pt;
2503  }
2504  }
2505 
2506  vpPoint p1 = getGravityCenter(corners_c1);
2507  vpPoint p2 = getGravityCenter(corners_c2);
2508 
2509  vpColVector dist(3);
2510  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
2511  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
2512  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
2513  double radius_c1 = sqrt(dist.sumSquare());
2514  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
2515  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
2516  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
2517  double radius_c2 = sqrt(dist.sumSquare());
2518 
2519  if (std::fabs(radius_c1 - radius_c2) >
2520  (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
2521  std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
2522  throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
2523  }
2524 
2525  // addPolygon(p1, p2, idFace, polygonName);
2526  // initCylinder(p1, p2, radius_c1, idFace++);
2527 
2528  int idRevolutionAxis = idFace;
2529  addPolygon(p1, p2, idFace, polygonName);
2530 
2531  addProjectionErrorPolygon(p1, p2, idFace++, polygonName);
2532 
2533  std::vector<std::vector<vpPoint> > listFaces;
2534  createCylinderBBox(p1, p2, radius_c1, listFaces);
2535  addPolygon(listFaces, idFace, polygonName);
2536 
2537  initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2538 
2539  addProjectionErrorPolygon(listFaces, idFace, polygonName);
2540  initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2541 
2542  idFace += 4;
2543 }
2544 
2553 void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
2554 {
2555  std::vector<vpPoint> corners;
2556  corners.resize(0);
2557 
2558  int indexListSize = line_set->coordIndex.getNum();
2559 
2560  SbVec3f point(0, 0, 0);
2561  vpPoint pt;
2562  SoVRMLCoordinate *coord;
2563 
2564  for (int i = 0; i < indexListSize; i++) {
2565  if (line_set->coordIndex[i] == -1) {
2566  if (corners.size() > 1) {
2567  addPolygon(corners, idFace, polygonName);
2568  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2569 
2570  addProjectionErrorPolygon(corners, idFace++, polygonName);
2572  corners.resize(0);
2573  }
2574  } else {
2575  coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2576  int index = line_set->coordIndex[i];
2577  point[0] = coord->point[index].getValue()[0];
2578  point[1] = coord->point[index].getValue()[1];
2579  point[2] = coord->point[index].getValue()[2];
2580 
2581  pt.setWorldCoordinates(point[0], point[1], point[2]);
2582  corners.push_back(pt);
2583  }
2584  }
2585 }
2586 
2587 #endif // VISP_HAVE_COIN3D
2588 
2598 vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
2599 {
2600  if (pts.empty()) {
2601  std::cout << "Cannot extract center of gravity of empty set." << std::endl;
2602  throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
2603  }
2604  double oX = 0;
2605  double oY = 0;
2606  double oZ = 0;
2607  vpPoint G;
2608 
2609  for (unsigned int i = 0; i < pts.size(); ++i) {
2610  oX += pts[i].get_oX();
2611  oY += pts[i].get_oY();
2612  oZ += pts[i].get_oZ();
2613  }
2614 
2615  G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
2616  return G;
2617 }
2618 
2631 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2632 vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
2633 {
2634  // Temporary variable to permit to order polygons by distance
2635  std::vector<vpPolygon> polygonsTmp;
2636  std::vector<std::vector<vpPoint> > roisPtTmp;
2637 
2638  // Pair containing the list of vpPolygon and the list of face corners
2639  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2640 
2641  for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
2642  // A face has at least 3 points
2643  if (faces.getPolygon()[i]->nbpt > 2) {
2644  if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
2645  std::vector<vpImagePoint> roiPts;
2646 
2647  if (clipPolygon) {
2648  faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo);
2649  } else {
2650  roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo);
2651  }
2652 
2653  if (roiPts.size() <= 2) {
2654  continue;
2655  }
2656 
2657  polygonsTmp.push_back(vpPolygon(roiPts));
2658 
2659  std::vector<vpPoint> polyPts;
2660  if (clipPolygon) {
2661  faces.getPolygon()[i]->getPolygonClipped(polyPts);
2662  } else {
2663  for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
2664  polyPts.push_back(faces.getPolygon()[i]->p[j]);
2665  }
2666  }
2667  roisPtTmp.push_back(polyPts);
2668  }
2669  }
2670  }
2671 
2672  if (orderPolygons) {
2673  // Order polygons by distance (near to far)
2674  std::vector<PolygonFaceInfo> listOfPolygonFaces;
2675  for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
2676  double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2677  for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2678  x_centroid += roisPtTmp[i][j].get_X();
2679  y_centroid += roisPtTmp[i][j].get_Y();
2680  z_centroid += roisPtTmp[i][j].get_Z();
2681  }
2682 
2683  x_centroid /= roisPtTmp[i].size();
2684  y_centroid /= roisPtTmp[i].size();
2685  z_centroid /= roisPtTmp[i].size();
2686 
2687  double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2688  listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2689  }
2690 
2691  // Sort the list of polygon faces
2692  std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2693 
2694  polygonsTmp.resize(listOfPolygonFaces.size());
2695  roisPtTmp.resize(listOfPolygonFaces.size());
2696 
2697  size_t cpt = 0;
2698  for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2699  ++it, cpt++) {
2700  polygonsTmp[cpt] = it->polygon;
2701  roisPtTmp[cpt] = it->faceCorners;
2702  }
2703 
2704  pairOfPolygonFaces.first = polygonsTmp;
2705  pairOfPolygonFaces.second = roisPtTmp;
2706  } else {
2707  pairOfPolygonFaces.first = polygonsTmp;
2708  pairOfPolygonFaces.second = roisPtTmp;
2709  }
2710 
2711  return pairOfPolygonFaces;
2712 }
2713 
2723 {
2724  useOgre = v;
2725  if (useOgre) {
2726 #ifndef VISP_HAVE_OGRE
2727  useOgre = false;
2728  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2729  "will be used. setOgreVisibilityTest() set to false."
2730  << std::endl;
2731 #endif
2732  }
2733 }
2734 
2740 void vpMbTracker::setFarClippingDistance(const double &dist)
2741 {
2743  vpTRACE("Far clipping value cannot be inferior than near clipping value. "
2744  "Far clipping won't be considered.");
2745  else if (dist < 0)
2746  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
2747  "won't be considered.");
2748  else {
2750  distFarClip = dist;
2751  for (unsigned int i = 0; i < faces.size(); i++) {
2752  faces[i]->setFarClippingDistance(distFarClip);
2753  }
2754 #ifdef VISP_HAVE_OGRE
2756 #endif
2757  }
2758 }
2759 
2770 void vpMbTracker::setLod(bool useLod, const std::string &name)
2771 {
2772  for (unsigned int i = 0; i < faces.size(); i++) {
2773  if (name.empty() || faces[i]->name == name) {
2774  faces[i]->setLod(useLod);
2775  }
2776  }
2777 }
2778 
2788 void vpMbTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
2789 {
2790  for (unsigned int i = 0; i < faces.size(); i++) {
2791  if (name.empty() || faces[i]->name == name) {
2792  faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2793  }
2794  }
2795 }
2796 
2805 void vpMbTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
2806 {
2807  for (unsigned int i = 0; i < faces.size(); i++) {
2808  if (name.empty() || faces[i]->name == name) {
2809  faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2810  }
2811  }
2812 }
2813 
2820 {
2822  vpTRACE("Near clipping value cannot be superior than far clipping value. "
2823  "Near clipping won't be considered.");
2824  else if (dist < 0)
2825  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
2826  "won't be considered.");
2827  else {
2829  distNearClip = dist;
2830  for (unsigned int i = 0; i < faces.size(); i++) {
2831  faces[i]->setNearClippingDistance(distNearClip);
2832  }
2833 #ifdef VISP_HAVE_OGRE
2835 #endif
2836  }
2837 }
2838 
2846 void vpMbTracker::setClipping(const unsigned int &flags)
2847 {
2848  clippingFlag = flags;
2849  for (unsigned int i = 0; i < faces.size(); i++)
2851 }
2852 
2853 void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true,
2854  const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
2855  const vpMatrix &LVJ_true, const vpColVector &error)
2856 {
2857  if (computeCovariance) {
2858  vpMatrix D;
2859  D.diag(w_true);
2860 
2861  // Note that here the covariance is computed on cMoPrev for time
2862  // computation efficiency
2863  if (isoJoIdentity_) {
2864  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
2865  } else {
2866  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
2867  }
2868  }
2869 }
2870 
2884 void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
2885 {
2886  if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
2887  throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
2888  }
2889 
2890  JTR.resize(6, false);
2891 
2892  SimdComputeJtR(interaction.data, interaction.getRows(), error.data, JTR.data);
2893 }
2894 
2896  const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
2897  double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
2898  const vpColVector *const m_w_prev)
2899 {
2901  if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
2902  mu *= 10.0;
2903 
2904  if (mu > 1.0)
2905  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
2906 
2907  m_cMo = cMoPrev;
2908  error = m_error_prev;
2909  if (w != NULL && m_w_prev != NULL) {
2910  *w = *m_w_prev;
2911  }
2912  reStartFromLastIncrement = true;
2913  }
2914  }
2915 }
2916 
2917 void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL,
2918  vpColVector &R, const vpColVector &error, vpColVector &error_prev,
2919  vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w,
2920  vpColVector *const m_w_prev)
2921 {
2922  if (isoJoIdentity_) {
2923  LTL = L.AtA();
2924  computeJTR(L, R, LTR);
2925 
2926  switch (m_optimizationMethod) {
2928  vpMatrix LMA(LTL.getRows(), LTL.getCols());
2929  LMA.eye();
2930  vpMatrix LTLmuI = LTL + (LMA * mu);
2931  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2932 
2933  if (iter != 0)
2934  mu /= 10.0;
2935 
2936  error_prev = error;
2937  if (w != NULL && m_w_prev != NULL)
2938  *m_w_prev = *w;
2939  break;
2940  }
2941 
2943  default:
2944  v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2945  break;
2946  }
2947  } else {
2949  cVo.buildFrom(m_cMo);
2950  vpMatrix LVJ = (L * (cVo * oJo));
2951  vpMatrix LVJTLVJ = (LVJ).AtA();
2952  vpColVector LVJTR;
2953  computeJTR(LVJ, R, LVJTR);
2954 
2955  switch (m_optimizationMethod) {
2957  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
2958  LMA.eye();
2959  vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2960  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2961  v = cVo * v;
2962 
2963  if (iter != 0)
2964  mu /= 10.0;
2965 
2966  error_prev = error;
2967  if (w != NULL && m_w_prev != NULL)
2968  *m_w_prev = *w;
2969  break;
2970  }
2972  default:
2973  v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2974  v = cVo * v;
2975  break;
2976  }
2977  }
2978 }
2979 
2981 {
2982  if (error.getRows() > 0)
2983  robust.MEstimator(vpRobust::TUKEY, error, w);
2984 }
2985 
2998 {
2999  vpColVector v(6);
3000  for (unsigned int i = 0; i < 6; i++)
3001  v[i] = oJo[i][i];
3002  return v;
3003 }
3004 
3021 {
3022  if (v.getRows() == 6) {
3023  isoJoIdentity = true;
3024  for (unsigned int i = 0; i < 6; i++) {
3025  // if(v[i] != 0){
3026  if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3027  oJo[i][i] = 1.0;
3028  } else {
3029  oJo[i][i] = 0.0;
3030  isoJoIdentity = false;
3031  }
3032  }
3033  }
3034 }
3035 
3036 void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
3037  std::vector<std::vector<vpPoint> > &listFaces)
3038 {
3039  listFaces.clear();
3040 
3041  // std::vector<vpPoint> revolutionAxis;
3042  // revolutionAxis.push_back(p1);
3043  // revolutionAxis.push_back(p2);
3044  // listFaces.push_back(revolutionAxis);
3045 
3046  vpColVector axis(3);
3047  axis[0] = p1.get_oX() - p2.get_oX();
3048  axis[1] = p1.get_oY() - p2.get_oY();
3049  axis[2] = p1.get_oZ() - p2.get_oZ();
3050 
3051  vpColVector randomVec(3);
3052  randomVec = 0;
3053 
3054  vpColVector axisOrtho(3);
3055 
3056  randomVec[0] = 1.0;
3057  axisOrtho = vpColVector::crossProd(axis, randomVec);
3058 
3059  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3060  randomVec = 0;
3061  randomVec[1] = 1.0;
3062  axisOrtho = vpColVector::crossProd(axis, randomVec);
3063  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3064  randomVec = 0;
3065  randomVec[2] = 1.0;
3066  axisOrtho = vpColVector::crossProd(axis, randomVec);
3067  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon())
3068  throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
3069  }
3070  }
3071 
3072  axisOrtho.normalize();
3073 
3074  vpColVector axisOrthoBis(3);
3075  axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
3076  axisOrthoBis.normalize();
3077 
3078  // First circle
3079  vpColVector p1Vec(3);
3080  p1Vec[0] = p1.get_oX();
3081  p1Vec[1] = p1.get_oY();
3082  p1Vec[2] = p1.get_oZ();
3083  vpColVector fc1 = p1Vec + axisOrtho * radius;
3084  vpColVector fc2 = p1Vec + axisOrthoBis * radius;
3085  vpColVector fc3 = p1Vec - axisOrtho * radius;
3086  vpColVector fc4 = p1Vec - axisOrthoBis * radius;
3087 
3088  vpColVector p2Vec(3);
3089  p2Vec[0] = p2.get_oX();
3090  p2Vec[1] = p2.get_oY();
3091  p2Vec[2] = p2.get_oZ();
3092  vpColVector sc1 = p2Vec + axisOrtho * radius;
3093  vpColVector sc2 = p2Vec + axisOrthoBis * radius;
3094  vpColVector sc3 = p2Vec - axisOrtho * radius;
3095  vpColVector sc4 = p2Vec - axisOrthoBis * radius;
3096 
3097  std::vector<vpPoint> pointsFace;
3098  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3099  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3100  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3101  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3102  listFaces.push_back(pointsFace);
3103 
3104  pointsFace.clear();
3105  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3106  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3107  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3108  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3109  listFaces.push_back(pointsFace);
3110 
3111  pointsFace.clear();
3112  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3113  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3114  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3115  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3116  listFaces.push_back(pointsFace);
3117 
3118  pointsFace.clear();
3119  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3120  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3121  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3122  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3123  listFaces.push_back(pointsFace);
3124 }
3125 
3135 bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
3136 {
3137  double dx = fabs(P1.get_oX() - P2.get_oX());
3138  double dy = fabs(P1.get_oY() - P2.get_oY());
3139  double dz = fabs(P1.get_oZ() - P2.get_oZ());
3140 
3141  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3142  dz <= std::numeric_limits<double>::epsilon())
3143  return true;
3144  else
3145  return false;
3146 }
3147 
3148 void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, int idFace,
3149  const std::string &polygonName, bool useLod, double minPolygonAreaThreshold,
3150  double minLineLengthThreshold)
3151 {
3152  std::vector<vpPoint> corners_without_duplicates;
3153  corners_without_duplicates.push_back(corners[0]);
3154  for (unsigned int i = 0; i < corners.size() - 1; i++) {
3155  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3156  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3157  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3158  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3159  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3160  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3161  corners_without_duplicates.push_back(corners[i + 1]);
3162  }
3163  }
3164 
3165  vpMbtPolygon polygon;
3166  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
3167  polygon.setIndex((int)idFace);
3168  polygon.setName(polygonName);
3169  polygon.setLod(useLod);
3170 
3171  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3172  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3173 
3174  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3175  polygon.addPoint(j, corners_without_duplicates[j]);
3176  }
3177 
3179 
3181  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3182 
3184  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3185 
3187  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3188 }
3189 
3190 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3191  int idFace, const std::string &polygonName, bool useLod,
3192  double minPolygonAreaThreshold)
3193 {
3194  vpMbtPolygon polygon;
3195  polygon.setNbPoint(4);
3196  polygon.setName(polygonName);
3197  polygon.setLod(useLod);
3198 
3199  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3200  // Non sense to set minLineLengthThreshold for circle
3201  // but used to be coherent when applying LOD settings for all polygons
3203 
3204  {
3205  // Create the 4 points of the circle bounding box
3206  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
3207 
3208  // Matrice de passage entre world et circle frame
3209  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
3210  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
3211  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
3212  vpRotationMatrix wRc;
3213  vpColVector x(3), y(3), z(3);
3214  // X axis is P2-P1
3215  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
3216  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
3217  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
3218  // Y axis is the normal of the plane
3219  y[0] = plane.getA() / norm_Y;
3220  y[1] = plane.getB() / norm_Y;
3221  y[2] = plane.getC() / norm_Y;
3222  // Z axis = X ^ Y
3223  z = vpColVector::crossProd(x, y);
3224  for (unsigned int i = 0; i < 3; i++) {
3225  wRc[i][0] = x[i];
3226  wRc[i][1] = y[i];
3227  wRc[i][2] = z[i];
3228  }
3229 
3230  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
3231  vpHomogeneousMatrix wMc(wtc, wRc);
3232 
3233  vpColVector c_p(4); // A point in the circle frame that is on the bbox
3234  c_p[0] = radius;
3235  c_p[1] = 0;
3236  c_p[2] = radius;
3237  c_p[3] = 1;
3238 
3239  // Matrix to rotate a point by 90 deg around Y in the circle frame
3240  for (unsigned int i = 0; i < 4; i++) {
3241  vpColVector w_p(4); // A point in the word frame
3243  w_p = wMc * cMc_90 * c_p;
3244 
3245  vpPoint w_P;
3246  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
3247 
3248  polygon.addPoint(i, w_P);
3249  }
3250  }
3251 
3252  polygon.setIndex(idFace);
3254 
3256  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3257 
3259  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3260 
3262  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3263 }
3264 
3265 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace,
3266  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3267 {
3268  // A polygon as a single line that corresponds to the revolution axis of the
3269  // cylinder
3270  vpMbtPolygon polygon;
3271  polygon.setNbPoint(2);
3272 
3273  polygon.addPoint(0, p1);
3274  polygon.addPoint(1, p2);
3275 
3276  polygon.setIndex(idFace);
3277  polygon.setName(polygonName);
3278  polygon.setLod(useLod);
3279 
3280  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3281  // Non sense to set minPolygonAreaThreshold for cylinder
3282  // but used to be coherent when applying LOD settings for all polygons
3284 
3286 
3288  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3289 
3291  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3292 
3294  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3295 }
3296 
3297 void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
3298  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3299 {
3300  int id = idFace;
3301  for (unsigned int i = 0; i < listFaces.size(); i++) {
3302  vpMbtPolygon polygon;
3303  polygon.setNbPoint((unsigned int)listFaces[i].size());
3304  for (unsigned int j = 0; j < listFaces[i].size(); j++)
3305  polygon.addPoint(j, listFaces[i][j]);
3306 
3307  polygon.setIndex(id);
3308  polygon.setName(polygonName);
3309  polygon.setIsPolygonOriented(false);
3310  polygon.setLod(useLod);
3311  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3313 
3315 
3317  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3318 
3320  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3321 
3323  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3324 
3325  id++;
3326  }
3327 }
3328 
3329 void vpMbTracker::addProjectionErrorLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
3330 {
3331  // suppress line already in the model
3332  bool already_here = false;
3333  vpMbtDistanceLine *l;
3334 
3335  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3336  it != m_projectionErrorLines.end(); ++it) {
3337  l = *it;
3338  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
3339  already_here = true;
3340  l->addPolygon(polygon);
3342  }
3343  }
3344 
3345  if (!already_here) {
3346  l = new vpMbtDistanceLine;
3347 
3349  l->buildFrom(P1, P2, m_rand);
3350  l->addPolygon(polygon);
3353  l->useScanLine = useScanLine;
3354 
3355  l->setIndex((unsigned int)m_projectionErrorLines.size());
3356  l->setName(name);
3357 
3360 
3363 
3366 
3367  m_projectionErrorLines.push_back(l);
3368  }
3369 }
3370 
3371 void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
3372  int idFace, const std::string &name)
3373 {
3374  bool already_here = false;
3375  vpMbtDistanceCircle *ci;
3376 
3377  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3378  it != m_projectionErrorCircles.end(); ++it) {
3379  ci = *it;
3380  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
3381  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
3382  already_here =
3383  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
3384  }
3385  }
3386 
3387  if (!already_here) {
3388  ci = new vpMbtDistanceCircle;
3389 
3391  ci->buildFrom(P1, P2, P3, r);
3393  ci->setIndex((unsigned int)m_projectionErrorCircles.size());
3394  ci->setName(name);
3395  ci->index_polygon = idFace;
3397 
3398  m_projectionErrorCircles.push_back(ci);
3399  }
3400 }
3401 
3402 void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace,
3403  const std::string &name)
3404 {
3405  bool already_here = false;
3407 
3408  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3409  it != m_projectionErrorCylinders.end(); ++it) {
3410  cy = *it;
3411  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
3412  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
3413  already_here =
3414  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
3415  }
3416  }
3417 
3418  if (!already_here) {
3419  cy = new vpMbtDistanceCylinder;
3420 
3422  cy->buildFrom(P1, P2, r);
3424  cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
3425  cy->setName(name);
3426  cy->index_polygon = idFace;
3428  m_projectionErrorCylinders.push_back(cy);
3429  }
3430 }
3431 
3432 void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3433  int idFace, const std::string &name)
3434 {
3435  addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
3436 }
3437 
3438 void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
3439  const std::string &name)
3440 {
3441  addProjectionErrorCylinder(p1, p2, radius, idFace, name);
3442 }
3443 
3445 {
3446  unsigned int nbpt = polygon.getNbPoint();
3447  if (nbpt > 0) {
3448  for (unsigned int i = 0; i < nbpt - 1; i++)
3449  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3450  addProjectionErrorLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
3451  }
3452 }
3453 
3455 {
3456  unsigned int nbpt = polygon.getNbPoint();
3457  if (nbpt > 0) {
3458  for (unsigned int i = 0; i < nbpt - 1; i++)
3459  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3460  }
3461 }
3462 
3481  const vpCameraParameters &_cam)
3482 {
3483  if (!modelInitialised) {
3484  throw vpException(vpException::fatalError, "model not initialized");
3485  }
3486 
3487  unsigned int nbFeatures = 0;
3488  double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);
3489 
3490  if (nbFeatures > 0) {
3491  return vpMath::deg(totalProjectionError / (double)nbFeatures);
3492  }
3493 
3494  return 90.0;
3495 }
3496 
3498  const vpCameraParameters &_cam, unsigned int &nbFeatures)
3499 {
3500  bool update_cam = m_projectionErrorCam != _cam;
3501  if (update_cam) {
3502  m_projectionErrorCam = _cam;
3503 
3504  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3505  it != m_projectionErrorLines.end(); ++it) {
3506  vpMbtDistanceLine *l = *it;
3508  }
3509 
3510  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3511  it != m_projectionErrorCylinders.end(); ++it) {
3512  vpMbtDistanceCylinder *cy = *it;
3514  }
3515 
3516  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3517  it != m_projectionErrorCircles.end(); ++it) {
3518  vpMbtDistanceCircle *ci = *it;
3520  }
3521  }
3522 
3523 #ifdef VISP_HAVE_OGRE
3524  if (useOgre) {
3525  if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
3529  // Turn off Ogre config dialog display for the next call to this
3530  // function since settings are saved in the ogre.cfg file and used
3531  // during the next call
3533  }
3534  }
3535 #endif
3536 
3537  if (clippingFlag > 2)
3539 
3541 
3543 
3544  if (useScanLine) {
3545  if (clippingFlag <= 2)
3547 
3550  }
3551 
3553 
3554  double totalProjectionError = 0.0;
3555  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3556  it != m_projectionErrorLines.end(); ++it) {
3557  vpMbtDistanceLine *l = *it;
3558  if (l->isVisible() && l->isTracked()) {
3559  for (size_t a = 0; a < l->meline.size(); a++) {
3560  if (l->meline[a] != NULL) {
3561  double lineNormGradient;
3562  unsigned int lineNbFeatures;
3563  l->meline[a]->computeProjectionError(I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
3566  totalProjectionError += lineNormGradient;
3567  nbFeatures += lineNbFeatures;
3568  }
3569  }
3570  }
3571  }
3572 
3573  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3574  it != m_projectionErrorCylinders.end(); ++it) {
3575  vpMbtDistanceCylinder *cy = *it;
3576  if (cy->isVisible() && cy->isTracked()) {
3577  if (cy->meline1 != NULL) {
3578  double cylinderNormGradient = 0;
3579  unsigned int cylinderNbFeatures = 0;
3580  cy->meline1->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3583  totalProjectionError += cylinderNormGradient;
3584  nbFeatures += cylinderNbFeatures;
3585  }
3586 
3587  if (cy->meline2 != NULL) {
3588  double cylinderNormGradient = 0;
3589  unsigned int cylinderNbFeatures = 0;
3590  cy->meline2->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3593  totalProjectionError += cylinderNormGradient;
3594  nbFeatures += cylinderNbFeatures;
3595  }
3596  }
3597  }
3598 
3599  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3600  it != m_projectionErrorCircles.end(); ++it) {
3601  vpMbtDistanceCircle *c = *it;
3602  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
3603  double circleNormGradient = 0;
3604  unsigned int circleNbFeatures = 0;
3605  c->meEllipse->computeProjectionError(I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
3608  totalProjectionError += circleNormGradient;
3609  nbFeatures += circleNbFeatures;
3610  }
3611  }
3612 
3613  return totalProjectionError;
3614 }
3615 
3616 void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
3617 {
3618  bool changed = false;
3619 
3620  if (!useOgre) {
3622  changed);
3623  } else {
3624 #ifdef VISP_HAVE_OGRE
3626  changed);
3627 #else
3629  changed);
3630 #endif
3631  }
3632 }
3633 
3635 {
3636  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3637  it != m_projectionErrorLines.end(); ++it) {
3638  for (size_t a = 0; a < (*it)->meline.size(); a++) {
3639  if ((*it)->meline[a] != NULL) {
3640  delete (*it)->meline[a];
3641  (*it)->meline[a] = NULL;
3642  }
3643  }
3644 
3645  (*it)->meline.clear();
3646  (*it)->nbFeature.clear();
3647  (*it)->nbFeatureTotal = 0;
3648  }
3649 
3650  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3651  it != m_projectionErrorCylinders.end(); ++it) {
3652  if ((*it)->meline1 != NULL) {
3653  delete (*it)->meline1;
3654  (*it)->meline1 = NULL;
3655  }
3656  if ((*it)->meline2 != NULL) {
3657  delete (*it)->meline2;
3658  (*it)->meline2 = NULL;
3659  }
3660 
3661  (*it)->nbFeature = 0;
3662  (*it)->nbFeaturel1 = 0;
3663  (*it)->nbFeaturel2 = 0;
3664  }
3665 
3666  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3667  it != m_projectionErrorCircles.end(); ++it) {
3668  if ((*it)->meEllipse != NULL) {
3669  delete (*it)->meEllipse;
3670  (*it)->meEllipse = NULL;
3671  }
3672  (*it)->nbFeature = 0;
3673  }
3674 }
3675 
3677 {
3678  const bool doNotTrack = true;
3679 
3680  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3681  it != m_projectionErrorLines.end(); ++it) {
3682  vpMbtDistanceLine *l = *it;
3683  bool isvisible = false;
3684 
3685  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
3686  ++itindex) {
3687  int index = *itindex;
3688  if (index == -1)
3689  isvisible = true;
3690  else {
3691  if (l->hiddenface->isVisible((unsigned int)index))
3692  isvisible = true;
3693  }
3694  }
3695 
3696  // Si la ligne n'appartient a aucune face elle est tout le temps visible
3697  if (l->Lindex_polygon.empty())
3698  isvisible = true; // Not sure that this can occur
3699 
3700  if (isvisible) {
3701  l->setVisible(true);
3702  l->updateTracked();
3703  if (l->meline.empty() && l->isTracked())
3704  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3705  } else {
3706  l->setVisible(false);
3707  for (size_t a = 0; a < l->meline.size(); a++) {
3708  if (l->meline[a] != NULL)
3709  delete l->meline[a];
3710  if (a < l->nbFeature.size())
3711  l->nbFeature[a] = 0;
3712  }
3713  l->nbFeatureTotal = 0;
3714  l->meline.clear();
3715  l->nbFeature.clear();
3716  }
3717  }
3718 
3719  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3720  it != m_projectionErrorCylinders.end(); ++it) {
3721  vpMbtDistanceCylinder *cy = *it;
3722 
3723  bool isvisible = false;
3724 
3725  int index = cy->index_polygon;
3726  if (index == -1)
3727  isvisible = true;
3728  else {
3729  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
3730  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
3731  isvisible = true;
3732  }
3733 
3734  if (isvisible) {
3735  cy->setVisible(true);
3736  if (cy->meline1 == NULL || cy->meline2 == NULL) {
3737  if (cy->isTracked())
3738  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3739  }
3740  } else {
3741  cy->setVisible(false);
3742  if (cy->meline1 != NULL)
3743  delete cy->meline1;
3744  if (cy->meline2 != NULL)
3745  delete cy->meline2;
3746  cy->meline1 = NULL;
3747  cy->meline2 = NULL;
3748  cy->nbFeature = 0;
3749  cy->nbFeaturel1 = 0;
3750  cy->nbFeaturel2 = 0;
3751  }
3752  }
3753 
3754  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3755  it != m_projectionErrorCircles.end(); ++it) {
3756  vpMbtDistanceCircle *ci = *it;
3757  bool isvisible = false;
3758 
3759  int index = ci->index_polygon;
3760  if (index == -1)
3761  isvisible = true;
3762  else {
3763  if (ci->hiddenface->isVisible((unsigned int)index))
3764  isvisible = true;
3765  }
3766 
3767  if (isvisible) {
3768  ci->setVisible(true);
3769  if (ci->meEllipse == NULL) {
3770  if (ci->isTracked())
3771  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3772  }
3773  } else {
3774  ci->setVisible(false);
3775  if (ci->meEllipse != NULL)
3776  delete ci->meEllipse;
3777  ci->meEllipse = NULL;
3778  ci->nbFeature = 0;
3779  }
3780  }
3781 }
3782 
3783 void vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose)
3784 {
3786  xmlp.setVerbose(verbose);
3789 
3790  try {
3791  if (verbose) {
3792  std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl;
3793  }
3794  xmlp.parse(configFile);
3795  } catch (...) {
3796  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
3797  }
3798 
3799  vpMe meParser;
3800  xmlp.getProjectionErrorMe(meParser);
3801 
3802  setProjectionErrorMovingEdge(meParser);
3804 }
3805 
3812 {
3813  m_projectionErrorMe = me;
3814 
3815  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3816  it != m_projectionErrorLines.end(); ++it) {
3817  vpMbtDistanceLine *l = *it;
3819  }
3820 
3821  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3822  it != m_projectionErrorCylinders.end(); ++it) {
3823  vpMbtDistanceCylinder *cy = *it;
3825  }
3826 
3827  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3828  it != m_projectionErrorCircles.end(); ++it) {
3829  vpMbtDistanceCircle *ci = *it;
3831  }
3832 }
3833 
3839 void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
3840 {
3842 
3843  m_SobelX.resize(size * 2 + 1, size * 2 + 1, false, false);
3845 
3846  m_SobelY.resize(size * 2 + 1, size * 2 + 1, false, false);
3848 }
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:278
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:303
unsigned int getRows() const
Definition: vpArray2D.h:288
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 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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
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 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:247
unsigned int getHeight() const
Definition: vpImage.h:189
vpDisplay * display
Definition: vpImage.h:145
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1929
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:1020
static std::string getAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1657
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:807
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1740
static std::string trim(std::string s)
Definition: vpIoTools.cpp:2195
static bool parseBoolean(std::string input)
Definition: vpIoTools.cpp:2181
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1699
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1633
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1528
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:103
static double rad(double deg)
Definition: vpMath.h:121
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:175
static double sqr(double x)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:111
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:447
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:2232
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