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. This class declares the methods to implement
33 *in order to have a model based tracker.
34 *
35 * Authors:
36 * Romain Tallonneau
37 * Aurelien Yol
38 *
39 *****************************************************************************/
40 #pragma once
41
42 #ifndef vpMbHiddenFaces_HH
43 #define vpMbHiddenFaces_HH
44
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
48 #include <visp3/mbt/vpMbScanLine.h>
49 #include <visp3/mbt/vpMbtPolygon.h>
50
51 #ifdef VISP_HAVE_OGRE
52 #include <visp3/ar/vpAROgre.h>
53 #endif
54
55 #include <limits>
56 #include <vector>
57
58 template <class PolygonType> class vpMbHiddenFaces;
59
60 template <class PolygonType> void swap(vpMbHiddenFaces<PolygonType> &first, vpMbHiddenFaces<PolygonType> &second);
61
62 /*!
63 \class vpMbHiddenFaces
64
65 \brief Implementation of the polygons management for the model-based
66 trackers.
67
68 \ingroup group_mbt_faces
69 */
70 template <class PolygonType = vpMbtPolygon> class vpMbHiddenFaces
71 {
72 private:
73 //! List of polygons
74 std::vector<PolygonType *> Lpol;
75 //! Number of visible polygon
76 unsigned int nbVisiblePolygon;
77 vpMbScanLine scanlineRender;
78
79 #ifdef VISP_HAVE_OGRE
80 vpImage<unsigned char> ogreBackground;
81 bool ogreInitialised;
82 unsigned int nbRayAttempts;
83 double ratioVisibleRay;
84 vpAROgre *ogre;
85 std::vector<Ogre::ManualObject *> lOgrePolygons;
86 bool ogreShowConfigDialog;
87 #endif
88
89 unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears,
90 const double &angleDisappears, bool &changed, bool useOgre = false,
91 bool not_used = false, unsigned int width=0, unsigned int height=0,
92 const vpCameraParameters &cam = vpCameraParameters());
93
94 public:
95 vpMbHiddenFaces();
96 virtual ~vpMbHiddenFaces();
97 vpMbHiddenFaces(const vpMbHiddenFaces ©);
98 vpMbHiddenFaces &operator=(vpMbHiddenFaces other);
99 friend void swap<PolygonType>(vpMbHiddenFaces &first, vpMbHiddenFaces &second);
100
101 void addPolygon(PolygonType *p);
102
103 bool computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
104 bool &changed, bool useOgre, bool not_used, unsigned int width, unsigned int height,
105 const vpCameraParameters &cam, const vpTranslationVector &cameraPos, unsigned int index);
106
107 void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
108
109 void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h);
110
111 void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector<std::pair<vpPoint, vpPoint> > &lines,
112 const bool &displayResults = false);
113
getMbScanLineRenderer()114 vpMbScanLine &getMbScanLineRenderer() { return scanlineRender; }
115
116 #ifdef VISP_HAVE_OGRE
117 void displayOgre(const vpHomogeneousMatrix &cMo);
118 #endif
119
120 /*!
121 Get the list of polygons.
122
123 \return Mbt Klt polygons list.
124 */
getPolygon()125 std::vector<PolygonType *> &getPolygon() { return Lpol; }
126
127 #ifdef VISP_HAVE_OGRE
128 void initOgre(const vpCameraParameters &cam = vpCameraParameters());
129 #endif
130
131 /*!
132 get the number of visible polygons.
133
134 \return number of visible polygons.
135 */
getNbVisiblePolygon()136 unsigned int getNbVisiblePolygon() const { return nbVisiblePolygon; }
137
138 #ifdef VISP_HAVE_OGRE
139 /*!
140 Get the number of rays that will be sent toward each polygon for
141 visibility test. Each ray will go from the optic center of the camera to a
142 random point inside the considered polygon.
143
144 \sa getGoodNbRayCastingAttemptsRatio()
145
146 \return Number of rays sent.
147 */
getNbRayCastingAttemptsForVisibility()148 unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; }
149
150 /*!
151 Get the Ogre3D Context.
152
153 \return A pointer on a vpAROgre instance.
154 */
getOgreContext()155 vpAROgre *getOgreContext() { return ogre; }
156
157 /*!
158 Get the ratio of visibility attempts that has to be successful to consider
159 a polygon as visible.
160
161 \sa getNbRayCastingAttemptsForVisibility()
162
163 \return Ratio of succesful attempts that has to be considered. Value will
164 be between 0.0 (0%) and 1.0 (100%).
165 */
getGoodNbRayCastingAttemptsRatio()166 double getGoodNbRayCastingAttemptsRatio() { return ratioVisibleRay; }
167 #endif
168
isAppearing(unsigned int i)169 bool isAppearing(unsigned int i) { return Lpol[i]->isAppearing(); }
170
171 #ifdef VISP_HAVE_OGRE
172 /*!
173 Tell whether if Ogre Context is initialised or not.
174
175 \return True if it does, false otherwise.
176 */
isOgreInitialised()177 bool isOgreInitialised() { return ogreInitialised; }
178 #endif
179
180 /*!
181 Check if the polygon at position i in the list is visible.
182
183 \param i : TPosition in the list.
184
185 \return Return true if the polygon is visible.
186 */
isVisible(unsigned int i)187 bool isVisible(unsigned int i) { return Lpol[i]->isVisible(); }
188
189 #ifdef VISP_HAVE_OGRE
190 bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
191 #endif
192
193 //! operator[] as modifier.
194 inline PolygonType *operator[](unsigned int i) { return Lpol[i]; }
195 //! operator[] as reader.
196 inline const PolygonType *operator[](unsigned int i) const { return Lpol[i]; }
197
198 void reset();
199
200 #ifdef VISP_HAVE_OGRE
201 /*!
202 Set the background size (by default it is 640x480).
203 The background size has to match with the size of the image that you are
204 using for the traking.
205
206 \warning This function has to be called before initOgre().
207
208 \param h : Height of the background
209 \param w : Width of the background
210 */
setBackgroundSizeOgre(const unsigned int & h,const unsigned int & w)211 void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
212 {
213 ogreBackground = vpImage<unsigned char>(h, w, 0);
214 }
215
216 /*!
217 Set the number of rays that will be sent toward each polygon for
218 visibility test. Each ray will go from the optic center of the camera to a
219 random point inside the considered polygon.
220
221 \sa setGoodNbRayCastingAttemptsRatio(const double &)
222
223 \param attempts Number of rays to be sent.
224 */
setNbRayCastingAttemptsForVisibility(const unsigned int & attempts)225 void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; }
226
227 /*!
228 Set the ratio of visibility attempts that has to be successful to consider
229 a polygon as visible.
230
231 \sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
232
233 \param ratio : Ratio of succesful attempts that has to be considered.
234 Value has to be between 0.0 (0%) and 1.0 (100%).
235 */
setGoodNbRayCastingAttemptsRatio(const double & ratio)236 void setGoodNbRayCastingAttemptsRatio(const double &ratio)
237 {
238 ratioVisibleRay = ratio;
239 if (ratioVisibleRay > 1.0)
240 ratioVisibleRay = 1.0;
241 if (ratioVisibleRay < 0.0)
242 ratioVisibleRay = 0.0;
243 }
244 /*!
245 Enable/Disable the appearance of Ogre config dialog on startup.
246
247 \warning This method has only effect when Ogre is used and Ogre visibility
248 test is enabled using setOgreVisibilityTest() with true parameter.
249
250 \param showConfigDialog : if true, shows Ogre dialog window (used to set
251 Ogre rendering options) when Ogre visibility is enabled. By default, this
252 functionality is turned off.
253 */
setOgreShowConfigDialog(bool showConfigDialog)254 inline void setOgreShowConfigDialog(bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }
255 #endif
256
257 unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
258 const vpHomogeneousMatrix &cMo, const double &angle, bool &changed);
259 unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
260 const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
261 bool &changed);
262 unsigned int setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
263 bool &changed);
264
265 #ifdef VISP_HAVE_OGRE
266 unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam,
267 const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
268 bool &changed);
269 unsigned int setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
270 bool &changed);
271 #endif
272 /*!
273 Get the number of polygons.
274
275 \return Size of the list.
276 */
size()277 inline unsigned int size() const { return (unsigned int)Lpol.size(); }
278 };
279
280 /*!
281 Basic constructor.
282 */
283 template <class PolygonType>
vpMbHiddenFaces()284 vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces() : Lpol(), nbVisiblePolygon(0), scanlineRender()
285 {
286 #ifdef VISP_HAVE_OGRE
287 ogreInitialised = false;
288 nbRayAttempts = 1;
289 ratioVisibleRay = 1.0;
290 ogreShowConfigDialog = false;
291 ogre = new vpAROgre();
292 ogreBackground = vpImage<unsigned char>(480, 640, 0);
293 #endif
294 }
295
296 /*!
297 Basic destructor.
298 */
~vpMbHiddenFaces()299 template <class PolygonType> vpMbHiddenFaces<PolygonType>::~vpMbHiddenFaces()
300 {
301 for (unsigned int i = 0; i < Lpol.size(); i++) {
302 if (Lpol[i] != NULL) {
303 delete Lpol[i];
304 }
305 Lpol[i] = NULL;
306 }
307 Lpol.resize(0);
308
309 #ifdef VISP_HAVE_OGRE
310 if (ogre != NULL) {
311 delete ogre;
312 ogre = NULL;
313 }
314
315 // This is already done by calling "delete ogre"
316 // for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
317 // if (lOgrePolygons[i]!=NULL){
318 // delete lOgrePolygons[i];
319 // }
320 // lOgrePolygons[i] = NULL;
321 // }
322
323 lOgrePolygons.resize(0);
324 #endif
325 }
326
327 /*!
328 \relates vpMbHiddenFaces
329 */
330 template <class PolygonType>
vpMbHiddenFaces(const vpMbHiddenFaces<PolygonType> & copy)331 vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces(const vpMbHiddenFaces<PolygonType> ©)
332 : Lpol(), nbVisiblePolygon(copy.nbVisiblePolygon), scanlineRender(copy.scanlineRender)
333 #ifdef VISP_HAVE_OGRE
334 ,
335 ogreBackground(copy.ogreBackground), ogreInitialised(copy.ogreInitialised), nbRayAttempts(copy.nbRayAttempts),
336 ratioVisibleRay(copy.ratioVisibleRay), ogre(NULL), lOgrePolygons(), ogreShowConfigDialog(copy.ogreShowConfigDialog)
337 #endif
338 {
339 // Copy the list of polygons
340 for (unsigned int i = 0; i < copy.Lpol.size(); i++) {
341 PolygonType *poly = new PolygonType(*copy.Lpol[i]);
342 Lpol.push_back(poly);
343 }
344 }
345
swap(vpMbHiddenFaces<PolygonType> & first,vpMbHiddenFaces<PolygonType> & second)346 template <class PolygonType> void swap(vpMbHiddenFaces<PolygonType> &first, vpMbHiddenFaces<PolygonType> &second)
347 {
348 using std::swap;
349 swap(first.Lpol, second.Lpol);
350 swap(first.nbVisiblePolygon, second.nbVisiblePolygon);
351 swap(first.scanlineRender, second.scanlineRender);
352 #ifdef VISP_HAVE_OGRE
353 swap(first.ogreInitialised, second.ogreInitialised);
354 swap(first.nbRayAttempts, second.nbRayAttempts);
355 swap(first.ratioVisibleRay, second.ratioVisibleRay);
356 swap(first.ogreShowConfigDialog, second.ogreShowConfigDialog);
357 swap(first.ogre, second.ogre);
358 swap(first.ogreBackground, second.ogreBackground);
359 #endif
360 }
361
362 /*!
363 Copy assignment operator.
364 */
365 template <class PolygonType>
366 vpMbHiddenFaces<PolygonType> &vpMbHiddenFaces<PolygonType>::operator=(vpMbHiddenFaces<PolygonType> other)
367 {
368 swap(*this, other);
369
370 return *this;
371 }
372
373 /*!
374 Add a polygon to the list of polygons.
375
376 \param p : The polygon to add.
377 */
addPolygon(PolygonType * p)378 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
379 {
380 PolygonType *p_new = new PolygonType;
381 p_new->index = p->index;
382 p_new->setNbPoint(p->nbpt);
383 p_new->isvisible = p->isvisible;
384 p_new->useLod = p->useLod;
385 p_new->minLineLengthThresh = p->minLineLengthThresh;
386 p_new->minPolygonAreaThresh = p->minPolygonAreaThresh;
387 p_new->setName(p->name);
388 p_new->hasOrientation = p->hasOrientation;
389
390 for (unsigned int i = 0; i < p->nbpt; i++)
391 p_new->p[i] = p->p[i];
392 Lpol.push_back(p_new);
393 }
394
395 /*!
396 Reset the Hidden faces (remove the list of PolygonType)
397 */
reset()398 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::reset()
399 {
400 nbVisiblePolygon = 0;
401 for (unsigned int i = 0; i < Lpol.size(); i++) {
402 if (Lpol[i] != NULL) {
403 delete Lpol[i];
404 }
405 Lpol[i] = NULL;
406 }
407 Lpol.resize(0);
408
409 #ifdef VISP_HAVE_OGRE
410 if (ogre != NULL) {
411 delete ogre;
412 ogre = NULL;
413 }
414
415 // This is already done by calling "delete ogre"
416 // for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
417 // if (lOgrePolygons[i]!=NULL){
418 // delete lOgrePolygons[i];
419 // }
420 // lOgrePolygons[i] = NULL;
421 // }
422
423 lOgrePolygons.resize(0);
424
425 ogreInitialised = false;
426 nbRayAttempts = 1;
427 ratioVisibleRay = 1.0;
428 ogre = new vpAROgre();
429 ogreBackground = vpImage<unsigned char>(480, 640);
430 #endif
431 }
432
433 /*!
434 Compute the clipped points of the polygons that have been added via
435 addPolygon().
436
437 \param cMo : Pose that will be used to clip the polygons.
438 \param cam : Camera parameters that will be used to clip the polygons.
439 */
440 template <class PolygonType>
computeClippedPolygons(const vpHomogeneousMatrix & cMo,const vpCameraParameters & cam)441 void vpMbHiddenFaces<PolygonType>::computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
442 {
443 for (unsigned int i = 0; i < Lpol.size(); i++) {
444 // For fast result we could just clip visible polygons.
445 // However clipping all of them gives us the possibility to return more
446 // information in the scanline visibility results
447 // if(Lpol[i]->isVisible())
448 {
449 Lpol[i]->changeFrame(cMo);
450 Lpol[i]->computePolygonClipped(cam);
451 }
452 }
453 }
454
455 /*!
456 Render the scene in order to perform, later via computeScanLineQuery(),
457 visibility tests.
458
459 \param cam : Camera parameters that will be used to render the scene.
460 \param w : Width of the render window.
461 \param h : Height of the render window.
462 */
463 template <class PolygonType>
computeScanLineRender(const vpCameraParameters & cam,const unsigned int & w,const unsigned int & h)464 void vpMbHiddenFaces<PolygonType>::computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w,
465 const unsigned int &h)
466 {
467 std::vector<std::vector<std::pair<vpPoint, unsigned int> > > polyClipped(Lpol.size());
468 std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> listPolyClipped;
469 std::vector<int> listPolyIndices;
470
471 for (unsigned int i = 0; i < Lpol.size(); i++) {
472 // For fast result we could just use visible polygons.
473 // However using all of them gives us the possibility to return more
474 // information in the scanline visibility results
475 // if(Lpol[i]->isVisible())
476 {
477 polyClipped[i].clear();
478 Lpol[i]->getPolygonClipped(polyClipped[i]);
479 if (polyClipped[i].size() != 0) {
480 listPolyClipped.push_back(&polyClipped[i]);
481 listPolyIndices.push_back(Lpol[i]->getIndex());
482 }
483 }
484 }
485
486 scanlineRender.drawScene(listPolyClipped, listPolyIndices, cam, w, h);
487 }
488
489 /*!
490 Compute Scanline visibility results for a line.
491
492 \warning computeScanLineRender() function has to be called before
493
494 \param a : First point of the line.
495 \param b : Second point of the line.
496 \param lines : Result of the scanline visibility. List of the visible parts
497 of the line. \param displayResults : True if the results have to be
498 displayed. False otherwise
499 */
500 template <class PolygonType>
computeScanLineQuery(const vpPoint & a,const vpPoint & b,std::vector<std::pair<vpPoint,vpPoint>> & lines,const bool & displayResults)501 void vpMbHiddenFaces<PolygonType>::computeScanLineQuery(const vpPoint &a, const vpPoint &b,
502 std::vector<std::pair<vpPoint, vpPoint> > &lines,
503 const bool &displayResults)
504 {
505 scanlineRender.queryLineVisibility(a, b, lines, displayResults);
506 }
507
508 /*!
509 Compute the number of visible polygons.
510
511 \param cMo : The pose of the camera
512 \param angleAppears : Angle used to test the appearance of a face
513 \param angleDisappears : Angle used to test the disappearance of a face
514 \param changed : True if a face appeared, disappeared or too many points
515 have been lost. False otherwise \param useOgre : True if a Ogre is used to
516 test the visibility, False otherwise \param not_used : Unused parameter.
517 \param I : Image used to test if a face is entirely projected in the image.
518 \param cam : Camera parameters.
519
520 \return Return the number of visible polygons
521 */
522 template <class PolygonType>
setVisiblePrivate(const vpHomogeneousMatrix & cMo,const double & angleAppears,const double & angleDisappears,bool & changed,bool useOgre,bool not_used,unsigned int width,unsigned int height,const vpCameraParameters & cam)523 unsigned int vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears,
524 const double &angleDisappears, bool &changed, bool useOgre,
525 bool not_used, unsigned int width, unsigned int height,
526 const vpCameraParameters &cam)
527 {
528 nbVisiblePolygon = 0;
529 changed = false;
530
531 vpTranslationVector cameraPos;
532
533 if (useOgre) {
534 #ifdef VISP_HAVE_OGRE
535 cMo.inverse().extract(cameraPos);
536 ogre->renderOneFrame(ogreBackground, cMo);
537 #else
538 vpTRACE("ViSP doesn't have Ogre3D, simple visibility test used");
539 #endif
540 }
541
542 for (unsigned int i = 0; i < Lpol.size(); i++) {
543 // std::cout << "Calling poly: " << i << std::endl;
544 if (computeVisibility(cMo, angleAppears, angleDisappears, changed, useOgre, not_used, width, height, cam, cameraPos, i))
545 nbVisiblePolygon++;
546 }
547 return nbVisiblePolygon;
548 }
549
550 /*!
551 Compute the visibility of a given face index.
552
553 \param cMo : The pose of the camera
554 \param angleAppears : Angle used to test the appearance of a face
555 \param angleDisappears : Angle used to test the disappearance of a face
556 \param changed : True if a face appeared, disappeared or too many points
557 have been lost. False otherwise
558 \param useOgre : True if a Ogre is used to test the visibility, False otherwise.
559 \param not_used : Unused parameter.
560 \param width, height Image size.
561 \param cam : Camera parameters.
562 \param cameraPos : Position of the camera. Used only when Ogre is used as
563 3rd party.
564 \param index : Index of the face to consider.
565
566 \return Return true if the face is visible.
567 */
568 template <class PolygonType>
computeVisibility(const vpHomogeneousMatrix & cMo,const double & angleAppears,const double & angleDisappears,bool & changed,bool useOgre,bool not_used,unsigned int width,unsigned int height,const vpCameraParameters & cam,const vpTranslationVector & cameraPos,unsigned int index)569 bool vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears,
570 const double &angleDisappears, bool &changed, bool useOgre,
571 bool not_used, unsigned int width, unsigned int height,
572 const vpCameraParameters &cam,
573 const vpTranslationVector &cameraPos, unsigned int index)
574 {
575 (void)not_used;
576 unsigned int i = index;
577 Lpol[i]->changeFrame(cMo);
578 Lpol[i]->isappearing = false;
579
580 // Commented because we need to compute visibility
581 // even when dealing with line in level of detail case
582 /*if(Lpol[i]->getNbPoint() <= 2)
583 {
584 Lpol[i]->isvisible = true;
585 }
586 else*/ {
587 if (Lpol[i]->isVisible()) {
588 bool testDisappear = false;
589 // unsigned int nbCornerInsidePrev = 0;
590
591 if (!testDisappear) {
592 if (useOgre)
593 #ifdef VISP_HAVE_OGRE
594 testDisappear = ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, width, height)) || !isVisibleOgre(cameraPos, i));
595 #else
596 {
597 (void)cameraPos; // Avoid warning
598 testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, width, height));
599 }
600 #endif
601 else
602 testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, width, height));
603 }
604
605 // test if the face is still visible
606 if (testDisappear) {
607 // std::cout << "Face " << i << " disappears" <<
608 // std::endl;
609 changed = true;
610 Lpol[i]->isvisible = false;
611 } else {
612 // nbVisiblePolygon++;
613 Lpol[i]->isvisible = true;
614
615 // if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
616 // changed = true;
617 }
618 } else {
619 bool testAppear = true;
620
621 if (testAppear) {
622 if (useOgre)
623 #ifdef VISP_HAVE_OGRE
624 testAppear = ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, width, height)) && isVisibleOgre(cameraPos, i));
625 #else
626 testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, width, height));
627 #endif
628 else
629 testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, width, height));
630 }
631
632 if (testAppear) {
633 // std::cout << "Face " << i << " appears" << std::endl;
634 Lpol[i]->isvisible = true;
635 changed = true;
636 // nbVisiblePolygon++;
637 } else {
638 // std::cout << "Problem" << std::endl;
639 Lpol[i]->isvisible = false;
640 }
641 }
642 }
643 // std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon <<
644 // std::endl;
645 return Lpol[i]->isvisible;
646 }
647
648 /*!
649 Compute the number of visible polygons.
650
651 \param width, height : Image size used to check if the region of interest is inside the
652 image.
653 \param cam : Camera parameters.
654 \param cMo : The pose of the camera.
655 \param angle : Angle used to test the appearance and disappearance of a face.
656 \param changed : True if a face appeared, disappeared or too many
657 points have been lost. False otherwise.
658
659 \return Return the number of visible polygons
660 */
661 template <class PolygonType>
setVisible(unsigned int width,unsigned int height,const vpCameraParameters & cam,const vpHomogeneousMatrix & cMo,const double & angle,bool & changed)662 unsigned int vpMbHiddenFaces<PolygonType>::setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
663 const vpHomogeneousMatrix &cMo, const double &angle,
664 bool &changed)
665 {
666 return setVisible(width, height, cam, cMo, angle, angle, changed);
667 }
668
669 /*!
670 Compute the number of visible polygons.
671
672 \param width, height : Image size used to check if the region of interest is inside the
673 image.
674 \param cam : Camera parameters.
675 \param cMo : The pose of the camera.
676 \param changed : True if a face appeared, disappeared or too many points
677 have been lost. False otherwise.
678 \param angleAppears : Angle used to test the appearance of a face.
679 \param angleDisappears : Angle used to test the disappearance of a face.
680
681 \return Return the number of visible polygons
682 */
683 template <class PolygonType>
setVisible(unsigned int width,unsigned int height,const vpCameraParameters & cam,const vpHomogeneousMatrix & cMo,const double & angleAppears,const double & angleDisappears,bool & changed)684 unsigned int vpMbHiddenFaces<PolygonType>::setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
685 const vpHomogeneousMatrix &cMo, const double &angleAppears,
686 const double &angleDisappears, bool &changed)
687 {
688 return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false, true, width, height, cam);
689 }
690
691 /*!
692 Compute the number of visible polygons.
693
694 \param cMo : The pose of the camera
695 \param angleAppears : Angle used to test the appearance of a face
696 \param angleDisappears : Angle used to test the disappearance of a face
697 \param changed : True if a face appeared, disappeared or too many points
698 have been lost. False otherwise
699
700 \return Return the number of visible polygons
701 */
702 template <class PolygonType>
setVisible(const vpHomogeneousMatrix & cMo,const double & angleAppears,const double & angleDisappears,bool & changed)703 unsigned int vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears,
704 const double &angleDisappears, bool &changed)
705 {
706 return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false);
707 }
708
709 #ifdef VISP_HAVE_OGRE
710 /*!
711 Initialise the ogre context for face visibility tests.
712
713 \param cam : Camera parameters.
714 */
initOgre(const vpCameraParameters & cam)715 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
716 {
717 ogreInitialised = true;
718 ogre->setCameraParameters(cam);
719 ogre->setShowConfigDialog(ogreShowConfigDialog);
720 ogre->init(ogreBackground, false, true);
721
722 for (unsigned int n = 0; n < Lpol.size(); n++) {
723 Ogre::ManualObject *manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));
724
725 manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
726 for (unsigned int i = 0; i < Lpol[n]->nbpt; i++) {
727 manual->position((Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(),
728 (Ogre::Real)Lpol[n]->p[i].get_oZ());
729 manual->colour(1.0, 1.0, 1.0);
730 manual->index(i);
731 }
732
733 manual->index(0);
734 manual->end();
735
736 ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual);
737
738 lOgrePolygons.push_back(manual);
739 }
740 }
741
742 /*!
743 Update the display in Ogre Window.
744
745 \param cMo : Pose used to display.
746 */
displayOgre(const vpHomogeneousMatrix & cMo)747 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
748 {
749 if (ogreInitialised && !ogre->isWindowHidden()) {
750 for (unsigned int i = 0; i < Lpol.size(); i++) {
751 if (Lpol[i]->isVisible()) {
752 lOgrePolygons[i]->setVisible(true);
753 } else
754 lOgrePolygons[i]->setVisible(false);
755 }
756 ogre->display(ogreBackground, cMo);
757 }
758 }
759
760 /*!
761 Compute the number of visible polygons through Ogre3D.
762
763 \param width, height : Image size used to check if the region of interest is inside the
764 image.
765 \param cam : Camera parameters.
766 \param cMo : The pose of the camera.
767 \param changed : True if a face appeared, disappeared or too many points
768 have been lost. False otherwise.
769 \param angleAppears : Angle used to test the appearance of a face.
770 \param angleDisappears : Angle used to test the disappearance of a face.
771
772 \return Return the number of visible polygons
773 */
774 template <class PolygonType>
setVisibleOgre(unsigned int width,unsigned int height,const vpCameraParameters & cam,const vpHomogeneousMatrix & cMo,const double & angleAppears,const double & angleDisappears,bool & changed)775 unsigned int vpMbHiddenFaces<PolygonType>::setVisibleOgre(unsigned int width, unsigned int height,
776 const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo,
777 const double &angleAppears, const double &angleDisappears,
778 bool &changed)
779 {
780 return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true, true, width, height, cam);
781 }
782
783 /*!
784 Compute the number of visible polygons through Ogre3D.
785
786 \param cMo : The pose of the camera
787 \param angleAppears : Angle used to test the appearance of a face
788 \param angleDisappears : Angle used to test the disappearance of a face
789 \param changed : True if a face appeared, disappeared or too many points
790 have been lost. False otherwise
791
792 \return Return the number of visible polygons
793 */
794 template <class PolygonType>
setVisibleOgre(const vpHomogeneousMatrix & cMo,const double & angleAppears,const double & angleDisappears,bool & changed)795 unsigned int vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears,
796 const double &angleDisappears, bool &changed)
797 {
798 return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true);
799 }
800
801 /*!
802 Test the visibility of a polygon through Ogre3D via RayCasting.
803
804 \param cameraPos : Position of the camera in the 3D world.
805 \param index : Index of the polygon.
806
807 \return Return true if the polygon is visible, False otherwise.
808 */
809 template <class PolygonType>
isVisibleOgre(const vpTranslationVector & cameraPos,const unsigned int & index)810 bool vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
811 {
812 Ogre::Vector3 camera((Ogre::Real)cameraPos[0], (Ogre::Real)cameraPos[1], (Ogre::Real)cameraPos[2]);
813 if (!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())) {
814 lOgrePolygons[index]->setVisible(false);
815 Lpol[index]->isvisible = false;
816 return false;
817 }
818
819 // Get the center of gravity
820 bool visible = false;
821 unsigned int nbVisible = 0;
822
823 for (unsigned int i = 0; i < nbRayAttempts; i++) {
824 Ogre::Vector3 origin(0, 0, 0);
825 Ogre::Real totalFactor = 0.0f;
826
827 for (unsigned int j = 0; j < Lpol[index]->getNbPoint(); j++) {
828 Ogre::Real factor = 1.0f;
829
830 if (nbRayAttempts > 1) {
831 int r = rand() % 101;
832
833 if (r != 0)
834 factor = ((Ogre::Real)r) / 100.0f;
835 }
836
837 Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(),
838 (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
839 tmp *= factor;
840 origin += tmp;
841 totalFactor += factor;
842 }
843
844 origin /= totalFactor;
845
846 Ogre::Vector3 direction = origin - camera;
847 Ogre::Real distanceCollision = direction.length();
848
849 direction.normalise();
850 Ogre::RaySceneQuery *mRaySceneQuery = ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction));
851 mRaySceneQuery->setSortByDistance(true);
852
853 Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute();
854 Ogre::RaySceneQueryResult::iterator it = result.begin();
855
856 // while(it != result.end()){
857 // std::cout << it->movable->getName() << "(" << it->distance<< ") :
858 // " << std::flush; it++;
859 // }
860 // std::cout << std::endl;
861 // it = result.begin();
862
863 if (it != result.end())
864 if (it->movable->getName().find("SimpleRenderable") !=
865 Ogre::String::npos) // Test if the ogreBackground is intersect in
866 // first
867 ++it;
868
869 double distance;
870 // In a case of a two-axis aligned segment, ray collision is not always
871 // working.
872 if (Lpol[index]->getNbPoint() == 2 &&
873 (((std::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) <
874 std::numeric_limits<double>::epsilon()) +
875 (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) <
876 std::numeric_limits<double>::epsilon()) +
877 (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) <
878 std::numeric_limits<double>::epsilon())) >= 2)) {
879 if (it != result.end()) {
880 if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
881 nbVisible++;
882 } else {
883 distance = it->distance;
884 // Cannot use epsilon for comparison as ray lenght is slightly
885 // different from the collision distance returned by
886 // Ogre::RaySceneQueryResult.
887 if (distance > distanceCollision || std::fabs(distance - distanceCollision) <
888 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/)
889 nbVisible++;
890 }
891 } else
892 nbVisible++; // Collision not detected but present.
893 } else {
894 if (it != result.end()) {
895 distance = it->distance;
896 double distancePrev = distance;
897
898 // std::cout << "For " << Ogre::StringConverter::toString(index) << ":
899 // " << it->movable->getName() << " / " << std::flush;
900
901 if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
902 nbVisible++;
903 } else {
904 ++it;
905 while (it != result.end()) {
906 distance = it->distance;
907
908 if (std::fabs(distance - distancePrev) <
909 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/) {
910 // std::cout << it->movable->getName() << " / " << std::flush;
911 if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
912 nbVisible++;
913 break;
914 }
915 ++it;
916 distancePrev = distance;
917 } else
918 break;
919 }
920 }
921 }
922 }
923
924 ogre->getSceneManager()->destroyQuery(mRaySceneQuery);
925 }
926
927 if (((double)nbVisible) / ((double)nbRayAttempts) > ratioVisibleRay ||
928 std::fabs(((double)nbVisible) / ((double)nbRayAttempts) - ratioVisibleRay) <
929 ratioVisibleRay * std::numeric_limits<double>::epsilon())
930 visible = true;
931 else
932 visible = false;
933
934 if (visible) {
935 lOgrePolygons[index]->setVisible(true);
936 Lpol[index]->isvisible = true;
937 } else {
938 lOgrePolygons[index]->setVisible(false);
939 Lpol[index]->isvisible = false;
940 }
941
942 return Lpol[index]->isvisible;
943 }
944 #endif // VISP_HAVE_OGRE
945
946 #endif // vpMbHiddenFaces
947