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