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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 1e710274b Thu May 28 15:30:14 2020 +0200 at jue may 28 15:45:10 CEST 2020