Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbTracker.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Generic model based tracker
33 *
34*****************************************************************************/
35
41#include <algorithm>
42#include <iostream>
43#include <limits>
44#include <sstream>
45
46#include <Simd/SimdLib.hpp>
47
48#include <visp3/core/vpColVector.h>
49#include <visp3/core/vpDisplay.h>
50#include <visp3/core/vpMath.h>
51#include <visp3/core/vpMatrix.h>
52#include <visp3/core/vpPoint.h>
53#include <visp3/vision/vpPose.h>
54#ifdef VISP_HAVE_MODULE_GUI
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayOpenCV.h>
57#include <visp3/gui/vpDisplayX.h>
58#endif
59#include <visp3/core/vpCameraParameters.h>
60#include <visp3/core/vpColor.h>
61#include <visp3/core/vpException.h>
62#include <visp3/core/vpIoTools.h>
63#include <visp3/core/vpPixelMeterConversion.h>
64#ifdef VISP_HAVE_MODULE_IO
65#include <visp3/io/vpImageIo.h>
66#endif
67#include <visp3/core/vpCPUFeatures.h>
68#include <visp3/core/vpIoTools.h>
69#include <visp3/core/vpMatrixException.h>
70#include <visp3/core/vpTrackingException.h>
71#include <visp3/mbt/vpMbTracker.h>
72
73#include <visp3/core/vpImageFilter.h>
74#include <visp3/mbt/vpMbtXmlGenericParser.h>
75
76#ifdef VISP_HAVE_COIN3D
77// Inventor includes
78#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
79#include <Inventor/VRMLnodes/SoVRMLGroup.h>
80#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
81#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
82#include <Inventor/VRMLnodes/SoVRMLShape.h>
83#include <Inventor/VRMLnodes/SoVRMLTransform.h>
84#include <Inventor/actions/SoGetMatrixAction.h>
85#include <Inventor/actions/SoGetPrimitiveCountAction.h>
86#include <Inventor/actions/SoSearchAction.h>
87#include <Inventor/actions/SoToVRML2Action.h>
88#include <Inventor/actions/SoWriteAction.h>
89#include <Inventor/misc/SoChildList.h>
90#include <Inventor/nodes/SoSeparator.h>
91#endif
92
93#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
94#include <mutex>
95#endif
96
97#ifndef DOXYGEN_SHOULD_SKIP_THIS
98
99namespace
100{
101#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
102 std::mutex g_mutex_cout;
103#endif
107struct 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
120struct 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
140std::istream &safeGetline(std::istream &is, std::string &t)
141{
142 t.clear();
143
144 // The characters in the stream are read one-by-one using a std::streambuf.
145 // That is faster than reading them one-by-one using the std::istream.
146 // Code that uses streambuf this way must be guarded by a sentry object.
147 // The sentry object performs various tasks,
148 // such as thread synchronization and updating the stream state.
149
150 std::istream::sentry se(is, true);
151 std::streambuf *sb = is.rdbuf();
152
153 for (;;) {
154 int c = sb->sbumpc();
155 if (c == '\n') {
156 return is;
157 } else if (c == '\r') {
158 if (sb->sgetc() == '\n')
159 sb->sbumpc();
160 return is;
161 } else if (c == std::streambuf::traits_type::eof()) {
162 // Also handle the case when the last line has no line ending
163 if (t.empty())
164 is.setstate(std::ios::eofbit);
165 return is;
166 } else { // default case
167 t += (char)c;
168 }
169 }
170}
171} // namespace
172#endif // DOXYGEN_SHOULD_SKIP_THIS
173
180 : m_cam(), m_cMo(), oJo(6, 6), m_isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
181 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
182 displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
183 angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
184 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
185 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
186 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
187 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
188 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
189 m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
190 m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
191 m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false),
192 m_rand()
193{
194 oJo.eye();
195 // Map used to parse additional information in CAO model files,
196 // like name of faces or LOD setting
197 mapOfParameterNames["name"] = "string";
198 mapOfParameterNames["minPolygonAreaThreshold"] = "number";
199 mapOfParameterNames["minLineLengthThreshold"] = "number";
200 mapOfParameterNames["useLod"] = "boolean";
201
204}
205
207{
208 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
209 it != m_projectionErrorLines.end(); ++it) {
210 vpMbtDistanceLine *l = *it;
211 if (l != NULL)
212 delete l;
213 l = NULL;
214 }
215
216 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
217 it != m_projectionErrorCylinders.end(); ++it) {
218 vpMbtDistanceCylinder *cy = *it;
219 if (cy != NULL)
220 delete cy;
221 cy = NULL;
222 }
223
224 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
225 it != m_projectionErrorCircles.end(); ++it) {
226 vpMbtDistanceCircle *ci = *it;
227 if (ci != NULL)
228 delete ci;
229 ci = NULL;
230 }
231#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
232 if (m_sodb_init_called) {
233 // Cleanup memory allocated by Coin library used to load a vrml model
234 SoDB::finish();
235 }
236#endif
237}
238
239#ifdef VISP_HAVE_MODULE_GUI
240void vpMbTracker::initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
241 const std::string &initFile, bool displayHelp, const vpHomogeneousMatrix &T)
242{
243 vpHomogeneousMatrix last_cMo;
244 vpPoseVector init_pos;
245 vpImagePoint ip;
247
248 std::string ext = ".init";
249 std::string str_pose = "";
250 size_t pos = initFile.rfind(ext);
251
252 // Load the last poses from files
253 std::fstream finitpos;
254 std::ifstream finit;
255 std::stringstream ss;
256 if (poseSavingFilename.empty()) {
257 if (pos != std::string::npos)
258 str_pose = initFile.substr(0, pos) + ".0.pos";
259 else
260 str_pose = initFile + ".0.pos";
261
262 finitpos.open(str_pose.c_str(), std::ios::in);
263 ss << str_pose;
264 } else {
265 finitpos.open(poseSavingFilename.c_str(), std::ios::in);
266 ss << poseSavingFilename;
267 }
268 if (finitpos.fail()) {
269 std::cout << "Cannot read " << ss.str() << std::endl << "cMo set to identity" << std::endl;
270 last_cMo.eye();
271 } else {
272 for (unsigned int i = 0; i < 6; i += 1) {
273 finitpos >> init_pos[i];
274 }
275
276 finitpos.close();
277 last_cMo.buildFrom(init_pos);
278
279 std::cout << "Tracker initial pose read from " << ss.str() << ": " << std::endl << last_cMo << std::endl;
280
281 if (I) {
283 display(*I, last_cMo, m_cam, vpColor::green, 1, true);
284 vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green);
286 } else {
287 vpDisplay::display(*I_color);
288 display(*I_color, last_cMo, m_cam, vpColor::green, 1, true);
289 vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green);
290 vpDisplay::flush(*I_color);
291 }
292
293 std::cout << "No modification : left click " << std::endl;
294 std::cout << "Modify initial pose : right click " << std::endl;
295
296 if (I) {
297 vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);
298
300
301 while (!vpDisplay::getClick(*I, ip, button)) {
302 }
303 } else {
304 vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose",
306
307 vpDisplay::flush(*I_color);
308
309 while (!vpDisplay::getClick(*I_color, ip, button)) {
310 }
311 }
312 }
313
314 if (!finitpos.fail() && button == vpMouseButton::button1) {
315 m_cMo = last_cMo;
316 } else {
317 vpDisplay *d_help = NULL;
318
319 if (I) {
322 } else {
323 vpDisplay::display(*I_color);
324 vpDisplay::flush(*I_color);
325 }
326
327 vpPose pose;
328
329 pose.clearPoint();
330
331 // Clear string stream that previously contained the path to the "object.0.pos" file.
332 ss.str(std::string());
333
334 // file parser
335 // number of points
336 // X Y Z
337 // X Y Z
338 if (pos != std::string::npos) {
339 ss << initFile;
340 } else {
341 ss << initFile;
342 ss << ".init";
343 }
344
345 std::cout << "Load 3D points from: " << ss.str() << std::endl;
346#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
347 finit.open(ss.str());
348#else
349 finit.open(ss.str().c_str());
350#endif
351 if (finit.fail()) {
352 std::cout << "Cannot read " << ss.str() << std::endl;
353 throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str());
354 }
355
356#ifdef VISP_HAVE_MODULE_IO
357 // Display window creation and initialisation
358 try {
359 if (displayHelp) {
360 const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"};
361 std::string dispF;
362 bool foundHelpImg = false;
363 if (pos != std::string::npos) {
364 for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
365 dispF = initFile.substr(0, pos) + imgExtVec[i];
366 foundHelpImg = vpIoTools::checkFilename(dispF);
367 }
368 } else {
369 for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
370 dispF = initFile + imgExtVec[i];
371 foundHelpImg = vpIoTools::checkFilename(dispF);
372 }
373 }
374
375 if (foundHelpImg) {
376 std::cout << "Load image to help initialization: " << dispF << std::endl;
377#if defined(VISP_HAVE_X11)
378 d_help = new vpDisplayX;
379#elif defined(VISP_HAVE_GDI)
380 d_help = new vpDisplayGDI;
381#elif defined(HAVE_OPENCV_HIGHGUI)
382 d_help = new vpDisplayOpenCV;
383#endif
384
385 vpImage<vpRGBa> Iref;
386 vpImageIo::read(Iref, dispF);
387#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
388 const int winXPos = I != NULL ? I->display->getWindowXPosition() : I_color->display->getWindowXPosition();
389 const int winYPos = I != NULL ? I->display->getWindowYPosition() : I_color->display->getWindowYPosition();
390 unsigned int width = I != NULL ? I->getWidth() : I_color->getWidth();
391 d_help->init(Iref, winXPos + (int)width + 80, winYPos, "Where to initialize...");
392 vpDisplay::display(Iref);
393 vpDisplay::flush(Iref);
394#endif
395 }
396 }
397 } catch (...) {
398 if (d_help != NULL) {
399 delete d_help;
400 d_help = NULL;
401 }
402 }
403#else //#ifdef VISP_HAVE_MODULE_IO
404 (void)(displayHelp);
405#endif //#ifdef VISP_HAVE_MODULE_IO
406 // skip lines starting with # as comment
407 removeComment(finit);
408
409 unsigned int n3d;
410 finit >> n3d;
411 finit.ignore(256, '\n'); // skip the rest of the line
412 std::cout << "Number of 3D points " << n3d << std::endl;
413 if (n3d > 100000) {
414 throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed",
415 ss.str().c_str());
416 }
417
418 std::vector<vpPoint> P(n3d);
419 for (unsigned int i = 0; i < n3d; i++) {
420 // skip lines starting with # as comment
421 removeComment(finit);
422
423 vpColVector pt_3d(4, 1.0);
424 finit >> pt_3d[0];
425 finit >> pt_3d[1];
426 finit >> pt_3d[2];
427 finit.ignore(256, '\n'); // skip the rest of the line
428
429 vpColVector pt_3d_tf = T * pt_3d;
430 std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " "
431 << pt_3d_tf[2] << std::endl;
433 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
434 }
436 finit.close();
438 bool isWellInit = false;
439 while (!isWellInit) {
440 std::vector<vpImagePoint> mem_ip;
441 for (unsigned int i = 0; i < n3d; i++) {
442 std::ostringstream text;
443 text << "Click on point " << i + 1;
444 if (I) {
446 vpDisplay::displayText(*I, 15, 10, text.str(), vpColor::red);
447 for (unsigned int k = 0; k < mem_ip.size(); k++) {
448 vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2);
451 } else {
452 vpDisplay::display(*I_color);
453 vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red);
454 for (unsigned int k = 0; k < mem_ip.size(); k++) {
455 vpDisplay::displayCross(*I_color, mem_ip[k], 10, vpColor::green, 2);
457 vpDisplay::flush(*I_color);
458 }
459
460 std::cout << "Click on point " << i + 1 << " ";
461 double x = 0, y = 0;
462 if (I) {
463 vpDisplay::getClick(*I, ip);
464 mem_ip.push_back(ip);
466 } else {
467 vpDisplay::getClick(*I_color, ip);
468 mem_ip.push_back(ip);
469 vpDisplay::flush(*I_color);
470 }
472 P[i].set_x(x);
473 P[i].set_y(y);
474
475 std::cout << "with 2D coordinates: " << ip << std::endl;
476
477 pose.addPoint(P[i]); // and added to the pose computation point list
478 }
479 if (I) {
482 } else {
483 vpDisplay::flush(*I_color);
484 vpDisplay::display(*I_color);
485 }
486
488
489 if (I) {
490 display(*I, m_cMo, m_cam, vpColor::green, 1, true);
491 vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
492
494
495 button = vpMouseButton::button1;
496 while (!vpDisplay::getClick(*I, ip, button)) {
497 }
498
499 if (button == vpMouseButton::button1) {
500 isWellInit = true;
501 } else {
502 pose.clearPoint();
505 }
506 } else {
507 display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
508 vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object",
510
511 vpDisplay::flush(*I_color);
512
513 button = vpMouseButton::button1;
514 while (!vpDisplay::getClick(*I_color, ip, button)) {
515 }
516
517 if (button == vpMouseButton::button1) {
518 isWellInit = true;
519 } else {
520 pose.clearPoint();
521 vpDisplay::display(*I_color);
522 vpDisplay::flush(*I_color);
523 }
524 }
525 }
526 if (I)
528 else
530
531 // save the pose into file
532 if (poseSavingFilename.empty())
533 savePose(str_pose);
534 else
536
537 if (d_help != NULL) {
538 delete d_help;
539 d_help = NULL;
540 }
541 }
542
543 std::cout << "cMo : " << std::endl << m_cMo << std::endl;
544
545 if (I)
546 init(*I);
547 else {
548 vpImageConvert::convert(*I_color, m_I);
549 init(m_I);
550 }
551}
552
584void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp,
585 const vpHomogeneousMatrix &T)
586{
587 initClick(&I, NULL, initFile, displayHelp, T);
588}
589
621void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, bool displayHelp,
622 const vpHomogeneousMatrix &T)
623{
624 initClick(NULL, &I_color, initFile, displayHelp, T);
625}
626
627void vpMbTracker::initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
628 const std::vector<vpPoint> &points3D_list, const std::string &displayFile)
629{
630 if (I) {
633 } else {
634 vpDisplay::display(*I_color);
635 vpDisplay::flush(*I_color);
636 }
637
638 vpDisplay *d_help = NULL;
639
640 vpPose pose;
641 std::vector<vpPoint> P;
642 for (unsigned int i = 0; i < points3D_list.size(); i++)
643 P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
644
645#ifdef VISP_HAVE_MODULE_IO
646 vpImage<vpRGBa> Iref;
647 // Display window creation and initialisation
648 if (vpIoTools::checkFilename(displayFile)) {
649 try {
650 std::cout << "Load image to help initialization: " << displayFile << std::endl;
651#if defined(VISP_HAVE_X11)
652 d_help = new vpDisplayX;
653#elif defined(VISP_HAVE_GDI)
654 d_help = new vpDisplayGDI;
655#elif defined VISP_HAVE_OPENCV
656 d_help = new vpDisplayOpenCV;
657#endif
658
659 vpImageIo::read(Iref, displayFile);
660#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
661 if (I) {
662 d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(),
663 "Where to initialize...");
664 } else {
665 d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80,
666 I_color->display->getWindowYPosition(), "Where to initialize...");
667 }
668 vpDisplay::display(Iref);
669 vpDisplay::flush(Iref);
670#endif
671 } catch (...) {
672 if (d_help != NULL) {
673 delete d_help;
674 d_help = NULL;
675 }
676 }
677 }
678#else //#ifdef VISP_HAVE_MODULE_IO
679 (void)(displayFile);
680#endif //#ifdef VISP_HAVE_MODULE_IO
681
682 vpImagePoint ip;
683 bool isWellInit = false;
684 while (!isWellInit) {
685 for (unsigned int i = 0; i < points3D_list.size(); i++) {
686 std::cout << "Click on point " << i + 1 << std::endl;
687 double x = 0, y = 0;
688 if (I) {
689 vpDisplay::getClick(*I, ip);
692 } else {
693 vpDisplay::getClick(*I_color, ip);
694 vpDisplay::displayCross(*I_color, ip, 5, vpColor::green);
695 vpDisplay::flush(*I_color);
696 }
698 P[i].set_x(x);
699 P[i].set_y(y);
700
701 std::cout << "Click on point " << ip << std::endl;
702
703 if (I) {
704 vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point
705 } else {
706 vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point
707 }
708 pose.addPoint(P[i]); // and added to the pose computation point list
709 }
710 if (I) {
712 } else {
713 vpDisplay::flush(*I_color);
714 }
715
717
718 if (I) {
719 display(*I, m_cMo, m_cam, vpColor::green, 1, true);
720 vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
721
723
725 while (!vpDisplay::getClick(*I, ip, button)) {
726 };
727
728 if (button == vpMouseButton::button1) {
729 isWellInit = true;
730 } else {
731 pose.clearPoint();
734 }
735 } else {
736 display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
737 vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object",
739
740 vpDisplay::flush(*I_color);
741
743 while (!vpDisplay::getClick(*I_color, ip, button)) {
744 };
745
746 if (button == vpMouseButton::button1) {
747 isWellInit = true;
748 } else {
749 pose.clearPoint();
750 vpDisplay::display(*I_color);
751 vpDisplay::flush(*I_color);
752 }
753 }
754 }
755
756 if (I) {
758 } else {
760 }
761
762 if (d_help != NULL) {
763 delete d_help;
764 d_help = NULL;
765 }
766
767 if (I)
768 init(*I);
769 else {
770 vpImageConvert::convert(*I_color, m_I);
771 init(m_I);
772 }
773}
774
786void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
787 const std::string &displayFile)
788{
789 initClick(&I, NULL, points3D_list, displayFile);
790}
791
803void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::vector<vpPoint> &points3D_list,
804 const std::string &displayFile)
805{
806 initClick(NULL, &I_color, points3D_list, displayFile);
807}
808#endif //#ifdef VISP_HAVE_MODULE_GUI
809
811 const std::string &initFile)
813 std::stringstream ss;
814 std::fstream finit;
815
816 std::string ext = ".init";
817 size_t pos = initFile.rfind(ext);
818
819 if (pos == initFile.size() - ext.size() && pos != 0) {
820 ss << initFile;
821 } else {
822 ss << initFile;
823 ss << ".init";
824 }
825
826 std::cout << "Load 2D/3D points from: " << ss.str() << std::endl;
827 finit.open(ss.str().c_str(), std::ios::in);
828 if (finit.fail()) {
829 std::cout << "cannot read " << ss.str() << std::endl;
830 throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str());
831 }
832
833 //********
834 // Read 3D points coordinates
835 //********
836 char c;
837 // skip lines starting with # as comment
838 finit.get(c);
839 while (!finit.fail() && (c == '#')) {
840 finit.ignore(256, '\n');
841 finit.get(c);
842 }
843 finit.unget();
844
845 unsigned int n3d;
846 finit >> n3d;
847 finit.ignore(256, '\n'); // skip the rest of the line
848 std::cout << "Number of 3D points " << n3d << std::endl;
849 if (n3d > 100000) {
850 throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed",
851 ss.str().c_str());
852 }
853
854 vpPoint *P = new vpPoint[n3d];
855 for (unsigned int i = 0; i < n3d; i++) {
856 // skip lines starting with # as comment
857 finit.get(c);
858 while (!finit.fail() && (c == '#')) {
859 finit.ignore(256, '\n');
860 finit.get(c);
862 finit.unget();
863 double X, Y, Z;
864 finit >> X;
865 finit >> Y;
866 finit >> Z;
867 finit.ignore(256, '\n'); // skip the rest of the line
868
869 std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
870 P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
871 }
872
873 //********
874 // Read 3D points coordinates
875 //********
876 // skip lines starting with # as comment
877 finit.get(c);
878 while (!finit.fail() && (c == '#')) {
879 finit.ignore(256, '\n');
880 finit.get(c);
881 }
882 finit.unget();
883
884 unsigned int n2d;
885 finit >> n2d;
886 finit.ignore(256, '\n'); // skip the rest of the line
887 std::cout << "Number of 2D points " << n2d << std::endl;
888 if (n2d > 100000) {
889 delete[] P;
890 throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed",
891 ss.str().c_str());
892 }
893
894 if (n3d != n2d) {
895 delete[] P;
897 "In %s file, number of 2D points %d and number of 3D "
898 "points %d are not equal",
899 ss.str().c_str(), n2d, n3d);
900 }
901
902 vpPose pose;
903 for (unsigned int i = 0; i < n2d; i++) {
904 // skip lines starting with # as comment
905 finit.get(c);
906 while (!finit.fail() && (c == '#')) {
907 finit.ignore(256, '\n');
908 finit.get(c);
909 }
910 finit.unget();
911 double u, v, x = 0, y = 0;
912 finit >> v;
913 finit >> u;
914 finit.ignore(256, '\n'); // skip the rest of the line
915
916 vpImagePoint ip(v, u);
917 std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
919 P[i].set_x(x);
920 P[i].set_y(y);
921 pose.addPoint(P[i]);
922 }
923
924 finit.close();
925
927
928 delete[] P;
929
930 if (I) {
931 init(*I);
932 } else {
933 vpImageConvert::convert(*I_color, m_I);
934 init(m_I);
935 }
936}
937
962void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
963{
964 initFromPoints(&I, NULL, initFile);
965}
966
991void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::string &initFile)
992{
993 initFromPoints(NULL, &I_color, initFile);
994}
995
997 const std::vector<vpImagePoint> &points2D_list,
998 const std::vector<vpPoint> &points3D_list)
999{
1000 if (points2D_list.size() != points3D_list.size())
1001 vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
1002 "different to the number of 3D points.");
1003
1004 size_t size = points3D_list.size();
1005 std::vector<vpPoint> P;
1006 vpPose pose;
1007
1008 for (size_t i = 0; i < size; i++) {
1009 P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1010 double x = 0, y = 0;
1011 vpPixelMeterConversion::convertPoint(m_cam, points2D_list[i], x, y);
1012 P[i].set_x(x);
1013 P[i].set_y(y);
1014 pose.addPoint(P[i]);
1015 }
1016
1018
1019 if (I) {
1020 init(*I);
1021 } else {
1022 vpImageConvert::convert(*I_color, m_I);
1023 init(m_I);
1024 }
1025}
1026
1035void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
1036 const std::vector<vpPoint> &points3D_list)
1037{
1038 initFromPoints(&I, NULL, points2D_list, points3D_list);
1039}
1040
1049void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::vector<vpImagePoint> &points2D_list,
1050 const std::vector<vpPoint> &points3D_list)
1051{
1052 initFromPoints(NULL, &I_color, points2D_list, points3D_list);
1053}
1054
1055void vpMbTracker::initFromPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
1056 const std::string &initFile)
1057{
1058 std::stringstream ss;
1059 std::fstream finit;
1060 vpPoseVector init_pos;
1061
1062 std::string ext = ".pos";
1063 size_t pos = initFile.rfind(ext);
1064
1065 if (pos == initFile.size() - ext.size() && pos != 0) {
1066 ss << initFile;
1067 } else {
1068 ss << initFile;
1069 ss << ".pos";
1070 }
1071
1072 finit.open(ss.str().c_str(), std::ios::in);
1073 if (finit.fail()) {
1074 std::cout << "Cannot read " << ss.str() << std::endl;
1075 throw vpException(vpException::ioError, "cannot read init file");
1076 }
1077
1078 for (unsigned int i = 0; i < 6; i += 1) {
1079 finit >> init_pos[i];
1080 }
1081
1082 m_cMo.buildFrom(init_pos);
1083
1084 if (I) {
1085 init(*I);
1086 } else {
1087 vpImageConvert::convert(*I_color, m_I);
1088 init(m_I);
1089 }
1090}
1091
1110void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
1111{
1112 initFromPose(&I, NULL, initFile);
1113}
1114
1133void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const std::string &initFile)
1134{
1135 initFromPose(NULL, &I_color, initFile);
1136}
1137
1145{
1146 m_cMo = cMo;
1147 init(I);
1148}
1149
1157{
1158 m_cMo = cMo;
1159 vpImageConvert::convert(I_color, m_I);
1160 init(m_I);
1161}
1162
1170{
1171 vpHomogeneousMatrix _cMo(cPo);
1172 initFromPose(I, _cMo);
1173}
1174
1182{
1183 vpHomogeneousMatrix _cMo(cPo);
1184 vpImageConvert::convert(I_color, m_I);
1185 initFromPose(m_I, _cMo);
1186}
1187
1193void vpMbTracker::savePose(const std::string &filename) const
1194{
1195 vpPoseVector init_pos;
1196 std::fstream finitpos;
1197 finitpos.open(filename.c_str(), std::ios::out);
1198
1199 init_pos.buildFrom(m_cMo);
1200 finitpos << init_pos;
1201 finitpos.close();
1202}
1203
1204void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
1205 bool useLod, double minPolygonAreaThreshold, double minLineLengthThreshold)
1206{
1207 std::vector<vpPoint> corners_without_duplicates;
1208 corners_without_duplicates.push_back(corners[0]);
1209 for (unsigned int i = 0; i < corners.size() - 1; i++) {
1210 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1211 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1212 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1213 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1214 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1215 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1216 corners_without_duplicates.push_back(corners[i + 1]);
1217 }
1218 }
1219
1220 vpMbtPolygon polygon;
1221 polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
1222 polygon.setIndex((int)idFace);
1223 polygon.setName(polygonName);
1224 polygon.setLod(useLod);
1225
1226 // //if(minPolygonAreaThreshold != -1.0) {
1227 // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1228 // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1229 // {
1230 // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1231 // }
1232 //
1233 // //if(minLineLengthThreshold != -1.0) {
1234 // if(std::fabs(minLineLengthThreshold + 1.0) >
1235 // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1236 // {
1237 // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1238 // }
1239
1240 polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1241 polygon.setMinLineLengthThresh(minLineLengthThreshold);
1242
1243 for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1244 polygon.addPoint(j, corners_without_duplicates[j]);
1245 }
1246
1247 faces.addPolygon(&polygon);
1248
1250 faces.getPolygon().back()->setClipping(clippingFlag);
1251
1253 faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1254
1256 faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1257}
1258
1259void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1260 const std::string &polygonName, bool useLod, double minPolygonAreaThreshold)
1261{
1262 vpMbtPolygon polygon;
1263 polygon.setNbPoint(4);
1264 polygon.setName(polygonName);
1265 polygon.setLod(useLod);
1266
1267 // //if(minPolygonAreaThreshold != -1.0) {
1268 // if(std::fabs(minPolygonAreaThreshold + 1.0) >
1269 // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
1270 // {
1271 // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1272 // }
1273 polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
1274 // Non sense to set minLineLengthThreshold for circle
1275 // but used to be coherent when applying LOD settings for all polygons
1277
1278 {
1279 // Create the 4 points of the circle bounding box
1280 vpPlane plane(p1, p2, p3, vpPlane::object_frame);
1281
1282 // Matrice de passage entre world et circle frame
1283 double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
1284 vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
1285 double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
1286 vpRotationMatrix wRc;
1287 vpColVector x(3), y(3), z(3);
1288 // X axis is P2-P1
1289 x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
1290 x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
1291 x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
1292 // Y axis is the normal of the plane
1293 y[0] = plane.getA() / norm_Y;
1294 y[1] = plane.getB() / norm_Y;
1295 y[2] = plane.getC() / norm_Y;
1296 // Z axis = X ^ Y
1297 z = vpColVector::crossProd(x, y);
1298 for (unsigned int i = 0; i < 3; i++) {
1299 wRc[i][0] = x[i];
1300 wRc[i][1] = y[i];
1301 wRc[i][2] = z[i];
1302 }
1303
1304 vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
1305 vpHomogeneousMatrix wMc(wtc, wRc);
1306
1307 vpColVector c_p(4); // A point in the circle frame that is on the bbox
1308 c_p[0] = radius;
1309 c_p[1] = 0;
1310 c_p[2] = radius;
1311 c_p[3] = 1;
1312
1313 // Matrix to rotate a point by 90 deg around Y in the circle frame
1314 for (unsigned int i = 0; i < 4; i++) {
1315 vpColVector w_p(4); // A point in the word frame
1317 w_p = wMc * cMc_90 * c_p;
1318
1319 vpPoint w_P;
1320 w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
1321
1322 polygon.addPoint(i, w_P);
1323 }
1324 }
1325
1326 polygon.setIndex(idFace);
1327 faces.addPolygon(&polygon);
1328
1330 faces.getPolygon().back()->setClipping(clippingFlag);
1331
1333 faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1334
1336 faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1337}
1338
1339void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
1340 bool useLod, double minLineLengthThreshold)
1341{
1342 // A polygon as a single line that corresponds to the revolution axis of the
1343 // cylinder
1344 vpMbtPolygon polygon;
1345 polygon.setNbPoint(2);
1346
1347 polygon.addPoint(0, p1);
1348 polygon.addPoint(1, p2);
1349
1350 polygon.setIndex(idFace);
1351 polygon.setName(polygonName);
1352 polygon.setLod(useLod);
1353
1354 // //if(minLineLengthThreshold != -1.0) {
1355 // if(std::fabs(minLineLengthThreshold + 1.0) >
1356 // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
1357 // {
1358 // polygon.setMinLineLengthThresh(minLineLengthThreshold);
1359 // }
1360 polygon.setMinLineLengthThresh(minLineLengthThreshold);
1361 // Non sense to set minPolygonAreaThreshold for cylinder
1362 // but used to be coherent when applying LOD settings for all polygons
1364
1365 faces.addPolygon(&polygon);
1366
1368 faces.getPolygon().back()->setClipping(clippingFlag);
1369
1371 faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1372
1374 faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1375}
1376
1377void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
1378 const std::string &polygonName, bool useLod, double minLineLengthThreshold)
1379{
1380 int id = idFace;
1381 for (unsigned int i = 0; i < listFaces.size(); i++) {
1382 vpMbtPolygon polygon;
1383 polygon.setNbPoint((unsigned int)listFaces[i].size());
1384 for (unsigned int j = 0; j < listFaces[i].size(); j++)
1385 polygon.addPoint(j, listFaces[i][j]);
1386
1387 polygon.setIndex(id);
1388 polygon.setName(polygonName);
1389 polygon.setIsPolygonOriented(false);
1390 polygon.setLod(useLod);
1391 polygon.setMinLineLengthThresh(minLineLengthThreshold);
1393
1394 faces.addPolygon(&polygon);
1395
1397 faces.getPolygon().back()->setClipping(clippingFlag);
1398
1400 faces.getPolygon().back()->setNearClippingDistance(distNearClip);
1401
1403 faces.getPolygon().back()->setFarClippingDistance(distFarClip);
1404
1405 id++;
1406 }
1407}
1408
1424void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &odTo)
1425{
1426 std::string::const_iterator it;
1427
1428 if (vpIoTools::checkFilename(modelFile)) {
1429 it = modelFile.end();
1430 if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
1431 (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
1432 std::vector<std::string> vectorOfModelFilename;
1433 int startIdFace = (int)faces.size();
1434 nbPoints = 0;
1435 nbLines = 0;
1436 nbPolygonLines = 0;
1437 nbPolygonPoints = 0;
1438 nbCylinders = 0;
1439 nbCircles = 0;
1440 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo);
1441 } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
1442 (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
1443 loadVRMLModel(modelFile);
1444 } else {
1445 throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
1446 }
1447 } else {
1448 throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
1449 }
1450
1451 this->modelInitialised = true;
1452 this->modelFileName = modelFile;
1453}
1454
1473void vpMbTracker::loadVRMLModel(const std::string &modelFile)
1474{
1475#ifdef VISP_HAVE_COIN3D
1476 m_sodb_init_called = true;
1477 SoDB::init(); // Call SoDB::finish() before ending the program.
1478
1479 SoInput in;
1480 SbBool ok = in.openFile(modelFile.c_str());
1481 SoVRMLGroup *sceneGraphVRML2;
1482
1483 if (!ok) {
1484 vpERROR_TRACE("can't open file to load model");
1485 throw vpException(vpException::fatalError, "can't open file to load model");
1486 }
1487
1488 if (!in.isFileVRML2()) {
1489 SoSeparator *sceneGraph = SoDB::readAll(&in);
1490 if (sceneGraph == NULL) { /*return -1;*/
1491 }
1492 sceneGraph->ref();
1493
1494 SoToVRML2Action tovrml2;
1495 tovrml2.apply(sceneGraph);
1496
1497 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1498 sceneGraphVRML2->ref();
1499 sceneGraph->unref();
1500 } else {
1501 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1502 if (sceneGraphVRML2 == NULL) { /*return -1;*/
1503 }
1504 sceneGraphVRML2->ref();
1505 }
1506
1507 in.closeFile();
1508
1509 vpHomogeneousMatrix transform;
1510 int indexFace = (int)faces.size();
1511 extractGroup(sceneGraphVRML2, transform, indexFace);
1512
1513 sceneGraphVRML2->unref();
1514#else
1515 vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1516 throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
1517#endif
1518}
1519
1520void vpMbTracker::removeComment(std::ifstream &fileId)
1521{
1522 char c;
1523
1524 fileId.get(c);
1525 while (!fileId.fail() && (c == '#')) {
1526 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));
1527 fileId.get(c);
1528 }
1529 if (fileId.fail()) {
1530 throw(vpException(vpException::ioError, "Reached end of file"));
1531 }
1532 fileId.unget();
1533}
1534
1535std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
1536{
1537 std::map<std::string, std::string> mapOfParams;
1538
1539 bool exit = false;
1540 while (!endLine.empty() && !exit) {
1541 exit = true;
1542
1543 for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
1544 it != mapOfParameterNames.end(); ++it) {
1545 endLine = vpIoTools::trim(endLine);
1546 std::string param(it->first + "=");
1547
1548 // Compare with a potential parameter
1549 if (endLine.compare(0, param.size(), param) == 0) {
1550 exit = false;
1551 endLine = endLine.substr(param.size());
1552
1553 bool parseQuote = false;
1554 if (it->second == "string") {
1555 // Check if the string is between quotes
1556 if (endLine.size() > 2 && endLine[0] == '"') {
1557 parseQuote = true;
1558 endLine = endLine.substr(1);
1559 size_t pos = endLine.find_first_of('"');
1560
1561 if (pos != std::string::npos) {
1562 mapOfParams[it->first] = endLine.substr(0, pos);
1563 endLine = endLine.substr(pos + 1);
1564 } else {
1565 parseQuote = false;
1566 }
1567 }
1568 }
1569
1570 if (!parseQuote) {
1571 // Deal with space or tabulation after parameter value to substring
1572 // to the next sequence
1573 size_t pos1 = endLine.find_first_of(' ');
1574 size_t pos2 = endLine.find_first_of('\t');
1575 size_t pos = pos1 < pos2 ? pos1 : pos2;
1576
1577 mapOfParams[it->first] = endLine.substr(0, pos);
1578 endLine = endLine.substr(pos + 1);
1579 }
1580 }
1581 }
1582 }
1583
1584 return mapOfParams;
1585}
1586
1636void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
1637 int &startIdFace, bool verbose, bool parent, const vpHomogeneousMatrix &odTo)
1638{
1639 std::ifstream fileId;
1640 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1641 fileId.open(modelFile.c_str(), std::ifstream::in);
1642 if (fileId.fail()) {
1643 std::cout << "cannot read CAO model file: " << modelFile << std::endl;
1644 throw vpException(vpException::ioError, "cannot read CAO model file");
1645 }
1646
1647 if (verbose) {
1648 std::cout << "Model file : " << modelFile << std::endl;
1649 }
1650 vectorOfModelFilename.push_back(modelFile);
1651
1652 try {
1653 char c;
1654 // Extraction of the version (remove empty line and commented ones
1655 // (comment line begin with the #)).
1656 // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
1657 removeComment(fileId);
1658
1660 int caoVersion;
1661 fileId.get(c);
1662 if (c == 'V') {
1663 fileId >> caoVersion;
1664 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1665 } else {
1666 std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
1667 "file : use V0, V1, ...";
1668 throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
1669 "header file : use V0, V1, ...");
1670 }
1671
1672 removeComment(fileId);
1673
1675 std::string line;
1676 const std::string prefix_load = "load";
1677
1678 fileId.get(c);
1679 fileId.unget();
1680 bool header = false;
1681 while (c == 'l' || c == 'L') {
1682 getline(fileId, line);
1683
1684 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1685 // remove "load("
1686 std::string paramsStr = line.substr(5);
1687 // get parameters inside load()
1688 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(")"));
1689 // split by comma
1690 std::vector<std::string> params = vpIoTools::splitChain(paramsStr, ",");
1691 // remove whitespaces
1692 for (size_t i = 0; i < params.size(); i++) {
1693 params[i] = vpIoTools::trim(params[i]);
1694 }
1695
1696 if (!params.empty()) {
1697 // Get the loaded model pathname
1698 std::string headerPathRead = params[0];
1699 headerPathRead = headerPathRead.substr(1);
1700 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of("\""));
1701
1702 std::string headerPath = headerPathRead;
1703 if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
1704 std::string parentDirectory = vpIoTools::getParent(modelFile);
1705 headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
1706 }
1707
1708 // Normalize path
1709 headerPath = vpIoTools::path(headerPath);
1710
1711 // Get real path
1712 headerPath = vpIoTools::getAbsolutePathname(headerPath);
1713
1714 vpHomogeneousMatrix oTo_local;
1716 vpThetaUVector tu;
1717 for (size_t i = 1; i < params.size(); i++) {
1718 std::string param = params[i];
1719 {
1720 const std::string prefix = "t=[";
1721 if (!param.compare(0, prefix.size(), prefix)) {
1722 param = param.substr(prefix.size());
1723 param = param.substr(0, param.find_first_of("]"));
1724
1725 std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1726 if (values.size() == 3) {
1727 t[0] = atof(values[0].c_str());
1728 t[1] = atof(values[1].c_str());
1729 t[2] = atof(values[2].c_str());
1730 }
1731 }
1732 }
1733 {
1734 const std::string prefix = "tu=[";
1735 if (!param.compare(0, prefix.size(), prefix)) {
1736 param = param.substr(prefix.size());
1737 param = param.substr(0, param.find_first_of("]"));
1738
1739 std::vector<std::string> values = vpIoTools::splitChain(param, ";");
1740 if (values.size() == 3) {
1741 for (size_t j = 0; j < values.size(); j++) {
1742 std::string value = values[j];
1743 bool radian = true;
1744 size_t unitPos = value.find("deg");
1745 if (unitPos != std::string::npos) {
1746 value = value.substr(0, unitPos);
1747 radian = false;
1748 }
1749
1750 unitPos = value.find("rad");
1751 if (unitPos != std::string::npos) {
1752 value = value.substr(0, unitPos);
1753 }
1754 tu[static_cast<unsigned int>(j)] = !radian ? vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1755 }
1756 }
1757 }
1758 }
1759 }
1760 oTo_local.buildFrom(t, tu);
1761
1762 bool cyclic = false;
1763 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1764 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1765 if (headerPath == *it) {
1766 cyclic = true;
1767 }
1768 }
1769
1770 if (!cyclic) {
1771 if (vpIoTools::checkFilename(headerPath)) {
1772 header = true;
1773 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, odTo * oTo_local);
1774 } else {
1775 throw vpException(vpException::ioError, "file cannot be open");
1776 }
1777 } else {
1778 std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
1779 << std::endl;
1780 }
1781 }
1782 }
1783
1784 removeComment(fileId);
1785 fileId.get(c);
1786 fileId.unget();
1787 }
1788
1790 unsigned int caoNbrPoint;
1791 fileId >> caoNbrPoint;
1792 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1793
1794 nbPoints += caoNbrPoint;
1795 if (verbose || (parent && !header)) {
1796#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1797 std::lock_guard<std::mutex> lock(g_mutex_cout);
1798#endif
1799 std::cout << "> " << caoNbrPoint << " points" << std::endl;
1800 }
1801
1802 if (caoNbrPoint > 100000) {
1803 throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
1804 }
1805
1806 if (caoNbrPoint == 0 && !header) {
1807 throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
1808 }
1809 vpPoint *caoPoints = new vpPoint[caoNbrPoint];
1810
1811 int i; // image coordinate (used for matching)
1812 int j;
1813
1814 for (unsigned int k = 0; k < caoNbrPoint; k++) {
1815 removeComment(fileId);
1816
1817 vpColVector pt_3d(4, 1.0);
1818 fileId >> pt_3d[0];
1819 fileId >> pt_3d[1];
1820 fileId >> pt_3d[2];
1821
1822 if (caoVersion == 2) {
1823 fileId >> i;
1824 fileId >> j;
1825 }
1826
1827 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1828
1829 vpColVector pt_3d_tf = odTo * pt_3d;
1830 caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1831 }
1832
1833 removeComment(fileId);
1834
1836 // Store in a map the potential segments to add
1837 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1838 unsigned int caoNbrLine;
1839 fileId >> caoNbrLine;
1840 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1841
1842 nbLines += caoNbrLine;
1843 unsigned int *caoLinePoints = NULL;
1844 if (verbose || (parent && !header)) {
1845#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1846 std::lock_guard<std::mutex> lock(g_mutex_cout);
1847#endif
1848 std::cout << "> " << caoNbrLine << " lines" << std::endl;
1849 }
1850
1851 if (caoNbrLine > 100000) {
1852 delete[] caoPoints;
1853 throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
1854 }
1855
1856 if (caoNbrLine > 0)
1857 caoLinePoints = new unsigned int[2 * caoNbrLine];
1858
1859 unsigned int index1, index2;
1860 // Initialization of idFace with startIdFace for dealing with recursive
1861 // load in header
1862 int idFace = startIdFace;
1863
1864 for (unsigned int k = 0; k < caoNbrLine; k++) {
1865 removeComment(fileId);
1866
1867 fileId >> index1;
1868 fileId >> index2;
1869
1871 // Get the end of the line
1872 std::string endLine = "";
1873 if (safeGetline(fileId, endLine).good()) {
1874 std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1875
1876 std::string segmentName = "";
1877 double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
1878 bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1879 if (mapOfParams.find("name") != mapOfParams.end()) {
1880 segmentName = mapOfParams["name"];
1881 }
1882 if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
1883 minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
1884 }
1885 if (mapOfParams.find("useLod") != mapOfParams.end()) {
1886 useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
1887 }
1888
1889 SegmentInfo segmentInfo;
1890 segmentInfo.name = segmentName;
1891 segmentInfo.useLod = useLod;
1892 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1893
1894 caoLinePoints[2 * k] = index1;
1895 caoLinePoints[2 * k + 1] = index2;
1896
1897 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1898 std::vector<vpPoint> extremities;
1899 extremities.push_back(caoPoints[index1]);
1900 extremities.push_back(caoPoints[index2]);
1901 segmentInfo.extremities = extremities;
1902
1903 std::pair<unsigned int, unsigned int> key(index1, index2);
1904
1905 segmentTemporaryMap[key] = segmentInfo;
1906 } else {
1907 vpTRACE(" line %d has wrong coordinates.", k);
1908 }
1909 }
1910 }
1911
1912 removeComment(fileId);
1913
1915 /* Load polygon from the lines extracted earlier (the first point of the
1916 * line is used)*/
1917 // Store in a vector the indexes of the segments added in the face segment
1918 // case
1919 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1920 unsigned int caoNbrPolygonLine;
1921 fileId >> caoNbrPolygonLine;
1922 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
1923
1924 nbPolygonLines += caoNbrPolygonLine;
1925 if (verbose || (parent && !header)) {
1926#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1927 std::lock_guard<std::mutex> lock(g_mutex_cout);
1928#endif
1929 std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
1930 }
1931
1932 if (caoNbrPolygonLine > 100000) {
1933 delete[] caoPoints;
1934 delete[] caoLinePoints;
1935 throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
1936 }
1937
1938 unsigned int index;
1939 for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1940 removeComment(fileId);
1941
1942 unsigned int nbLinePol;
1943 fileId >> nbLinePol;
1944 std::vector<vpPoint> corners;
1945 if (nbLinePol > 100000) {
1946 throw vpException(vpException::badValue, "Exceed the max number of lines.");
1947 }
1948
1949 for (unsigned int n = 0; n < nbLinePol; n++) {
1950 fileId >> index;
1951
1952 if (index >= caoNbrLine) {
1953 throw vpException(vpException::badValue, "Exceed the max number of lines.");
1954 }
1955 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
1956 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
1957
1958 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
1959 faceSegmentKeyVector.push_back(key);
1960 }
1961
1963 // Get the end of the line
1964 std::string endLine = "";
1965 if (safeGetline(fileId, endLine).good()) {
1966 std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
1967
1968 std::string polygonName = "";
1969 bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
1970 double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
1971 if (mapOfParams.find("name") != mapOfParams.end()) {
1972 polygonName = mapOfParams["name"];
1973 }
1974 if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
1975 minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
1976 }
1977 if (mapOfParams.find("useLod") != mapOfParams.end()) {
1978 useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
1979 }
1980
1981 addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
1982 initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
1983
1984 addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold,
1987 }
1988 }
1989
1990 // Add the segments which were not already added in the face segment case
1991 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
1992 it != segmentTemporaryMap.end(); ++it) {
1993 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
1994 faceSegmentKeyVector.end()) {
1995 addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
1996 it->second.minLineLengthThresh);
1997 initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
1998
1999 addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod,
2000 minPolygonAreaThresholdGeneral, it->second.minLineLengthThresh);
2002 }
2003 }
2004
2005 removeComment(fileId);
2006
2008 /* Extract the polygon using the point coordinates (top of the file) */
2009 unsigned int caoNbrPolygonPoint;
2010 fileId >> caoNbrPolygonPoint;
2011 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2012
2013 nbPolygonPoints += caoNbrPolygonPoint;
2014 if (verbose || (parent && !header)) {
2015#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2016 std::lock_guard<std::mutex> lock(g_mutex_cout);
2017#endif
2018 std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
2019 }
2020
2021 if (caoNbrPolygonPoint > 100000) {
2022 throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
2023 }
2024
2025 for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2026 removeComment(fileId);
2027
2028 unsigned int nbPointPol;
2029 fileId >> nbPointPol;
2030 if (nbPointPol > 100000) {
2031 throw vpException(vpException::badValue, "Exceed the max number of points.");
2032 }
2033 std::vector<vpPoint> corners;
2034 for (unsigned int n = 0; n < nbPointPol; n++) {
2035 fileId >> index;
2036 if (index > caoNbrPoint - 1) {
2037 throw vpException(vpException::badValue, "Exceed the max number of points.");
2038 }
2039 corners.push_back(caoPoints[index]);
2040 }
2041
2043 // Get the end of the line
2044 std::string endLine = "";
2045 if (safeGetline(fileId, endLine).good()) {
2046 std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2047
2048 std::string polygonName = "";
2049 bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2050 double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2051 if (mapOfParams.find("name") != mapOfParams.end()) {
2052 polygonName = mapOfParams["name"];
2053 }
2054 if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2055 minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2056 }
2057 if (mapOfParams.find("useLod") != mapOfParams.end()) {
2058 useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2059 }
2060
2061 addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
2062 initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2063
2064 addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold,
2067 }
2068 }
2069
2071 unsigned int caoNbCylinder;
2072 try {
2073 removeComment(fileId);
2074
2075 if (fileId.eof()) { // check if not at the end of the file (for old
2076 // style files)
2077 delete[] caoPoints;
2078 delete[] caoLinePoints;
2079 return;
2080 }
2081
2082 /* Extract the cylinders */
2083 fileId >> caoNbCylinder;
2084 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2085
2086 nbCylinders += caoNbCylinder;
2087 if (verbose || (parent && !header)) {
2088#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2089 std::lock_guard<std::mutex> lock(g_mutex_cout);
2090#endif
2091 std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
2092 }
2093
2094 if (caoNbCylinder > 100000) {
2095 throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
2096 }
2097
2098 for (unsigned int k = 0; k < caoNbCylinder; ++k) {
2099 removeComment(fileId);
2100
2101 double radius;
2102 unsigned int indexP1, indexP2;
2103 fileId >> indexP1;
2104 fileId >> indexP2;
2105 fileId >> radius;
2106
2108 // Get the end of the line
2109 std::string endLine = "";
2110 if (safeGetline(fileId, endLine).good()) {
2111 std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2112
2113 std::string polygonName = "";
2114 bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2115 double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
2116 if (mapOfParams.find("name") != mapOfParams.end()) {
2117 polygonName = mapOfParams["name"];
2118 }
2119 if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
2120 minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
2121 }
2122 if (mapOfParams.find("useLod") != mapOfParams.end()) {
2123 useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2124 }
2125
2126 int idRevolutionAxis = idFace;
2127 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2128
2129 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod,
2130 minLineLengthThreshold);
2131
2132 std::vector<std::vector<vpPoint> > listFaces;
2133 createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
2134 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2135
2136 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2137
2138 addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2139 initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2140
2141 idFace += 4;
2142 }
2143 }
2144
2145 } catch (const std::exception &e) {
2146 std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2147 std::cerr << "Exception: " << e.what() << std::endl;
2148 caoNbCylinder = 0;
2149 }
2150
2152 unsigned int caoNbCircle;
2153 try {
2154 removeComment(fileId);
2155
2156 if (fileId.eof()) { // check if not at the end of the file (for old
2157 // style files)
2158 delete[] caoPoints;
2159 delete[] caoLinePoints;
2160 return;
2161 }
2162
2163 /* Extract the circles */
2164 fileId >> caoNbCircle;
2165 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line
2166
2167 nbCircles += caoNbCircle;
2168 if (verbose || (parent && !header)) {
2169#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2170 std::lock_guard<std::mutex> lock(g_mutex_cout);
2171#endif
2172 std::cout << "> " << caoNbCircle << " circles" << std::endl;
2173 }
2174
2175 if (caoNbCircle > 100000) {
2176 throw vpException(vpException::badValue, "Exceed the max number of cicles.");
2177 }
2178
2179 for (unsigned int k = 0; k < caoNbCircle; ++k) {
2180 removeComment(fileId);
2181
2182 double radius;
2183 unsigned int indexP1, indexP2, indexP3;
2184 fileId >> radius;
2185 fileId >> indexP1;
2186 fileId >> indexP2;
2187 fileId >> indexP3;
2188
2190 // Get the end of the line
2191 std::string endLine = "";
2192 if (safeGetline(fileId, endLine).good()) {
2193 std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
2194
2195 std::string polygonName = "";
2196 bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
2197 double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
2198 if (mapOfParams.find("name") != mapOfParams.end()) {
2199 polygonName = mapOfParams["name"];
2200 }
2201 if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
2202 minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
2203 }
2204 if (mapOfParams.find("useLod") != mapOfParams.end()) {
2205 useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
2206 }
2207
2208 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2209 minPolygonAreaThreshold);
2210
2211 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2212
2213 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace,
2214 polygonName, useLod, minPolygonAreaThreshold);
2215 initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++,
2216 polygonName);
2217 }
2218 }
2219
2220 } catch (const std::exception &e) {
2221 std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
2222 std::cerr << "Exception: " << e.what() << std::endl;
2223 caoNbCircle = 0;
2224 }
2225
2226 startIdFace = idFace;
2227
2228 delete[] caoPoints;
2229 delete[] caoLinePoints;
2230
2231 if (header && parent) {
2232 if (verbose) {
2233#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2234 std::lock_guard<std::mutex> lock(g_mutex_cout);
2235#endif
2236 std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
2237 std::cout << "Total nb of points : " << nbPoints << std::endl;
2238 std::cout << "Total nb of lines : " << nbLines << std::endl;
2239 std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
2240 std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
2241 std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
2242 std::cout << "Total nb of circles : " << nbCircles << std::endl;
2243 } else {
2244#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2245 std::lock_guard<std::mutex> lock(g_mutex_cout);
2246#endif
2247 std::cout << "> " << nbPoints << " points" << std::endl;
2248 std::cout << "> " << nbLines << " lines" << std::endl;
2249 std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
2250 std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
2251 std::cout << "> " << nbCylinders << " cylinders" << std::endl;
2252 std::cout << "> " << nbCircles << " circles" << std::endl;
2253 }
2254 }
2255
2256 // Go up: remove current model
2257 vectorOfModelFilename.pop_back();
2258 } catch (const std::exception &e) {
2259 std::cerr << "Cannot read line!" << std::endl;
2260 std::cerr << "Exception: " << e.what() << std::endl;
2261 throw vpException(vpException::ioError, "cannot read line");
2262 }
2263}
2264
2265#ifdef VISP_HAVE_COIN3D
2273void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
2274{
2275 vpHomogeneousMatrix transformCur;
2276 SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
2277 if (sceneGraphVRML2Trasnform) {
2278 float rx, ry, rz, rw;
2279 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2280 vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
2281 // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
2282 // rw << std::endl;
2283
2284 float tx, ty, tz;
2285 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2286 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2287 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2288 vpTranslationVector transVec(tx, ty, tz);
2289 // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
2290 // std::endl;
2291
2292 float sx, sy, sz;
2293 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2294 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2295 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2296 // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
2297 // std::endl;
2298
2299 for (unsigned int i = 0; i < 3; i++)
2300 rotMat[0][i] *= sx;
2301 for (unsigned int i = 0; i < 3; i++)
2302 rotMat[1][i] *= sy;
2303 for (unsigned int i = 0; i < 3; i++)
2304 rotMat[2][i] *= sz;
2305
2306 transformCur = vpHomogeneousMatrix(transVec, rotMat);
2307 transform = transform * transformCur;
2308 }
2309
2310 int nbShapes = sceneGraphVRML2->getNumChildren();
2311 // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
2312 // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
2313 // std::endl;
2314
2315 SoNode *child;
2316
2317 for (int i = 0; i < nbShapes; i++) {
2318 vpHomogeneousMatrix transform_recursive(transform);
2319 child = sceneGraphVRML2->getChild(i);
2320
2321 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2322 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2323 }
2324
2325 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2326 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2327 }
2328
2329 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2330 SoChildList *child2list = child->getChildren();
2331 std::string name = child->getName().getString();
2332
2333 for (int j = 0; j < child2list->getLength(); j++) {
2334 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2335 SoVRMLIndexedFaceSet *face_set;
2336 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2337 if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
2338 extractCylinders(face_set, transform, idFace, name);
2339 } else {
2340 extractFaces(face_set, transform, idFace, name);
2341 }
2342 }
2343 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2344 SoVRMLIndexedLineSet *line_set;
2345 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2346 extractLines(line_set, idFace, name);
2347 }
2348 }
2349 }
2350 }
2351}
2352
2362void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2363 const std::string &polygonName)
2364{
2365 std::vector<vpPoint> corners;
2366
2367 // SoMFInt32 indexList = _face_set->coordIndex;
2368 // int indexListSize = indexList.getNum();
2369 int indexListSize = face_set->coordIndex.getNum();
2370
2371 vpColVector pointTransformed(4);
2372 vpPoint pt;
2373 SoVRMLCoordinate *coord;
2374
2375 for (int i = 0; i < indexListSize; i++) {
2376 if (face_set->coordIndex[i] == -1) {
2377 if (corners.size() > 1) {
2378 addPolygon(corners, idFace, polygonName);
2379 initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2380
2381 addProjectionErrorPolygon(corners, idFace++, polygonName);
2383 corners.resize(0);
2384 }
2385 } else {
2386 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2387 int index = face_set->coordIndex[i];
2388 pointTransformed[0] = coord->point[index].getValue()[0];
2389 pointTransformed[1] = coord->point[index].getValue()[1];
2390 pointTransformed[2] = coord->point[index].getValue()[2];
2391 pointTransformed[3] = 1.0;
2392
2393 pointTransformed = transform * pointTransformed;
2394
2395 pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2396 corners.push_back(pt);
2397 }
2398 }
2399}
2400
2415void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
2416 const std::string &polygonName)
2417{
2418 std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
2419 // first circle and to the
2420 // second one.
2421 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2422
2423 unsigned int indexListSize = (unsigned int)coords->point.getNum();
2424
2425 if (indexListSize % 2 == 1) {
2426 std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
2427 throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
2428 }
2429 corners_c1.resize(indexListSize / 2);
2430 corners_c2.resize(indexListSize / 2);
2431 vpColVector pointTransformed(4);
2432 vpPoint pt;
2433
2434 // extract all points and fill the two sets.
2435
2436 for (int i = 0; i < coords->point.getNum(); ++i) {
2437 pointTransformed[0] = coords->point[i].getValue()[0];
2438 pointTransformed[1] = coords->point[i].getValue()[1];
2439 pointTransformed[2] = coords->point[i].getValue()[2];
2440 pointTransformed[3] = 1.0;
2441
2442 pointTransformed = transform * pointTransformed;
2443
2444 pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
2445
2446 if (i < (int)corners_c1.size()) {
2447 corners_c1[(unsigned int)i] = pt;
2448 } else {
2449 corners_c2[(unsigned int)i - corners_c1.size()] = pt;
2450 }
2451 }
2452
2453 vpPoint p1 = getGravityCenter(corners_c1);
2454 vpPoint p2 = getGravityCenter(corners_c2);
2455
2456 vpColVector dist(3);
2457 dist[0] = p1.get_oX() - corners_c1[0].get_oX();
2458 dist[1] = p1.get_oY() - corners_c1[0].get_oY();
2459 dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
2460 double radius_c1 = sqrt(dist.sumSquare());
2461 dist[0] = p2.get_oX() - corners_c2[0].get_oX();
2462 dist[1] = p2.get_oY() - corners_c2[0].get_oY();
2463 dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
2464 double radius_c2 = sqrt(dist.sumSquare());
2465
2466 if (std::fabs(radius_c1 - radius_c2) >
2467 (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
2468 std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
2469 throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
2470 }
2471
2472 // addPolygon(p1, p2, idFace, polygonName);
2473 // initCylinder(p1, p2, radius_c1, idFace++);
2474
2475 int idRevolutionAxis = idFace;
2476 addPolygon(p1, p2, idFace, polygonName);
2477
2478 addProjectionErrorPolygon(p1, p2, idFace++, polygonName);
2479
2480 std::vector<std::vector<vpPoint> > listFaces;
2481 createCylinderBBox(p1, p2, radius_c1, listFaces);
2482 addPolygon(listFaces, idFace, polygonName);
2483
2484 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2485
2486 addProjectionErrorPolygon(listFaces, idFace, polygonName);
2487 initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2488
2489 idFace += 4;
2490}
2491
2500void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
2501{
2502 std::vector<vpPoint> corners;
2503 corners.resize(0);
2504
2505 int indexListSize = line_set->coordIndex.getNum();
2506
2507 SbVec3f point(0, 0, 0);
2508 vpPoint pt;
2509 SoVRMLCoordinate *coord;
2510
2511 for (int i = 0; i < indexListSize; i++) {
2512 if (line_set->coordIndex[i] == -1) {
2513 if (corners.size() > 1) {
2514 addPolygon(corners, idFace, polygonName);
2515 initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
2516
2517 addProjectionErrorPolygon(corners, idFace++, polygonName);
2519 corners.resize(0);
2520 }
2521 } else {
2522 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2523 int index = line_set->coordIndex[i];
2524 point[0] = coord->point[index].getValue()[0];
2525 point[1] = coord->point[index].getValue()[1];
2526 point[2] = coord->point[index].getValue()[2];
2527
2528 pt.setWorldCoordinates(point[0], point[1], point[2]);
2529 corners.push_back(pt);
2530 }
2531 }
2532}
2533
2534#endif // VISP_HAVE_COIN3D
2535
2545vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
2546{
2547 if (pts.empty()) {
2548 std::cout << "Cannot extract center of gravity of empty set." << std::endl;
2549 throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
2550 }
2551 double oX = 0;
2552 double oY = 0;
2553 double oZ = 0;
2554 vpPoint G;
2555
2556 for (unsigned int i = 0; i < pts.size(); ++i) {
2557 oX += pts[i].get_oX();
2558 oY += pts[i].get_oY();
2559 oZ += pts[i].get_oZ();
2560 }
2561
2562 G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
2563 return G;
2564}
2565
2578std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2579vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
2580{
2581 // Temporary variable to permit to order polygons by distance
2582 std::vector<vpPolygon> polygonsTmp;
2583 std::vector<std::vector<vpPoint> > roisPtTmp;
2584
2585 // Pair containing the list of vpPolygon and the list of face corners
2586 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2587
2588 for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
2589 // A face has at least 3 points
2590 if (faces.getPolygon()[i]->nbpt > 2) {
2591 if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
2592 std::vector<vpImagePoint> roiPts;
2593
2594 if (clipPolygon) {
2595 faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo);
2596 } else {
2597 roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo);
2598 }
2599
2600 if (roiPts.size() <= 2) {
2601 continue;
2602 }
2603
2604 polygonsTmp.push_back(vpPolygon(roiPts));
2605
2606 std::vector<vpPoint> polyPts;
2607 if (clipPolygon) {
2608 faces.getPolygon()[i]->getPolygonClipped(polyPts);
2609 } else {
2610 for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
2611 polyPts.push_back(faces.getPolygon()[i]->p[j]);
2612 }
2613 }
2614 roisPtTmp.push_back(polyPts);
2615 }
2616 }
2617 }
2618
2619 if (orderPolygons) {
2620 // Order polygons by distance (near to far)
2621 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2622 for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
2623 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2624 for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2625 x_centroid += roisPtTmp[i][j].get_X();
2626 y_centroid += roisPtTmp[i][j].get_Y();
2627 z_centroid += roisPtTmp[i][j].get_Z();
2628 }
2629
2630 x_centroid /= roisPtTmp[i].size();
2631 y_centroid /= roisPtTmp[i].size();
2632 z_centroid /= roisPtTmp[i].size();
2633
2634 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2635 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2636 }
2637
2638 // Sort the list of polygon faces
2639 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2640
2641 polygonsTmp.resize(listOfPolygonFaces.size());
2642 roisPtTmp.resize(listOfPolygonFaces.size());
2643
2644 size_t cpt = 0;
2645 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2646 ++it, cpt++) {
2647 polygonsTmp[cpt] = it->polygon;
2648 roisPtTmp[cpt] = it->faceCorners;
2649 }
2650
2651 pairOfPolygonFaces.first = polygonsTmp;
2652 pairOfPolygonFaces.second = roisPtTmp;
2653 } else {
2654 pairOfPolygonFaces.first = polygonsTmp;
2655 pairOfPolygonFaces.second = roisPtTmp;
2656 }
2657
2658 return pairOfPolygonFaces;
2659}
2660
2670{
2671 useOgre = v;
2672 if (useOgre) {
2673#ifndef VISP_HAVE_OGRE
2674 useOgre = false;
2675 std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2676 "will be used. setOgreVisibilityTest() set to false."
2677 << std::endl;
2678#endif
2679 }
2680}
2681
2688{
2690 vpTRACE("Far clipping value cannot be inferior than near clipping value. "
2691 "Far clipping won't be considered.");
2692 else if (dist < 0)
2693 vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
2694 "won't be considered.");
2695 else {
2697 distFarClip = dist;
2698 for (unsigned int i = 0; i < faces.size(); i++) {
2699 faces[i]->setFarClippingDistance(distFarClip);
2700 }
2701#ifdef VISP_HAVE_OGRE
2703#endif
2704 }
2705}
2706
2717void vpMbTracker::setLod(bool useLod, const std::string &name)
2718{
2719 for (unsigned int i = 0; i < faces.size(); i++) {
2720 if (name.empty() || faces[i]->name == name) {
2721 faces[i]->setLod(useLod);
2722 }
2723 }
2724}
2725
2735void vpMbTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
2736{
2737 for (unsigned int i = 0; i < faces.size(); i++) {
2738 if (name.empty() || faces[i]->name == name) {
2739 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2740 }
2741 }
2742}
2743
2752void vpMbTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
2753{
2754 for (unsigned int i = 0; i < faces.size(); i++) {
2755 if (name.empty() || faces[i]->name == name) {
2756 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2757 }
2758 }
2759}
2760
2767{
2769 vpTRACE("Near clipping value cannot be superior than far clipping value. "
2770 "Near clipping won't be considered.");
2771 else if (dist < 0)
2772 vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
2773 "won't be considered.");
2774 else {
2776 distNearClip = dist;
2777 for (unsigned int i = 0; i < faces.size(); i++) {
2778 faces[i]->setNearClippingDistance(distNearClip);
2779 }
2780#ifdef VISP_HAVE_OGRE
2782#endif
2783 }
2784}
2785
2793void vpMbTracker::setClipping(const unsigned int &flags)
2794{
2795 clippingFlag = flags;
2796 for (unsigned int i = 0; i < faces.size(); i++)
2798}
2799
2800void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true,
2801 const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
2802 const vpMatrix &LVJ_true, const vpColVector &error)
2803{
2804 if (computeCovariance) {
2805 vpMatrix D;
2806 D.diag(w_true);
2807
2808 // Note that here the covariance is computed on cMoPrev for time
2809 // computation efficiency
2810 if (isoJoIdentity) {
2811 covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
2812 } else {
2813 covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
2814 }
2815 }
2816}
2817
2830void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
2831{
2832 if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
2833 throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
2834 }
2835
2836 JTR.resize(6, false);
2837
2838 SimdComputeJtR(interaction.data, interaction.getRows(), error.data, JTR.data);
2839}
2840
2842 const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
2843 double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
2844 const vpColVector *const m_w_prev)
2845{
2847 if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
2848 mu *= 10.0;
2849
2850 if (mu > 1.0)
2851 throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
2852
2853 m_cMo = cMoPrev;
2854 error = m_error_prev;
2855 if (w != NULL && m_w_prev != NULL) {
2856 *w = *m_w_prev;
2857 }
2858 reStartFromLastIncrement = true;
2859 }
2860 }
2861}
2862
2863void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix &LTL,
2864 vpColVector &R, const vpColVector &error, vpColVector &error_prev,
2865 vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w,
2866 vpColVector *const m_w_prev)
2867{
2868 if (isoJoIdentity) {
2869 LTL = L.AtA();
2870 computeJTR(L, R, LTR);
2871
2872 switch (m_optimizationMethod) {
2874 vpMatrix LMA(LTL.getRows(), LTL.getCols());
2875 LMA.eye();
2876 vpMatrix LTLmuI = LTL + (LMA * mu);
2877 v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2878
2879 if (iter != 0)
2880 mu /= 10.0;
2881
2882 error_prev = error;
2883 if (w != NULL && m_w_prev != NULL)
2884 *m_w_prev = *w;
2885 break;
2886 }
2887
2889 default:
2890 v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
2891 break;
2892 }
2893 } else {
2895 cVo.buildFrom(m_cMo);
2896 vpMatrix LVJ = (L * (cVo * oJo));
2897 vpMatrix LVJTLVJ = (LVJ).AtA();
2898 vpColVector LVJTR;
2899 computeJTR(LVJ, R, LVJTR);
2900
2901 switch (m_optimizationMethod) {
2903 vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
2904 LMA.eye();
2905 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2906 v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2907 v = cVo * v;
2908
2909 if (iter != 0)
2910 mu /= 10.0;
2911
2912 error_prev = error;
2913 if (w != NULL && m_w_prev != NULL)
2914 *m_w_prev = *w;
2915 break;
2916 }
2918 default:
2919 v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
2920 v = cVo * v;
2921 break;
2922 }
2923 }
2924}
2925
2927{
2928 if (error.getRows() > 0)
2929 robust.MEstimator(vpRobust::TUKEY, error, w);
2930}
2931
2944{
2945 vpColVector v(6);
2946 for (unsigned int i = 0; i < 6; i++)
2947 v[i] = oJo[i][i];
2948 return v;
2949}
2950
2967{
2968 if (v.getRows() == 6) {
2969 m_isoJoIdentity = true;
2970 for (unsigned int i = 0; i < 6; i++) {
2971 // if(v[i] != 0){
2972 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
2973 oJo[i][i] = 1.0;
2974 } else {
2975 oJo[i][i] = 0.0;
2976 m_isoJoIdentity = false;
2977 }
2978 }
2979 }
2980}
2981
2982void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
2983 std::vector<std::vector<vpPoint> > &listFaces)
2984{
2985 listFaces.clear();
2986
2987 // std::vector<vpPoint> revolutionAxis;
2988 // revolutionAxis.push_back(p1);
2989 // revolutionAxis.push_back(p2);
2990 // listFaces.push_back(revolutionAxis);
2991
2992 vpColVector axis(3);
2993 axis[0] = p1.get_oX() - p2.get_oX();
2994 axis[1] = p1.get_oY() - p2.get_oY();
2995 axis[2] = p1.get_oZ() - p2.get_oZ();
2996
2997 vpColVector randomVec(3);
2998 randomVec = 0;
2999
3000 vpColVector axisOrtho(3);
3001
3002 randomVec[0] = 1.0;
3003 axisOrtho = vpColVector::crossProd(axis, randomVec);
3004
3005 if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3006 randomVec = 0;
3007 randomVec[1] = 1.0;
3008 axisOrtho = vpColVector::crossProd(axis, randomVec);
3009 if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3010 randomVec = 0;
3011 randomVec[2] = 1.0;
3012 axisOrtho = vpColVector::crossProd(axis, randomVec);
3013 if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon())
3014 throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
3015 }
3016 }
3017
3018 axisOrtho.normalize();
3019
3020 vpColVector axisOrthoBis(3);
3021 axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
3022 axisOrthoBis.normalize();
3023
3024 // First circle
3025 vpColVector p1Vec(3);
3026 p1Vec[0] = p1.get_oX();
3027 p1Vec[1] = p1.get_oY();
3028 p1Vec[2] = p1.get_oZ();
3029 vpColVector fc1 = p1Vec + axisOrtho * radius;
3030 vpColVector fc2 = p1Vec + axisOrthoBis * radius;
3031 vpColVector fc3 = p1Vec - axisOrtho * radius;
3032 vpColVector fc4 = p1Vec - axisOrthoBis * radius;
3033
3034 vpColVector p2Vec(3);
3035 p2Vec[0] = p2.get_oX();
3036 p2Vec[1] = p2.get_oY();
3037 p2Vec[2] = p2.get_oZ();
3038 vpColVector sc1 = p2Vec + axisOrtho * radius;
3039 vpColVector sc2 = p2Vec + axisOrthoBis * radius;
3040 vpColVector sc3 = p2Vec - axisOrtho * radius;
3041 vpColVector sc4 = p2Vec - axisOrthoBis * radius;
3042
3043 std::vector<vpPoint> pointsFace;
3044 pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3045 pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3046 pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3047 pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3048 listFaces.push_back(pointsFace);
3049
3050 pointsFace.clear();
3051 pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
3052 pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
3053 pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3054 pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3055 listFaces.push_back(pointsFace);
3056
3057 pointsFace.clear();
3058 pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
3059 pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
3060 pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3061 pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3062 listFaces.push_back(pointsFace);
3063
3064 pointsFace.clear();
3065 pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
3066 pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
3067 pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
3068 pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
3069 listFaces.push_back(pointsFace);
3070}
3071
3081bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
3082{
3083 double dx = fabs(P1.get_oX() - P2.get_oX());
3084 double dy = fabs(P1.get_oY() - P2.get_oY());
3085 double dz = fabs(P1.get_oZ() - P2.get_oZ());
3086
3087 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3088 dz <= std::numeric_limits<double>::epsilon())
3089 return true;
3090 else
3091 return false;
3092}
3093
3094void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, int idFace,
3095 const std::string &polygonName, bool useLod, double minPolygonAreaThreshold,
3096 double minLineLengthThreshold)
3097{
3098 std::vector<vpPoint> corners_without_duplicates;
3099 corners_without_duplicates.push_back(corners[0]);
3100 for (unsigned int i = 0; i < corners.size() - 1; i++) {
3101 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3102 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3103 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3104 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3105 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3106 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3107 corners_without_duplicates.push_back(corners[i + 1]);
3108 }
3109 }
3110
3111 vpMbtPolygon polygon;
3112 polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
3113 polygon.setIndex((int)idFace);
3114 polygon.setName(polygonName);
3115 polygon.setLod(useLod);
3116
3117 polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3118 polygon.setMinLineLengthThresh(minLineLengthThreshold);
3119
3120 for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3121 polygon.addPoint(j, corners_without_duplicates[j]);
3122 }
3123
3125
3127 m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3128
3130 m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3131
3133 m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3134}
3135
3136void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3137 int idFace, const std::string &polygonName, bool useLod,
3138 double minPolygonAreaThreshold)
3139{
3140 vpMbtPolygon polygon;
3141 polygon.setNbPoint(4);
3142 polygon.setName(polygonName);
3143 polygon.setLod(useLod);
3144
3145 polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
3146 // Non sense to set minLineLengthThreshold for circle
3147 // but used to be coherent when applying LOD settings for all polygons
3149
3150 {
3151 // Create the 4 points of the circle bounding box
3152 vpPlane plane(p1, p2, p3, vpPlane::object_frame);
3153
3154 // Matrice de passage entre world et circle frame
3155 double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
3156 vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
3157 double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
3158 vpRotationMatrix wRc;
3159 vpColVector x(3), y(3), z(3);
3160 // X axis is P2-P1
3161 x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
3162 x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
3163 x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
3164 // Y axis is the normal of the plane
3165 y[0] = plane.getA() / norm_Y;
3166 y[1] = plane.getB() / norm_Y;
3167 y[2] = plane.getC() / norm_Y;
3168 // Z axis = X ^ Y
3169 z = vpColVector::crossProd(x, y);
3170 for (unsigned int i = 0; i < 3; i++) {
3171 wRc[i][0] = x[i];
3172 wRc[i][1] = y[i];
3173 wRc[i][2] = z[i];
3174 }
3175
3176 vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
3177 vpHomogeneousMatrix wMc(wtc, wRc);
3178
3179 vpColVector c_p(4); // A point in the circle frame that is on the bbox
3180 c_p[0] = radius;
3181 c_p[1] = 0;
3182 c_p[2] = radius;
3183 c_p[3] = 1;
3184
3185 // Matrix to rotate a point by 90 deg around Y in the circle frame
3186 for (unsigned int i = 0; i < 4; i++) {
3187 vpColVector w_p(4); // A point in the word frame
3189 w_p = wMc * cMc_90 * c_p;
3190
3191 vpPoint w_P;
3192 w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
3193
3194 polygon.addPoint(i, w_P);
3195 }
3196 }
3197
3198 polygon.setIndex(idFace);
3200
3202 m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3203
3205 m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3206
3208 m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3209}
3210
3211void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace,
3212 const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3213{
3214 // A polygon as a single line that corresponds to the revolution axis of the
3215 // cylinder
3216 vpMbtPolygon polygon;
3217 polygon.setNbPoint(2);
3218
3219 polygon.addPoint(0, p1);
3220 polygon.addPoint(1, p2);
3221
3222 polygon.setIndex(idFace);
3223 polygon.setName(polygonName);
3224 polygon.setLod(useLod);
3225
3226 polygon.setMinLineLengthThresh(minLineLengthThreshold);
3227 // Non sense to set minPolygonAreaThreshold for cylinder
3228 // but used to be coherent when applying LOD settings for all polygons
3230
3232
3234 m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3235
3237 m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3238
3240 m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3241}
3242
3243void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
3244 const std::string &polygonName, bool useLod, double minLineLengthThreshold)
3245{
3246 int id = idFace;
3247 for (unsigned int i = 0; i < listFaces.size(); i++) {
3248 vpMbtPolygon polygon;
3249 polygon.setNbPoint((unsigned int)listFaces[i].size());
3250 for (unsigned int j = 0; j < listFaces[i].size(); j++)
3251 polygon.addPoint(j, listFaces[i][j]);
3252
3253 polygon.setIndex(id);
3254 polygon.setName(polygonName);
3255 polygon.setIsPolygonOriented(false);
3256 polygon.setLod(useLod);
3257 polygon.setMinLineLengthThresh(minLineLengthThreshold);
3259
3261
3263 m_projectionErrorFaces.getPolygon().back()->setClipping(clippingFlag);
3264
3266 m_projectionErrorFaces.getPolygon().back()->setNearClippingDistance(distNearClip);
3267
3269 m_projectionErrorFaces.getPolygon().back()->setFarClippingDistance(distFarClip);
3270
3271 id++;
3272 }
3273}
3274
3275void vpMbTracker::addProjectionErrorLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
3276{
3277 // suppress line already in the model
3278 bool already_here = false;
3280
3281 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3282 it != m_projectionErrorLines.end(); ++it) {
3283 l = *it;
3284 if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
3285 already_here = true;
3286 l->addPolygon(polygon);
3288 }
3289 }
3290
3291 if (!already_here) {
3292 l = new vpMbtDistanceLine;
3293
3295 l->buildFrom(P1, P2, m_rand);
3296 l->addPolygon(polygon);
3300
3301 l->setIndex((unsigned int)m_projectionErrorLines.size());
3302 l->setName(name);
3303
3306
3309
3312
3313 m_projectionErrorLines.push_back(l);
3314 }
3315}
3316
3317void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
3318 int idFace, const std::string &name)
3319{
3320 bool already_here = false;
3322
3323 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3324 it != m_projectionErrorCircles.end(); ++it) {
3325 ci = *it;
3326 if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
3327 (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
3328 already_here =
3329 (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
3330 }
3331 }
3332
3333 if (!already_here) {
3334 ci = new vpMbtDistanceCircle;
3335
3337 ci->buildFrom(P1, P2, P3, r);
3339 ci->setIndex((unsigned int)m_projectionErrorCircles.size());
3340 ci->setName(name);
3341 ci->index_polygon = idFace;
3343
3344 m_projectionErrorCircles.push_back(ci);
3345 }
3346}
3347
3348void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace,
3349 const std::string &name)
3350{
3351 bool already_here = false;
3353
3354 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3355 it != m_projectionErrorCylinders.end(); ++it) {
3356 cy = *it;
3357 if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
3358 (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
3359 already_here =
3360 (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
3361 }
3362 }
3363
3364 if (!already_here) {
3365 cy = new vpMbtDistanceCylinder;
3366
3368 cy->buildFrom(P1, P2, r);
3370 cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
3371 cy->setName(name);
3372 cy->index_polygon = idFace;
3374 m_projectionErrorCylinders.push_back(cy);
3375 }
3376}
3377
3378void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
3379 int idFace, const std::string &name)
3380{
3381 addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
3382}
3383
3384void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
3385 const std::string &name)
3386{
3387 addProjectionErrorCylinder(p1, p2, radius, idFace, name);
3388}
3389
3391{
3392 unsigned int nbpt = polygon.getNbPoint();
3393 if (nbpt > 0) {
3394 for (unsigned int i = 0; i < nbpt - 1; i++)
3395 addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3396 addProjectionErrorLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
3397 }
3398}
3399
3401{
3402 unsigned int nbpt = polygon.getNbPoint();
3403 if (nbpt > 0) {
3404 for (unsigned int i = 0; i < nbpt - 1; i++)
3405 addProjectionErrorLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
3406 }
3407}
3408
3427 const vpCameraParameters &_cam)
3428{
3429 if (!modelInitialised) {
3430 throw vpException(vpException::fatalError, "model not initialized");
3431 }
3432
3433 unsigned int nbFeatures = 0;
3434 double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);
3435
3436 if (nbFeatures > 0) {
3437 return vpMath::deg(totalProjectionError / (double)nbFeatures);
3438 }
3439
3440 return 90.0;
3441}
3442
3444 const vpCameraParameters &_cam, unsigned int &nbFeatures)
3445{
3446 bool update_cam = m_projectionErrorCam != _cam;
3447 if (update_cam) {
3448 m_projectionErrorCam = _cam;
3449
3450 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3451 it != m_projectionErrorLines.end(); ++it) {
3452 vpMbtDistanceLine *l = *it;
3454 }
3455
3456 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3457 it != m_projectionErrorCylinders.end(); ++it) {
3458 vpMbtDistanceCylinder *cy = *it;
3460 }
3461
3462 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3463 it != m_projectionErrorCircles.end(); ++it) {
3464 vpMbtDistanceCircle *ci = *it;
3466 }
3467 }
3468
3469#ifdef VISP_HAVE_OGRE
3470 if (useOgre) {
3471 if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
3475 // Turn off Ogre config dialog display for the next call to this
3476 // function since settings are saved in the ogre.cfg file and used
3477 // during the next call
3479 }
3480 }
3481#endif
3482
3483 if (clippingFlag > 2)
3485
3487
3489
3490 if (useScanLine) {
3491 if (clippingFlag <= 2)
3493
3496 }
3497
3499
3500 double totalProjectionError = 0.0;
3501 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3502 it != m_projectionErrorLines.end(); ++it) {
3503 vpMbtDistanceLine *l = *it;
3504 if (l->isVisible() && l->isTracked()) {
3505 for (size_t a = 0; a < l->meline.size(); a++) {
3506 if (l->meline[a] != NULL) {
3507 double lineNormGradient;
3508 unsigned int lineNbFeatures;
3509 l->meline[a]->computeProjectionError(I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
3512 totalProjectionError += lineNormGradient;
3513 nbFeatures += lineNbFeatures;
3514 }
3515 }
3516 }
3517 }
3518
3519 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3520 it != m_projectionErrorCylinders.end(); ++it) {
3521 vpMbtDistanceCylinder *cy = *it;
3522 if (cy->isVisible() && cy->isTracked()) {
3523 if (cy->meline1 != NULL) {
3524 double cylinderNormGradient = 0;
3525 unsigned int cylinderNbFeatures = 0;
3526 cy->meline1->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3529 totalProjectionError += cylinderNormGradient;
3530 nbFeatures += cylinderNbFeatures;
3531 }
3532
3533 if (cy->meline2 != NULL) {
3534 double cylinderNormGradient = 0;
3535 unsigned int cylinderNbFeatures = 0;
3536 cy->meline2->computeProjectionError(I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
3539 totalProjectionError += cylinderNormGradient;
3540 nbFeatures += cylinderNbFeatures;
3541 }
3542 }
3543 }
3544
3545 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3546 it != m_projectionErrorCircles.end(); ++it) {
3547 vpMbtDistanceCircle *c = *it;
3548 if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
3549 double circleNormGradient = 0;
3550 unsigned int circleNbFeatures = 0;
3551 c->meEllipse->computeProjectionError(I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
3554 totalProjectionError += circleNormGradient;
3555 nbFeatures += circleNbFeatures;
3556 }
3557 }
3558
3559 return totalProjectionError;
3560}
3561
3562void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
3563{
3564 bool changed = false;
3565
3566 if (!useOgre) {
3568 changed);
3569 } else {
3570#ifdef VISP_HAVE_OGRE
3572 changed);
3573#else
3575 changed);
3576#endif
3577 }
3578}
3579
3581{
3582 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3583 it != m_projectionErrorLines.end(); ++it) {
3584 for (size_t a = 0; a < (*it)->meline.size(); a++) {
3585 if ((*it)->meline[a] != NULL) {
3586 delete (*it)->meline[a];
3587 (*it)->meline[a] = NULL;
3588 }
3589 }
3590
3591 (*it)->meline.clear();
3592 (*it)->nbFeature.clear();
3593 (*it)->nbFeatureTotal = 0;
3594 }
3595
3596 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3597 it != m_projectionErrorCylinders.end(); ++it) {
3598 if ((*it)->meline1 != NULL) {
3599 delete (*it)->meline1;
3600 (*it)->meline1 = NULL;
3601 }
3602 if ((*it)->meline2 != NULL) {
3603 delete (*it)->meline2;
3604 (*it)->meline2 = NULL;
3605 }
3606
3607 (*it)->nbFeature = 0;
3608 (*it)->nbFeaturel1 = 0;
3609 (*it)->nbFeaturel2 = 0;
3610 }
3611
3612 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3613 it != m_projectionErrorCircles.end(); ++it) {
3614 if ((*it)->meEllipse != NULL) {
3615 delete (*it)->meEllipse;
3616 (*it)->meEllipse = NULL;
3617 }
3618 (*it)->nbFeature = 0;
3619 }
3620}
3621
3623{
3624 const bool doNotTrack = true;
3625
3626 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3627 it != m_projectionErrorLines.end(); ++it) {
3628 vpMbtDistanceLine *l = *it;
3629 bool isvisible = false;
3630
3631 for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
3632 ++itindex) {
3633 int index = *itindex;
3634 if (index == -1)
3635 isvisible = true;
3636 else {
3637 if (l->hiddenface->isVisible((unsigned int)index))
3638 isvisible = true;
3639 }
3640 }
3641
3642 // Si la ligne n'appartient a aucune face elle est tout le temps visible
3643 if (l->Lindex_polygon.empty())
3644 isvisible = true; // Not sure that this can occur
3645
3646 if (isvisible) {
3647 l->setVisible(true);
3648 l->updateTracked();
3649 if (l->meline.empty() && l->isTracked())
3650 l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3651 } else {
3652 l->setVisible(false);
3653 for (size_t a = 0; a < l->meline.size(); a++) {
3654 if (l->meline[a] != NULL)
3655 delete l->meline[a];
3656 if (a < l->nbFeature.size())
3657 l->nbFeature[a] = 0;
3658 }
3659 l->nbFeatureTotal = 0;
3660 l->meline.clear();
3661 l->nbFeature.clear();
3662 }
3663 }
3664
3665 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3666 it != m_projectionErrorCylinders.end(); ++it) {
3667 vpMbtDistanceCylinder *cy = *it;
3668
3669 bool isvisible = false;
3670
3671 int index = cy->index_polygon;
3672 if (index == -1)
3673 isvisible = true;
3674 else {
3675 if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
3676 cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
3677 isvisible = true;
3678 }
3679
3680 if (isvisible) {
3681 cy->setVisible(true);
3682 if (cy->meline1 == NULL || cy->meline2 == NULL) {
3683 if (cy->isTracked())
3684 cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3685 }
3686 } else {
3687 cy->setVisible(false);
3688 if (cy->meline1 != NULL)
3689 delete cy->meline1;
3690 if (cy->meline2 != NULL)
3691 delete cy->meline2;
3692 cy->meline1 = NULL;
3693 cy->meline2 = NULL;
3694 cy->nbFeature = 0;
3695 cy->nbFeaturel1 = 0;
3696 cy->nbFeaturel2 = 0;
3697 }
3698 }
3699
3700 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3701 it != m_projectionErrorCircles.end(); ++it) {
3702 vpMbtDistanceCircle *ci = *it;
3703 bool isvisible = false;
3704
3705 int index = ci->index_polygon;
3706 if (index == -1)
3707 isvisible = true;
3708 else {
3709 if (ci->hiddenface->isVisible((unsigned int)index))
3710 isvisible = true;
3711 }
3712
3713 if (isvisible) {
3714 ci->setVisible(true);
3715 if (ci->meEllipse == NULL) {
3716 if (ci->isTracked())
3717 ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
3718 }
3719 } else {
3720 ci->setVisible(false);
3721 if (ci->meEllipse != NULL)
3722 delete ci->meEllipse;
3723 ci->meEllipse = NULL;
3724 ci->nbFeature = 0;
3725 }
3726 }
3727}
3728
3729void vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose)
3730{
3732 xmlp.setVerbose(verbose);
3735
3736 try {
3737 if (verbose) {
3738 std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl;
3739 }
3740 xmlp.parse(configFile);
3741 } catch (...) {
3742 throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
3743 }
3744
3745 vpMe meParser;
3746 xmlp.getProjectionErrorMe(meParser);
3747
3750}
3751
3758{
3760
3761 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
3762 it != m_projectionErrorLines.end(); ++it) {
3763 vpMbtDistanceLine *l = *it;
3765 }
3766
3767 for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
3768 it != m_projectionErrorCylinders.end(); ++it) {
3769 vpMbtDistanceCylinder *cy = *it;
3771 }
3772
3773 for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
3774 it != m_projectionErrorCircles.end(); ++it) {
3775 vpMbtDistanceCircle *ci = *it;
3777 }
3778}
3779
3785void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
3786{
3788
3789 m_SobelX.resize(size * 2 + 1, size * 2 + 1, false, false);
3791
3792 m_SobelY.resize(size * 2 + 1, size * 2 + 1, false, false);
3794}
void setFarClippingDistance(const double &dist)
Definition vpAROgre.h:199
void setNearClippingDistance(const double &dist)
Definition vpAROgre.h:210
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
vpColVector & normalize()
double sumSquare() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
static const vpColor red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
int getWindowXPosition() const
Definition vpDisplay.h:247
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
int getWindowYPosition() const
Definition vpDisplay.h:252
static void flush(const vpImage< unsigned char > &I)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static FilterType getSobelKernelX(FilterType *filter, unsigned int size)
static FilterType getSobelKernelY(FilterType *filter, unsigned int size)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
vpDisplay * display
Definition vpImage.h:140
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
static std::string path(const std::string &pathname)
static std::string getAbsolutePathname(const std::string &pathname)
static bool checkFilename(const std::string &filename)
static bool isAbsolutePathname(const std::string &pathname)
static std::string trim(std::string s)
static bool parseBoolean(std::string input)
static std::string createFilePath(const std::string &parent, const std::string &child)
static std::string getParent(const std::string &pathname)
static std::string getName(const std::string &pathname)
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition vpMath.h:98
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static double sqr(double x)
Definition vpMath.h:124
static double deg(double rad)
Definition vpMath.h:106
error that can be emitted by the vpMatrix class and its derivatives
@ incorrectMatrixSizeError
Incorrect matrix size.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void eye()
Definition vpMatrix.cpp:446
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int size() const
std::vector< PolygonType * > & getPolygon()
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void addPolygon(PolygonType *p)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpAROgre * getOgreContext()
void setOgreShowConfigDialog(bool showConfigDialog)
Main methods for a model-based tracker.
std::map< std::string, std::string > parseParameters(std::string &endLine)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
double m_lambda
Gain of the virtual visual servoing stage.
virtual vpColVector getEstimatedDoF() const
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
virtual void setEstimatedDoF(const vpColVector &v)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
void removeComment(std::ifstream &fileId)
bool modelInitialised
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
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)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
void projectionErrorResetMovingEdges()
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
@ LEVENBERG_MARQUARDT_OPT
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void init(const vpImage< unsigned char > &I)=0
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
std::map< std::string, std::string > mapOfParameterNames
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void loadVRMLModel(const std::string &modelFile)
unsigned int nbLines
Number of lines in CAO model.
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void savePose(const std::string &filename) const
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
vpMatrix covarianceMatrix
Covariance matrix.
vpHomogeneousMatrix m_cMo
The current pose.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
vpMatrix m_SobelX
Sobel kernel in X.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
unsigned int nbPoints
Number of points in CAO model.
vpCameraParameters m_cam
The camera parameters.
std::string modelFileName
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
virtual void setOgreVisibilityTest(const bool &v)
std::string poseSavingFilename
void setProjectionErrorKernelSize(const unsigned int &size)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
double angleDisappears
Angle used to detect a face disappearance.
virtual void setNearClippingDistance(const double &dist)
void setProjectionErrorMovingEdge(const vpMe &me)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
virtual ~vpMbTracker()
bool useScanLine
Use Scanline for visibility tests.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
vpMatrix m_SobelY
Sobel kernel in Y.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
bool m_projectionErrorOgreShowConfigDialog
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
double distNearClip
Distance for near clipping.
bool m_sodb_init_called
Flag that indicates that SoDB::init(); was called.
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
unsigned int nbCylinders
Number of cylinders in CAO model.
unsigned int clippingFlag
Flags specifying which clipping to used.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
unsigned int nbCircles
Number of circles in CAO model.
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void setCameraParameters(const vpCameraParameters &camera)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
void setIndex(unsigned int i)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Manage a cylinder used in the model-based tracker.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
unsigned int nbFeaturel2
The number of moving edges on line 2.
vpPoint * p2
The second extremity on the axe.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpMbtPolygon & getPolygon()
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
Implementation of a polygon of the model used by the model-based tracker.
void setMinPolygonAreaThresh(double min_polygon_area)
std::string getName() const
void setName(const std::string &face_name)
void setLod(bool use_lod)
virtual void setIndex(int i)
void setMinLineLengthThresh(double min_line_length)
void setIsPolygonOriented(const bool &oriented)
int getIndex() const
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setProjectionErrorMe(const vpMe &me)
unsigned int getProjectionErrorKernelSize() const
void setProjectionErrorKernelSize(const unsigned int &size)
void parse(const std::string &filename)
void getProjectionErrorMe(vpMe &me) const
Definition vpMe.h:122
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Definition vpPlane.h:54
@ object_frame
Definition vpPlane.h:65
double getA() const
Definition vpPlane.h:100
double getC() const
Definition vpPlane.h:104
double getB() const
Definition vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Implements a 3D polygon with render functionalities like clipping.
Definition vpPolygon3D.h:55
void setFarClippingDistance(const double &dist)
unsigned int getNbPoint() const
void setNearClippingDistance(const double &dist)
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
virtual void setNbPoint(unsigned int nb)
void setClipping(const unsigned int &flags)
void addPoint(unsigned int n, const vpPoint &P)
Defines a generic 2D polygon.
Definition vpPolygon.h:97
Implementation of a pose vector and operations on poses.
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
void clearPoint()
Definition vpPose.cpp:125
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
Implementation of a rotation vector as quaternion angle minimal representation.
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388