Main MRPT website > C++ reference for MRPT 1.5.9
CFaceDetection.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "detectors-precomp.h" // Precompiled headers
11 #include <mrpt/gui.h>
12 #include <mrpt/maps/CColouredPointsMap.h>
13 
14 #include <mrpt/detectors/CFaceDetection.h>
15 #include <mrpt/math.h>
16 #include <mrpt/opengl/CPointCloudColoured.h>
17 #include <mrpt/opengl/CGridPlaneXY.h>
18 #include <mrpt/opengl/CSphere.h>
19 #include <mrpt/opengl/CArrow.h>
20 #include <mrpt/opengl/CSetOfLines.h>
21 #include <mrpt/opengl/CAxis.h>
22 
23 #include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
24 #include <mrpt/slam/CICP.h>
25 
26 // Universal include for all versions of OpenCV
27 #include <mrpt/otherlibs/do_opencv_includes.h>
28 
29 
30 using namespace std;
31 using namespace mrpt;
32 using namespace mrpt::detectors;
33 using namespace mrpt::math;
34 using namespace mrpt::gui;
35 using namespace mrpt::math;
36 using namespace mrpt::utils;
37 using namespace mrpt::opengl;
38 using namespace mrpt::system;
39 using namespace mrpt::maps;
40 using namespace mrpt::obs;
41 
42 
43 //------------------------------------------------------------------------
44 // CFaceDetection
45 //------------------------------------------------------------------------
46 CFaceDetection::CFaceDetection() :
47  m_end_threads(false),
48  m_enter_checkIfFaceRegions(0,1),
49  m_enter_checkIfFacePlaneCov(0,1),
50  m_enter_checkIfDiagonalSurface(0,1),
51  m_leave_checkIfFaceRegions(0,1),
52  m_leave_checkIfFacePlaneCov(0,1),
53  m_leave_checkIfDiagonalSurface(0,1)
54 {
55  m_measure.numPossibleFacesDetected = 0;
56  m_measure.numRealFacesDetected = 0;
57 
58  m_measure.faceNum = 0;
59 
60  m_timeLog.enable();
61 }
62 
63 
64 //------------------------------------------------------------------------
65 // ~CFaceDetection
66 //------------------------------------------------------------------------
67 CFaceDetection::~CFaceDetection()
68 {
69  // Stop filters threads
70 
71  m_end_threads = true;
72 
73  m_enter_checkIfFacePlaneCov.release();
74  m_enter_checkIfFaceRegions.release();
75  m_enter_checkIfDiagonalSurface.release();
76 
77  joinThread(m_thread_checkIfFaceRegions);
78  joinThread(m_thread_checkIfFacePlaneCov);
79  joinThread(m_thread_checkIfDiagonalSurface);
80 }
81 
82 //------------------------------------------------------------------------
83 // init
84 //------------------------------------------------------------------------
85 void CFaceDetection::init(const mrpt::utils::CConfigFileBase &cfg )
86 {
87  m_options.confidenceThreshold = cfg.read_int( "FaceDetection", "confidenceThreshold", 240 );
88  m_options.multithread = cfg.read_bool( "FaceDetection", "multithread", true );
89  m_options.useCovFilter = cfg.read_bool( "FaceDetection", "useCovFilter", true );
90  m_options.useRegionsFilter = cfg.read_bool( "FaceDetection", "useRegionsFilter", true );
91  m_options.useSizeDistanceRelationFilter = cfg.read_bool( "FaceDetection", "useSizeDistanceRelationFilter", true );
92  m_options.useDiagonalDistanceFilter = cfg.read_bool( "FaceDetection", "useDiagonalDistanceFilter", true );
93 
94  m_testsOptions.planeThreshold = cfg.read_double( "FaceDetection", "planeThreshold", 50 );
95  m_testsOptions.planeTest_eigenVal_top = cfg.read_double( "FaceDetection", "planeTest_eigenVal_top", 0.011 );
96  m_testsOptions.planeTest_eigenVal_bottom = cfg.read_double( "FaceDetection", "planeTest_eigenVal_bottom", 0.0002 );
97  m_testsOptions.regionsTest_sumDistThreshold_top = cfg.read_double( "FaceDetection", "regionsTest_sumDistThreshold_top", 0.5 );
98  m_testsOptions.regionsTest_sumDistThreshold_bottom = cfg.read_double( "FaceDetection", "regionsTest_sumDistThreshold_bottom", 0.04 );
99 
100  m_measure.takeTime = cfg.read_bool( "FaceDetection", "takeTime", false );
101  m_measure.takeMeasures = cfg.read_bool( "FaceDetection", "takeMeasures", false );
102  m_measure.saveMeasurementsToFile= cfg.read_bool( "FaceDetection", "saveMeasurementsToFile", false );
103 
104  // Run filters threads
105  if ( m_options.multithread )
106  {
107  if ( m_options.useRegionsFilter )
108  m_thread_checkIfFaceRegions = createThread( dummy_checkIfFaceRegions, this );
109  if ( m_options.useCovFilter )
110  m_thread_checkIfFacePlaneCov = createThread( dummy_checkIfFacePlaneCov, this );
111  if ( m_options.useSizeDistanceRelationFilter || m_options.useDiagonalDistanceFilter )
112  m_thread_checkIfDiagonalSurface = createThread( dummy_checkIfDiagonalSurface, this );
113 
114  m_checkIfFacePlaneCov_res = false;
115  m_checkIfFaceRegions_res = true;
116  m_checkIfDiagonalSurface_res = true;
117  }
118 
119  cascadeClassifier.init( cfg );
120 }
121 
122 
123 //------------------------------------------------------------------------
124 // detectObjects
125 //------------------------------------------------------------------------
126 void CFaceDetection::detectObjects_Impl(const mrpt::obs::CObservation *obs, vector_detectable_object &detected)
127 {
128  MRPT_START
129 
130  // Detect possible faces
131  vector_detectable_object localDetected;
132 
133  // To obtain experimental results
134  {
135  if ( m_measure.takeTime )
136  m_timeLog.enter("Detection time");
137  }
138 
139  cascadeClassifier.detectObjects( obs, localDetected );
140 
141  // To obtain experimental results
142  {
143  if ( m_measure.takeTime )
144  m_timeLog.leave("Detection time");
145 
146  //if ( m_measure.takeMeasures )
147  m_measure.numPossibleFacesDetected += localDetected.size();
148  }
149 
150 
151  // Check if we are using a 3D Camera and 3D points are saved
152  if ( (IS_CLASS(obs, CObservation3DRangeScan )) && ( localDetected.size() > 0 ) )
153  {
154  // To obtain experimental results
155  {
156  if ( m_measure.takeTime )
157  m_timeLog.enter("Check if real face time");
158  }
159 
160  CObservation3DRangeScan* o = static_cast<CObservation3DRangeScan*>( const_cast<CObservation*>(obs) );
161 
162  if ( o->hasPoints3D )
163  {
164  // Vector to save detected objects to delete if they aren't a face
165  vector<size_t> deleteDetected;
166 
167  // Check if all possible detected faces satisfy a serial of constrains
168  for ( unsigned int i = 0; i < localDetected.size(); i++ )
169  {
170  CDetectable2DPtr rec = CDetectable2DPtr(localDetected[i]);
171 
172  // Calculate initial and final rows and columns
173  unsigned int r1 = rec->m_y;
174  unsigned int r2 = rec->m_y + rec->m_height;
175  unsigned int c1 = rec->m_x;
176  unsigned int c2 = rec->m_x + rec->m_width;
177 
178  o->getZoneAsObs( m_lastFaceDetected, r1, r2, c1, c2 );
179 
180  if ( m_options.multithread )
181  {
182  // To obtain experimental results
183  {
184  if ( m_measure.takeTime )
185  m_timeLog.enter("Multithread filters application");
186  }
187 
188  // Semaphores signal
189  if ( m_options.useCovFilter )
190  m_enter_checkIfFacePlaneCov.release();
191  if ( m_options.useRegionsFilter )
192  m_enter_checkIfFaceRegions.release();
193  if ( m_options.useSizeDistanceRelationFilter || m_options.useDiagonalDistanceFilter )
194  m_enter_checkIfDiagonalSurface.release();
195 
196  // Semaphores wait
197  if ( m_options.useCovFilter )
198  m_leave_checkIfFacePlaneCov.waitForSignal();
199  if ( m_options.useRegionsFilter )
200  m_leave_checkIfFaceRegions.waitForSignal();
201  if ( m_options.useSizeDistanceRelationFilter || m_options.useDiagonalDistanceFilter )
202  m_leave_checkIfDiagonalSurface.waitForSignal();
203 
204  // Check resutls
205  if ( !m_checkIfFacePlaneCov_res || !m_checkIfFaceRegions_res
206  || !m_checkIfDiagonalSurface_res )
207  deleteDetected.push_back( i );
208 
209  // To obtain experimental results
210  {
211  if ( m_measure.takeTime )
212  m_timeLog.leave("Multithread filters application");
213  }
214 
215  m_measure.faceNum++;
216 
217  }
218  else
219  {
220  // To obtain experimental results
221  {
222  if ( m_measure.takeTime )
223  m_timeLog.enter("Secuential filters application");
224  }
225 
226  /////////////////////////////////////////////////////
227  //CMatrixTemplate<bool> region;
228  //experimental_segmentFace( m_lastFaceDetected, region);
229  /////////////////////////////////////////////////////
230 
231  //m_lastFaceDetected.intensityImage.saveToFile(format("%i.jpg",m_measure.faceNum));
232 
233  bool remove = false;
234 
235  // First check if we can adjust a plane to detected region as face, if yes it isn't a face!
236  if ( m_options.useCovFilter && !checkIfFacePlaneCov( &m_lastFaceDetected ) )
237  {
238  deleteDetected.push_back( i );
239  remove = true;
240  }
241  else if ( m_options.useRegionsFilter && !checkIfFaceRegions( &m_lastFaceDetected ) )
242  {
243  deleteDetected.push_back( i );
244  remove = true;
245  }
246  else if ( ( m_options.useSizeDistanceRelationFilter || m_options.useDiagonalDistanceFilter )
247  && !checkIfDiagonalSurface( &m_lastFaceDetected ) )
248  {
249  deleteDetected.push_back( i );
250  remove = true;
251  }
252 
253  if ( remove )
254  {
255  /*ofstream f;
256  f.open("deleted.txt", ofstream::app);
257  f << "Deleted: " << m_measure.faceNum << endl;
258  f.close();*/
259  m_measure.deletedRegions.push_back( m_measure.faceNum );
260 
261  }
262 
263  m_measure.faceNum++;
264 
265  // To obtain experimental results
266  {
267  if ( m_measure.takeTime )
268  m_timeLog.leave("Secuential filters application");
269  }
270 
271  }
272 
273  }
274 
275  // Delete non faces
276  for ( unsigned int i = deleteDetected.size(); i > 0; i-- )
277  localDetected.erase( localDetected.begin() + deleteDetected[i-1] );
278  }
279 
280  // Convert 2d detected objects to 3d
281  for ( unsigned int i = 0; i < localDetected.size(); i++ )
282  {
283  CDetectable3DPtr object3d =
284  CDetectable3DPtr( new CDetectable3D((CDetectable2DPtr)localDetected[i]) );
285  detected.push_back( object3d );
286  }
287 
288  // To obtain experimental results
289  {
290  if ( m_measure.takeTime )
291  m_timeLog.leave("Check if real face time");
292  }
293  }
294  else // Not using a 3D camera
295  {
296  detected = localDetected;
297  }
298 
299  // To obtain experimental results
300  {
301  //if ( m_measure.takeMeasures )
302  m_measure.numRealFacesDetected += detected.size();
303  }
304 
305  MRPT_END
306 
307 }
308 
309 
310 //------------------------------------------------------------------------
311 // checkIfFacePlane
312 //------------------------------------------------------------------------
313 bool CFaceDetection::checkIfFacePlane( CObservation3DRangeScan *face )
314 {
315 
316  vector<TPoint3D> points;
317 
318  size_t N = face->points3D_x.size();
319 
320  points.resize( N );
321 
322  for ( size_t i = 0; i < N; i++ )
323  points[i] = TPoint3D( face->points3D_x.at(i), face->points3D_y.at(i), face->points3D_z.at(i) );
324 
325  // Try to ajust a plane
326  TPlane plane;
327 
328  // To obtain experimental results
329  {
330  if ( m_measure.takeMeasures )
331  m_measure.errorEstimations.push_back( (double)getRegressionPlane(points,plane) );
332  }
333 
334  if ( getRegressionPlane(points,plane) < m_testsOptions.planeThreshold )
335  return true;
336 
337  return false;
338 }
339 
340 void CFaceDetection::dummy_checkIfFacePlaneCov( CFaceDetection *obj )
341 {
342  obj->thread_checkIfFacePlaneCov( );
343 }
344 
345 void CFaceDetection::thread_checkIfFacePlaneCov( )
346 {
347  for(;;)
348  {
349  m_enter_checkIfFacePlaneCov.waitForSignal();
350 
351  if ( m_end_threads )
352  break;
353 
354  // Perform filter
355  m_checkIfFacePlaneCov_res = checkIfFacePlaneCov( &m_lastFaceDetected );
356 
357  m_leave_checkIfFacePlaneCov.release();
358  }
359 }
360 
361 //------------------------------------------------------------------------
362 // checkIfFacePlaneCov
363 //------------------------------------------------------------------------
364 bool CFaceDetection::checkIfFacePlaneCov( CObservation3DRangeScan* face )
365 {
366  MRPT_TRY_START
367 
368  // To obtain experimental results
369  {
370  if ( m_measure.takeTime )
371  m_timeLog.enter("Check if face plane: covariance");
372  }
373 
374  // Get face region size
375  const unsigned int faceWidth = face->intensityImage.getWidth();
376  const unsigned int faceHeight = face->intensityImage.getHeight();
377 
378  // We work with a confidence image?
379  const bool confidence = face->hasConfidenceImage;
380 
381  // To fill with valid points
382  vector<CArrayDouble<3> > pointsVector;
383 
384  CMatrixTemplate<bool> region; // To save the segmented region
385  experimental_segmentFace( *face, region);
386 
387  for ( unsigned int j = 0; j < faceHeight; j++ )
388  {
389  for ( unsigned int k = 0; k < faceWidth; k++ )
390  {
391  CArrayDouble<3> aux;
392 
393  if ( region.get_unsafe( j,k ) && (( (!confidence) || (( confidence ) &&
394  ( *(face->confidenceImage.get_unsafe( k, j, 0 )) > m_options.confidenceThreshold )
395  && ( *(face->intensityImage.get_unsafe( k, j )) > 50 ))))) // Don't take in account dark pixels
396  {
397  int position = faceWidth*j + k;
398  aux[0] = face->points3D_x[position];
399  aux[1] = face->points3D_y[position];
400  aux[2] = face->points3D_z[position];
401  pointsVector.push_back( aux );
402  }
403  }
404  }
405 
406  // Check if points vector is empty to avoid a future crash
407  if ( pointsVector.empty() )
408  return false;
409 
410  //experimental_viewFacePointsScanned( *face );
411 
412  // To obtain the covariance vector and eigenvalues
413  CMatrixDouble cov;
414  CMatrixDouble eVects, m_eVals;
415  CVectorDouble eVals;
416 
417  cov = covVector<vector<CArrayDouble<3> >,CMatrixDouble>( pointsVector );
418 
419  cov.eigenValues( eVals );
420 
421  cov.eigenVectors( eVects, m_eVals );
422 
423  // To obtain experimental results
424  {
425  if ( m_measure.takeMeasures )
426  m_measure.lessEigenVals.push_back(eVals[0]);
427 
428  if ( m_measure.takeTime )
429  m_timeLog.leave("Check if face plane: covariance");
430 
431  // Uncomment if you want to analyze the calculated eigenvalues
432  //ofstream f;
433  /*f.open("eigenvalues.txt", ofstream::app);
434  f << m_measure.faceNum << " " << eVals[0] << endl;
435  f.close();*/
436 
437  //f.open("eigenvalues2.txt", ofstream::app);
438  cout << eVals[0] << " " << eVals[1] << " " << eVals[2] << " > " ;
439  cout << eVals[0]/eVals[2] << endl;
440  //f << eVals[0]/eVals[2] << endl;
441  //f.close();
442  }
443 
444  if ( m_measure.faceNum >= 314 )
445  experimental_viewFacePointsAndEigenVects( pointsVector, eVects, eVals );
446 
447  // Check if the less eigenvalue is out of the permited area
448  //if ( ( eVals[0] > m_options.planeEigenValThreshold_down )
449  // && ( eVals[0] < m_options.planeEigenValThreshold_up ) )
450  if ( eVals[0]/eVals[2] > 0.06 )
451  {
452 
453  //Uncomment if you want to save the face regions discarted by this filter
454  /*ofstream f;
455  f.open("deletedCOV.txt", ofstream::app);
456  f << m_measure.faceNum << endl;
457  f.close();*/
458 
459  return true; // Filter not passed
460  }
461 
462  return false; // Filter passed
463 
464  MRPT_TRY_END
465 }
466 
467 
468 void CFaceDetection::dummy_checkIfFaceRegions( CFaceDetection *obj )
469 {
470  obj->thread_checkIfFaceRegions( );
471 }
472 
473 void CFaceDetection::thread_checkIfFaceRegions( )
474 {
475  for(;;)
476  {
477  m_enter_checkIfFaceRegions.waitForSignal();
478 
479  if ( m_end_threads )
480  break;
481 
482  // Perform filter
483  m_checkIfFaceRegions_res = checkIfFaceRegions( &m_lastFaceDetected );
484 
485  m_leave_checkIfFaceRegions.release();
486  }
487 }
488 
489 
490 //------------------------------------------------------------------------
491 // checkIfFaceRegions
492 //------------------------------------------------------------------------
493 
494 bool CFaceDetection::checkIfFaceRegions( CObservation3DRangeScan* face )
495 {
496  MRPT_START
497 
498  // To obtain experimental results
499  {
500  if ( m_measure.takeTime )
501  m_timeLog.enter("Check if face plane: regions");
502  }
503 
504  // To obtain region size
505  const unsigned int faceWidth = face->intensityImage.getWidth();
506  const unsigned int faceHeight = face->intensityImage.getHeight();
507 
508  // Initial vertical size of a region
509  unsigned int sectionVSize = faceHeight/3.0;
510 
511  // Steps of this filter
512  // 1. To segment the region detected as face using a regions growing algorithm
513  // 2. To obtain the first and last column to work (a profile face detected can have a lateral area without to use)
514  // 3. To calculate the histogram of the upper zone of the region for determine if we use it (if this zone present
515  // a lot of dark pixels the measurements can be wrong)
516  // 4. To obtain the coordinates of pixels that form each subregion
517  // 5. To calculate medians or means of each subregion
518  // 6. To check subregions constrains
519 
520  vector<TPoint3D> points;
521 
522  TPoint3D meanPos[3][3] = {
523  { TPoint3D(0,0,0),TPoint3D(0,0,0),TPoint3D(0,0,0)},
524  { TPoint3D(0,0,0),TPoint3D(0,0,0),TPoint3D(0,0,0)},
525  { TPoint3D(0,0,0),TPoint3D(0,0,0),TPoint3D(0,0,0)} };
526  int numPoints[3][3] = { {0,0,0}, {0,0,0}, {0,0,0} };
527 
528  vector<TPoint3D> regions2[9];
529 
530  //
531  // 1. To segment the region detected as face using a regions growing algorithm
532  //
533 
534  CMatrixTemplate<bool> region; // To save the segmented region
535  experimental_segmentFace( *face, region);
536 
537  //
538  // 2. To obtain the first and last column to work (a profile face detected can have a lateral area without to use)
539  //
540 
541  size_t start=faceWidth, end=0;
542 
543  for ( size_t r = 0; r < region.getRowCount() ; r++ )
544  for ( size_t c = 1; c < region.getColCount() ; c++ )
545  {
546  if ( ( !(region.get_unsafe( r, c-1 )) ) && ( region.get_unsafe( r, c )) )
547  {
548  if ( c < start )
549  start = c;
550  }
551  else if ( ( region.get_unsafe( r, c-1 ) ) && ( !(region.get_unsafe( r, c )) ) )
552  if ( c > end )
553  end = c;
554 
555  if ( ( c > end ) && ( region.get_unsafe( r, c ) ) )
556  end = c;
557 
558  }
559 
560  if ( end == 0 ) end = faceWidth-1; // Check if the end has't changed
561  if ( end < 3*(faceWidth/4) ) end = 3*(faceWidth/4); // To avoid spoiler
562  if ( start == faceWidth ) start = 0; // Check if the start has't changed
563  if ( start > faceWidth/4 ) start = faceWidth/4; // To avoid spoiler
564 
565  //cout << "Start: " << start << " End: " << end << endl;
566 
567  // To use the start and end calculated to obtain the final regions limits
568  unsigned int utilWidth = faceWidth - start - ( faceWidth - end );
569  unsigned int c1 = ceil( utilWidth/3.0 + start );
570  unsigned int c2 = ceil( 2*(utilWidth/3.0) + start );
571 
572  //
573  // 3. To calculate the histogram of the upper zone of the region for determine if we use it
574  //
575 
576  CMatrixTemplate<unsigned int> hist;
577  hist.setSize(1,256,true);
578  experimental_calcHist( face->intensityImage, start, 0, end, ceil(faceHeight*0.1), hist );
579 
580  size_t countHist = 0;
581  for ( size_t i = 0; i < 60; i++ )
582  {
583  countHist += hist.get_unsafe(0,i);
584  }
585 
586  size_t upLimit = 0;
587  size_t downLimit = faceHeight-1;
588 
589  if ( countHist > 10 )
590  {
591  upLimit = floor(faceHeight*0.1);
592  downLimit = floor(faceHeight*0.9);
593  }
594 
595  // Uncomment it if you want to analyze the number of pixels that have more dark that the 60 gray tone
596  //m_meanHist.push_back( countHist );
597 
598  //
599  // 4. To obtain the coordinates of pixels that form each region
600  //
601 
602  unsigned int cont = 0;
603 
604  for ( unsigned int r = 0; r < faceHeight; r++ )
605  {
606  for ( unsigned int c = 0; c < faceWidth; c++, cont++ )
607  {
608  if ( ( r >= upLimit ) && ( r <= downLimit )
609  && ( region.get_unsafe( r, c ) )
610  && (*(face->confidenceImage.get_unsafe( c, r, 0 )) > m_options.confidenceThreshold )
611  && ( *(face->intensityImage.get_unsafe( c, r )) > 50) )
612  {
613  unsigned int row, col;
614  if ( r < sectionVSize + upLimit*0.3)
615  row = 0;
616  else if ( r < sectionVSize*2 - upLimit*0.15 )
617  row = 1;
618  else
619  row = 2;
620 
621  if ( c < c1)
622  col = 0;
623  else if ( c < c2 )
624  col = 1;
625  else
626  col = 2;
627 
628 
629  TPoint3D point( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont]);
630  meanPos[row][col] = meanPos[row][col] + point;
631 
632  ++numPoints[row][col];
633 
634  if ( row == 0 && col == 0 )
635  regions2[0].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
636  else if ( row == 0 && col == 1 )
637  regions2[1].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
638  else if ( row == 0 && col == 2 )
639  regions2[2].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
640  else if ( row == 1 && col == 0 )
641  regions2[3].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
642  else if ( row == 1 && col == 1 )
643  regions2[4].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
644  else if ( row == 1 && col == 2 )
645  regions2[5].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
646  else if ( row == 2 && col == 0 )
647  regions2[6].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
648  else if ( row == 2 && col == 1 )
649  regions2[7].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
650  else
651  regions2[8].push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
652 
653  }
654 
655  }
656  }
657 
658  //
659  // 5. To calculate medians or means of each subregion
660  //
661 
662  vector<double> oldPointsX1;
663 
664  size_t middle1=0;
665  size_t middle2=0;
666 
667  if ( regions2[0].size() > 0 )
668  {
669  for ( size_t i = 0; i < regions2[0].size(); i++ )
670  oldPointsX1.push_back( regions2[0][i].x );
671 
672  middle1 = floor((double)oldPointsX1.size()/2);
673  nth_element(oldPointsX1.begin(),oldPointsX1.begin()+middle1,oldPointsX1.end()); // Obtain center element
674  }
675 
676  vector<double> oldPointsX2;
677 
678  if ( regions2[2].size() > 0 )
679  {
680  for ( size_t i = 0; i < regions2[2].size(); i++ )
681  oldPointsX2.push_back( regions2[2][i].x );
682 
683  middle2 = floor((double)oldPointsX2.size()/2);
684  nth_element(oldPointsX2.begin(),oldPointsX2.begin()+middle2,oldPointsX2.end()); // Obtain center element
685  }
686 
687  for ( size_t i = 0; i < 3; i++ )
688  for ( size_t j = 0; j < 3; j++ )
689  if ( !numPoints[i][j] )
690  meanPos[i][j] = TPoint3D( 0, 0, 0 );
691  else
692  meanPos[i][j] = meanPos[i][j] / numPoints[i][j];
693 
694  if ( regions2[0].size() > 0 )
695  meanPos[0][0].x = oldPointsX1.at(middle1);
696 
697  if ( regions2[2].size() > 0 )
698  meanPos[0][2].x = oldPointsX2.at(middle2);
699 
700  //
701  // 6. To check subregions constrains
702  //
703  vector<double> dist(5);
704  size_t res = checkRelativePosition( meanPos[1][0], meanPos[1][2], meanPos[1][1], dist[0] );
705  res += res && checkRelativePosition( meanPos[2][0], meanPos[2][2], meanPos[2][1], dist[1] );
706  res += res && checkRelativePosition( meanPos[0][0], meanPos[0][2], meanPos[0][1], dist[2] );
707  res += res && checkRelativePosition( meanPos[0][0], meanPos[2][2], meanPos[1][1], dist[3] );
708  res += res && checkRelativePosition( meanPos[2][0], meanPos[0][2], meanPos[1][1], dist[4] );
709 
710  ofstream f;
711  f.open("dist.txt", ofstream::app);
712  f << sum(dist) << endl;
713  f.close();
714 
715  bool real = false;
716  if ( !res )
717  real = true;
718  else if (( res = 1 ) && ( sum(dist) > 0.04 ))
719  real = true;
720 
721  f.open("tam.txt",ofstream::app);
722  f << meanPos[0][1].distanceTo( meanPos[2][1] ) << endl;
723  f.close();
724 
725 
726  //experimental_viewRegions( regions2, meanPos );
727 
728  //cout << endl << meanPos[0][0] << "\t" << meanPos[0][1] << "\t" << meanPos[0][2];
729  // cout << endl << meanPos[1][0] << "\t" << meanPos[1][1] << "\t" << meanPos[1][2];
730  // cout << endl << meanPos[2][0] << "\t" << meanPos[2][1] << "\t" << meanPos[2][2] << endl;
731 
732 
733  // To obtain experimental results
734  {
735  if ( m_measure.takeTime )
736  m_timeLog.leave("Check if face plane: regions");
737  }
738 
739  if ( real )
740  return true; // Filter passed
741  else
742  {
743 
744  // Uncomment if you want to known what regions was discarted by this filter
745  /*ofstream f;
746  f.open("deletedSTRUCTURES.txt", ofstream::app);
747  f << m_measure.faceNum << endl;
748  f.close();*/
749 
750  return false; // Filter not passed
751  }
752 
753  MRPT_END
754 }
755 
756 
757 //------------------------------------------------------------------------
758 // checkRelativePosition
759 //------------------------------------------------------------------------
760 
761 size_t CFaceDetection::checkRelativePosition( const TPoint3D &p1, const TPoint3D &p2, const TPoint3D &p, double &dist )
762 {
763  double x1 = -p1.y;
764  double y1 = p1.x;
765 
766  double x2 = -p2.y;
767  double y2 = p2.x;
768 
769  double x = -p.y;
770  double y = p.x;
771 
772  double yIdeal = y1 + ( ((x-x1)*(y2-y1)) / (x2-x1) );
773 
774  //////////////////////////////////
775 
776  /*double xaux = x2;
777  double yaux = y1;
778 
779  cout << "Grados= " << RAD2DEG(acos( (xaux-x1)/(sqrt(pow(x1-x2,2)+pow(y1-y2,2))) )) << endl;*/
780 
781  ///////////////////////////////////////
782 
783  dist = yIdeal-y;
784 
785  if ( y < yIdeal )
786  return 0;
787  else
788  return 1;
789 }
790 
791 
792 void CFaceDetection::dummy_checkIfDiagonalSurface( CFaceDetection *obj )
793 {
794  obj->thread_checkIfDiagonalSurface( );
795 }
796 
797 void CFaceDetection::thread_checkIfDiagonalSurface( )
798 {
799  for(;;)
800  {
801  m_enter_checkIfDiagonalSurface.waitForSignal();
802 
803  if ( m_end_threads )
804  break;
805 
806  // Perform filter
807  m_checkIfDiagonalSurface_res = checkIfDiagonalSurface( &m_lastFaceDetected );
808 
809  m_leave_checkIfDiagonalSurface.release();
810  }
811 }
812 
813 
814 //------------------------------------------------------------------------
815 // checkIfDiagonalSurface
816 //------------------------------------------------------------------------
817 
818 bool CFaceDetection::checkIfDiagonalSurface( CObservation3DRangeScan* face )
819 {
820  MRPT_START
821 
822  // To obtain experimental results
823  {
824  if ( m_options.useDiagonalDistanceFilter && m_measure.takeTime )
825  m_timeLog.enter("Check if face plane: diagonal distances");
826 
827  if ( m_options.useSizeDistanceRelationFilter && m_measure.takeTime )
828  m_timeLog.enter("Check if face plane: size-distance relation");
829  }
830 
831  const unsigned int faceWidth = face->intensityImage.getWidth();
832  const unsigned int faceHeight = face->intensityImage.getHeight();
833 
834  //const float max_desv = 0.2;
835 
836  unsigned int x1 = ceil(faceWidth*0.25);
837  unsigned int x2 = floor(faceWidth*0.75);
838  unsigned int y1 = ceil(faceHeight*0.15);
839  unsigned int y2 = floor(faceHeight*0.85);
840 
841  vector<TPoint3D> points;
842  unsigned int cont = ( y1 == 0 ? 0 : faceHeight*(y1-1));
843  CMatrixBool valids;
844 
845  valids.setSize(faceHeight,faceWidth);
846 
847  int total = 0;
848  double sumDepth = 0;
849 
850  for ( unsigned int i = y1; i <= y2; i++ )
851  {
852  cont += x1;
853 
854  for ( unsigned int j = x1; j <= x2; j++, cont++ )
855  {
856  if (*(face->confidenceImage.get_unsafe( j, i, 0 )) > m_options.confidenceThreshold )
857  {
858  sumDepth += face->points3D_x[cont];
859  total++;
860  points.push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
861  }
862  }
863  cont += faceWidth-x2-1;
864  }
865 
866  double meanDepth = sumDepth / total;
867 
868 
869 
870  /*if ( m_measure.faceNum == 434 )
871  experimental_viewFacePointsScanned( *face );*/
872 
873  //experimental_viewFacePointsScanned( points );
874 
875  bool res = true;
876 
877  if ( m_options.useSizeDistanceRelationFilter )
878  {
879  double maxFaceDistance = 0.5 + 1000 / ( pow( faceWidth, 1.9 ) );
880 
881  // To obtain experimental results
882  {
883  if ( m_measure.takeTime )
884  m_timeLog.leave("Check if face plane: size-distance relation");
885 
886  if ( m_options.useDiagonalDistanceFilter && m_measure.takeTime )
887  m_timeLog.leave("Check if face plane: diagonal distances");
888  }
889 
890  /*if ( maxFaceDistance > meanDepth )
891  return true;
892 
893  if ( !m_options.useDiagonalDistanceFilter )
894  return false;*/
895 
896  if ( maxFaceDistance < meanDepth )
897  {
898  // Uncomment if you want to analyze the regions discarted by this filter
899  /*ofstream f;
900  f.open("deletedSIZEDISTANCE.txt", ofstream::app);
901  f << m_measure.faceNum << endl;
902  f.close();*/
903 
904  //if ( !m_options.useDiagonalDistanceFilter )
905  return false;
906  //else
907  // res = false;
908  }
909 
910  if ( !m_options.useDiagonalDistanceFilter )
911  return true;
912  }
913 
914  ofstream f;
915  /*f.open("relaciones1.txt", ofstream::app);
916  f << faceWidth << endl;
917  f.close();*/
918 
919  f.open("relaciones2.txt", ofstream::app);
920  f << meanDepth << endl;
921  f.close();
922 
923  //cout << m_measure.faceNum ;
924 
925  //experimental_viewFacePointsScanned( points );
926 
927  points.clear();
928 
929  cont = ( y1 == 1 ? 0 : faceHeight*(y1-1));
930 
931  for ( unsigned int i = y1; i <= y2; i++ )
932  {
933  cont += x1;
934 
935  for ( unsigned int j = x1; j <= x2; j++, cont++ )
936  {
937  if ( (*(face->confidenceImage.get_unsafe( j, i, 0 )) > m_options.confidenceThreshold ) )
938  //&& ( face->points3D_x[cont] > meanDepth - max_desv )
939  //&& ( face->points3D_x[cont] < meanDepth + max_desv ) )
940  {
941  valids.set_unsafe( i, j, true );
942  points.push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
943  }
944  else
945  valids.set_unsafe( i, j, false );
946 
947 
948  }
949  cont += faceWidth-x2-1;
950  }
951 
952  /*if ( m_measure.faceNum > 838 )
953  experimental_viewFacePointsScanned( points );*/
954 
955  //if ( ( m_measure.faceNum == 225 ) || ( m_measure.faceNum == 226 ) )
956  //experimental_viewFacePointsScanned( points );
957 
958  double sumDistances = 0;
959  double distance;
960  int offsetIndex;
961 
962  cont = 0;
963 
964  for ( unsigned int i = y1; i <= y2; i++ )
965  {
966  cont += x1;
967 
968  for ( unsigned int j = x1; j <= x2; j++, cont++ )
969  {
970  if ( valids.get_unsafe( i, j ) )
971  {
972  //experimental_calcDiagDist( face, i, j, faceWidth, faceHeight, valids, distance );
973 
974  distance = 0;
975  if ( ( i+1 <= y2 ) && ( j+1 <= x2 ) )
976  {
977  if ( valids.get_unsafe( i+1, j+1 ) )
978  {
979  TPoint3D p1( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] );
980  offsetIndex = cont + faceWidth + 1;
981  distance = p1.distanceTo( TPoint3D(face->points3D_x[offsetIndex], face->points3D_y[offsetIndex], face->points3D_z[offsetIndex]) );
982  }
983  else
984  {
985  bool validOffset = true;
986  int offset = 2;
987 
988  while ( validOffset )
989  {
990  if ( ( i + offset <= y2 ) && ( j + offset <= x2 ) )
991  {
992  if ( valids.get_unsafe( i+offset, j+offset ) )
993  {
994  TPoint3D p1( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] );
995  offsetIndex = cont + faceWidth + offset;
996  distance = p1.distanceTo( TPoint3D(face->points3D_x[offsetIndex], face->points3D_y[offsetIndex], face->points3D_z[offsetIndex]) );
997  break;
998  }
999  offset++;
1000  }
1001  else
1002  validOffset = false;
1003  }
1004  }
1005  }
1006 
1007  sumDistances += distance;
1008  }
1009  }
1010  cont += faceWidth-x2-1;
1011  }
1012 
1013  // For experimental results
1014  {
1015  if ( m_measure.takeMeasures )
1016  m_measure.sumDistances.push_back( sumDistances );
1017 
1018  ofstream f;
1019  f.open("distances.txt", ofstream::app);
1020  //f << m_measure.faceNum << " " << sumDistances << endl;
1021  f << sumDistances << endl;
1022  f.close();
1023 
1024  f.open("distances2.txt", ofstream::app);
1025  f << m_measure.faceNum << " " << sumDistances << endl;
1026  f.close();
1027  }
1028 
1029  //double yMax = 3 + 3.8 / ( pow( meanDepth, 2 ) );
1030  //double yMax = 3 + 7 /( pow( meanDepth, 2) ) ;
1031  double yMax = 3 + 6 /( pow( meanDepth, 2) ) ;
1032  double yMin = 1 + 3.8 / ( pow( meanDepth+1.2, 2 ) );
1033 
1034  // To obtain experimental results
1035  {
1036  if ( m_measure.takeTime )
1037  m_timeLog.leave("Check if face plane: diagonal distances");
1038  }
1039 
1040  if ((( sumDistances <= yMax ) && ( sumDistances >= yMin ))&&( res ) )
1041  {
1042  /* Uncomment if you want to analyze the real size of each studied region
1043  / *ofstream f;
1044  f.open("sizes.txt", ofstream::app);
1045  double h = meanDepth/cos(DEG2RAD(faceHeight*0.2361111111111111));
1046  double realHigh = sin(DEG2RAD(faceHeight*0.2361111111111111))*h;
1047  f << realHigh << endl;
1048  f.close();*/
1049 
1050  return true;
1051  }
1052 
1053  // Uncomment if you want to analyze regions discarted by this filter
1054  /*if (( sumDistances > yMax ) || ( sumDistances < yMin ))
1055  {
1056  ofstream f;
1057  f.open("deletedDIAGONAL.txt", ofstream::app);
1058  f << m_measure.faceNum << endl;
1059  f.close();
1060  }*/
1061 
1062  return false;
1063 
1064 
1065  MRPT_END
1066 }
1067 
1068 
1069 //------------------------------------------------------------------------
1070 // checkIfDiagonalSurface2
1071 //------------------------------------------------------------------------
1072 
1073 bool CFaceDetection::checkIfDiagonalSurface2( CObservation3DRangeScan* face )
1074 {
1075  MRPT_START
1076 
1077  // To obtain experimental results
1078  {
1079  if ( m_options.useDiagonalDistanceFilter && m_measure.takeTime )
1080  m_timeLog.enter("Check if face plane: diagonal distances");
1081 
1082  if ( m_options.useSizeDistanceRelationFilter && m_measure.takeTime )
1083  m_timeLog.enter("Check if face plane: size-distance relation");
1084  }
1085 
1086  const unsigned int faceWidth = face->intensityImage.getWidth();
1087  const unsigned int faceHeight = face->intensityImage.getHeight();
1088 
1089  CMatrixTemplate<bool> region; // To save the segmented region
1090  experimental_segmentFace( *face, region);
1091 
1092  size_t cont = 0;
1093  size_t total = 0;
1094  float sumDepth = 0;
1095 
1096  vector<TPoint3D> points;
1097 
1098  for ( unsigned int row = 0; row < faceHeight; row++ )
1099  {
1100  for ( unsigned int col = 0; col < faceWidth; col++, cont++ )
1101  {
1102  if ( ( region.get_unsafe( row, col ) ) &&
1103  (*(face->confidenceImage.get_unsafe( col, row, 0 )) > m_options.confidenceThreshold ))
1104  {
1105  sumDepth += face->points3D_x[cont];
1106  total++;
1107  points.push_back( TPoint3D( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] ) );
1108  }
1109  }
1110  }
1111 
1112  double meanDepth = sumDepth / total;
1113 
1114  bool res = true;
1115 
1116  if ( m_options.useSizeDistanceRelationFilter )
1117  {
1118  double maxFaceDistance = 0.5 + 1000 / ( pow( faceWidth, 1.9 ) );
1119 
1120  // To obtain experimental results
1121  {
1122  if ( m_measure.takeTime )
1123  m_timeLog.leave("Check if face plane: size-distance relation");
1124 
1125  if ( m_options.useDiagonalDistanceFilter && m_measure.takeTime )
1126  m_timeLog.leave("Check if face plane: diagonal distances");
1127  }
1128 
1129  /*if ( maxFaceDistance > meanDepth )
1130  return true;
1131 
1132  if ( !m_options.useDiagonalDistanceFilter )
1133  return false;*/
1134 
1135  if ( maxFaceDistance < meanDepth )
1136  {
1137  // Uncomment if you want to analyze the regions discarted by this filter
1138  /*ofstream f;
1139  f.open("deletedSIZEDISTANCE.txt", ofstream::app);
1140  f << m_measure.faceNum << endl;
1141  f.close();*/
1142 
1143  //if ( !m_options.useDiagonalDistanceFilter )
1144  return false;
1145  //else
1146  // res = false;
1147  }
1148 
1149  if ( !m_options.useDiagonalDistanceFilter )
1150  return true;
1151  }
1152 
1153  ofstream f;
1154  /*f.open("relaciones1.txt", ofstream::app);
1155  f << faceWidth << endl;
1156  f.close();*/
1157 
1158  f.open("relaciones2.txt", ofstream::app);
1159  f << meanDepth << endl;
1160  f.close();
1161 
1162  //cout << m_measure.faceNum ;
1163 
1164  //experimental_viewFacePointsScanned( points );
1165 
1166  points.clear();
1167 
1168  /*if ( m_measure.faceNum > 838 )
1169  experimental_viewFacePointsScanned( points );*/
1170 
1171  //if ( ( m_measure.faceNum == 225 ) || ( m_measure.faceNum == 226 ) )
1172  //experimental_viewFacePointsScanned( points );
1173 
1174  double sumDistances = 0;
1175  double distance;
1176  size_t offsetIndex = 0;
1177 
1178  cont = 0;
1179 
1180  for ( unsigned int i = 0; i < faceHeight; i++ )
1181  {
1182  for ( unsigned int j = 0; j < faceWidth; j++, cont++ )
1183  {
1184  if ( region.get_unsafe( i, j ) )
1185  {
1186  distance = 0;
1187  if ( ( i+1 < faceHeight ) && ( j+1 < faceWidth ) )
1188  {
1189  if ( region.get_unsafe( i+1, j+1 ) )
1190  {
1191  TPoint3D p1( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] );
1192  offsetIndex = cont + faceWidth + 1;
1193  distance = p1.distanceTo( TPoint3D(face->points3D_x[offsetIndex], face->points3D_y[offsetIndex], face->points3D_z[offsetIndex]) );
1194  }
1195  else
1196  {
1197  bool validOffset = true;
1198  int offset = 2;
1199 
1200  while ( validOffset )
1201  {
1202  if ( ( i + offset < faceHeight ) && ( j + offset < faceWidth ) )
1203  {
1204  if ( region.get_unsafe( i+offset, j+offset ) )
1205  {
1206  TPoint3D p1( face->points3D_x[cont], face->points3D_y[cont], face->points3D_z[cont] );
1207  offsetIndex = cont + faceWidth + offset;
1208  distance = p1.distanceTo( TPoint3D(face->points3D_x[offsetIndex], face->points3D_y[offsetIndex], face->points3D_z[offsetIndex]) );
1209  break;
1210  }
1211  offset++;
1212  }
1213  else
1214  validOffset = false;
1215  }
1216  }
1217  }
1218 
1219  sumDistances += distance;
1220  }
1221  }
1222  }
1223 
1224  // For experimental results
1225  {
1226  if ( m_measure.takeMeasures )
1227  m_measure.sumDistances.push_back( sumDistances );
1228 
1229  ofstream f;
1230  f.open("distances.txt", ofstream::app);
1231  //f << m_measure.faceNum << " " << sumDistances << endl;
1232  f << sumDistances << endl;
1233  f.close();
1234 
1235  /*f.open("distances2.txt", ofstream::app);
1236  f << m_measure.faceNum << " " << sumDistances << endl;
1237  f.close();*/
1238  }
1239 
1240  //double yMax = 3 + 3.8 / ( pow( meanDepth, 2 ) );
1241  //double yMax = 3 + 7 /( pow( meanDepth, 2) ) ;
1242  double yMax = 3 + 11.8 /( pow( meanDepth, 0.9) ) ;
1243  double yMin = 1 + 3.8 / ( pow( meanDepth+7, 6 ) );
1244 
1245  // To obtain experimental results
1246  {
1247  if ( m_measure.takeTime )
1248  m_timeLog.leave("Check if face plane: diagonal distances");
1249  }
1250 
1251  if ((( sumDistances <= yMax ) && ( sumDistances >= yMin ))&&( res ) )
1252  {
1253  /* Uncomment if you want to analyze the real size of each studied region
1254  / *ofstream f;
1255  f.open("sizes.txt", ofstream::app);
1256  double h = meanDepth/cos(DEG2RAD(faceHeight*0.2361111111111111));
1257  double realHigh = sin(DEG2RAD(faceHeight*0.2361111111111111))*h;
1258  f << realHigh << endl;
1259  f.close();*/
1260 
1261  return true;
1262  }
1263 
1264  // Uncomment if you want to analyze regions discarted by this filter
1265  /*if (( sumDistances > yMax ) || ( sumDistances < yMin ))
1266  {
1267  ofstream f;
1268  f.open("deletedDIAGONAL.txt", ofstream::app);
1269  f << m_measure.faceNum << endl;
1270  f.close();
1271  }*/
1272 
1273  return false;
1274 
1275 
1276  MRPT_END
1277 }
1278 
1279 
1280 //------------------------------------------------------------------------
1281 // experimental_viewFacePointsScanned
1282 //------------------------------------------------------------------------
1283 
1284 void CFaceDetection::experimental_viewFacePointsScanned( const CObservation3DRangeScan &face )
1285 {
1286  vector<float> xs, ys, zs;
1287 
1288  unsigned int N = face.points3D_x.size();
1289 
1290  xs.resize(N);
1291  ys.resize(N);
1292  zs.resize(N);
1293 
1294  for ( unsigned int i = 0; i < N; i++ )
1295  {
1296  xs[i] = face.points3D_x[i];
1297  ys[i] = face.points3D_y[i];
1298  zs[i] = face.points3D_z[i];
1299  }
1300 
1301  experimental_viewFacePointsScanned( xs, ys, zs );
1302 }
1303 
1304 
1305 //------------------------------------------------------------------------
1306 // experimental_ViewFacePointsScanned
1307 //------------------------------------------------------------------------
1308 
1309 void CFaceDetection::experimental_viewFacePointsScanned( const vector<TPoint3D> &points )
1310 {
1311  vector<float> xs, ys, zs;
1312 
1313  unsigned int N = points.size();
1314 
1315  xs.resize(N);
1316  ys.resize(N);
1317  zs.resize(N);
1318 
1319  for ( unsigned int i = 0; i < N; i++ )
1320  {
1321  xs[i] = points[i].x;
1322  ys[i] = points[i].y;
1323  zs[i] = points[i].z;
1324  }
1325 
1326  experimental_viewFacePointsScanned( xs, ys, zs );
1327 }
1328 
1329 
1330 //------------------------------------------------------------------------
1331 // experimental_viewFacePointsScanned
1332 //------------------------------------------------------------------------
1333 
1334 void CFaceDetection::experimental_viewFacePointsScanned( const vector<float> &xs, const vector<float> &ys, const vector<float> &zs )
1335 {
1336  mrpt::gui::CDisplayWindow3D win3D;
1337 
1338  win3D.setWindowTitle("3D Face detected (Scanned points)");
1339 
1340  win3D.resize(400,300);
1341 
1342  win3D.setCameraAzimuthDeg(140);
1343  win3D.setCameraElevationDeg(20);
1344  win3D.setCameraZoom(6.0);
1345  win3D.setCameraPointingToPoint(2.5,0,0);
1346 
1347  mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create();
1348  gl_points->setPointSize(4.5);
1349 
1350  mrpt::opengl::COpenGLScenePtr scene = win3D.get3DSceneAndLock();
1351 
1352  scene->insert( gl_points );
1353  scene->insert( mrpt::opengl::CGridPlaneXY::Create() );
1354 
1355  CColouredPointsMap pntsMap;
1356 
1357  pntsMap.setAllPoints( xs, ys, zs );
1358 
1359  gl_points->loadFromPointsMap(&pntsMap);
1360 
1361  //gl_points->setColor(0,0.7,0.7,1);
1362 
1363  /*static int i = 0;
1364 
1365  if ( i == 2 )
1366  {
1367  mapa.setAllPoints( xs, ys, zs );
1368  i++;
1369  }
1370  else if ( i > 2 )
1371  {
1372  float run_time;
1373  CICP icp;
1374  CICP::TReturnInfo icp_info;
1375 
1376  icp.options.thresholdDist = 0.40;
1377  icp.options.thresholdAng = 0.40;
1378 
1379  CPose3DPDFPtr pdf= icp.Align3D(
1380  &mapa, // Map to align
1381  &pntsMap, // Reference map
1382  CPose3D(), // Initial gross estimate
1383  &run_time,
1384  &icp_info);
1385 
1386  cout << "ICP run took " << run_time << " secs." << endl;
1387  cout << "Goodness: " << 100*icp_info.goodness << "%" << endl;
1388  }
1389 
1390  i++;*/
1391 
1392  win3D.unlockAccess3DScene();
1393  win3D.repaint();
1394 
1395  system::pause();
1396 }
1397 
1398 
1399 //------------------------------------------------------------------------
1400 // experimental_viewFacePointsAndEigenVects
1401 //------------------------------------------------------------------------
1402 
1403 void CFaceDetection::experimental_viewFacePointsAndEigenVects( const vector<CArrayDouble<3> > &pointsVector, const CMatrixDouble &eigenVect, const CVectorDouble &eigenVal )
1404 {
1405 
1406  vector<float> xs, ys, zs;
1407 
1408  const size_t size = pointsVector.size();
1409 
1410  xs.resize( size );
1411  ys.resize( size );
1412  zs.resize( size );
1413 
1414  for ( size_t i = 0; i < size; i++ )
1415  {
1416  xs[i] = pointsVector[i][0];
1417  ys[i] = pointsVector[i][1];
1418  zs[i] = pointsVector[i][2];
1419  }
1420 
1421  TPoint3D center( sum(xs)/size, sum(ys)/size, sum(zs)/size );
1422 
1423  mrpt::gui::CDisplayWindow3D win3D;
1424 
1425  win3D.setWindowTitle("3D Face detected (Scanned points)");
1426 
1427  win3D.resize(400,300);
1428 
1429  win3D.setCameraAzimuthDeg(140);
1430  win3D.setCameraElevationDeg(20);
1431  win3D.setCameraZoom(6.0);
1432  win3D.setCameraPointingToPoint(2.5,0,0);
1433 
1434  mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create();
1435  gl_points->setPointSize(4.5);
1436 
1437  mrpt::opengl::COpenGLScenePtr scene = win3D.get3DSceneAndLock();
1438 
1439  CSpherePtr sphere = CSphere::Create(0.005f);
1440  sphere->setLocation( center );
1441  sphere->setColor( TColorf(0,1,0) );
1442  scene->insert( sphere );
1443 
1444  //TPoint3D E1( eigenVect.get_unsafe(0,0), eigenVect.get_unsafe(1,0), eigenVect.get_unsafe(2,0) );
1445  //TPoint3D E2( eigenVect.get_unsafe(0,1), eigenVect.get_unsafe(1,1), eigenVect.get_unsafe(2,1) );
1446  //TPoint3D E3( eigenVect.get_unsafe(0,2), eigenVect.get_unsafe(1,2), eigenVect.get_unsafe(2,2) );
1447 
1448  TPoint3D E1( eigenVect.get_unsafe(0,0), eigenVect.get_unsafe(0,1), eigenVect.get_unsafe(0,2) );
1449  TPoint3D E2( eigenVect.get_unsafe(1,0), eigenVect.get_unsafe(1,1), eigenVect.get_unsafe(1,2) );
1450  TPoint3D E3( eigenVect.get_unsafe(2,0), eigenVect.get_unsafe(2,1), eigenVect.get_unsafe(2,2) );
1451 
1452  //vector<TSegment3D> sgms;
1453 
1454  TPoint3D p1( center + E1*eigenVal[0]*100 );
1455  TPoint3D p2( center + E2*eigenVal[1]*100 );
1456  TPoint3D p3( center + E3*eigenVal[2]*100 );
1457 
1458  CArrowPtr arrow1 = CArrow::Create( center.x, center.y, center.z, p1.x, p1.y, p1.z );
1459  CArrowPtr arrow2 = CArrow::Create( center.x, center.y, center.z, p2.x, p2.y, p2.z );
1460  CArrowPtr arrow3 = CArrow::Create( center.x, center.y, center.z, p3.x, p3.y, p3.z );
1461 
1462  arrow1->setColor( TColorf(0,1,0) );
1463  arrow2->setColor( TColorf(1,0,0) );
1464  arrow3->setColor( TColorf(0,0,1) );
1465 
1466  scene->insert( arrow1 );
1467  scene->insert( arrow2 );
1468  scene->insert( arrow3 );
1469 
1470 
1471  //sgms.push_back( TSegment3D(center,center + E1*eigenVal[0]*100) );
1472  //sgms.push_back( TSegment3D(center,center + E2*eigenVal[1]*100) );
1473  //sgms.push_back( TSegment3D(center,center + E3*eigenVal[2]*100) );
1474  //mrpt::opengl::CSetOfLinesPtr lines = mrpt::opengl::CSetOfLines::Create( sgms );
1475  //lines->setColor(0,0,1,1);
1476  //lines->setLineWidth( 10 );
1477 
1478  //scene->insert( lines );
1479 
1480  scene->insert( gl_points );
1481  scene->insert( mrpt::opengl::CGridPlaneXY::Create() );
1482 
1483  CColouredPointsMap pntsMap;
1484 
1485  pntsMap.setAllPoints( xs, ys, zs );
1486 
1487  gl_points->loadFromPointsMap(&pntsMap);
1488 
1489  win3D.unlockAccess3DScene();
1490  win3D.repaint();
1491 
1492  system::pause();
1493 }
1494 
1495 
1496 //------------------------------------------------------------------------
1497 // experimental_viewRegions
1498 //------------------------------------------------------------------------
1499 
1500 void CFaceDetection::experimental_viewRegions( const vector<TPoint3D> regions[9], const TPoint3D meanPos[3][3] )
1501 {
1502  mrpt::gui::CDisplayWindow3D win3D;
1503 
1504  win3D.setWindowTitle("3D Face detected (Scanned points)");
1505 
1506  win3D.resize(400,300);
1507 
1508  win3D.setCameraAzimuthDeg(140);
1509  win3D.setCameraElevationDeg(20);
1510  win3D.setCameraZoom(6.0);
1511  win3D.setCameraPointingToPoint(2.5,0,0);
1512 
1513  mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create();
1514  gl_points->setPointSize(6);
1515 
1516  mrpt::opengl::COpenGLScenePtr scene = win3D.get3DSceneAndLock();
1517 
1518  if ( meanPos != NULL )
1519  {
1520  for ( size_t i = 0; i < 3; i++ )
1521  for ( size_t j = 0; j < 3; j++ )
1522  {
1523  CSpherePtr sphere = CSphere::Create(0.005f);
1524  sphere->setLocation( meanPos[i][j] );
1525  sphere->setColor( TColorf(0,1,0) );
1526  scene->insert( sphere );
1527  }
1528  }
1529 
1530  vector<TSegment3D> sgms;
1531  sgms.push_back( TSegment3D(meanPos[0][0],meanPos[0][1]) );
1532  sgms.push_back( TSegment3D(meanPos[0][1],meanPos[0][2]) );
1533  sgms.push_back( TSegment3D(meanPos[1][0],meanPos[1][1]) );
1534  sgms.push_back( TSegment3D(meanPos[1][1],meanPos[1][2]) );
1535  sgms.push_back( TSegment3D(meanPos[2][0],meanPos[2][1]) );
1536  sgms.push_back( TSegment3D(meanPos[2][1],meanPos[2][2]) );
1537  sgms.push_back( TSegment3D(meanPos[0][0],meanPos[1][1]) );
1538  sgms.push_back( TSegment3D(meanPos[1][1],meanPos[2][2]) );
1539  sgms.push_back( TSegment3D(meanPos[2][0],meanPos[1][1]) );
1540  sgms.push_back( TSegment3D(meanPos[1][1],meanPos[0][2]) );
1541  mrpt::opengl::CSetOfLinesPtr lines = mrpt::opengl::CSetOfLines::Create( sgms );
1542  lines->setColor(0,0,1,1);
1543  lines->setLineWidth( 10 );
1544 
1545  scene->insert( lines );
1546 
1547  scene->insert( gl_points );
1548  scene->insert( mrpt::opengl::CGridPlaneXY::Create() );
1549  scene->insert(mrpt::opengl::CAxis::Create(-5,-5,-5,5,5,5,2.5,3,true));
1550 
1551  CColouredPointsMap pntsMap;
1552 
1553  vector<float> xs, ys, zs;
1554 
1555  for ( size_t i = 0; i < 9; i++ )
1556  for ( unsigned int j = 0; j < regions[i].size(); j++ )
1557  {
1558  xs.push_back( regions[i][j].x );
1559  ys.push_back( regions[i][j].y );
1560  zs.push_back( regions[i][j].z );
1561  }
1562 
1563  pntsMap.setAllPoints( xs, ys, zs );
1564 
1565  int cont = 0;
1566  float colors[9][3] = {{1,0,0},{0,1,0},{0,0,1},{1,1,0},{1,0,1},{0,1,1},{0.5f,0.25f,0},{0.5f,0,0.25f},{0,0.35f,0.5f}};
1567  for ( size_t i = 0; i < 9; i++ )
1568  {
1569  float R = colors[i][0];
1570  float G = colors[i][1];
1571  float B = colors[i][2];
1572 
1573  for ( unsigned int j = 0; j < regions[i].size(); j++, cont++ )
1574  pntsMap.setPointColor( cont, R, G, B);
1575  }
1576 
1577  gl_points->loadFromPointsMap(&pntsMap);
1578  //gl_points->setColorA(0.5);
1579 
1580  win3D.unlockAccess3DScene();
1581  win3D.repaint();
1582 
1583  system::pause();
1584 }
1585 
1586 
1587 //------------------------------------------------------------------------
1588 // experimental_segmentFace
1589 //------------------------------------------------------------------------
1590 
1591 void CFaceDetection::experimental_segmentFace( const CObservation3DRangeScan &face, CMatrixTemplate<bool> &region )
1592 {
1593  const unsigned int faceWidth = face.intensityImage.getWidth();
1594  const unsigned int faceHeight = face.intensityImage.getHeight();
1595 
1596  region.setSize( faceWidth, faceHeight, true);
1597 
1598  unsigned int x1 = ceil(faceWidth*0.4);
1599  unsigned int x2 = floor(faceWidth*0.6);
1600  unsigned int y1 = ceil(faceHeight*0.4);
1601  unsigned int y2 = floor(faceHeight*0.6);
1602 
1603  region.setSize(faceHeight,faceWidth);
1604  CMatrixTemplate<size_t> toExpand;
1605  toExpand.setSize(faceHeight,faceWidth,true);
1606 
1607  unsigned int cont = ( y1 <= 1 ? 0 : faceHeight*(y1-1));
1608 
1609  //int total = 0; // JL: Unused var
1610  //int numPoints = 0; // JL: Unused var
1611 
1612  mrpt::utils::CImage img;
1613  // Normalize the image
1614  CMatrixFloat range2D = m_lastFaceDetected.rangeImage;
1615  range2D *= 1.0f/5;
1616  img.setFromMatrix(range2D);
1617 
1618  // INITIALIZATION
1619  for ( unsigned int i = y1; i <= y2; i++ )
1620  {
1621  cont += x1;
1622 
1623  for ( unsigned int j = x1; j <= x2; j++, cont++ )
1624  {
1625  if (*(face.confidenceImage.get_unsafe( j, i, 0 )) > m_options.confidenceThreshold )
1626  {
1627  //unsigned char *c = img.get_unsafe(i,j);
1628  //size_t value = (size_t)*c;
1629  //total += value;
1630  //++numPoints;
1631  toExpand.set_unsafe(i,j,1);
1632  }
1633  }
1634  cont += faceWidth-x2;
1635  }
1636 
1637  //int mean = total / numPoints;
1638 
1639  //cout << "Mean: " << mean << endl;
1640  //system::pause();
1641 
1642  // UMBRALIZATION
1643 /*
1644  for ( unsigned int row = 0; row < faceWidth; row++ )
1645  {
1646  for ( unsigned int col = 0; col < faceHeight; col++ )
1647  {
1648  unsigned char *c = img.get_unsafe(row,col);
1649  size_t value = (size_t)*c;
1650 
1651  if ( ( value < mean+7 ) && ( value > mean-7 ) )
1652  {
1653  region.set_unsafe( row, col, true );
1654  }else{
1655  img.setPixel( row, col, 0 );
1656  }
1657  }
1658  }
1659 */
1660 
1661  // REGIONS GROWING
1662 
1663  bool newExpanded = true;
1664 
1665  while ( newExpanded )
1666  {
1667  newExpanded = false;
1668 
1669  for ( size_t row = 0; row < faceHeight; row++ )
1670  {
1671  for ( size_t col = 0; col < faceWidth; col++ )
1672  {
1673  //cout << toExpand.get_unsafe( row, col ) << "" ;
1674 
1675  if ( toExpand.get_unsafe( row, col ) == 1 )
1676  {
1677  region.set_unsafe( row, col, true );
1678 
1679  unsigned char *c = img.get_unsafe(col,row);
1680  int value = (int)*c;
1681 
1682  if (( row > 0 ) && ( toExpand.get_unsafe(row-1,col) != 2 ))
1683  {
1684  unsigned char *c = img.get_unsafe(col, row-1);
1685  int value2 = (int)*c;
1686  if ( abs( value - value2 ) < 2 )
1687  {
1688  toExpand.set_unsafe(row-1,col,1);
1689  newExpanded = true;
1690  }
1691  }
1692 
1693  if (( row < faceWidth-1 ) && ( toExpand.get_unsafe(row+1,col) != 2 ))
1694  {
1695  unsigned char *c = img.get_unsafe(col, row+1);
1696  int value2 = (int)*c;
1697  if ( abs( value - value2 ) < 2 )
1698  {
1699  toExpand.set_unsafe(row+1,col,1);
1700  newExpanded = true;
1701  }
1702  }
1703 
1704  if (( col > 0) && ( toExpand.get_unsafe(row,col-1) != 2 ))
1705  {
1706  unsigned char *c = img.get_unsafe(col-1,row);
1707  int value2 = (int)*c;
1708  if ( abs( value - value2 ) < 2 )
1709  {
1710  toExpand.set_unsafe(row,col-1,1);
1711  newExpanded = true;
1712  }
1713  }
1714 
1715  if (( col < faceHeight-1) && ( toExpand.get_unsafe(row,col+1) != 2 ))
1716  {
1717  unsigned char *c = img.get_unsafe(col+1,row);
1718  int value2 = (int)*c;
1719  if ( abs( value - value2 ) < 2 )
1720  {
1721  toExpand.set_unsafe(row,col+1,1);
1722  newExpanded = true;
1723  }
1724  }
1725 
1726  toExpand.set_unsafe( row, col, 2 );
1727  }
1728  }
1729  }
1730  }
1731 
1732  for ( unsigned int row = 0; row < faceHeight; row++ )
1733  {
1734  for ( unsigned int col = 0; col < faceWidth; col++ )
1735  {
1736  if ( !(region.get_unsafe( row, col)) )
1737  {
1738  img.setPixel( col, row, 0 );
1739  }
1740  }
1741  }
1742 
1743  // Uncomment if you want to see the resultant region segmented
1744  if ( m_measure.faceNum >= 314 )
1745  {
1746  CDisplayWindow win("Live video");
1747 
1748  win.showImage( img );
1749  system::pause();
1750  }
1751 }
1752 
1753 
1754 //------------------------------------------------------------------------
1755 // experimental_calcHist
1756 //------------------------------------------------------------------------
1757 
1758 void CFaceDetection::experimental_calcHist( const CImage &face, const size_t &c1, const size_t &r1, const size_t &c2,
1759  const size_t &r2, CMatrixTemplate<unsigned int> &hist )
1760 {
1761  TImageSize size;
1762  face.getSize( size );
1763  for ( size_t row = r1; row <= r2 ; row++ )
1764  for ( size_t col = c1; col <= c2; col++ )
1765  {
1766  unsigned char *c = face.get_unsafe( col, row );
1767  size_t value = (size_t)*c;
1768  int count = hist.get_unsafe( 0, value ) + 1;
1769  hist.set_unsafe( 0, value, count );
1770  }
1771 
1772 }
1773 
1774 
1775 //------------------------------------------------------------------------
1776 // experimental_showMeasurements
1777 //------------------------------------------------------------------------
1778 
1779 void CFaceDetection::experimental_showMeasurements()
1780 {
1781  // This method execution time is not critical because it's executed only at the end
1782  // or a few times in user application
1783 
1784  ofstream f;
1785  f.open("statistics.txt", ofstream::app);
1786 
1787  if ( m_measure.lessEigenVals.size() > 0 )
1788  {
1789  double meanEigenVal, stdEigenVal;
1790  double minEigenVal = *min_element( m_measure.lessEigenVals.begin(), m_measure.lessEigenVals.end() );
1791  double maxEigenVal = *max_element( m_measure.lessEigenVals.begin(), m_measure.lessEigenVals.end() );
1792 
1793  meanAndStd( m_measure.lessEigenVals, meanEigenVal, stdEigenVal );
1794 
1795  cout << endl << "Statistical data about eigen values calculated of regions detected as faces" << endl;
1796  cout << "Min eigenVal: " << minEigenVal << endl;
1797  cout << "Max eigenVal: " << maxEigenVal << endl;
1798  cout << "Mean eigenVal: " << meanEigenVal << endl;
1799  cout << "Standard Desv: " << stdEigenVal << endl;
1800 
1801  if ( m_measure.saveMeasurementsToFile )
1802  {
1803  f << endl << "Statistical data about eigen values calculated of regions detected as faces" << endl;
1804  f << "Min eigenVal: " << minEigenVal << endl;
1805  f << "Max eigenVal: " << maxEigenVal << endl;
1806  f << "Mean eigenVal: " << meanEigenVal << endl;
1807  f << "Standard Desv: " << stdEigenVal << endl;
1808  }
1809  }
1810 
1811  if ( m_measure.sumDistances.size() > 0 )
1812  {
1813  double meanSumDist, stdSumDist;
1814  double minSumDist = *min_element( m_measure.sumDistances.begin(), m_measure.sumDistances.end() );
1815  double maxSumDist = *max_element( m_measure.sumDistances.begin(), m_measure.sumDistances.end() );
1816 
1817  meanAndStd( m_measure.sumDistances, meanSumDist, stdSumDist );
1818 
1819  cout << endl << "Statistical data about sum of distances" << endl;
1820  cout << "Min sumDistances: " << minSumDist << endl;
1821  cout << "Max sumDistances: " << maxSumDist << endl;
1822  cout << "Mean sumDistances: " << meanSumDist << endl;
1823  cout << "Standard Desv: " << stdSumDist << endl;
1824 
1825  if ( m_measure.saveMeasurementsToFile )
1826  {
1827  f << endl << "Statistical data about sum of distances" << endl;
1828  f << "Min sumDistances: " << minSumDist << endl;
1829  f << "Max sumDistances: " << maxSumDist << endl;
1830  f << "Mean sumDistances: " << meanSumDist << endl;
1831  f << "Standard Desv: " << stdSumDist << endl;
1832  }
1833  }
1834 
1835  if ( m_measure.errorEstimations.size() > 0 )
1836  {
1837  double meanEstimationErr, stdEstimationErr;
1838  double minEstimationErr = *min_element( m_measure.errorEstimations.begin(), m_measure.errorEstimations.end() );
1839  double maxEstimationErr = *max_element( m_measure.errorEstimations.begin(), m_measure.errorEstimations.end() );
1840 
1841  meanAndStd( m_measure.errorEstimations, meanEstimationErr, stdEstimationErr );
1842 
1843  cout << endl << "Statistical data about estimation error adjusting a plane of regions detected as faces" << endl;
1844  cout << "Min estimation: " << minEstimationErr << endl;
1845  cout << "Max estimation: " << maxEstimationErr << endl;
1846  cout << "Mean estimation: " << meanEstimationErr << endl;
1847  cout << "Standard Desv: " << stdEstimationErr << endl;
1848 
1849  if ( m_measure.saveMeasurementsToFile )
1850  {
1851  f << endl << "Statistical data about estimation error adjusting a plane of regions detected as faces" << endl;
1852  f << "Min estimation: " << minEstimationErr << endl;
1853  f << "Max estimation: " << maxEstimationErr << endl;
1854  f << "Mean estimation: " << meanEstimationErr << endl;
1855  f << "Standard Desv: " << stdEstimationErr << endl;
1856  }
1857  }
1858 
1859  cout << endl << "Data about number of faces" << endl;
1860  cout << "Possible faces detected: " << m_measure.numPossibleFacesDetected << endl;
1861  cout << "Real faces detected: " << m_measure.numRealFacesDetected << endl;
1862 
1863  if ( m_meanHist.size() > 0 )
1864  {
1865  double minHist = *min_element( m_meanHist.begin(), m_meanHist.end() );
1866  double maxHist = *max_element( m_meanHist.begin(), m_meanHist.end() );
1867  double meanHist;
1868  double stdHist;
1869  meanAndStd( m_meanHist, meanHist, stdHist );
1870 
1871 
1872  cout << endl << "Mean hist: " << meanHist << endl;
1873  cout << "Min hist: " << minHist << endl;
1874  cout << "Max hist: " << maxHist << endl;
1875  cout << "Stdv: " << stdHist << endl;
1876  }
1877 
1878  if ( m_measure.saveMeasurementsToFile )
1879  {
1880  f << endl << "Data about number of faces" << endl;
1881  f << "Possible faces detected: " << m_measure.numPossibleFacesDetected << endl;
1882  f << "Real faces detected: " << m_measure.numRealFacesDetected << endl;
1883  }
1884 
1885  if ( m_measure.takeTime && m_measure.saveMeasurementsToFile )
1886  f << endl << m_timeLog.getStatsAsText();
1887 
1888  f.close();
1889 
1890  mrpt::system::pause();
1891 }
1892 
1893 
1894 //------------------------------------------------------------------------
1895 // debug_returnResults
1896 //------------------------------------------------------------------------
1897 
1898 void CFaceDetection::debug_returnResults( const vector_uint &falsePositives, const vector_uint &ignore, unsigned int &falsePositivesDeleted, unsigned int &realFacesDeleted )
1899 {
1900  const unsigned int numDeleted = m_measure.deletedRegions.size();
1901  const unsigned int numFalsePositives = falsePositives.size();
1902  const unsigned int numIgnored = ignore.size();
1903  unsigned int ignoredDetected = 0;
1904 
1905  falsePositivesDeleted = 0;
1906 
1907  for ( unsigned int i = 0; i < numDeleted; i++ )
1908  {
1909  unsigned int region = m_measure.deletedRegions[i];
1910 
1911  bool falsePositive = false;
1912 
1913  unsigned int j = 0;
1914  while (!falsePositive && ( j < numFalsePositives ) )
1915  {
1916  if ( region == falsePositives[j] ) falsePositive = true;
1917  j++;
1918  }
1919 
1920  if ( falsePositive )
1921  falsePositivesDeleted++;
1922  else
1923  {
1924  bool igno = false;
1925 
1926  j = 0;
1927  while (!igno && ( j < numIgnored ) )
1928  {
1929  if ( region == ignore[j] ) igno = true;
1930  j++;
1931  }
1932 
1933  if ( igno )
1934  ignoredDetected++;
1935  }
1936  }
1937 
1938  realFacesDeleted = numDeleted - falsePositivesDeleted - ignoredDetected;
1939 
1940  m_measure.faceNum = 0;
1941  m_measure.deletedRegions.clear();
1942 }
mrpt::math::CMatrixTemplate::get_unsafe
const T & get_unsafe(size_t row, size_t col) const
Fast but unsafe method to read a value from the matrix.
Definition: CMatrixTemplate.h:358
mrpt::detectors::CFaceDetection::m_timeLog
mrpt::utils::CTimeLogger m_timeLog
Definition: CFaceDetection.h:118
meanAndStd
void meanAndStd(VEC &outMeanVector, VEC &outStdVector, const bool unbiased_variance=true) const
Computes a row with the mean values of each column in the matrix and the associated vector with the s...
Definition: eigen_plugins.h:366
count
GLuint GLuint GLsizei count
Definition: glext.h:3512
mrpt::gui::CDisplayWindow3D::unlockAccess3DScene
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
Definition: CDisplayWindow3D.cpp:537
CAxis.h
mrpt::utils::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:99
mrpt::detectors::CFaceDetection::m_end_threads
bool m_end_threads
Indicates to all threads that must finish their execution.
Definition: CFaceDetection.h:84
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPointCloudColoured.h
z
GLdouble GLdouble z
Definition: glext.h:3734
mrpt::obs::CObservation3DRangeScan::getZoneAsObs
void getZoneAsObs(CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2)
Extract a ROI of the 3D observation as a new one.
Definition: CObservation3DRangeScan.cpp:724
mrpt::detectors::CFaceDetection::m_thread_checkIfFacePlaneCov
mrpt::system::TThreadHandle m_thread_checkIfFacePlaneCov
Thread that execute checkIfFacePlaneCov filter.
Definition: CFaceDetection.h:77
mrpt::math::CMatrixBool
Declares a matrix of booleans (non serializable).
Definition: CMatrixTemplate.h:572
mrpt::vector_uint
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
mrpt::detectors::CFaceDetection::cascadeClassifier
CCascadeClassifierDetection cascadeClassifier
Definition: CFaceDetection.h:35
mrpt::detectors::CFaceDetection::m_checkIfFaceRegions_res
bool m_checkIfFaceRegions_res
Save result of checkIfFaceRegions filter.
Definition: CFaceDetection.h:80
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
CMetricMapsAlignmentAlgorithm.h
mrpt::utils::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
mrpt::detectors::CFaceDetection::debug_returnResults
void debug_returnResults(const vector_uint &falsePositives, const vector_uint &ignore, unsigned int &falsePositivesDeleted, unsigned int &realFacesDeleted)
Definition: CFaceDetection.cpp:1898
mrpt::detectors::CObjectDetection::detectObjects
void detectObjects(const mrpt::obs::CObservationPtr obs, vector_detectable_object &detected)
Definition: CObjectDetection.h:38
mrpt::system::pause
void BASE_IMPEXP pause(const std::string &msg=std::string("Press any key to continue...")) MRPT_NO_THROWS
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:435
mrpt::detectors::vector_detectable_object
std::vector< CDetectableObjectPtr > vector_detectable_object
Definition: CObjectDetection.h:22
CSetOfLines.h
mrpt::detectors::CFaceDetection::dummy_checkIfDiagonalSurface
static void dummy_checkIfDiagonalSurface(CFaceDetection *obj)
Definition: CFaceDetection.cpp:792
mrpt::detectors::CFaceDetection::m_testsOptions
struct mrpt::detectors::CFaceDetection::TTestsOptions m_testsOptions
mrpt::utils::TPixelCoord
A pair (x,y) of pixel coordinates (integer resolution).
Definition: TPixelCoord.h:37
mrpt::gui::CDisplayWindow3D::setCameraPointingToPoint
void setCameraPointingToPoint(float x, float y, float z)
Changes the camera parameters programmatically.
Definition: CDisplayWindow3D.cpp:605
offset
GLintptr offset
Definition: glext.h:3780
mrpt::obs::CObservation3DRangeScan
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
Definition: obs/CObservation3DRangeScan.h:145
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::detectors::CFaceDetection::m_meanHist
std::vector< double > m_meanHist
Definition: CFaceDetection.h:120
CICP.h
mrpt::detectors::CFaceDetection::checkRelativePosition
size_t checkRelativePosition(const mrpt::math::TPoint3D &p1, const mrpt::math::TPoint3D &p2, const mrpt::math::TPoint3D &p, double &dist)
Definition: CFaceDetection.cpp:761
mrpt::synch::CSemaphore::waitForSignal
bool waitForSignal(unsigned int timeout_ms=0)
Blocks until the count of the semaphore to be non-zero.
Definition: CSemaphore.cpp:24
mrpt::detectors::CFaceDetection::m_leave_checkIfFacePlaneCov
mrpt::synch::CSemaphore m_leave_checkIfFacePlaneCov
Indicates to main thread that thread_checkIfFacePlaneCov has been completed analisis of the last face...
Definition: CFaceDetection.h:91
mrpt::detectors::CFaceDetection::experimental_viewFacePointsAndEigenVects
void experimental_viewFacePointsAndEigenVects(const std::vector< mrpt::math::CArrayDouble< 3 > > &pointsVector, const mrpt::math::CMatrixDouble &eigenVect, const mrpt::math::CVectorDouble &eigenVal)
Definition: CFaceDetection.cpp:1403
std
STL namespace.
CGridPlaneXY.h
mrpt::math::TPoint3D::z
double z
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:248
mrpt::detectors::CFaceDetection::m_measure
struct mrpt::detectors::CFaceDetection::TMeasurement m_measure
mrpt::detectors::CFaceDetection::m_thread_checkIfFaceRegions
mrpt::system::TThreadHandle m_thread_checkIfFaceRegions
Thread that execute checkIfFaceRegions filter.
Definition: CFaceDetection.h:76
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
mrpt::system::createThread
TThreadHandle createThread(void(*func)(T), T param)
Creates a new thread from a function (or static method) with one generic parameter.
Definition: threads.h:80
mrpt::math::TSegment3D
struct BASE_IMPEXP TSegment3D
Definition: lightweight_geom_data.h:585
mrpt::detectors::CFaceDetection::m_lastFaceDetected
mrpt::obs::CObservation3DRangeScan m_lastFaceDetected
Last face detected.
Definition: CFaceDetection.h:94
mrpt::utils::CTimeLogger::enable
void enable(bool enabled=true)
Definition: CTimeLogger.h:88
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:33
points
GLsizei const GLfloat * points
Definition: glext.h:4797
mrpt::obs::CObservation3DRangeScan::rangeImage
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
Definition: obs/CObservation3DRangeScan.h:316
mrpt::utils::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:81
mrpt::opengl::CSetOfLines::Create
static CSetOfLinesPtr Create()
MRPT_TRY_END
#define MRPT_TRY_END
Definition: maps/CColouredPointsMap.h:352
mrpt::gui::CDisplayWindow3D::get3DSceneAndLock
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
Definition: CDisplayWindow3D.cpp:528
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::detectors::CFaceDetection::m_leave_checkIfDiagonalSurface
mrpt::synch::CSemaphore m_leave_checkIfDiagonalSurface
Indicates to main thread that thread_checkIfDiagonalSurface has been completed analisis of the last f...
Definition: CFaceDetection.h:92
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
mrpt::math::cov
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:135
mrpt::detectors::CFaceDetection::checkIfDiagonalSurface2
bool checkIfDiagonalSurface2(mrpt::obs::CObservation3DRangeScan *face)
Definition: CFaceDetection.cpp:1073
c
const GLubyte * c
Definition: glext.h:5590
mrpt::math::TPoint3D::distanceTo
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
Definition: lightweight_geom_data.h:288
mrpt::maps::CPointsMap::setAllPoints
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y, const std::vector< float > &Z)
Set all the points at once from vectors with X,Y and Z coordinates.
Definition: maps/CPointsMap.h:446
img
GLint GLvoid * img
Definition: glext.h:3645
mrpt::detectors::CFaceDetection::checkIfFacePlaneCov
bool checkIfFacePlaneCov(mrpt::obs::CObservation3DRangeScan *face)
Definition: CFaceDetection.cpp:364
mrpt::utils::CTimeLogger::getStatsAsText
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
Definition: CTimeLogger.cpp:146
mrpt::gui::CDisplayWindow
This class creates a window as a graphical user interface (GUI) for displaying images to the user...
Definition: CDisplayWindow.h:29
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
end
GLuint GLuint end
Definition: glext.h:3512
mrpt::detectors::CFaceDetection::m_options
struct mrpt::detectors::CFaceDetection::TOptions m_options
mrpt::synch::CSemaphore::release
void release(unsigned int increaseCount=1)
Increments the count of the semaphore by a given amount.
Definition: CSemaphore.cpp:41
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::detectors::CFaceDetection::checkIfFaceRegions
bool checkIfFaceRegions(mrpt::obs::CObservation3DRangeScan *face)
Definition: CFaceDetection.cpp:494
mrpt::math::TPlane
3D Plane, represented by its equation
Definition: lightweight_geom_data.h:893
mrpt::detectors::CFaceDetection::experimental_segmentFace
void experimental_segmentFace(const mrpt::obs::CObservation3DRangeScan &face, mrpt::math::CMatrixTemplate< bool > &region)
Definition: CFaceDetection.cpp:1591
mrpt::math::getRegressionPlane
double BASE_IMPEXP getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:1703
mrpt::detectors::CFaceDetection::checkIfDiagonalSurface
bool checkIfDiagonalSurface(mrpt::obs::CObservation3DRangeScan *face)
Definition: CFaceDetection.cpp:818
mrpt::detectors::CFaceDetection
Specific class for face detection.
Definition: CFaceDetection.h:31
mrpt::maps::CColouredPointsMap
A map of 2D/3D points with individual colours (RGB).
Definition: maps/CColouredPointsMap.h:32
mrpt::math::CMatrixTemplate::set_unsafe
void set_unsafe(size_t row, size_t col, const T &v)
Fast but unsafe method to write a value in the matrix.
Definition: CMatrixTemplate.h:347
mrpt::obs::CObservation3DRangeScan::hasPoints3D
bool hasPoints3D
true means the field points3D contains valid data.
Definition: obs/CObservation3DRangeScan.h:292
mrpt::system::joinThread
void BASE_IMPEXP joinThread(TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:74
mrpt::detectors::CFaceDetection::init
virtual void init(const mrpt::utils::CConfigFileBase &cfg)
Initialize the object with parameters loaded from the given config source.
Definition: CFaceDetection.cpp:85
CSphere.h
mrpt::gui::CDisplayWindow::showImage
void showImage(const mrpt::utils::CImage &img)
Show a given color or grayscale image on the window.
Definition: CDisplayWindow.cpp:390
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
mrpt::opengl::CGridPlaneXY::Create
static CGridPlaneXYPtr Create()
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::gui::CDisplayWindow3D::setWindowTitle
void setWindowTitle(const std::string &str) MRPT_OVERRIDE
Changes the window title.
Definition: CDisplayWindow3D.cpp:504
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
mrpt::detectors::CFaceDetection::checkIfFacePlane
bool checkIfFacePlane(mrpt::obs::CObservation3DRangeScan *face)
Definition: CFaceDetection.cpp:313
mrpt::detectors::CFaceDetection::experimental_viewFacePointsScanned
void experimental_viewFacePointsScanned(const std::vector< float > &xs, const std::vector< float > &ys, const std::vector< float > &zs)
Definition: CFaceDetection.cpp:1334
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::detectors::CCascadeClassifierDetection::init
virtual void init(const mrpt::utils::CConfigFileBase &cfg)
Initialize cascade classifier detection.
Definition: CCascadeClassifierDetection.cpp:60
mrpt::detectors::CFaceDetection::m_leave_checkIfFaceRegions
mrpt::synch::CSemaphore m_leave_checkIfFaceRegions
Indicates to main thread that thread_checkIfFaceRegions has been completed analisis of the last face ...
Definition: CFaceDetection.h:90
mrpt::detectors::CFaceDetection::detectObjects_Impl
virtual void detectObjects_Impl(const mrpt::obs::CObservation *obs, vector_detectable_object &detected)
Definition: CFaceDetection.cpp:126
mrpt::obs::CObservation
Declares a class that represents any robot&#39;s observation.
Definition: obs/CObservation.h:40
mrpt::utils::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:65
IS_CLASS
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
row
GLenum GLenum GLvoid * row
Definition: glext.h:3533
CFaceDetection.h
mrpt::detectors::CFaceDetection::dummy_checkIfFacePlaneCov
static void dummy_checkIfFacePlaneCov(CFaceDetection *obj)
Definition: CFaceDetection.cpp:340
mrpt::detectors::CFaceDetection::experimental_calcHist
void experimental_calcHist(const mrpt::utils::CImage &face, const size_t &c1, const size_t &r1, const size_t &c2, const size_t &r2, mrpt::math::CMatrixTemplate< unsigned int > &hist)
Definition: CFaceDetection.cpp:1758
mrpt::gui::CDisplayWindow3D::setCameraZoom
void setCameraZoom(float zoom)
Changes the camera parameters programmatically.
Definition: CDisplayWindow3D.cpp:623
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CAngularObservationMesh.h:23
MRPT_TRY_START
#define MRPT_TRY_START
Definition: maps/CColouredPointsMap.h:351
mrpt::math::CMatrixTemplate::getColCount
size_t getColCount() const
Number of columns in the matrix.
Definition: CMatrixTemplate.h:244
mrpt::utils::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
mrpt::detectors::CFaceDetection::dummy_checkIfFaceRegions
static void dummy_checkIfFaceRegions(CFaceDetection *obj)
Definition: CFaceDetection.cpp:468
mrpt::utils::CTimeLogger::leave
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
mrpt::math::CMatrixTemplate::getRowCount
size_t getRowCount() const
Number of rows in the matrix.
Definition: CMatrixTemplate.h:239
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::math::CMatrixTemplate::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixTemplate.h:256
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: CBaseGUIWindow.h:26
mrpt::detectors::CFaceDetection::m_enter_checkIfDiagonalSurface
mrpt::synch::CSemaphore m_enter_checkIfDiagonalSurface
Indicates to thread_checkIfDiagonalSurface that exist a new face to analyze.
Definition: CFaceDetection.h:88
mrpt::detectors::CFaceDetection::m_enter_checkIfFaceRegions
mrpt::synch::CSemaphore m_enter_checkIfFaceRegions
Indicates to thread_checkIfFaceRegions that exist a new face to analyze.
Definition: CFaceDetection.h:86
value
GLsizei const GLfloat * value
Definition: glext.h:3929
size
GLsizeiptr size
Definition: glext.h:3779
res
GLuint res
Definition: glext.h:6298
x
GLenum GLint x
Definition: glext.h:3516
start
GLuint start
Definition: glext.h:3512
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:246
mrpt::utils::CTimeLogger::enter
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
mrpt::detectors::CFaceDetection::m_thread_checkIfDiagonalSurface
mrpt::system::TThreadHandle m_thread_checkIfDiagonalSurface
Thread that execute checkIfDiagonalSurface filter.
Definition: CFaceDetection.h:78
mrpt::detectors::CFaceDetection::m_checkIfDiagonalSurface_res
bool m_checkIfDiagonalSurface_res
Save result of checkIfDiagonalSurface filter.
Definition: CFaceDetection.h:82
mrpt::gui::CDisplayWindow3D::resize
void resize(unsigned int width, unsigned int height) MRPT_OVERRIDE
Resizes the window, stretching the image to fit into the display area.
Definition: CDisplayWindow3D.cpp:453
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::detectors::CFaceDetection::m_enter_checkIfFacePlaneCov
mrpt::synch::CSemaphore m_enter_checkIfFacePlaneCov
Indicates to thread_checkIfFacePlaneCov that exist a new face to analyze.
Definition: CFaceDetection.h:87
mrpt::detectors::CFaceDetection::m_checkIfFacePlaneCov_res
bool m_checkIfFacePlaneCov_res
Save result of checkIfFacePlaneCov filter.
Definition: CFaceDetection.h:81
mrpt::math::distance
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
CArrow.h
mrpt::gui::CDisplayWindow3D::setCameraAzimuthDeg
void setCameraAzimuthDeg(float deg)
Changes the camera parameters programmatically.
Definition: CDisplayWindow3D.cpp:591
face
GLenum GLuint GLint GLenum face
Definition: glext.h:7014
mrpt::maps::CColouredPointsMap::setPointColor
void setPointColor(size_t index, float R, float G, float B)
Changes just the color of a given point from the map.
Definition: CColouredPointsMap.cpp:375
mrpt::detectors::CFaceDetection::experimental_viewRegions
void experimental_viewRegions(const std::vector< mrpt::math::TPoint3D > regions[9], const mrpt::math::TPoint3D meanPos[3][3])
Definition: CFaceDetection.cpp:1500
mrpt::opengl::CPointCloudColoured::Create
static CPointCloudColouredPtr Create()
mrpt::gui::CDisplayWindow3D::setCameraElevationDeg
void setCameraElevationDeg(float deg)
Changes the camera parameters programmatically.
Definition: CDisplayWindow3D.cpp:566
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:99
mrpt::opengl::CAxis::Create
static CAxisPtr Create()


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020