Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpMbTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Generic model based tracker
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  * Eric Marchand
38  *
39  *****************************************************************************/
40 
46 #include <algorithm>
47 #include <iostream>
48 #include <limits>
49 
50 #include <visp3/core/vpColVector.h>
51 #include <visp3/core/vpDisplay.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpMatrix.h>
54 #include <visp3/core/vpPoint.h>
55 #include <visp3/vision/vpPose.h>
56 #ifdef VISP_HAVE_MODULE_GUI
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayOpenCV.h>
59 #include <visp3/gui/vpDisplayX.h>
60 #endif
61 #include <visp3/core/vpCameraParameters.h>
62 #include <visp3/core/vpColor.h>
63 #include <visp3/core/vpException.h>
64 #include <visp3/core/vpIoTools.h>
65 #include <visp3/core/vpPixelMeterConversion.h>
66 #ifdef VISP_HAVE_MODULE_IO
67 #include <visp3/io/vpImageIo.h>
68 #endif
69 #include <visp3/core/vpCPUFeatures.h>
70 #include <visp3/core/vpIoTools.h>
71 #include <visp3/core/vpMatrixException.h>
72 #include <visp3/core/vpTrackingException.h>
73 #include <visp3/mbt/vpMbTracker.h>
74 
75 #include <visp3/core/vpImageFilter.h>
76 #include <visp3/mbt/vpMbtXmlGenericParser.h>
77 
78 #ifdef VISP_HAVE_COIN3D
79 // Inventor includes
80 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
81 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
82 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
83 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
84 #include <Inventor/VRMLnodes/SoVRMLShape.h>
85 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
86 #include <Inventor/actions/SoGetMatrixAction.h>
87 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
88 #include <Inventor/actions/SoSearchAction.h>
89 #include <Inventor/actions/SoToVRML2Action.h>
90 #include <Inventor/actions/SoWriteAction.h>
91 #include <Inventor/misc/SoChildList.h>
92 #include <Inventor/nodes/SoSeparator.h>
93 #endif
94 
95 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
96 #include <emmintrin.h>
97 #define VISP_HAVE_SSE2 1
98 #endif
99 
100 #ifndef DOXYGEN_SHOULD_SKIP_THIS
101 
102 namespace
103 {
107 struct SegmentInfo {
108  SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
109 
110  std::vector<vpPoint> extremities;
111  std::string name;
112  bool useLod;
113  double minLineLengthThresh;
114 };
115 
120 struct PolygonFaceInfo {
121  PolygonFaceInfo(double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
122  : distanceToCamera(dist), polygon(poly), faceCorners(corners)
123  {
124  }
125 
126  bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }
127 
128  double distanceToCamera;
129  vpPolygon polygon;
130  std::vector<vpPoint> faceCorners;
131 };
132 }
133 #endif // DOXYGEN_SHOULD_SKIP_THIS
134 
141  : m_cam(), m_cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
142  computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
143  displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
144  angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
145  useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
146  nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
147  minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
148  m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
149  m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(),
150  m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
151  m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
152  m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
153  m_projectionErrorCam(), m_mask(NULL), m_I()
154 {
155  oJo.eye();
156  // Map used to parse additional information in CAO model files,
157  // like name of faces or LOD setting
158  mapOfParameterNames["name"] = "string";
159  mapOfParameterNames["minPolygonAreaThreshold"] = "number";
160  mapOfParameterNames["minLineLengthThreshold"] = "number";
161  mapOfParameterNames["useLod"] = "boolean";
162 
165 }
166 
168  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
169  vpMbtDistanceLine *l = *it;
170  if (l != NULL)
171  delete l;
172  l = NULL;
173  }
174 
175  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end(); ++it) {
176  vpMbtDistanceCylinder *cy = *it;
177  if (cy != NULL)
178  delete cy;
179  cy = NULL;
180  }
181 
182  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
183  vpMbtDistanceCircle *ci = *it;
184  if (ci != NULL)
185  delete ci;
186  ci = NULL;
187  }
188 }
189 
190 #ifdef VISP_HAVE_MODULE_GUI
191 void vpMbTracker::initClick(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
192  const std::string &initFile, bool displayHelp, const vpHomogeneousMatrix &T)
193 {
194  vpHomogeneousMatrix last_cMo;
195  vpPoseVector init_pos;
196  vpImagePoint ip;
198 
199  std::string ext = ".init";
200  std::string str_pose = "";
201  size_t pos = initFile.rfind(ext);
202 
203  // Load the last poses from files
204  std::fstream finitpos;
205  std::ifstream finit;
206  char s[FILENAME_MAX];
207  if (poseSavingFilename.empty()) {
208  if (pos != std::string::npos)
209  str_pose = initFile.substr(0, pos) + ".0.pos";
210  else
211  str_pose = initFile + ".0.pos";
212 
213  finitpos.open(str_pose.c_str(), std::ios::in);
214  sprintf(s, "%s", str_pose.c_str());
215  } else {
216  finitpos.open(poseSavingFilename.c_str(), std::ios::in);
217  sprintf(s, "%s", poseSavingFilename.c_str());
218  }
219  if (finitpos.fail()) {
220  std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
221  last_cMo.eye();
222  } else {
223  for (unsigned int i = 0; i < 6; i += 1) {
224  finitpos >> init_pos[i];
225  }
226 
227  finitpos.close();
228  last_cMo.buildFrom(init_pos);
229 
230  std::cout << "last_cMo : " << std::endl << last_cMo << std::endl;
231 
232  if (I) {
233  vpDisplay::display(*I);
234  display(*I, last_cMo, m_cam, vpColor::green, 1, true);
235  vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green);
236  vpDisplay::flush(*I);
237  } else {
238  vpDisplay::display(*I_color);
239  display(*I_color, last_cMo, m_cam, vpColor::green, 1, true);
240  vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green);
241  vpDisplay::flush(*I_color);
242  }
243 
244  std::cout << "No modification : left click " << std::endl;
245  std::cout << "Modify initial pose : right click " << std::endl;
246 
247  if (I) {
248  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
249 
250  vpDisplay::flush(*I );
251 
252  while (!vpDisplay::getClick(*I, ip, button))
253  ;
254  } else {
255  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
256 
257  vpDisplay::flush(*I_color);
258 
259  while (!vpDisplay::getClick(*I_color, ip, button))
260  ;
261  }
262 
263  }
264 
265  if (!finitpos.fail() && button == vpMouseButton::button1) {
266  m_cMo = last_cMo;
267  } else {
268  vpDisplay *d_help = NULL;
269 
270  if (I) {
271  vpDisplay::display(*I);
272  vpDisplay::flush(*I);
273  }
274  else {
275  vpDisplay::display(*I_color);
276  vpDisplay::flush(*I_color);
277  }
278 
279  vpPose pose;
280 
281  pose.clearPoint();
282 
283  // file parser
284  // number of points
285  // X Y Z
286  // X Y Z
287  if (pos != std::string::npos)
288  sprintf(s, "%s", initFile.c_str());
289  else
290  sprintf(s, "%s.init", initFile.c_str());
291 
292  std::cout << "Load 3D points from: " << s << std::endl;
293  finit.open(s);
294  if (finit.fail()) {
295  std::cout << "cannot read " << s << std::endl;
296  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
297  }
298 
299 #ifdef VISP_HAVE_MODULE_IO
300  // Display window creation and initialisation
301  try {
302  if (displayHelp) {
303  const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"};
304  std::string dispF;
305  bool foundHelpImg = false;
306  if (pos != std::string::npos) {
307  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
308  dispF = initFile.substr(0, pos) + imgExtVec[i];
309  foundHelpImg = vpIoTools::checkFilename(dispF);
310  }
311  } else {
312  for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
313  dispF = initFile + imgExtVec[i];
314  foundHelpImg = vpIoTools::checkFilename(dispF);
315  }
316  }
317 
318  if (foundHelpImg) {
319  std::cout << "Load image to help initialization: " << dispF << std::endl;
320 #if defined VISP_HAVE_X11
321  d_help = new vpDisplayX;
322 #elif defined VISP_HAVE_GDI
323  d_help = new vpDisplayGDI;
324 #elif defined VISP_HAVE_OPENCV
325  d_help = new vpDisplayOpenCV;
326 #endif
327 
328  vpImage<vpRGBa> Iref;
329  vpImageIo::read(Iref, dispF);
330 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
331  const int winXPos = I != NULL ? I->display->getWindowXPosition() : I_color->display->getWindowXPosition();
332  const int winYPos = I != NULL ? I->display->getWindowYPosition() : I_color->display->getWindowYPosition();
333  unsigned int width = I != NULL ? I->getWidth() : I_color->getWidth();
334  d_help->init(Iref, winXPos + (int)width + 80, winYPos,
335  "Where to initialize...");
336  vpDisplay::display(Iref);
337  vpDisplay::flush(Iref);
338 #endif
339  }
340  }
341  } catch (...) {
342  if (d_help != NULL) {
343  delete d_help;
344  d_help = NULL;
345  }
346  }
347 #else //#ifdef VISP_HAVE_MODULE_IO
348  (void)(displayHelp);
349 #endif //#ifdef VISP_HAVE_MODULE_IO
350  // skip lines starting with # as comment
351  removeComment(finit);
352 
353  unsigned int n3d;
354  finit >> n3d;
355  finit.ignore(256, '\n'); // skip the rest of the line
356  std::cout << "Number of 3D points " << n3d << std::endl;
357  if (n3d > 100000) {
358  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
359  }
360 
361  std::vector<vpPoint> P(n3d);
362  for (unsigned int i = 0; i < n3d; i++) {
363  // skip lines starting with # as comment
364  removeComment(finit);
365 
366  vpColVector pt_3d(4, 1.0);
367  finit >> pt_3d[0];
368  finit >> pt_3d[1];
369  finit >> pt_3d[2];
370  finit.ignore(256, '\n'); // skip the rest of the line
371 
372  vpColVector pt_3d_tf = T*pt_3d;
373  std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " " << pt_3d_tf[2] << std::endl;
374 
375  P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
376  }
377 
378  finit.close();
379 
380  bool isWellInit = false;
381  while (!isWellInit) {
382  std::vector<vpImagePoint> mem_ip;
383  for (unsigned int i = 0; i < n3d; i++) {
384  std::ostringstream text;
385  text << "Click on point " << i + 1;
386  if (I) {
387  vpDisplay::display(*I);
388  vpDisplay::displayText(*I, 15, 10, text.str(), vpColor::red);
389  for (unsigned int k = 0; k < mem_ip.size(); k++) {
390  vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2);
391  }
392  vpDisplay::flush(*I);
393  } else {
394  vpDisplay::display(*I_color);
395  vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red);
396  for (unsigned int k = 0; k < mem_ip.size(); k++) {
397  vpDisplay::displayCross(*I_color, mem_ip[k], 10, vpColor::green, 2);
398  }
399  vpDisplay::flush(*I_color);
400  }
401 
402  std::cout << "Click on point " << i + 1 << " ";
403  double x = 0, y = 0;
404  if (I) {
405  vpDisplay::getClick(*I, ip);
406  mem_ip.push_back(ip);
407  vpDisplay::flush(*I);
408  } else {
409  vpDisplay::getClick(*I_color, ip);
410  mem_ip.push_back(ip);
411  vpDisplay::flush(*I_color);
412  }
414  P[i].set_x(x);
415  P[i].set_y(y);
416 
417  std::cout << "with 2D coordinates: " << ip << std::endl;
418 
419  pose.addPoint(P[i]); // and added to the pose computation point list
420  }
421  if (I) {
422  vpDisplay::flush(*I);
423  vpDisplay::display(*I);
424  } else {
425  vpDisplay::flush(*I_color);
426  vpDisplay::display(*I_color);
427  }
428 
429  vpHomogeneousMatrix cMo1, cMo2;
430  double d1, d2;
431  d1 = d2 = std::numeric_limits<double>::max();
432  try {
433  pose.computePose(vpPose::LAGRANGE, cMo1);
434  d1 = pose.computeResidual(cMo1);
435  }
436  catch(...) {
437  // Lagrange non-planar cannot work with less than 6 points
438  }
439  try {
440  pose.computePose(vpPose::DEMENTHON, cMo2);
441  d2 = pose.computeResidual(cMo2);
442  }
443  catch(...) {
444  // Should not occur
445  }
446 
447  if (d1 < d2) {
448  m_cMo = cMo1;
449  } else {
450  m_cMo = cMo2;
451  }
453 
454  if (I) {
455  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
456  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
457 
458  vpDisplay::flush(*I);
459 
460  button = vpMouseButton::button1;
461  while (!vpDisplay::getClick(*I, ip, button))
462  ;
463 
464  if (button == vpMouseButton::button1) {
465  isWellInit = true;
466  } else {
467  pose.clearPoint();
468  vpDisplay::display(*I);
469  vpDisplay::flush(*I);
470  }
471  } else {
472  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
473  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
474 
475  vpDisplay::flush(*I_color);
476 
477  button = vpMouseButton::button1;
478  while (!vpDisplay::getClick(*I_color, ip, button))
479  ;
480 
481  if (button == vpMouseButton::button1) {
482  isWellInit = true;
483  } else {
484  pose.clearPoint();
485  vpDisplay::display(*I_color);
486  vpDisplay::flush(*I_color);
487  }
488  }
489  }
490  if (I)
492  else
493  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
494 
495  // save the pose into file
496  if (poseSavingFilename.empty())
497  savePose(str_pose);
498  else
500 
501  if (d_help != NULL) {
502  delete d_help;
503  d_help = NULL;
504  }
505  }
506 
507  std::cout << "cMo : " << std::endl << m_cMo << std::endl;
508 
509  if (I)
510  init(*I);
511  else {
512  vpImageConvert::convert(*I_color, m_I);
513  init(m_I);
514  }
515 }
516 
548 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp,
549  const vpHomogeneousMatrix &T)
550 {
551  initClick(&I, NULL, initFile, displayHelp, T);
552 }
553 
585 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, bool displayHelp,
586  const vpHomogeneousMatrix &T)
587 {
588  initClick(NULL, &I_color, initFile, displayHelp, T);
589 }
590 
591 void vpMbTracker::initClick(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
592  const std::vector<vpPoint> &points3D_list, const std::string &displayFile)
593 {
594  if (I) {
595  vpDisplay::display(*I);
596  vpDisplay::flush(*I);
597  } else {
598  vpDisplay::display(*I_color);
599  vpDisplay::flush(*I_color);
600  }
601 
602  vpDisplay *d_help = NULL;
603 
604  vpPose pose;
605  std::vector<vpPoint> P;
606  for (unsigned int i = 0; i < points3D_list.size(); i++)
607  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
608 
609 #ifdef VISP_HAVE_MODULE_IO
610  vpImage<vpRGBa> Iref;
611  // Display window creation and initialisation
612  if (vpIoTools::checkFilename(displayFile)) {
613  try {
614  std::cout << "Load image to help initialization: " << displayFile << std::endl;
615 #if defined VISP_HAVE_X11
616  d_help = new vpDisplayX;
617 #elif defined VISP_HAVE_GDI
618  d_help = new vpDisplayGDI;
619 #elif defined VISP_HAVE_OPENCV
620  d_help = new vpDisplayOpenCV;
621 #endif
622 
623  vpImageIo::read(Iref, displayFile);
624 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
625  if (I) {
626  d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(),
627  "Where to initialize...");
628  } else {
629  d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80, I_color->display->getWindowYPosition(),
630  "Where to initialize...");
631  }
632  vpDisplay::display(Iref);
633  vpDisplay::flush(Iref);
634 #endif
635  } catch (...) {
636  if (d_help != NULL) {
637  delete d_help;
638  d_help = NULL;
639  }
640  }
641  }
642 #else //#ifdef VISP_HAVE_MODULE_IO
643  (void)(displayFile);
644 #endif //#ifdef VISP_HAVE_MODULE_IO
645 
646  vpImagePoint ip;
647  bool isWellInit = false;
648  while (!isWellInit) {
649  for (unsigned int i = 0; i < points3D_list.size(); i++) {
650  std::cout << "Click on point " << i + 1 << std::endl;
651  double x = 0, y = 0;
652  if (I) {
653  vpDisplay::getClick(*I, ip);
655  vpDisplay::flush(*I);
656  } else {
657  vpDisplay::getClick(*I_color, ip);
658  vpDisplay::displayCross(*I_color, ip, 5, vpColor::green);
659  vpDisplay::flush(*I_color);
660  }
662  P[i].set_x(x);
663  P[i].set_y(y);
664 
665  std::cout << "Click on point " << ip << std::endl;
666 
667  if (I) {
668  vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point
669  } else {
670  vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point
671  }
672  pose.addPoint(P[i]); // and added to the pose computation point list
673  }
674  if (I) {
675  vpDisplay::flush(*I);
676  } else {
677  vpDisplay::flush(*I_color);
678  }
679 
680  vpHomogeneousMatrix cMo1, cMo2;
681  double d1, d2;
682  d1 = d2 = std::numeric_limits<double>::max();
683  try {
684  pose.computePose(vpPose::LAGRANGE, cMo1);
685  d1 = pose.computeResidual(cMo1);
686  }
687  catch(...) {
688  // Lagrange non-planar cannot work with less than 6 points
689  }
690  try {
691  pose.computePose(vpPose::DEMENTHON, cMo2);
692  d2 = pose.computeResidual(cMo2);
693  }
694  catch(...) {
695  // Should not occur
696  }
697 
698  if (d1 < d2) {
699  m_cMo = cMo1;
700  } else {
701  m_cMo = cMo2;
702  }
704 
705  if (I) {
706  display(*I, m_cMo, m_cam, vpColor::green, 1, true);
707  vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
708 
709  vpDisplay::flush(*I);
710 
712  while (!vpDisplay::getClick(*I, ip, button)) {
713  };
714 
715  if (button == vpMouseButton::button1) {
716  isWellInit = true;
717  } else {
718  pose.clearPoint();
719  vpDisplay::display(*I);
720  vpDisplay::flush(*I);
721  }
722  } else {
723  display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
724  vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
725 
726  vpDisplay::flush(*I_color);
727 
729  while (!vpDisplay::getClick(*I_color, ip, button)) {
730  };
731 
732  if (button == vpMouseButton::button1) {
733  isWellInit = true;
734  } else {
735  pose.clearPoint();
736  vpDisplay::display(*I_color);
737  vpDisplay::flush(*I_color);
738  }
739  }
740  }
741 
742  if (I) {
744  } else {
745  vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
746  }
747 
748  if (d_help != NULL) {
749  delete d_help;
750  d_help = NULL;
751  }
752 
753  if (I)
754  init(*I);
755  else {
756  vpImageConvert::convert(*I_color, m_I);
757  init(m_I);
758  }
759 }
760 
772 void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
773  const std::string &displayFile)
774 {
775  initClick(&I, NULL, points3D_list, displayFile);
776 }
777 
789 void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::vector<vpPoint> &points3D_list,
790  const std::string &displayFile)
791 {
792  initClick(NULL, &I_color, points3D_list, displayFile);
793 }
794 #endif //#ifdef VISP_HAVE_MODULE_GUI
795 
796 void vpMbTracker::initFromPoints(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
797  const std::string &initFile)
798 {
799  char s[FILENAME_MAX];
800  std::fstream finit;
801 
802  std::string ext = ".init";
803  size_t pos = initFile.rfind(ext);
804 
805  if (pos == initFile.size() - ext.size() && pos != 0)
806  sprintf(s, "%s", initFile.c_str());
807  else
808  sprintf(s, "%s.init", initFile.c_str());
809 
810  std::cout << "Load 2D/3D points from: " << s << std::endl;
811  finit.open(s, std::ios::in);
812  if (finit.fail()) {
813  std::cout << "cannot read " << s << std::endl;
814  throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
815  }
816 
817  //********
818  // Read 3D points coordinates
819  //********
820  char c;
821  // skip lines starting with # as comment
822  finit.get(c);
823  while (!finit.fail() && (c == '#')) {
824  finit.ignore(256, '\n');
825  finit.get(c);
826  }
827  finit.unget();
828 
829  unsigned int n3d;
830  finit >> n3d;
831  finit.ignore(256, '\n'); // skip the rest of the line
832  std::cout << "Number of 3D points " << n3d << std::endl;
833  if (n3d > 100000) {
834  throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
835  }
836 
837  vpPoint *P = new vpPoint[n3d];
838  for (unsigned int i = 0; i < n3d; i++) {
839  // skip lines starting with # as comment
840  finit.get(c);
841  while (!finit.fail() && (c == '#')) {
842  finit.ignore(256, '\n');
843  finit.get(c);
844  }
845  finit.unget();
846  double X, Y, Z;
847  finit >> X;
848  finit >> Y;
849  finit >> Z;
850  finit.ignore(256, '\n'); // skip the rest of the line
851 
852  std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
853  P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
854  }
855 
856  //********
857  // Read 3D points coordinates
858  //********
859  // skip lines starting with # as comment
860  finit.get(c);
861  while (!finit.fail() && (c == '#')) {
862  finit.ignore(256, '\n');
863  finit.get(c);
864  }
865  finit.unget();
866 
867  unsigned int n2d;
868  finit >> n2d;
869  finit.ignore(256, '\n'); // skip the rest of the line
870  std::cout << "Number of 2D points " << n2d << std::endl;
871  if (n2d > 100000) {
872  delete[] P;
873  throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed", s);
874  }
875 
876  if (n3d != n2d) {
877  delete[] P;
879  "In %s file, number of 2D points %d and number of 3D "
880  "points %d are not equal",
881  s, n2d, n3d);
882  }
883 
884  vpPose pose;
885  for (unsigned int i = 0; i < n2d; i++) {
886  // skip lines starting with # as comment
887  finit.get(c);
888  while (!finit.fail() && (c == '#')) {
889  finit.ignore(256, '\n');
890  finit.get(c);
891  }
892  finit.unget();
893  double u, v, x = 0, y = 0;
894  finit >> v;
895  finit >> u;
896  finit.ignore(256, '\n'); // skip the rest of the line
897 
898  vpImagePoint ip(v, u);
899  std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
901  P[i].set_x(x);
902  P[i].set_y(y);
903  pose.addPoint(P[i]);
904  }
905 
906  finit.close();
907 
908  vpHomogeneousMatrix cMo1, cMo2;
909  double d1, d2;
910  d1 = d2 = std::numeric_limits<double>::max();
911  try {
912  pose.computePose(vpPose::LAGRANGE, cMo1);
913  d1 = pose.computeResidual(cMo1);
914  }
915  catch(...) {
916  // Lagrange non-planar cannot work with less than 6 points
917  }
918  try {
919  pose.computePose(vpPose::DEMENTHON, cMo2);
920  d2 = pose.computeResidual(cMo2);
921  }
922  catch(...) {
923  // Should not occur
924  }
925 
926  if (d1 < d2)
927  m_cMo = cMo1;
928  else
929  m_cMo = cMo2;
930 
932 
933  delete[] P;
934 
935  if (I) {
936  init(*I);
937  } else {
938  vpImageConvert::convert(*I_color, m_I);
939  init(m_I);
940  }
941 }
942 
967 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
968 {
969  initFromPoints(&I, NULL, initFile);
970 }
971 
996 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::string &initFile)
997 {
998  initFromPoints(NULL, &I_color, initFile);
999 }
1000 
1001 void vpMbTracker::initFromPoints(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
1002  const std::vector<vpImagePoint> &points2D_list, const std::vector<vpPoint> &points3D_list)
1003 {
1004  if (points2D_list.size() != points3D_list.size())
1005  vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
1006  "different to the number of 3D points.");
1007 
1008  size_t size = points3D_list.size();
1009  std::vector<vpPoint> P;
1010  vpPose pose;
1011 
1012  for (size_t i = 0; i < size; i++) {
1013  P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1014  double x = 0, y = 0;
1015  vpPixelMeterConversion::convertPoint(m_cam, points2D_list[i], x, y);
1016  P[i].set_x(x);
1017  P[i].set_y(y);
1018  pose.addPoint(P[i]);
1019  }
1020 
1021  vpHomogeneousMatrix cMo1, cMo2;
1022  double d1, d2;
1023  d1 = d2 = std::numeric_limits<double>::max();
1024  try {
1025  pose.computePose(vpPose::LAGRANGE, cMo1);
1026  d1 = pose.computeResidual(cMo1);
1027  }
1028  catch(...) {
1029  // Lagrange non-planar cannot work with less than 6 points
1030  }
1031  try {
1032  pose.computePose(vpPose::DEMENTHON, cMo2);
1033  d2 = pose.computeResidual(cMo2);
1034  }
1035  catch(...) {
1036  // Should not occur
1037  }
1038 
1039  if (d1 < d2)
1040  m_cMo = cMo1;
1041  else
1042  m_cMo = cMo2;
1043 
1045 
1046  if (I) {
1047  init(*I);
1048  } else {
1049  vpImageConvert::convert(*I_color, m_I);
1050  init(m_I);
1051  }
1052 }
1053 
1062 void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
1063  const std::vector<vpPoint> &points3D_list)
1064 {
1065  initFromPoints(&I, NULL, points2D_list, points3D_list);
1066 }
1067 
1076 void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::vector<vpImagePoint> &points2D_list,
1077  const std::vector<vpPoint> &points3D_list)
1078 {
1079  initFromPoints(NULL, &I_color, points2D_list, points3D_list);
1080 }
1081 
1082 void vpMbTracker::initFromPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
1083  const std::string &initFile)
1084 {
1085  char s[FILENAME_MAX];
1086  std::fstream finit;
1087  vpPoseVector init_pos;
1088 
1089  std::string ext = ".pos";
1090  size_t pos = initFile.rfind(ext);
1091 
1092  if (pos == initFile.size() - ext.size() && pos != 0)
1093  sprintf(s, "%s", initFile.c_str());
1094  else
1095  sprintf(s, "%s.pos", initFile.c_str());
1096 
1097  finit.open(s, std::ios::in);
1098  if (finit.fail()) {
1099  std::cout << "cannot read " << s << std::endl;
1100  throw vpException(vpException::ioError, "cannot read init file");
1101  }
1102 
1103  for (unsigned int i = 0; i < 6; i += 1) {
1104  finit >> init_pos[i];
1105  }
1106 
1107  m_cMo.buildFrom(init_pos);
1108 
1109  if (I) {
1110  init(*I);
1111  } else {
1112  vpImageConvert::convert(*I_color, m_I);
1113  init(m_I);
1114  }
1115 }
1116 
1135 void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1136 {
1137  initFromPose(&I, NULL, initFile);
1138 }
1139 
1158 void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1159 {
1160  initFromPose(NULL, &I_color, initFile);
1161 }
1162 
1170 {
1171  m_cMo = cMo;
1172  init(I);
1173 }
1174 
1182 {
1183  m_cMo = cMo;
1184  vpImageConvert::convert(I_color, m_I);
1185  init(m_I);
1186 }
1187 
1195 {
1196  vpHomogeneousMatrix _cMo(cPo);
1197  initFromPose(I, _cMo);
1198 }
1199 
1207 {
1208  vpHomogeneousMatrix _cMo(cPo);
1209  vpImageConvert::convert(I_color, m_I);
1210  initFromPose(m_I, _cMo);
1211 }
1212 
1218 void vpMbTracker::savePose(const std::string &filename) const
1219 {
1220  vpPoseVector init_pos;
1221  std::fstream finitpos;
1222  char s[FILENAME_MAX];
1223 
1224  sprintf(s, "%s", filename.c_str());
1225  finitpos.open(s, std::ios::out);
1226 
1227  init_pos.buildFrom(m_cMo);
1228  finitpos << init_pos;
1229  finitpos.close();
1230 }
1231 
1232 void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
1233  bool useLod, double minPolygonAreaThreshold,
1234  double minLineLengthThreshold)
1235 {
1236  std::vector<vpPoint> corners_without_duplicates;
1237  corners_without_duplicates.push_back(corners[0]);
1238  for (unsigned int i = 0; i < corners.size() - 1; i++) {
1239  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1240  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1241  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1242  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1243  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1244  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1245  corners_without_duplicates.push_back(corners[i + 1]);
1246  }
1247  }
1248 
1249  vpMbtPolygon polygon;
1250  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
1251  polygon.setIndex((int)idFace);
1252  polygon.setName(polygonName);
1253  polygon.setLod(useLod);
1254 
1255  // //if(minPolygonAreaThreshold != -1.0) {
1256  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1257  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1258  // {
1259  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1260  // }
1261  //
1262  // //if(minLineLengthThreshold != -1.0) {
1263  // if(std::fabs(minLineLengthThreshold + 1.0) >
1264  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1265  // {
1266  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1267  // }
1268 
1269  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1270  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1271 
1272  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1273  polygon.addPoint(j, corners_without_duplicates[j]);
1274  }
1275 
1276  faces.addPolygon(&polygon);
1277 
1279  faces.getPolygon().back()->setClipping(clippingFlag);
1280 
1281  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1282  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1283 
1284  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1285  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1286 }
1287 
1288 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1289  int idFace, const std::string &polygonName, bool useLod,
1290  double minPolygonAreaThreshold)
1291 {
1292  vpMbtPolygon polygon;
1293  polygon.setNbPoint(4);
1294  polygon.setName(polygonName);
1295  polygon.setLod(useLod);
1296 
1297  // //if(minPolygonAreaThreshold != -1.0) {
1298  // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1299  // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1300  // {
1301  // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1302  // }
1303  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1304  // Non sense to set minLineLengthThreshold for circle
1305  // but used to be coherent when applying LOD settings for all polygons
1307 
1308  {
1309  // Create the 4 points of the circle bounding box
1310  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
1311 
1312  // Matrice de passage entre world et circle frame
1313  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
1314  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
1315  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
1316  vpRotationMatrix wRc;
1317  vpColVector x(3), y(3), z(3);
1318  // X axis is P2-P1
1319  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
1320  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
1321  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
1322  // Y axis is the normal of the plane
1323  y[0] = plane.getA() / norm_Y;
1324  y[1] = plane.getB() / norm_Y;
1325  y[2] = plane.getC() / norm_Y;
1326  // Z axis = X ^ Y
1327  z = vpColVector::crossProd(x, y);
1328  for (unsigned int i = 0; i < 3; i++) {
1329  wRc[i][0] = x[i];
1330  wRc[i][1] = y[i];
1331  wRc[i][2] = z[i];
1332  }
1333 
1334  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
1335  vpHomogeneousMatrix wMc(wtc, wRc);
1336 
1337  vpColVector c_p(4); // A point in the circle frame that is on the bbox
1338  c_p[0] = radius;
1339  c_p[1] = 0;
1340  c_p[2] = radius;
1341  c_p[3] = 1;
1342 
1343  // Matrix to rotate a point by 90 deg around Y in the circle frame
1344  for (unsigned int i = 0; i < 4; i++) {
1345  vpColVector w_p(4); // A point in the word frame
1347  w_p = wMc * cMc_90 * c_p;
1348 
1349  vpPoint w_P;
1350  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
1351 
1352  polygon.addPoint(i, w_P);
1353  }
1354  }
1355 
1356  polygon.setIndex(idFace);
1357  faces.addPolygon(&polygon);
1358 
1360  faces.getPolygon().back()->setClipping(clippingFlag);
1361 
1362  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1363  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1364 
1365  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1366  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1367 }
1368 
1369 void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
1370  bool useLod, double minLineLengthThreshold)
1371 {
1372  // A polygon as a single line that corresponds to the revolution axis of the
1373  // cylinder
1374  vpMbtPolygon polygon;
1375  polygon.setNbPoint(2);
1376 
1377  polygon.addPoint(0, p1);
1378  polygon.addPoint(1, p2);
1379 
1380  polygon.setIndex(idFace);
1381  polygon.setName(polygonName);
1382  polygon.setLod(useLod);
1383 
1384  // //if(minLineLengthThreshold != -1.0) {
1385  // if(std::fabs(minLineLengthThreshold + 1.0) >
1386  // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1387  // {
1388  // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1389  // }
1390  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1391  // Non sense to set minPolygonAreaThreshold for cylinder
1392  // but used to be coherent when applying LOD settings for all polygons
1394 
1395  faces.addPolygon(&polygon);
1396 
1398  faces.getPolygon().back()->setClipping(clippingFlag);
1399 
1400  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1401  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1402 
1403  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1404  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1405 }
1406 
1407 void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
1408  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
1409 {
1410  int id = idFace;
1411  for (unsigned int i = 0; i < listFaces.size(); i++) {
1412  vpMbtPolygon polygon;
1413  polygon.setNbPoint((unsigned int)listFaces[i].size());
1414  for (unsigned int j = 0; j < listFaces[i].size(); j++)
1415  polygon.addPoint(j, listFaces[i][j]);
1416 
1417  polygon.setIndex(id);
1418  polygon.setName(polygonName);
1419  polygon.setIsPolygonOriented(false);
1420  polygon.setLod(useLod);
1421  polygon.setMinLineLengthThresh(minLineLengthThreshold);
1423 
1424  faces.addPolygon(&polygon);
1425 
1427  faces.getPolygon().back()->setClipping(clippingFlag);
1428 
1429  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1430  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1431 
1432  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1433  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1434 
1435  id++;
1436  }
1437 }
1438 
1466 void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &odTo)
1467 {
1468  std::string::const_iterator it;
1469 
1470  if (vpIoTools::checkFilename(modelFile)) {
1471  it = modelFile.end();
1472  if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
1473  (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
1474  std::vector<std::string> vectorOfModelFilename;
1475  int startIdFace = (int)faces.size();
1476  nbPoints = 0;
1477  nbLines = 0;
1478  nbPolygonLines = 0;
1479  nbPolygonPoints = 0;
1480  nbCylinders = 0;
1481  nbCircles = 0;
1482  loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo);
1483  } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
1484  (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
1485  loadVRMLModel(modelFile);
1486  } else {
1487  throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
1488  }
1489  } else {
1490  throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
1491  }
1492 
1493  this->modelInitialised = true;
1494  this->modelFileName = modelFile;
1495 }
1496 
1527 void vpMbTracker::loadVRMLModel(const std::string &modelFile)
1528 {
1529 #ifdef VISP_HAVE_COIN3D
1530  SoDB::init(); // Call SoDB::finish() before ending the program.
1531 
1532  SoInput in;
1533  SbBool ok = in.openFile(modelFile.c_str());
1534  SoVRMLGroup *sceneGraphVRML2;
1535 
1536  if (!ok) {
1537  vpERROR_TRACE("can't open file to load model");
1538  throw vpException(vpException::fatalError, "can't open file to load model");
1539  }
1540 
1541  if (!in.isFileVRML2()) {
1542  SoSeparator *sceneGraph = SoDB::readAll(&in);
1543  if (sceneGraph == NULL) { /*return -1;*/
1544  }
1545  sceneGraph->ref();
1546 
1547  SoToVRML2Action tovrml2;
1548  tovrml2.apply(sceneGraph);
1549 
1550  sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1551  sceneGraphVRML2->ref();
1552  sceneGraph->unref();
1553  } else {
1554  sceneGraphVRML2 = SoDB::readAllVRML(&in);
1555  if (sceneGraphVRML2 == NULL) { /*return -1;*/
1556  }
1557  sceneGraphVRML2->ref();
1558  }
1559 
1560  in.closeFile();
1561 
1562  vpHomogeneousMatrix transform;
1563  int indexFace = (int)faces.size();
1564  extractGroup(sceneGraphVRML2, transform, indexFace);
1565 
1566  sceneGraphVRML2->unref();
1567 #else
1568  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1569  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
1570 #endif
1571 }
1572 
1573 void vpMbTracker::removeComment(std::ifstream &fileId)
1574 {
1575  char c;
1576 
1577  fileId.get(c);
1578  while (!fileId.fail() && (c == '#')) {
1579  fileId.ignore(256, '\n');
1580  fileId.get(c);
1581  }
1582  if (fileId.fail()) {
1583  throw(vpException(vpException::ioError, "Reached end of file"));
1584  }
1585  fileId.unget();
1586 }
1587 
1588 std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
1589 {
1590  std::map<std::string, std::string> mapOfParams;
1591 
1592  bool exit = false;
1593  while (!endLine.empty() && !exit) {
1594  exit = true;
1595 
1596  for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
1597  it != mapOfParameterNames.end(); ++it) {
1598  endLine = vpIoTools::trim(endLine);
1599  std::string param(it->first + "=");
1600 
1601  // Compare with a potential parameter
1602  if (endLine.compare(0, param.size(), param) == 0) {
1603  exit = false;
1604  endLine = endLine.substr(param.size());
1605 
1606  bool parseQuote = false;
1607  if (it->second == "string") {
1608  // Check if the string is between quotes
1609  if (endLine.size() > 2 && endLine[0] == '"') {
1610  parseQuote = true;
1611  endLine = endLine.substr(1);
1612  size_t pos = endLine.find_first_of('"');
1613 
1614  if (pos != std::string::npos) {
1615  mapOfParams[it->first] = endLine.substr(0, pos);
1616  endLine = endLine.substr(pos + 1);
1617  } else {
1618  parseQuote = false;
1619  }
1620  }
1621  }
1622 
1623  if (!parseQuote) {
1624  // Deal with space or tabulation after parameter value to substring
1625  // to the next sequence
1626  size_t pos1 = endLine.find_first_of(' ');
1627  size_t pos2 = endLine.find_first_of('\t');
1628  size_t pos = pos1 < pos2 ? pos1 : pos2;
1629 
1630  mapOfParams[it->first] = endLine.substr(0, pos);
1631  endLine = endLine.substr(pos + 1);
1632  }
1633  }
1634  }
1635  }
1636 
1637  return mapOfParams;
1638 }
1639 
1689 void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
1690  int &startIdFace, bool verbose, bool parent,
1691  const vpHomogeneousMatrix &odTo)
1692 {
1693  std::ifstream fileId;
1694  fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1695  fileId.open(modelFile.c_str(), std::ifstream::in);
1696  if (fileId.fail()) {
1697  std::cout << "cannot read CAO model file: " << modelFile << std::endl;
1698  throw vpException(vpException::ioError, "cannot read CAO model file");
1699  }
1700 
1701  if (verbose) {
1702  std::cout << "Model file : " << modelFile << std::endl;
1703  }
1704  vectorOfModelFilename.push_back(modelFile);
1705 
1706  try {
1707  char c;
1708  // Extraction of the version (remove empty line and commented ones
1709  // (comment line begin with the #)).
1710  // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
1711  removeComment(fileId);
1712 
1714  int caoVersion;
1715  fileId.get(c);
1716  if (c == 'V') {
1717  fileId >> caoVersion;
1718  fileId.ignore(256, '\n'); // skip the rest of the line
1719  } else {
1720  std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
1721  "file : use V0, V1, ...";
1722  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
1723  "header file : use V0, V1, ...");
1724  }
1725 
1726  removeComment(fileId);
1727 
1729  std::string line;
1730  const std::string prefix_load = "load";
1731 
1732  fileId.get(c);
1733  fileId.unget();
1734  bool header = false;
1735  while (c == 'l' || c == 'L') {
1736  getline(fileId, line);
1737 
1738  if (!line.compare(0, prefix_load.size(), prefix_load)) {
1739  //remove "load("
1740  std::string paramsStr = line.substr(5);
1741  //get parameters inside load()
1742  paramsStr = paramsStr.substr(0, paramsStr.find_first_of(")"));
1743  //split by comma
1744  std::vector<std::string> params = vpIoTools::splitChain(paramsStr, ",");
1745  //remove whitespaces
1746  for (size_t i = 0; i < params.size(); i++) {
1747  params[i] = vpIoTools::trim(params[i]);
1748  }
1749 
1750  if (!params.empty()) {
1751  // Get the loaded model pathname
1752  std::string headerPathRead = params[0];
1753  headerPathRead = headerPathRead.substr(1);
1754  headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of("\""));
1755 
1756  std::string headerPath = headerPathRead;
1757  if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
1758  std::string parentDirectory = vpIoTools::getParent(modelFile);
1759  headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
1760  }
1761 
1762  // Normalize path
1763  headerPath = vpIoTools::path(headerPath);
1764 
1765  // Get real path
1766  headerPath = vpIoTools::getAbsolutePathname(headerPath);
1767 
1768  vpHomogeneousMatrix oTo_local;
1770  vpThetaUVector tu;
1771  for (size_t i = 1; i < params.size(); i++) {
1772  std::string param = params[i];
1773  {
1774  const std::string prefix = "t=[";
1775  if (!param.compare(0, prefix.size(), prefix)) {
1776  param = param.substr(prefix.size());
1777  param = param.substr(0, param.find_first_of("]"));
1778 
1779  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1780  if (values.size() == 3) {
1781  t[0] = atof(values[0].c_str());
1782  t[1] = atof(values[1].c_str());
1783  t[2] = atof(values[2].c_str());
1784  }
1785  }
1786  }
1787  {
1788  const std::string prefix = "tu=[";
1789  if (!param.compare(0, prefix.size(), prefix)) {
1790  param = param.substr(prefix.size());
1791  param = param.substr(0, param.find_first_of("]"));
1792 
1793  std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1794  if (values.size() == 3) {
1795  for (size_t j = 0; j < values.size(); j++) {
1796  std::string value = values[j];
1797  bool radian = true;
1798  size_t unitPos = value.find("deg");
1799  if (unitPos != std::string::npos) {
1800  value = value.substr(0, unitPos);
1801  radian = false;
1802  }
1803 
1804  unitPos = value.find("rad");
1805  if (unitPos != std::string::npos) {
1806  value = value.substr(0, unitPos);
1807  }
1808  tu[static_cast<unsigned int>(j)] = !radian ? vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1809  }
1810  }
1811  }
1812  }
1813  }
1814  oTo_local.buildFrom(t, tu);
1815 
1816  bool cyclic = false;
1817  for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1818  it != vectorOfModelFilename.end() && !cyclic; ++it) {
1819  if (headerPath == *it) {
1820  cyclic = true;
1821  }
1822  }
1823 
1824  if (!cyclic) {
1825  if (vpIoTools::checkFilename(headerPath)) {
1826  header = true;
1827  loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, odTo*oTo_local);
1828  } else {
1829  throw vpException(vpException::ioError, "file cannot be open");
1830  }
1831  } else {
1832  std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
1833  << std::endl;
1834  }
1835  }
1836  }
1837 
1838  removeComment(fileId);
1839  fileId.get(c);
1840  fileId.unget();
1841  }
1842 
1844  unsigned int caoNbrPoint;
1845  fileId >> caoNbrPoint;
1846  fileId.ignore(256, '\n'); // skip the rest of the line
1847 
1848  nbPoints += caoNbrPoint;
1849  if (verbose || (parent && !header)) {
1850  std::cout << "> " << caoNbrPoint << " points" << std::endl;
1851  }
1852 
1853  if (caoNbrPoint > 100000) {
1854  throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
1855  }
1856 
1857  if (caoNbrPoint == 0 && !header) {
1858  throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
1859  }
1860  vpPoint *caoPoints = new vpPoint[caoNbrPoint];
1861 
1862  int i; // image coordinate (used for matching)
1863  int j;
1864 
1865  for (unsigned int k = 0; k < caoNbrPoint; k++) {
1866  removeComment(fileId);
1867 
1868  vpColVector pt_3d(4, 1.0);
1869  fileId >> pt_3d[0];
1870  fileId >> pt_3d[1];
1871  fileId >> pt_3d[2];
1872 
1873  if (caoVersion == 2) {
1874  fileId >> i;
1875  fileId >> j;
1876  }
1877 
1878  fileId.ignore(256, '\n'); // skip the rest of the line
1879 
1880  vpColVector pt_3d_tf = odTo*pt_3d;
1881  caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1882  }
1883 
1884  removeComment(fileId);
1885 
1887  // Store in a map the potential segments to add
1888  std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1889  unsigned int caoNbrLine;
1890  fileId >> caoNbrLine;
1891  fileId.ignore(256, '\n'); // skip the rest of the line
1892 
1893  nbLines += caoNbrLine;
1894  unsigned int *caoLinePoints = NULL;
1895  if (verbose || (parent && !header)) {
1896  std::cout << "> " << caoNbrLine << " lines" << std::endl;
1897  }
1898 
1899  if (caoNbrLine > 100000) {
1900  delete[] caoPoints;
1901  throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
1902  }
1903 
1904  if (caoNbrLine > 0)
1905  caoLinePoints = new unsigned int[2 * caoNbrLine];
1906 
1907  unsigned int index1, index2;
1908  // Initialization of idFace with startIdFace for dealing with recursive
1909  // load in header
1910  int idFace = startIdFace;
1911 
1912  for (unsigned int k = 0; k < caoNbrLine; k++) {
1913  removeComment(fileId);
1914 
1915  fileId >> index1;
1916  fileId >> index2;
1917 
1919  // Get the end of the line
1920  char buffer[256];
1921  fileId.getline(buffer, 256);
1922  std::string endLine(buffer);
1923  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1924 
1925  std::string segmentName = "";
1926  double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1927  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1928  if (mapOfParams.find("name") != mapOfParams.end()) {
1929  segmentName = mapOfParams["name"];
1930  }
1931  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1932  minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1933  }
1934  if (mapOfParams.find("useLod") != mapOfParams.end()) {
1935  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
1936  }
1937 
1938  SegmentInfo segmentInfo;
1939  segmentInfo.name = segmentName;
1940  segmentInfo.useLod = useLod;
1941  segmentInfo.minLineLengthThresh = minLineLengthThresh;
1942 
1943  caoLinePoints[2 * k] = index1;
1944  caoLinePoints[2 * k + 1] = index2;
1945 
1946  if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1947  std::vector<vpPoint> extremities;
1948  extremities.push_back(caoPoints[index1]);
1949  extremities.push_back(caoPoints[index2]);
1950  segmentInfo.extremities = extremities;
1951 
1952  std::pair<unsigned int, unsigned int> key(index1, index2);
1953 
1954  segmentTemporaryMap[key] = segmentInfo;
1955  } else {
1956  vpTRACE(" line %d has wrong coordinates.", k);
1957  }
1958  }
1959 
1960  removeComment(fileId);
1961 
1963  /* Load polygon from the lines extracted earlier (the first point of the
1964  * line is used)*/
1965  // Store in a vector the indexes of the segments added in the face segment
1966  // case
1967  std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1968  unsigned int caoNbrPolygonLine;
1969  fileId >> caoNbrPolygonLine;
1970  fileId.ignore(256, '\n'); // skip the rest of the line
1971 
1972  nbPolygonLines += caoNbrPolygonLine;
1973  if (verbose || (parent && !header)) {
1974  std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
1975  }
1976 
1977  if (caoNbrPolygonLine > 100000) {
1978  delete[] caoPoints;
1979  delete[] caoLinePoints;
1980  throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
1981  }
1982 
1983  unsigned int index;
1984  for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1985  removeComment(fileId);
1986 
1987  unsigned int nbLinePol;
1988  fileId >> nbLinePol;
1989  std::vector<vpPoint> corners;
1990  if (nbLinePol > 100000) {
1991  throw vpException(vpException::badValue, "Exceed the max number of lines.");
1992  }
1993 
1994  for (unsigned int n = 0; n < nbLinePol; n++) {
1995  fileId >> index;
1996 
1997  if (index >= caoNbrLine) {
1998  throw vpException(vpException::badValue, "Exceed the max number of lines.");
1999  }
2000  corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2001  corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2002 
2003  std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2004  faceSegmentKeyVector.push_back(key);
2005  }
2006 
2008  // Get the end of the line
2009  char buffer[256];
2010  fileId.getline(buffer, 256);
2011  std::string endLine(buffer);
2012  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2013 
2014  std::string polygonName = "";
2015  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2016  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2017  if (mapOfParams.find("name") != mapOfParams.end()) {
2018  polygonName = mapOfParams["name"];
2019  }
2020  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2021  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2022  }
2023  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2024  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2025  }
2026 
2027  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2028  initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
2029 
2030  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2032  }
2033 
2034  // Add the segments which were not already added in the face segment case
2035  for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2036  it != segmentTemporaryMap.end(); ++it) {
2037  if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2038  faceSegmentKeyVector.end()) {
2039  addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
2040  it->second.minLineLengthThresh);
2041  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2042 
2043  addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
2044  it->second.minLineLengthThresh);
2046  }
2047  }
2048 
2049  removeComment(fileId);
2050 
2052  /* Extract the polygon using the point coordinates (top of the file) */
2053  unsigned int caoNbrPolygonPoint;
2054  fileId >> caoNbrPolygonPoint;
2055  fileId.ignore(256, '\n'); // skip the rest of the line
2056 
2057  nbPolygonPoints += caoNbrPolygonPoint;
2058  if (verbose || (parent && !header)) {
2059  std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
2060  }
2061 
2062  if (caoNbrPolygonPoint > 100000) {
2063  throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
2064  }
2065 
2066  for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2067  removeComment(fileId);
2068 
2069  unsigned int nbPointPol;
2070  fileId >> nbPointPol;
2071  if (nbPointPol > 100000) {
2072  throw vpException(vpException::badValue, "Exceed the max number of points.");
2073  }
2074  std::vector<vpPoint> corners;
2075  for (unsigned int n = 0; n < nbPointPol; n++) {
2076  fileId >> index;
2077  if (index > caoNbrPoint - 1) {
2078  throw vpException(vpException::badValue, "Exceed the max number of points.");
2079  }
2080  corners.push_back(caoPoints[index]);
2081  }
2082 
2084  // Get the end of the line
2085  char buffer[256];
2086  fileId.getline(buffer, 256);
2087  std::string endLine(buffer);
2088  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2089 
2090  std::string polygonName = "";
2091  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2092  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2093  if (mapOfParams.find("name") != mapOfParams.end()) {
2094  polygonName = mapOfParams["name"];
2095  }
2096  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2097  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2098  }
2099  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2100  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2101  }
2102 
2103  addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2104  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2105 
2106  addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2108  }
2109 
2111  unsigned int caoNbCylinder;
2112  try {
2113  removeComment(fileId);
2114 
2115  if (fileId.eof()) { // check if not at the end of the file (for old
2116  // style files)
2117  delete[] caoPoints;
2118  delete[] caoLinePoints;
2119  return;
2120  }
2121 
2122  /* Extract the cylinders */
2123  fileId >> caoNbCylinder;
2124  fileId.ignore(256, '\n'); // skip the rest of the line
2125 
2126  nbCylinders += caoNbCylinder;
2127  if (verbose || (parent && !header)) {
2128  std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
2129  }
2130 
2131  if (caoNbCylinder > 100000) {
2132  throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
2133  }
2134 
2135  for (unsigned int k = 0; k < caoNbCylinder; ++k) {
2136  removeComment(fileId);
2137 
2138  double radius;
2139  unsigned int indexP1, indexP2;
2140  fileId >> indexP1;
2141  fileId >> indexP2;
2142  fileId >> radius;
2143 
2145  // Get the end of the line
2146  char buffer[256];
2147  fileId.getline(buffer, 256);
2148  std::string endLine(buffer);
2149  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2150 
2151  std::string polygonName = "";
2152  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2153  double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
2154  if (mapOfParams.find("name") != mapOfParams.end()) {
2155  polygonName = mapOfParams["name"];
2156  }
2157  if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
2158  minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
2159  }
2160  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2161  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2162  }
2163 
2164  int idRevolutionAxis = idFace;
2165  addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2166 
2167  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
2168 
2169  std::vector<std::vector<vpPoint> > listFaces;
2170  createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
2171  addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2172 
2173  initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2174 
2175  addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2176  initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2177 
2178  idFace += 4;
2179  }
2180 
2181  } catch (...) {
2182  std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2183  caoNbCylinder = 0;
2184  }
2185 
2187  unsigned int caoNbCircle;
2188  try {
2189  removeComment(fileId);
2190 
2191  if (fileId.eof()) { // check if not at the end of the file (for old
2192  // style files)
2193  delete[] caoPoints;
2194  delete[] caoLinePoints;
2195  return;
2196  }
2197 
2198  /* Extract the circles */
2199  fileId >> caoNbCircle;
2200  fileId.ignore(256, '\n'); // skip the rest of the line
2201 
2202  nbCircles += caoNbCircle;
2203  if (verbose || (parent && !header)) {
2204  std::cout << "> " << caoNbCircle << " circles" << std::endl;
2205  }
2206 
2207  if (caoNbCircle > 100000) {
2208  throw vpException(vpException::badValue, "Exceed the max number of cicles.");
2209  }
2210 
2211  for (unsigned int k = 0; k < caoNbCircle; ++k) {
2212  removeComment(fileId);
2213 
2214  double radius;
2215  unsigned int indexP1, indexP2, indexP3;
2216  fileId >> radius;
2217  fileId >> indexP1;
2218  fileId >> indexP2;
2219  fileId >> indexP3;
2220 
2222  // Get the end of the line
2223  char buffer[256];
2224  fileId.getline(buffer, 256);
2225  std::string endLine(buffer);
2226  std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2227 
2228  std::string polygonName = "";
2229  bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2230  double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2231  if (mapOfParams.find("name") != mapOfParams.end()) {
2232  polygonName = mapOfParams["name"];
2233  }
2234  if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2235  minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2236  }
2237  if (mapOfParams.find("useLod") != mapOfParams.end()) {
2238  useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2239  }
2240 
2241  addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2242  minPolygonAreaThreshold);
2243 
2244  initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2245 
2246  addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2247  minPolygonAreaThreshold);
2248  initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, polygonName);
2249  }
2250 
2251  } catch (...) {
2252  std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
2253  caoNbCircle = 0;
2254  }
2255 
2256  startIdFace = idFace;
2257 
2258  delete[] caoPoints;
2259  delete[] caoLinePoints;
2260 
2261  if (header && parent) {
2262  if (verbose) {
2263  std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
2264  std::cout << "Total nb of points : " << nbPoints << std::endl;
2265  std::cout << "Total nb of lines : " << nbLines << std::endl;
2266  std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
2267  std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
2268  std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
2269  std::cout << "Total nb of circles : " << nbCircles << std::endl;
2270  } else {
2271  std::cout << "> " << nbPoints << " points" << std::endl;
2272  std::cout << "> " << nbLines << " lines" << std::endl;
2273  std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
2274  std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
2275  std::cout << "> " << nbCylinders << " cylinders" << std::endl;
2276  std::cout << "> " << nbCircles << " circles" << std::endl;
2277  }
2278  }
2279 
2280  //Go up: remove current model
2281  vectorOfModelFilename.pop_back();
2282  } catch (...) {
2283  std::cerr << "Cannot read line!" << std::endl;
2284  throw vpException(vpException::ioError, "cannot read line");
2285  }
2286 }
2287 
2288 #ifdef VISP_HAVE_COIN3D
2289 
2296 void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
2297 {
2298  vpHomogeneousMatrix transformCur;
2299  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
2300  if (sceneGraphVRML2Trasnform) {
2301  float rx, ry, rz, rw;
2302  sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2303  vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
2304  // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
2305  // rw << std::endl;
2306 
2307  float tx, ty, tz;
2308  tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2309  ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2310  tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2311  vpTranslationVector transVec(tx, ty, tz);
2312  // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
2313  // std::endl;
2314 
2315  float sx, sy, sz;
2316  sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2317  sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2318  sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2319  // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
2320  // std::endl;
2321 
2322  for (unsigned int i = 0; i < 3; i++)
2323  rotMat[0][i] *= sx;
2324  for (unsigned int i = 0; i < 3; i++)
2325  rotMat[1][i] *= sy;
2326  for (unsigned int i = 0; i < 3; i++)
2327  rotMat[2][i] *= sz;
2328 
2329  transformCur = vpHomogeneousMatrix(transVec, rotMat);
2330  transform = transform * transformCur;
2331  }
2332 
2333  int nbShapes = sceneGraphVRML2->getNumChildren();
2334  // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
2335  // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
2336  // std::endl;
2337 
2338  SoNode *child;
2339 
2340  for (int i = 0; i < nbShapes; i++) {
2341  vpHomogeneousMatrix transform_recursive(transform);
2342  child = sceneGraphVRML2->getChild(i);
2343 
2344  if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2345  extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2346  }
2347 
2348  if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2349  extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2350  }
2351 
2352  if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2353  SoChildList *child2list = child->getChildren();
2354  std::string name = child->getName().getString();
2355 
2356  for (int j = 0; j < child2list->getLength(); j++) {
2357  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2358  SoVRMLIndexedFaceSet *face_set;
2359  face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2360  if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
2361  extractCylinders(face_set, transform, idFace, name);
2362  } else {
2363  extractFaces(face_set, transform, idFace, name);
2364  }
2365  }
2366  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2367  SoVRMLIndexedLineSet *line_set;
2368  line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2369  extractLines(line_set, idFace, name);
2370  }
2371  }
2372  }
2373  }
2374 }
2375 
2385 void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2386  const std::string &polygonName)
2387 {
2388  std::vector<vpPoint> corners;
2389 
2390  // SoMFInt32 indexList = _face_set->coordIndex;
2391  // int indexListSize = indexList.getNum();
2392  int indexListSize = face_set->coordIndex.getNum();
2393 
2394  vpColVector pointTransformed(4);
2395  vpPoint pt;
2396  SoVRMLCoordinate *coord;
2397 
2398  for (int i = 0; i < indexListSize; i++) {
2399  if (face_set->coordIndex[i] == -1) {
2400  if (corners.size() > 1) {
2401  addPolygon(corners, idFace, polygonName);
2402  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2403 
2404  addProjectionErrorPolygon(corners, idFace++, polygonName);
2406  corners.resize(0);
2407  }
2408  } else {
2409  coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2410  int index = face_set->coordIndex[i];
2411  pointTransformed[0] = coord->point[index].getValue()[0];
2412  pointTransformed[1] = coord->point[index].getValue()[1];
2413  pointTransformed[2] = coord->point[index].getValue()[2];
2414  pointTransformed[3] = 1.0;
2415 
2416  pointTransformed = transform * pointTransformed;
2417 
2418  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2419  corners.push_back(pt);
2420  }
2421  }
2422 }
2423 
2438 void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2439  const std::string &polygonName)
2440 {
2441  std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
2442  // first circle and to the
2443  // second one.
2444  SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2445 
2446  unsigned int indexListSize = (unsigned int)coords->point.getNum();
2447 
2448  if (indexListSize % 2 == 1) {
2449  std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
2450  throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
2451  }
2452  corners_c1.resize(indexListSize / 2);
2453  corners_c2.resize(indexListSize / 2);
2454  vpColVector pointTransformed(4);
2455  vpPoint pt;
2456 
2457  // extract all points and fill the two sets.
2458 
2459  for (int i = 0; i < coords->point.getNum(); ++i) {
2460  pointTransformed[0] = coords->point[i].getValue()[0];
2461  pointTransformed[1] = coords->point[i].getValue()[1];
2462  pointTransformed[2] = coords->point[i].getValue()[2];
2463  pointTransformed[3] = 1.0;
2464 
2465  pointTransformed = transform * pointTransformed;
2466 
2467  pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2468 
2469  if (i < (int)corners_c1.size()) {
2470  corners_c1[(unsigned int)i] = pt;
2471  } else {
2472  corners_c2[(unsigned int)i - corners_c1.size()] = pt;
2473  }
2474  }
2475 
2476  vpPoint p1 = getGravityCenter(corners_c1);
2477  vpPoint p2 = getGravityCenter(corners_c2);
2478 
2479  vpColVector dist(3);
2480  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
2481  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
2482  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
2483  double radius_c1 = sqrt(dist.sumSquare());
2484  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
2485  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
2486  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
2487  double radius_c2 = sqrt(dist.sumSquare());
2488 
2489  if (std::fabs(radius_c1 - radius_c2) >
2490  (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
2491  std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
2492  throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
2493  }
2494 
2495  // addPolygon(p1, p2, idFace, polygonName);
2496  // initCylinder(p1, p2, radius_c1, idFace++);
2497 
2498  int idRevolutionAxis = idFace;
2499  addPolygon(p1, p2, idFace, polygonName);
2500 
2501  addProjectionErrorPolygon(p1, p2, idFace++, polygonName);
2502 
2503  std::vector<std::vector<vpPoint> > listFaces;
2504  createCylinderBBox(p1, p2, radius_c1, listFaces);
2505  addPolygon(listFaces, idFace, polygonName);
2506 
2507  initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2508 
2509  addProjectionErrorPolygon(listFaces, idFace, polygonName);
2510  initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2511 
2512  idFace += 4;
2513 }
2514 
2523 void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
2524 {
2525  std::vector<vpPoint> corners;
2526  corners.resize(0);
2527 
2528  int indexListSize = line_set->coordIndex.getNum();
2529 
2530  SbVec3f point(0, 0, 0);
2531  vpPoint pt;
2532  SoVRMLCoordinate *coord;
2533 
2534  for (int i = 0; i < indexListSize; i++) {
2535  if (line_set->coordIndex[i] == -1) {
2536  if (corners.size() > 1) {
2537  addPolygon(corners, idFace, polygonName);
2538  initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2539 
2540  addProjectionErrorPolygon(corners, idFace++, polygonName);
2542  corners.resize(0);
2543  }
2544  } else {
2545  coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2546  int index = line_set->coordIndex[i];
2547  point[0] = coord->point[index].getValue()[0];
2548  point[1] = coord->point[index].getValue()[1];
2549  point[2] = coord->point[index].getValue()[2];
2550 
2551  pt.setWorldCoordinates(point[0], point[1], point[2]);
2552  corners.push_back(pt);
2553  }
2554  }
2555 }
2556 
2557 #endif // VISP_HAVE_COIN3D
2558 
2568 vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
2569 {
2570  if (pts.empty()) {
2571  std::cout << "Cannot extract center of gravity of empty set." << std::endl;
2572  throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
2573  }
2574  double oX = 0;
2575  double oY = 0;
2576  double oZ = 0;
2577  vpPoint G;
2578 
2579  for (unsigned int i = 0; i < pts.size(); ++i) {
2580  oX += pts[i].get_oX();
2581  oY += pts[i].get_oY();
2582  oZ += pts[i].get_oZ();
2583  }
2584 
2585  G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
2586  return G;
2587 }
2588 
2601 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2602 vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
2603 {
2604  // Temporary variable to permit to order polygons by distance
2605  std::vector<vpPolygon> polygonsTmp;
2606  std::vector<std::vector<vpPoint> > roisPtTmp;
2607 
2608  // Pair containing the list of vpPolygon and the list of face corners
2609  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2610 
2611  for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
2612  // A face has at least 3 points
2613  if (faces.getPolygon()[i]->nbpt > 2) {
2614  if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
2615  std::vector<vpImagePoint> roiPts;
2616 
2617  if (clipPolygon) {
2618  faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo);
2619  } else {
2620  roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo);
2621  }
2622 
2623  if (roiPts.size() <= 2) {
2624  continue;
2625  }
2626 
2627  polygonsTmp.push_back(vpPolygon(roiPts));
2628 
2629  std::vector<vpPoint> polyPts;
2630  if (clipPolygon) {
2631  faces.getPolygon()[i]->getPolygonClipped(polyPts);
2632  } else {
2633  for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
2634  polyPts.push_back(faces.getPolygon()[i]->p[j]);
2635  }
2636  }
2637  roisPtTmp.push_back(polyPts);
2638  }
2639  }
2640  }
2641 
2642  if (orderPolygons) {
2643  // Order polygons by distance (near to far)
2644  std::vector<PolygonFaceInfo> listOfPolygonFaces;
2645  for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
2646  double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2647  for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2648  x_centroid += roisPtTmp[i][j].get_X();
2649  y_centroid += roisPtTmp[i][j].get_Y();
2650  z_centroid += roisPtTmp[i][j].get_Z();
2651  }
2652 
2653  x_centroid /= roisPtTmp[i].size();
2654  y_centroid /= roisPtTmp[i].size();
2655  z_centroid /= roisPtTmp[i].size();
2656 
2657  double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2658  listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2659  }
2660 
2661  // Sort the list of polygon faces
2662  std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2663 
2664  polygonsTmp.resize(listOfPolygonFaces.size());
2665  roisPtTmp.resize(listOfPolygonFaces.size());
2666 
2667  size_t cpt = 0;
2668  for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2669  ++it, cpt++) {
2670  polygonsTmp[cpt] = it->polygon;
2671  roisPtTmp[cpt] = it->faceCorners;
2672  }
2673 
2674  pairOfPolygonFaces.first = polygonsTmp;
2675  pairOfPolygonFaces.second = roisPtTmp;
2676  } else {
2677  pairOfPolygonFaces.first = polygonsTmp;
2678  pairOfPolygonFaces.second = roisPtTmp;
2679  }
2680 
2681  return pairOfPolygonFaces;
2682 }
2683 
2693 {
2694  useOgre = v;
2695  if (useOgre) {
2696 #ifndef VISP_HAVE_OGRE
2697  useOgre = false;
2698  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2699  "will be used. setOgreVisibilityTest() set to false."
2700  << std::endl;
2701 #endif
2702  }
2703 }
2704 
2710 void vpMbTracker::setFarClippingDistance(const double &dist)
2711 {
2712  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2713  vpTRACE("Far clipping value cannot be inferior than near clipping value. "
2714  "Far clipping won't be considered.");
2715  else if (dist < 0)
2716  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
2717  "won't be considered.");
2718  else {
2720  distFarClip = dist;
2721  for (unsigned int i = 0; i < faces.size(); i++) {
2722  faces[i]->setFarClippingDistance(distFarClip);
2723  }
2724 #ifdef VISP_HAVE_OGRE
2726 #endif
2727  }
2728 }
2729 
2740 void vpMbTracker::setLod(bool useLod, const std::string &name)
2741 {
2742  for (unsigned int i = 0; i < faces.size(); i++) {
2743  if (name.empty() || faces[i]->name == name) {
2744  faces[i]->setLod(useLod);
2745  }
2746  }
2747 }
2748 
2758 void vpMbTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
2759 {
2760  for (unsigned int i = 0; i < faces.size(); i++) {
2761  if (name.empty() || faces[i]->name == name) {
2762  faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2763  }
2764  }
2765 }
2766 
2775 void vpMbTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
2776 {
2777  for (unsigned int i = 0; i < faces.size(); i++) {
2778  if (name.empty() || faces[i]->name == name) {
2779  faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2780  }
2781  }
2782 }
2783 
2790 {
2791  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2792  vpTRACE("Near clipping value cannot be superior than far clipping value. "
2793  "Near clipping won't be considered.");
2794  else if (dist < 0)
2795  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
2796  "won't be considered.");
2797  else {
2799  distNearClip = dist;
2800  for (unsigned int i = 0; i < faces.size(); i++) {
2801  faces[i]->setNearClippingDistance(distNearClip);
2802  }
2803 #ifdef VISP_HAVE_OGRE
2805 #endif
2806  }
2807 }
2808 
2816 void vpMbTracker::setClipping(const unsigned int &flags)
2817 {
2818  clippingFlag = flags;
2819  for (unsigned int i = 0; i < faces.size(); i++)
2821 }
2822 
2823 void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true,
2824  const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
2825  const vpMatrix &LVJ_true, const vpColVector &error)
2826 {
2827  if (computeCovariance) {
2828  vpMatrix D;
2829  D.diag(w_true);
2830 
2831  // Note that here the covariance is computed on cMoPrev for time
2832  // computation efficiency
2833  if (isoJoIdentity_) {
2834  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
2835  } else {
2836  covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
2837  }
2838  }
2839 }
2840 
2854 void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
2855 {
2856  if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
2857  throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
2858  }
2859 
2860  JTR.resize(6, false);
2861 
2862  bool checkSSE2 = vpCPUFeatures::checkSSE2();
2863 #if !VISP_HAVE_SSE2
2864  checkSSE2 = false;
2865 #endif
2866 
2867  if (checkSSE2) {
2868 #if VISP_HAVE_SSE2
2869  __m128d v_JTR_0_1 = _mm_setzero_pd();
2870  __m128d v_JTR_2_3 = _mm_setzero_pd();
2871  __m128d v_JTR_4_5 = _mm_setzero_pd();
2872 
2873  for (unsigned int i = 0; i < interaction.getRows(); i++) {
2874  const __m128d v_error = _mm_set1_pd(error[i]);
2875 
2876  __m128d v_interaction = _mm_loadu_pd(&interaction[i][0]);
2877  v_JTR_0_1 = _mm_add_pd(v_JTR_0_1, _mm_mul_pd(v_interaction, v_error));
2878 
2879  v_interaction = _mm_loadu_pd(&interaction[i][2]);
2880  v_JTR_2_3 = _mm_add_pd(v_JTR_2_3, _mm_mul_pd(v_interaction, v_error));
2881 
2882  v_interaction = _mm_loadu_pd(&interaction[i][4]);
2883  v_JTR_4_5 = _mm_add_pd(v_JTR_4_5, _mm_mul_pd(v_interaction, v_error));
2884  }
2885 
2886  _mm_storeu_pd(JTR.data, v_JTR_0_1);
2887  _mm_storeu_pd(JTR.data + 2, v_JTR_2_3);
2888  _mm_storeu_pd(JTR.data + 4, v_JTR_4_5);
2889 #endif
2890  } else {
2891  const unsigned int N = interaction.getRows();
2892 
2893  for (unsigned int i = 0; i < 6; i += 1) {
2894  double ssum = 0;
2895  for (unsigned int j = 0; j < N; j += 1) {
2896  ssum += interaction[j][i] * error[j];
2897  }
2898  JTR[i] = ssum;
2899  }
2900  }
2901 }
2902 
2904  const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
2905  double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
2906  const vpColVector *const m_w_prev)
2907 {
2909  if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
2910  mu *= 10.0;
2911 
2912  if (mu > 1.0)
2913  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
2914 
2915  m_cMo = cMoPrev;
2916  error = m_error_prev;
2917  if (w != NULL && m_w_prev != NULL) {
2918  *w = *m_w_prev;
2919  }
2920  reStartFromLastIncrement = true;
2921  }
2922  }
2923 }
2924 
2925 void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L,
2926  vpMatrix &LTL, vpColVector &R, const vpColVector &error,
2927  vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v,
2928  const vpColVector *const w, vpColVector *const m_w_prev)
2929 {
2930  if (isoJoIdentity_) {
2931  LTL = L.AtA();
2932  computeJTR(L, R, LTR);
2933 
2934  switch (m_optimizationMethod) {
2936  vpMatrix LMA(LTL.getRows(), LTL.getCols());
2937  LMA.eye();
2938  vpMatrix LTLmuI = LTL + (LMA * mu);
2939  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2940 
2941  if (iter != 0)
2942  mu /= 10.0;
2943 
2944  error_prev = error;
2945  if (w != NULL && m_w_prev != NULL)
2946  *m_w_prev = *w;
2947  break;
2948  }
2949 
2951  default:
2952  v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2953  break;
2954  }
2955  } else {
2957  cVo.buildFrom(m_cMo);
2958  vpMatrix LVJ = (L * (cVo * oJo));
2959  vpMatrix LVJTLVJ = (LVJ).AtA();
2960  vpColVector LVJTR;
2961  computeJTR(LVJ, R, LVJTR);
2962 
2963  switch (m_optimizationMethod) {
2965  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
2966  LMA.eye();
2967  vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2968  v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2969  v = cVo * v;
2970 
2971  if (iter != 0)
2972  mu /= 10.0;
2973 
2974  error_prev = error;
2975  if (w != NULL && m_w_prev != NULL)
2976  *m_w_prev = *w;
2977  break;
2978  }
2980  default:
2981  v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2982  v = cVo * v;
2983  break;
2984  }
2985  }
2986 }
2987 
2989 {
2990  if (error.getRows() > 0)
2991  robust.MEstimator(vpRobust::TUKEY, error, w);
2992 }
2993 
3006 {
3007  vpColVector v(6);
3008  for (unsigned int i = 0; i < 6; i++)
3009  v[i] = oJo[i][i];
3010  return v;
3011 }
3012 
3029 {
3030  if (v.getRows() == 6) {
3031  isoJoIdentity = true;
3032  for (unsigned int i = 0; i < 6; i++) {
3033  // if(v[i] != 0){
3034  if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3035  oJo[i][i] = 1.0;
3036  } else {
3037  oJo[i][i] = 0.0;
3038  isoJoIdentity = false;
3039  }
3040  }
3041  }
3042 }
3043 
3044 void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
3045  std::vector<std::vector<vpPoint> > &listFaces)
3046 {
3047  listFaces.clear();
3048 
3049  // std::vector<vpPoint> revolutionAxis;
3050  // revolutionAxis.push_back(p1);
3051  // revolutionAxis.push_back(p2);
3052  // listFaces.push_back(revolutionAxis);
3053 
3054  vpColVector axis(3);
3055  axis[0] = p1.get_oX() - p2.get_oX();
3056  axis[1] = p1.get_oY() - p2.get_oY();
3057  axis[2] = p1.get_oZ() - p2.get_oZ();
3058 
3059  vpColVector randomVec(3);
3060  randomVec = 0;
3061 
3062  vpColVector axisOrtho(3);
3063 
3064  randomVec[0] = 1.0;
3065  axisOrtho = vpColVector::crossProd(axis, randomVec);
3066 
3067  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3068  randomVec = 0;
3069  randomVec[1] = 1.0;
3070  axisOrtho = vpColVector::crossProd(axis, randomVec);
3071  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3072  randomVec = 0;
3073  randomVec[2] = 1.0;
3074  axisOrtho = vpColVector::crossProd(axis, randomVec);
3075  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon())
3076  throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
3077  }
3078  }
3079 
3080  axisOrtho.normalize();
3081 
3082  vpColVector axisOrthoBis(3);
3083  axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
3084  axisOrthoBis.normalize();
3085 
3086  // First circle
3087  vpColVector p1Vec(3);
3088  p1Vec[0] = p1.get_oX();
3089  p1Vec[1] = p1.get_oY();
3090  p1Vec[2] = p1.get_oZ();
3091  vpColVector fc1 = p1Vec + axisOrtho * radius;
3092  vpColVector fc2 = p1Vec + axisOrthoBis * radius;
3093  vpColVector fc3 = p1Vec - axisOrtho * radius;
3094  vpColVector fc4 = p1Vec - axisOrthoBis * radius;
3095 
3096  vpColVector p2Vec(3);
3097  p2Vec[0] = p2.get_oX();
3098  p2Vec[1] = p2.get_oY();
3099  p2Vec[2] = p2.get_oZ();
3100  vpColVector sc1 = p2Vec + axisOrtho * radius;
3101  vpColVector sc2 = p2Vec + axisOrthoBis * radius;
3102  vpColVector sc3 = p2Vec - axisOrtho * radius;
3103  vpColVector sc4 = p2Vec - axisOrthoBis * radius;
3104 
3105  std::vector<vpPoint> pointsFace;
3106  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3107  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3108  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3109  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3110  listFaces.push_back(pointsFace);
3111 
3112  pointsFace.clear();
3113  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3114  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3115  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3116  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3117  listFaces.push_back(pointsFace);
3118 
3119  pointsFace.clear();
3120  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3121  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3122  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3123  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3124  listFaces.push_back(pointsFace);
3125 
3126  pointsFace.clear();
3127  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3128  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3129  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3130  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3131  listFaces.push_back(pointsFace);
3132 }
3133 
3143 bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
3144 {
3145  double dx = fabs(P1.get_oX() - P2.get_oX());
3146  double dy = fabs(P1.get_oY() - P2.get_oY());
3147  double dz = fabs(P1.get_oZ() - P2.get_oZ());
3148 
3149  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3150  dz <= std::numeric_limits<double>::epsilon())
3151  return true;
3152  else
3153  return false;
3154 }
3155 
3156 void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
3157  bool useLod, double minPolygonAreaThreshold,
3158  double minLineLengthThreshold)
3159 {
3160  std::vector<vpPoint> corners_without_duplicates;
3161  corners_without_duplicates.push_back(corners[0]);
3162  for (unsigned int i = 0; i < corners.size() - 1; i++) {
3163  if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3164  std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3165  std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3166  std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3167  std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3168  std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3169  corners_without_duplicates.push_back(corners[i + 1]);
3170  }
3171  }
3172 
3173  vpMbtPolygon polygon;
3174  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
3175  polygon.setIndex((int)idFace);
3176  polygon.setName(polygonName);
3177  polygon.setLod(useLod);
3178 
3179  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3180  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3181 
3182  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3183  polygon.addPoint(j, corners_without_duplicates[j]);
3184  }
3185 
3187 
3189  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3190 
3191  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3192  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3193 
3194  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3195  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3196 }
3197 
3198 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3199  int idFace, const std::string &polygonName, bool useLod,
3200  double minPolygonAreaThreshold)
3201 {
3202  vpMbtPolygon polygon;
3203  polygon.setNbPoint(4);
3204  polygon.setName(polygonName);
3205  polygon.setLod(useLod);
3206 
3207  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3208  // Non sense to set minLineLengthThreshold for circle
3209  // but used to be coherent when applying LOD settings for all polygons
3211 
3212  {
3213  // Create the 4 points of the circle bounding box
3214  vpPlane plane(p1, p2, p3, vpPlane::object_frame);
3215 
3216  // Matrice de passage entre world et circle frame
3217  double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
3218  vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
3219  double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
3220  vpRotationMatrix wRc;
3221  vpColVector x(3), y(3), z(3);
3222  // X axis is P2-P1
3223  x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
3224  x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
3225  x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
3226  // Y axis is the normal of the plane
3227  y[0] = plane.getA() / norm_Y;
3228  y[1] = plane.getB() / norm_Y;
3229  y[2] = plane.getC() / norm_Y;
3230  // Z axis = X ^ Y
3231  z = vpColVector::crossProd(x, y);
3232  for (unsigned int i = 0; i < 3; i++) {
3233  wRc[i][0] = x[i];
3234  wRc[i][1] = y[i];
3235  wRc[i][2] = z[i];
3236  }
3237 
3238  vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
3239  vpHomogeneousMatrix wMc(wtc, wRc);
3240 
3241  vpColVector c_p(4); // A point in the circle frame that is on the bbox
3242  c_p[0] = radius;
3243  c_p[1] = 0;
3244  c_p[2] = radius;
3245  c_p[3] = 1;
3246 
3247  // Matrix to rotate a point by 90 deg around Y in the circle frame
3248  for (unsigned int i = 0; i < 4; i++) {
3249  vpColVector w_p(4); // A point in the word frame
3251  w_p = wMc * cMc_90 * c_p;
3252 
3253  vpPoint w_P;
3254  w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
3255 
3256  polygon.addPoint(i, w_P);
3257  }
3258  }
3259 
3260  polygon.setIndex(idFace);
3262 
3264  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3265 
3266  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3267  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3268 
3269  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3270  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3271 }
3272 
3273 void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
3274  bool useLod, double minLineLengthThreshold)
3275 {
3276  // A polygon as a single line that corresponds to the revolution axis of the
3277  // cylinder
3278  vpMbtPolygon polygon;
3279  polygon.setNbPoint(2);
3280 
3281  polygon.addPoint(0, p1);
3282  polygon.addPoint(1, p2);
3283 
3284  polygon.setIndex(idFace);
3285  polygon.setName(polygonName);
3286  polygon.setLod(useLod);
3287 
3288  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3289  // Non sense to set minPolygonAreaThreshold for cylinder
3290  // but used to be coherent when applying LOD settings for all polygons
3292 
3294 
3296  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3297 
3298  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3299  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3300 
3301  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3302  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3303 }
3304 
3305 void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
3306  const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3307 {
3308  int id = idFace;
3309  for (unsigned int i = 0; i < listFaces.size(); i++) {
3310  vpMbtPolygon polygon;
3311  polygon.setNbPoint((unsigned int)listFaces[i].size());
3312  for (unsigned int j = 0; j < listFaces[i].size(); j++)
3313  polygon.addPoint(j, listFaces[i][j]);
3314 
3315  polygon.setIndex(id);
3316  polygon.setName(polygonName);
3317  polygon.setIsPolygonOriented(false);
3318  polygon.setLod(useLod);
3319  polygon.setMinLineLengthThresh(minLineLengthThreshold);
3321 
3323 
3325  m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3326 
3327  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3328  m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3329 
3330  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3331  m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3332 
3333  id++;
3334  }
3335 }
3336 
3337 void vpMbTracker::addProjectionErrorLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
3338 {
3339  // suppress line already in the model
3340  bool already_here = false;
3341  vpMbtDistanceLine *l;
3342 
3343  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3344  l = *it;
3345  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
3346  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
3347  already_here = true;
3348  l->addPolygon(polygon);
3350  }
3351  }
3352 
3353  if (!already_here) {
3354  l = new vpMbtDistanceLine;
3355 
3357  l->buildFrom(P1, P2);
3358  l->addPolygon(polygon);
3361  l->useScanLine = useScanLine;
3362 
3363  l->setIndex((unsigned int)m_projectionErrorLines.size());
3364  l->setName(name);
3365 
3368 
3369  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
3371 
3372  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
3374 
3375  m_projectionErrorLines.push_back(l);
3376  }
3377 }
3378 
3379 void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
3380  const std::string &name)
3381 {
3382  bool already_here = false;
3383  vpMbtDistanceCircle *ci;
3384 
3385  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3386  ci = *it;
3387  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
3388  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
3389  already_here =
3390  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
3391  }
3392  }
3393 
3394  if (!already_here) {
3395  ci = new vpMbtDistanceCircle;
3396 
3398  ci->buildFrom(P1, P2, P3, r);
3400  ci->setIndex((unsigned int)m_projectionErrorCircles.size());
3401  ci->setName(name);
3402  ci->index_polygon = idFace;
3404 
3405  m_projectionErrorCircles.push_back(ci);
3406  }
3407 }
3408 
3409 void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace,
3410  const std::string &name)
3411 {
3412  bool already_here = false;
3414 
3415  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3416  ++it) {
3417  cy = *it;
3418  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
3419  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
3420  already_here =
3421  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
3422  }
3423  }
3424 
3425  if (!already_here) {
3426  cy = new vpMbtDistanceCylinder;
3427 
3429  cy->buildFrom(P1, P2, r);
3431  cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
3432  cy->setName(name);
3433  cy->index_polygon = idFace;
3435  m_projectionErrorCylinders.push_back(cy);
3436  }
3437 }
3438 
3439 void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
3440  double radius, int idFace, const std::string &name)
3441 {
3442  addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
3443 }
3444 
3445 void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius,
3446  int idFace, const std::string &name)
3447 {
3448  addProjectionErrorCylinder(p1, p2, radius, idFace, name);
3449 }
3450 
3452 {
3453  unsigned int nbpt = polygon.getNbPoint();
3454  if (nbpt > 0) {
3455  for (unsigned int i = 0; i < nbpt - 1; i++)
3456  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3457  addProjectionErrorLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
3458  }
3459 }
3460 
3462 {
3463  unsigned int nbpt = polygon.getNbPoint();
3464  if (nbpt > 0) {
3465  for (unsigned int i = 0; i < nbpt - 1; i++)
3466  addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3467  }
3468 }
3469 
3488  const vpCameraParameters &_cam)
3489 {
3490  if (!modelInitialised) {
3491  throw vpException(vpException::fatalError, "model not initialized");
3492  }
3493 
3494  unsigned int nbFeatures = 0;
3495  double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);
3496 
3497  if (nbFeatures > 0) {
3498  return vpMath::deg(totalProjectionError / (double)nbFeatures);
3499  }
3500 
3501  return 90.0;
3502 }
3503 
3505  const vpCameraParameters &_cam, unsigned int &nbFeatures)
3506 {
3507  bool update_cam = m_projectionErrorCam != _cam;
3508  if (update_cam) {
3509  m_projectionErrorCam = _cam;
3510 
3511  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3512  it != m_projectionErrorLines.end(); ++it) {
3513  vpMbtDistanceLine *l = *it;
3515  }
3516 
3517  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3518  it != m_projectionErrorCylinders.end(); ++it) {
3519  vpMbtDistanceCylinder *cy = *it;
3521  }
3522 
3523  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3524  it != m_projectionErrorCircles.end(); ++it) {
3525  vpMbtDistanceCircle *ci = *it;
3527  }
3528  }
3529 
3530 #ifdef VISP_HAVE_OGRE
3531  if (useOgre) {
3532  if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
3536  // Turn off Ogre config dialog display for the next call to this
3537  // function since settings are saved in the ogre.cfg file and used
3538  // during the next call
3540  }
3541  }
3542 #endif
3543 
3544  if (clippingFlag > 2)
3546 
3548 
3550 
3551  if (useScanLine) {
3552  if (clippingFlag <= 2)
3554 
3557  }
3558 
3560 
3561  double totalProjectionError = 0.0;
3562  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end();
3563  ++it) {
3564  vpMbtDistanceLine *l = *it;
3565  if (l->isVisible() && l->isTracked()) {
3566  for (size_t a = 0; a < l->meline.size(); a++) {
3567  if (l->meline[a] != NULL) {
3568  double lineNormGradient;
3569  unsigned int lineNbFeatures;
3570  l->meline[a]->computeProjectionError(I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
3573  totalProjectionError += lineNormGradient;
3574  nbFeatures += lineNbFeatures;
3575  }
3576  }
3577  }
3578  }
3579 
3580  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3581  it != m_projectionErrorCylinders.end(); ++it) {
3582  vpMbtDistanceCylinder *cy = *it;
3583  if (cy->isVisible() && cy->isTracked()) {
3584  if (cy->meline1 != NULL) {
3585  double cylinderNormGradient = 0;
3586  unsigned int cylinderNbFeatures = 0;
3587  cy->meline1->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3590  totalProjectionError += cylinderNormGradient;
3591  nbFeatures += cylinderNbFeatures;
3592  }
3593 
3594  if (cy->meline2 != NULL) {
3595  double cylinderNormGradient = 0;
3596  unsigned int cylinderNbFeatures = 0;
3597  cy->meline2->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3600  totalProjectionError += cylinderNormGradient;
3601  nbFeatures += cylinderNbFeatures;
3602  }
3603  }
3604  }
3605 
3606  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3607  it != m_projectionErrorCircles.end(); ++it) {
3608  vpMbtDistanceCircle *c = *it;
3609  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
3610  double circleNormGradient = 0;
3611  unsigned int circleNbFeatures = 0;
3612  c->meEllipse->computeProjectionError(I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
3615  totalProjectionError += circleNormGradient;
3616  nbFeatures += circleNbFeatures;
3617  }
3618  }
3619 
3620  return totalProjectionError;
3621 }
3622 
3623 void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
3624 {
3625  bool changed = false;
3626 
3627  if (!useOgre) {
3629  } else {
3630 #ifdef VISP_HAVE_OGRE
3632 #else
3634 #endif
3635  }
3636 }
3637 
3639 {
3640  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3641  for (size_t a = 0; a < (*it)->meline.size(); a++) {
3642  if ((*it)->meline[a] != NULL) {
3643  delete (*it)->meline[a];
3644  (*it)->meline[a] = NULL;
3645  }
3646  }
3647 
3648  (*it)->meline.clear();
3649  (*it)->nbFeature.clear();
3650  (*it)->nbFeatureTotal = 0;
3651  }
3652 
3653  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3654  ++it) {
3655  if ((*it)->meline1 != NULL) {
3656  delete (*it)->meline1;
3657  (*it)->meline1 = NULL;
3658  }
3659  if ((*it)->meline2 != NULL) {
3660  delete (*it)->meline2;
3661  (*it)->meline2 = NULL;
3662  }
3663 
3664  (*it)->nbFeature = 0;
3665  (*it)->nbFeaturel1 = 0;
3666  (*it)->nbFeaturel2 = 0;
3667  }
3668 
3669  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3670  if ((*it)->meEllipse != NULL) {
3671  delete (*it)->meEllipse;
3672  (*it)->meEllipse = NULL;
3673  }
3674  (*it)->nbFeature = 0;
3675  }
3676 }
3677 
3679 {
3680  const bool doNotTrack = true;
3681 
3682  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3683  it != m_projectionErrorLines.end(); ++it) {
3684  vpMbtDistanceLine *l = *it;
3685  bool isvisible = false;
3686 
3687  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
3688  ++itindex) {
3689  int index = *itindex;
3690  if (index == -1)
3691  isvisible = true;
3692  else {
3693  if (l->hiddenface->isVisible((unsigned int)index))
3694  isvisible = true;
3695  }
3696  }
3697 
3698  // Si la ligne n'appartient a aucune face elle est tout le temps visible
3699  if (l->Lindex_polygon.empty())
3700  isvisible = true; // Not sure that this can occur
3701 
3702  if (isvisible) {
3703  l->setVisible(true);
3704  l->updateTracked();
3705  if (l->meline.empty() && l->isTracked())
3706  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3707  } else {
3708  l->setVisible(false);
3709  for (size_t a = 0; a < l->meline.size(); a++) {
3710  if (l->meline[a] != NULL)
3711  delete l->meline[a];
3712  if (a < l->nbFeature.size())
3713  l->nbFeature[a] = 0;
3714  }
3715  l->nbFeatureTotal = 0;
3716  l->meline.clear();
3717  l->nbFeature.clear();
3718  }
3719  }
3720 
3721  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3722  it != m_projectionErrorCylinders.end(); ++it) {
3723  vpMbtDistanceCylinder *cy = *it;
3724 
3725  bool isvisible = false;
3726 
3727  int index = cy->index_polygon;
3728  if (index == -1)
3729  isvisible = true;
3730  else {
3731  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
3732  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
3733  isvisible = true;
3734  }
3735 
3736  if (isvisible) {
3737  cy->setVisible(true);
3738  if (cy->meline1 == NULL || cy->meline2 == NULL) {
3739  if (cy->isTracked())
3740  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3741  }
3742  } else {
3743  cy->setVisible(false);
3744  if (cy->meline1 != NULL)
3745  delete cy->meline1;
3746  if (cy->meline2 != NULL)
3747  delete cy->meline2;
3748  cy->meline1 = NULL;
3749  cy->meline2 = NULL;
3750  cy->nbFeature = 0;
3751  cy->nbFeaturel1 = 0;
3752  cy->nbFeaturel2 = 0;
3753  }
3754  }
3755 
3756  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3757  it != m_projectionErrorCircles.end(); ++it) {
3758  vpMbtDistanceCircle *ci = *it;
3759  bool isvisible = false;
3760 
3761  int index = ci->index_polygon;
3762  if (index == -1)
3763  isvisible = true;
3764  else {
3765  if (ci->hiddenface->isVisible((unsigned int)index))
3766  isvisible = true;
3767  }
3768 
3769  if (isvisible) {
3770  ci->setVisible(true);
3771  if (ci->meEllipse == NULL) {
3772  if (ci->isTracked())
3773  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3774  }
3775  } else {
3776  ci->setVisible(false);
3777  if (ci->meEllipse != NULL)
3778  delete ci->meEllipse;
3779  ci->meEllipse = NULL;
3780  ci->nbFeature = 0;
3781  }
3782  }
3783 }
3784 
3785 void vpMbTracker::loadConfigFile(const std::string &configFile)
3786 {
3787 #ifdef VISP_HAVE_PUGIXML
3791 
3792  try {
3793  std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl;
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 #else
3806  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
3807 #endif
3808 }
3809 
3816 {
3817  m_projectionErrorMe = me;
3818 
3819  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
3820  vpMbtDistanceLine *l = *it;
3822  }
3823 
3824  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
3825  ++it) {
3826  vpMbtDistanceCylinder *cy = *it;
3828  }
3829 
3830  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
3831  vpMbtDistanceCircle *ci = *it;
3833  }
3834 }
3835 
3841 void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
3842 {
3844 
3845  m_SobelX.resize(size*2+1, size*2+1, false, false);
3847 
3848  m_SobelY.resize(size*2+1, size*2+1, false, false);
3850 }
3851 
static double getSobelKernelY(double *filter, unsigned int size)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
vpDisplay * display
Definition: vpImage.h:142
void addPoint(unsigned int n, const vpPoint &P)
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
virtual ~vpMbTracker()
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:424
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:171
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
unsigned int nbLines
Number of lines in CAO model.
Definition: vpMbTracker.h:162
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
bool isVisible(unsigned int i)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double frobeniusNorm() const
void setCameraParameters(const vpCameraParameters &camera)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
int getIndex() const
Definition: vpMbtPolygon.h:101
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int size() const
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setIndex(unsigned int i)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::map< std::string, std::string > parseParameters(std::string &endLine)
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1578
unsigned int nbCircles
Number of circles in CAO model.
Definition: vpMbTracker.h:170
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
void setNearClippingDistance(const double &dist)
Definition: vpAROgre.h:210
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
std::map< std::string, std::string > mapOfParameterNames
Definition: vpMbTracker.h:182
std::string getName() const
Definition: vpMbtPolygon.h:108
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isTracked() const
unsigned int nbCylinders
Number of cylinders in CAO model.
Definition: vpMbTracker.h:168
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
vpMatrix AtA() const
Definition: vpMatrix.cpp:693
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
#define vpERROR_TRACE
Definition: vpDebug.h:393
unsigned int nbPoints
Number of points in CAO model.
Definition: vpMbTracker.h:160
virtual void loadConfigFile(const std::string &configFile)
virtual void setNbPoint(unsigned int nb)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpPoint * p1
The first extremity.
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
Definition: vpMbTracker.h:205
void setProjectionErrorKernelSize(const unsigned int &size)
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
bool modelInitialised
Definition: vpMbTracker.h:123
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Definition: vpArray2D.h:289
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition: vpMe.h:60
Manage the line of a polygon used in the model-based tracker.
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int nbFeature
The number of moving edges.
void setName(const std::string &face_name)
Definition: vpMbtPolygon.h:159
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:92
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
std::string modelFileName
Definition: vpMbTracker.h:120
virtual void setEstimatedDoF(const vpColVector &v)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1473
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
static const vpColor green
Definition: vpColor.h:182
vpPoint * p1
The first extremity on the axe.
static void flush(const vpImage< unsigned char > &I)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:422
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void setLod(bool useLod, const std::string &name="")
void setFarClippingDistance(const double &dist)
Definition: vpAROgre.h:199
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:931
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
static const vpColor red
Definition: vpColor.h:179
Class that defines what is a point.
Definition: vpPoint.h:58
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:472
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)=0
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:474
unsigned int getCols() const
Definition: vpArray2D.h:279
bool m_projectionErrorOgreShowConfigDialog
Definition: vpMbTracker.h:203
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
static bool parseBoolean(std::string input)
Definition: vpIoTools.cpp:2019
vpColVector & normalize()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1537
Error that can be emited by the vpTracker class and its derivates.
void setProjectionErrorKernelSize(const unsigned int &size)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setName(const std::string &circle_name)
vpPoint * p2
The second extremity.
void projectionErrorResetMovingEdges()
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:951
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:114
void setProjectionErrorMe(const vpMe &me)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
VISP_EXPORT bool checkSSE2()
static void display(const vpImage< unsigned char > &I)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
void clear()
Definition: vpColVector.h:175
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
double getB() const
Definition: vpPlane.h:104
void setMinPolygonAreaThresh(double min_polygon_area)
Definition: vpMbtPolygon.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
Generic class defining intrinsic camera parameters.
void setOgreShowConfigDialog(bool showConfigDialog)
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Main methods for a model-based tracker.
Definition: vpMbTracker.h:104
std::vector< PolygonType * > & getPolygon()
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:426
virtual void loadVRMLModel(const std::string &modelFile)
Implementation of a rotation vector as quaternion angle minimal representation.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
void setIsPolygonOriented(const bool &oriented)
Definition: vpMbtPolygon.h:166
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1767
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
Definition: vpMbTracker.h:164
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
void addPolygon(PolygonType *p)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1438
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
static double rad(double deg)
Definition: vpMath.h:108
std::string poseSavingFilename
Definition: vpMbTracker.h:126
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
Definition: vpMbTracker.h:196
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
void setVisible(bool _isvisible)
int getWindowXPosition() const
Definition: vpDisplay.h:245
double getA() const
Definition: vpPlane.h:102
void setName(const std::string &line_name)
int getWindowYPosition() const
Definition: vpDisplay.h:250
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
void setCameraParameters(const vpCameraParameters &camera)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
Definition: vpMbTracker.h:166
bool isVisible() const
static std::string trim(std::string s)
Definition: vpIoTools.cpp:2033
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:101
void setMinLineLengthThresh(double min_line_length)
Definition: vpMbtPolygon.h:141
virtual void setOgreVisibilityTest(const bool &v)
unsigned int getHeight() const
Definition: vpImage.h:186
void setIndex(unsigned int i)
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:243
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
Definition: vpMbTracker.h:219
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
void setMovingEdge(vpMe *Me)
void setLod(bool use_lod)
void setProjectionErrorMovingEdge(const vpMe &me)
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double sumSquare() const
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
error that can be emited by the vpMatrix class and its derivates
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual vpColVector getEstimatedDoF() const
double getC() const
Definition: vpPlane.h:106
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void addPolygon(const int &index)
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:730
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
static std::string getAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1495
static double getSobelKernelX(double *filter, unsigned int size)
virtual void setClipping(const unsigned int &flags)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double radius
The radius of the cylinder.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
std::vector< vpMbtMeLine * > meline
The moving edge container.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
unsigned int getProjectionErrorKernelSize() const
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
unsigned int nbFeaturel2
The number of moving edges on line 2.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
Definition: vpMbTracker.h:207
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...
Definition: vpPose.cpp:336
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void savePose(const std::string &filename) const
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
Definition: vpMbTracker.h:200
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:244
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< unsigned int > nbFeature
The number of moving edges.
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Class that consider the case of a translation vector.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void eye()
Definition: vpMatrix.cpp:492
Implementation of a rotation vector as axis-angle minimal representation.
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
virtual void setIndex(int i)
Definition: vpMbtPolygon.h:124
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
vpPoint * p1
The center of the circle.
void removeComment(std::ifstream &fileId)
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
Definition: vpMbTracker.h:202
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setIndex(unsigned int i)
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
Definition: vpMbTracker.h:198
void getProjectionErrorMe(vpMe &me) const
void clearPoint()
Definition: vpPose.cpp:134
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)