Main MRPT website > C++ reference for MRPT 1.9.9
PbMapMaker.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-2018, 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 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a
13  * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
14  */
15 
16 #include "pbmap-precomp.h" // precomp. hdr
17 
18 #if MRPT_HAS_PCL
19 
20 #include <mrpt/math/types_math.h> // Eigen
21 
22 //#include <pcl/io/io.h>
23 //#include <pcl/io/pcd_io.h>
24 #include <pcl/features/integral_image_normal.h>
25 #include <pcl/features/normal_3d.h>
26 #include <pcl/ModelCoefficients.h>
27 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
28 #include <pcl/segmentation/organized_connected_component_segmentation.h>
29 #include <pcl/filters/extract_indices.h>
30 #include <pcl/filters/voxel_grid.h>
31 #include <pcl/common/transforms.h>
32 #include <pcl/common/time.h>
33 #include <mrpt/config/CConfigFile.h>
34 #include <mrpt/pbmap/PbMapMaker.h>
35 
36 #include <mutex>
37 #include <iostream>
38 
39 //#define _VERBOSE 0
40 #define WATCH_UNARY 0
41 #define WATCH_BINARY 0
42 #define WATCH_COLOR 1
43 
44 using namespace std;
45 using namespace Eigen;
46 using namespace mrpt::pbmap;
47 
48 std::mutex CS_visualize;
49 
50 // Bhattacharyya histogram distance function
51 double BhattacharyyaDist(std::vector<float>& hist1, std::vector<float>& hist2)
52 {
53  assert(hist1.size() == hist2.size());
54  double BhattachDist;
55  double BhattachDist_aux = 0.0;
56  for (unsigned i = 0; i < hist1.size(); i++)
57  BhattachDist_aux += sqrt(hist1[i] * hist2[i]);
58 
59  BhattachDist = sqrt(1 - BhattachDist_aux);
60 
61  return BhattachDist;
62 }
63 
64 /*!Some parameters to specify input/output and some thresholds.*/
65 struct config_pbmap
66 {
67  // [global]
68  bool input_from_rawlog;
69  std::string rawlog_path;
70  bool record_rawlog;
71  float color_threshold;
72  float intensity_threshold;
73  float hue_threshold;
74 
75  // [plane_segmentation]
76  float dist_threshold; // Maximum distance to the plane between neighbor
77  // 3D-points
78  float angle_threshold; // = 0.017453 * 4.0 // Maximum angle between
79  // contiguous 3D-points
80  float minInliersRate; // Minimum ratio of inliers/image points required
81 
82  // [map_construction]
83  bool use_color; // Add color information to the planes
84  float proximity_neighbor_planes; // Two planar patches are considered
85  // neighbors when the closest distance
86  // between them is under
87  // proximity_neighbor_planes
88  // float max_angle_normals; // (10º) Two planar patches that represent the
89  // same surface must have similar normals // QUITAR
90  float max_cos_normal;
91  float max_dist_center_plane; // Two planar patches that represent the same
92  // surface must have their center in the same
93  // plane
94  float proximity_threshold; // Two planar patches that represent the same
95  // surface must overlap or be nearby
96  int graph_mode; // This var selects the condition to create edges in the
97  // graph, either proximity of planar patches or
98  // co-visibility in a single frame
99 
100  // [semantics]
101  bool inferStructure; // Infer if the planes correspond to the floor,
102  // ceiling or walls
103  bool makeClusters; // Should the PbMapMaker cluster the planes according to
104  // their co-visibility
105 
106  // [localisation]
107  bool detect_loopClosure; // Run PbMapLocaliser in a different threads to
108  // detect loop closures or preloaded PbMaps
109  string config_localiser;
110 
111  // [serialize]
112  std::string path_save_pbmap;
113  bool save_registered_cloud;
114  std::string path_save_registered_cloud;
115 
116 } configPbMap;
117 
118 void readConfigFile(const string& config_file_name)
119 {
120  mrpt::config::CConfigFile config_file(config_file_name);
121 
122  // Plane segmentation
123  configPbMap.dist_threshold = config_file.read_float(
124  "plane_segmentation", "dist_threshold", 0.04, true);
125  configPbMap.angle_threshold = config_file.read_float(
126  "plane_segmentation", "angle_threshold", 0.069812, true);
127  configPbMap.minInliersRate = config_file.read_float(
128  "plane_segmentation", "minInliersRate", 0.01, true);
129 
130  // map_construction
131  configPbMap.use_color =
132  config_file.read_bool("map_construction", "use_color", false);
133  configPbMap.color_threshold =
134  config_file.read_float("map_construction", "color_threshold", 0.09);
135  configPbMap.intensity_threshold = config_file.read_float(
136  "map_construction", "intensity_threshold", 255.0);
137  configPbMap.hue_threshold =
138  config_file.read_float("unary", "hue_threshold", 0.25);
139  configPbMap.proximity_neighbor_planes = config_file.read_float(
140  "map_construction", "proximity_neighbor_planes", 1.0);
141  configPbMap.graph_mode =
142  config_file.read_int("map_construction", "graph_mode", 1);
143  configPbMap.max_cos_normal = config_file.read_float(
144  "map_construction", "max_cos_normal", 0.9848, true);
145  configPbMap.max_dist_center_plane = config_file.read_float(
146  "map_construction", "max_dist_center_plane", 0.1, true);
147  configPbMap.proximity_threshold =
148  config_file.read_float("map_construction", "proximity_threshold", 0.15);
149 
150  // [semantics]
151  configPbMap.inferStructure =
152  config_file.read_bool("semantics", "inferStructure", true);
153  configPbMap.makeClusters =
154  config_file.read_bool("semantics", "makeCovisibilityClusters", false);
155  // configPbMap.path_prev_pbmap =
156  // config_file.read_string("localisation","path_prev_pbmap","",true);
157 
158  // [localisation]
159  configPbMap.detect_loopClosure =
160  config_file.read_bool("localisation", "detect_loopClosure", true);
161  if (configPbMap.detect_loopClosure)
162  configPbMap.config_localiser = config_file.read_string(
163  "localisation", "config_localiser", "", true);
164 
165  // serialize
166  configPbMap.path_save_pbmap =
167  config_file.read_string("serialize", "path_save_pbmap", "map");
168  configPbMap.save_registered_cloud =
169  config_file.read_bool("serialize", "save_registered_cloud", true);
170  configPbMap.path_save_registered_cloud = config_file.read_string(
171  "serialize", "path_save_registered_cloud",
172  "/home/edu/Projects/PbMaps/PbMaps.txt");
173 // cout << "path_save_registered_cloud " <<
174 // configPbMap.path_save_registered_cloud << endl;
175 
176 #ifdef _VERBOSE
177  cout << "readConfigFile configPbMap.ini dist_threshold "
178  << configPbMap.dist_threshold << endl;
179 #endif
180 }
181 
182 PbMapMaker::PbMapMaker(const string& config_file)
183  : cloudViewer("Cloud Viewer"),
184  mpPbMapLocaliser(nullptr),
185  m_pbmaker_must_stop(false),
186  m_pbmaker_finished(false)
187 {
188  // Load parameters
189  ASSERT_FILE_EXISTS_(config_file);
190  readConfigFile(config_file);
191 
192  mpPlaneInferInfo = new PlaneInferredInfo(mPbMap);
193 
194  if (configPbMap.detect_loopClosure)
195  mpPbMapLocaliser =
196  new PbMapLocaliser(mPbMap, configPbMap.config_localiser);
197 
198  if (configPbMap.makeClusters) clusterize = new SemanticClustering(mPbMap);
199 
200  pbmaker_hd = std::thread(&PbMapMaker::run, this);
201 
202  // Unary
203  rejectAreaF = 0, acceptAreaF = 0, rejectAreaT = 0, acceptAreaT = 0;
204  rejectElongF = 0, acceptElongF = 0, rejectElongT = 0, acceptElongT = 0;
205  rejectC1C2C3_F = 0, acceptC1C2C3_F = 0, rejectC1C2C3_T = 0,
206  acceptC1C2C3_T = 0;
207  rejectNrgb_F = 0, acceptNrgb_F = 0, rejectNrgb_T = 0, acceptNrgb_T = 0;
208  rejectIntensity_F = 0, acceptIntensity_F = 0, rejectIntensity_T = 0,
209  acceptIntensity_T = 0;
210  rejectColor_F = 0, acceptColor_F = 0, rejectColor_T = 0, acceptColor_T = 0;
211  rejectHistH_F = 0, acceptHistH_F = 0, rejectHistH_T = 0, acceptHistH_T = 0;
212 }
213 
214 bool PbMapMaker::arePlanesNearby(
215  Plane& plane1, Plane& plane2, const float distThreshold)
216 {
217  float distThres2 = distThreshold * distThreshold;
218 
219  // First we check distances between centroids and vertex to accelerate this
220  // check
221  if ((plane1.v3center - plane2.v3center).squaredNorm() < distThres2)
222  return true;
223 
224  for (unsigned i = 1; i < plane1.polygonContourPtr->size(); i++)
225  if ((getVector3fromPointXYZ(plane1.polygonContourPtr->points[i]) -
226  plane2.v3center)
227  .squaredNorm() < distThres2)
228  return true;
229 
230  for (unsigned j = 1; j < plane2.polygonContourPtr->size(); j++)
231  if ((plane1.v3center -
232  getVector3fromPointXYZ(plane2.polygonContourPtr->points[j]))
233  .squaredNorm() < distThres2)
234  return true;
235 
236  for (unsigned i = 1; i < plane1.polygonContourPtr->size(); i++)
237  for (unsigned j = 1; j < plane2.polygonContourPtr->size(); j++)
238  if ((diffPoints(
239  plane1.polygonContourPtr->points[i],
240  plane2.polygonContourPtr->points[j]))
241  .squaredNorm() < distThres2)
242  return true;
243 
244  // If not found yet, search properly by checking distances:
245  // a) Between an edge and a vertex
246  // b) Between two edges (imagine two polygons on perpendicular planes)
247  // c) Between a vertex and the inside of the polygon
248  // d) Or the polygons intersect
249 
250  // a) & b)
251  for (unsigned i = 1; i < plane1.polygonContourPtr->size(); i++)
252  for (unsigned j = 1; j < plane2.polygonContourPtr->size(); j++)
253  if (dist3D_Segment_to_Segment2(
254  Segment(
255  plane1.polygonContourPtr->points[i],
256  plane1.polygonContourPtr->points[i - 1]),
257  Segment(
258  plane2.polygonContourPtr->points[j],
259  plane2.polygonContourPtr->points[j - 1])) < distThres2)
260  return true;
261 
262  // c)
263  for (unsigned i = 1; i < plane1.polygonContourPtr->size(); i++)
264  if (plane2.v3normal.dot(
265  getVector3fromPointXYZ(plane1.polygonContourPtr->points[i]) -
266  plane2.v3center) < distThreshold)
267  if (isInHull(
268  plane1.polygonContourPtr->points[i],
269  plane2.polygonContourPtr))
270  return true;
271 
272  for (unsigned j = 1; j < plane2.polygonContourPtr->size(); j++)
273  if (plane1.v3normal.dot(
274  getVector3fromPointXYZ(plane2.polygonContourPtr->points[j]) -
275  plane1.v3center) < distThreshold)
276  if (isInHull(
277  plane2.polygonContourPtr->points[j],
278  plane1.polygonContourPtr))
279  return true;
280 
281  // cout << "Polygons intersect?\n";
282  // d)
283  // for(unsigned i=1; i < plane1.polygonContourPtr->size(); i++)
284  // if(
285  // plane2.v3normal.dot(getVector3fromPointXYZ(plane1.polygonContourPtr->points[i])
286  // - plane2.v3center) < distThreshold )
287  // {
288  //// cout << "Elements\n" << plane2.v3normal << "\n xyz " <<
289  /// plane1.polygonContourPtr->points[i].x << " " <<
290  /// plane1.polygonContourPtr->points[i].y << " " <<
291  /// plane1.polygonContourPtr->points[i].z
292  //// << " xyz2 " << plane1.polygonContourPtr->points[i-1].x << " "
293  ///<< plane1.polygonContourPtr->points[i-1].y << " " <<
294  /// plane1.polygonContourPtr->points[i-1].z << endl;
295  //// assert(
296  /// plane2.v3normal.dot(diffPoints(plane1.polygonContourPtr->points[i],
297  /// plane1.polygonContourPtr->points[i-1]) ) != 0 );
298  // float d = (plane2.v3normal.dot(plane2.v3center -
299  // getVector3fromPointXYZ(plane1.polygonContourPtr->points[i-1]) ) ) /
300  // (plane2.v3normal.dot(diffPoints(plane1.polygonContourPtr->points[i],
301  // plane1.polygonContourPtr->points[i-1]) ) );
302  // PointT intersection;
303  // intersection.x = d * plane1.polygonContourPtr->points[i].x + (1-d) *
304  // plane1.polygonContourPtr->points[i-1].x;
305  // intersection.y = d * plane1.polygonContourPtr->points[i].y + (1-d) *
306  // plane1.polygonContourPtr->points[i-1].y;
307  // intersection.z = d * plane1.polygonContourPtr->points[i].z + (1-d) *
308  // plane1.polygonContourPtr->points[i-1].z;
309  // if(isInHull(intersection, plane2.polygonContourPtr) )
310  // return true;
311  // }
312  // if (plane1.id==2 && frameQueue.size() == 12)
313  // cout << "pasaFinal\n";
314  return false;
315 }
316 
317 void PbMapMaker::checkProximity(Plane& plane, float proximity)
318 {
319  for (unsigned i = 0; i < mPbMap.vPlanes.size(); i++)
320  {
321  if (plane.id == mPbMap.vPlanes[i].id) continue;
322 
323  if (plane.nearbyPlanes.count(mPbMap.vPlanes[i].id)) continue;
324 
325  if (arePlanesNearby(
326  plane, mPbMap.vPlanes[i], proximity)) // If the planes are
327  // closer than proximity
328  // (in meters), then mark
329  // them as neighbors
330  {
331  plane.nearbyPlanes.insert(mPbMap.vPlanes[i].id);
332  mPbMap.vPlanes[i].nearbyPlanes.insert(plane.id);
333  }
334  }
335 }
336 
337 //// double area_THRESHOLD=10.0;
338 // double area_THRESHOLD_inv=1/configPbMap.area_THRESHOLD;
339 //// double elongation_THRESHOLD=1.15;
340 // double elongation_THRESHOLD_inv=1/configPbMap.elongation_THRESHOLD;
341 //// double color_threshold=0.3;
342 // double colorDev_THRESHOLD;
343 // double dist_THRESHOLD = 1.6; double dist_THRESHOLD_inv =
344 // 1/dist_THRESHOLD;
345 //// double dist_THRESHOLDFull = 1.2; double dist_THRESHOLDFull_inv =
346 /// 1/dist_THRESHOLDFull;
347 // double height_THRESHOLD=0.5;
348 // double height_THRESHOLD_parallel=0.03;
349 // double normal_THRESHOLD=1.0;
350 // double ppaldir_normal_THRESHOLD=8;
351 //
352 // double singleC_THRESHOLD = 0.2;
353 // double elong_rely_ppal_THRESHOLD=1.5;
354 // double elong_rely_ppal_THRESHOLD_tight=1.3;
355 
356 void PbMapMaker::watchProperties(
357  set<unsigned>& observedPlanes, Plane& observedPlane)
358 {
359  cout << "PbMapMaker::watchProperties..." << configPbMap.color_threshold
360  << " " << configPbMap.intensity_threshold << " "
361  << configPbMap.hue_threshold << "\n";
362  // Watch properties
363  // double tCondition;
364  // mrpt::system::CTicTac clock;
365  // colorDev_THRESHOLD=configPbMap.color_threshold/10;
366 
367  if (observedPlane.numObservations > 2)
368  {
369  // Previous instanes of same plane
370  for (size_t i = 0; i < observedPlane.prog_Nrgb.size(); i++)
371  {
372 #if WATCH_UNARY
373  // tCondition = pcl::getTime();
374  // clock.Tic();
375  // Check area unary
376  double rel_areas = observedPlane.area / observedPlane.prog_area[i];
377  if (rel_areas < area_THRESHOLD_inv ||
378  rel_areas > configPbMap.area_THRESHOLD)
379  rejectAreaT++;
380  else
381  acceptAreaT++;
382  // timeCompareArea.push_back( (pcl::getTime() - tCondition)*1e6
383  // );
384  // timeCompareArea.push_back( clock.Tac()*1e6 );
385 
386  // tCondition = pcl::getTime();
387  // Check ratio unary
388  double rel_ratios =
389  observedPlane.elongation / observedPlane.prog_elongation[i];
390  if (rel_ratios < elongation_THRESHOLD_inv ||
391  rel_ratios > configPbMap.elongation_THRESHOLD)
392  rejectElongT++;
393  else
394  acceptElongT++;
395 // timeCompareElong.push_back( (pcl::getTime() - tCondition)*1e6 );
396 #endif
397 
398 #if WATCH_COLOR
399  // tCondition = pcl::getTime();
400  // Check colorC1C2C3 unary
401  double dif_C1C2C3 =
402  (observedPlane.v3colorC1C2C3 - observedPlane.prog_C1C2C3[i])
403  .norm();
404  // cout << "color1 " << observedPlane.v3colorC1C2C3 << " color2 "
405  // << prevPlane.v3colorC1C2C3 << endl;
406  if (dif_C1C2C3 > configPbMap.color_threshold)
407  rejectC1C2C3_T++;
408  else
409  acceptC1C2C3_T++;
410  // timeCompareC1C2C3.push_back( (pcl::getTime() - tCondition)*1e6
411  // );
412 
413  double dif_Nrgb =
414  (observedPlane.v3colorNrgb - observedPlane.prog_Nrgb[i]).norm();
415  // cout << "color1 " << observedPlane.v3colorNrgb << " color2 "
416  // << prevPlane.v3colorNrgb << endl;
417  // if( dif_Nrgb > configPbMap.color_threshold ||
418  // fabs(observedPlane.dominantIntensity -
419  // observedPlane.prog_intensity[i]) > 255)
420  if (dif_Nrgb > configPbMap.color_threshold)
421  rejectNrgb_T++;
422  else
423  acceptNrgb_T++;
424  // timeCompareNrgb.push_back( (pcl::getTime() - tCondition)*1e6
425  // );
426 
427  if (fabs(
428  observedPlane.dominantIntensity -
429  observedPlane.prog_intensity[i]) >
430  configPbMap.intensity_threshold)
431  rejectIntensity_T++;
432  else
433  acceptIntensity_T++;
434 
435  if (fabs(
436  observedPlane.dominantIntensity -
437  observedPlane.prog_intensity[i]) >
438  configPbMap.intensity_threshold ||
439  dif_Nrgb > configPbMap.color_threshold)
440  rejectColor_T++;
441  else
442  acceptColor_T++;
443 
444  // Hue histogram
445  double hist_dist = BhattacharyyaDist(
446  observedPlane.hist_H, observedPlane.prog_hist_H[i]);
447  if (hist_dist > configPbMap.hue_threshold)
448  rejectHistH_T++;
449  else
450  acceptHistH_T++;
451 #endif
452  }
453 
454  // cout << "Watch rejection. Planes " << mPbMap.vPlanes.size() << "\n";
455  // Get unary rejection rate
456  for (size_t i = 0; i < mPbMap.vPlanes.size(); i++)
457  {
458  if (i == observedPlane.id) continue;
459 
460  Plane& prevPlane = mPbMap.vPlanes[i];
461 // cout << "color prev plane " <<
462 // prevPlane.v3colorNrgb.transpose() << " " <<
463 // prevPlane.dominantIntensity << " " << prevPlane.bDominantColor
464 // << endl;
465 
466 #if WATCH_UNARY
467  // tCondition = pcl::getTime();
468  // clock.Tic();
469  double rel_areas = observedPlane.area / prevPlane.area;
470  if (rel_areas < area_THRESHOLD_inv ||
471  rel_areas > configPbMap.area_THRESHOLD)
472  rejectAreaF++;
473  else
474  acceptAreaF++;
475  // timeCompareArea.push_back( (pcl::getTime() - tCondition)*1e6
476  // );
477  // timeCompareArea.push_back( clock.Tac()*1e6 );
478 
479  // tCondition = pcl::getTime();
480  double rel_ratios = observedPlane.elongation / prevPlane.elongation;
481  if (rel_ratios < elongation_THRESHOLD_inv ||
482  rel_ratios > configPbMap.elongation_THRESHOLD)
483  rejectElongF++;
484  else
485  acceptElongF++;
486 // timeCompareElong.push_back( (pcl::getTime() - tCondition)*1e6 );
487 #endif
488 
489 #if WATCH_COLOR
490  // tCondition = pcl::getTime();
491  double dif_C1C2C3 =
492  (observedPlane.v3colorC1C2C3 - prevPlane.v3colorC1C2C3).norm();
493  // cout << "color1 " << observedPlane.v3colorC1C2C3 << "
494  // color2 " << prevPlane.v3colorC1C2C3 << endl;
495  if (dif_C1C2C3 > configPbMap.color_threshold)
496  rejectC1C2C3_F++;
497  else
498  acceptC1C2C3_F++;
499  // timeCompareC1C2C3.push_back( (pcl::getTime() - tCondition)*1e6
500  // );
501 
502  // // Nrgb
503  double dif_Nrgb =
504  (observedPlane.v3colorNrgb - prevPlane.v3colorNrgb).norm();
505  // cout << "color1 " << observedPlane.v3colorNrgb << "
506  // color2 " << prevPlane.v3colorNrgb << endl;
507  // if( dif_Nrgb > configPbMap.color_threshold ||
508  // fabs(observedPlane.dominantIntensity -
509  // prevPlane.dominantIntensity) > 255)
510  if (dif_Nrgb > configPbMap.color_threshold)
511  rejectNrgb_F++;
512  else
513  acceptNrgb_F++;
514  // timeCompareNrgb.push_back( (pcl::getTime() - tCondition)*1e6
515  // );
516 
517  // cout << "intensity conditions\n";
518  // cout << " elements " << observedPlane.dominantIntensity <<" "
519  // << prevPlane.dominantIntensity << " " <<
520  // configPbMap.intensity_threshold;
521  // cout << " " << rejectIntensity_F << " " << acceptIntensity_F <<
522  // endl;
523 
524  if (fabs(
525  observedPlane.dominantIntensity -
526  prevPlane.dominantIntensity) >
527  configPbMap.intensity_threshold)
528  rejectIntensity_F++;
529  else
530  acceptIntensity_F++;
531 
532  if (fabs(
533  observedPlane.dominantIntensity -
534  prevPlane.dominantIntensity) >
535  configPbMap.intensity_threshold ||
536  dif_Nrgb > configPbMap.color_threshold)
537  rejectColor_F++;
538  else
539  acceptColor_F++;
540  // cout << "rejectColor_F " << rejectColor_F << "acceptColor_F " <<
541  // acceptColor_F << endl;
542 
543  // Hue histogram
544  double hist_dist =
545  BhattacharyyaDist(observedPlane.hist_H, prevPlane.hist_H);
546  if (hist_dist > configPbMap.hue_threshold)
547  rejectHistH_F++;
548  else
549  acceptHistH_F++;
550 // cout << "finish reject conditions\n";
551 
552 #endif
553  }
554 
555 #if WATCH_UNARY
556  observedPlane.prog_area.push_back(observedPlane.area);
557  // observedPlane.prog_v3center.push_back(observedPlane.v3center);
558  // observedPlane.prog_v3normal.push_back(observedPlane.v3normal);
559  observedPlane.prog_elongation.push_back(observedPlane.elongation);
560 // observedPlane.prog_v3PpalDir.push_back(observedPlane.v3PpalDir);
561 #endif
562 
563 // cout << "Update progression\n";
564 
565 #if WATCH_COLOR
566  observedPlane.prog_C1C2C3.push_back(observedPlane.v3colorC1C2C3);
567  observedPlane.prog_Nrgb.push_back(observedPlane.v3colorNrgb);
568  observedPlane.prog_intensity.push_back(observedPlane.dominantIntensity);
569  observedPlane.prog_hist_H.push_back(observedPlane.hist_H);
570 #endif
571  }
572  cout << " ...Watching finished\n";
573 }
574 
575 void PbMapMaker::saveInfoFiles()
576 {
577  cout << "PbMapMaker::saveInfoFiles(...)\n";
578 
579  // cout << "DiscAreaF rate " << rejectAreaF/(rejectAreaF+acceptAreaF) << "
580  // meas " << rejectAreaF+acceptAreaF << endl;
581  // cout << "DiscElongF rate " << rejectElongF/(rejectElongF+acceptElongF)
582  // << " meas " << rejectElongF+acceptElongF << endl;
583  // cout << "DiscC1C2C3_F rate " <<
584  // rejectC1C2C3_F/(rejectC1C2C3_F+acceptC1C2C3_F) << " meas " <<
585  // rejectC1C2C3_F+acceptC1C2C3_F << endl;
586  //
587  // cout << "DiscAreaT rate " << rejectAreaT/(rejectAreaT+acceptAreaT) << "
588  // meas " << rejectAreaT+acceptAreaT << endl;
589  // cout << "DiscElongT rate " << rejectElongT/(rejectElongT+acceptElongT)
590  // << " meas " << rejectElongT+acceptElongT << endl;
591  // cout << "DiscC1C2C3_T rate " <<
592  // rejectC1C2C3_T/(rejectC1C2C3_T+acceptC1C2C3_T) << " meas " <<
593  // rejectC1C2C3_T+acceptC1C2C3_T << endl;
594 
595  string results_file;
596  ofstream file;
597 
598 #if WATCH_UNARY
599  results_file = "results/areaRestriction.txt";
600  file.open(results_file.c_str(), ios::app);
601  file << configPbMap.area_THRESHOLD << " " << rejectAreaT << " "
602  << acceptAreaT << " " << rejectAreaF << " " << acceptAreaF << endl;
603  file.close();
604 
605  results_file = "results/elongRestriction.txt";
606  file.open(results_file.c_str(), ios::app);
607  file << configPbMap.elongation_THRESHOLD << " " << rejectElongT << " "
608  << acceptElongT << " " << rejectElongF << " " << acceptElongF << endl;
609  file.close();
610 #endif
611 
612 #if WATCH_COLOR
613  results_file = "results/c1c2c3Restriction.txt";
614  file.open(results_file.c_str(), ios::app);
615  file << configPbMap.color_threshold << " " << rejectC1C2C3_T << " "
616  << acceptC1C2C3_T << " " << rejectC1C2C3_F << " " << acceptC1C2C3_F
617  << endl;
618  file.close();
619 
620  results_file = "results/NrgbRestriction.txt";
621  file.open(results_file.c_str(), ios::app);
622  file << configPbMap.color_threshold << " " << rejectNrgb_T << " "
623  << acceptNrgb_T << " " << rejectNrgb_F << " " << acceptNrgb_F << endl;
624  file.close();
625 
626  results_file = "results/IntensityRestriction.txt";
627  file.open(results_file.c_str(), ios::app);
628  file << configPbMap.intensity_threshold << " " << rejectIntensity_T << " "
629  << acceptIntensity_T << " " << rejectIntensity_F << " "
630  << acceptIntensity_F << endl;
631  file.close();
632 
633  results_file = "results/ColorRestriction.txt";
634  file.open(results_file.c_str(), ios::app);
635  file << configPbMap.intensity_threshold << " " << rejectColor_T << " "
636  << acceptColor_T << " " << rejectColor_F << " " << acceptColor_F
637  << endl;
638  file.close();
639 
640  results_file = "results/HueRestriction.txt";
641  file.open(results_file.c_str(), ios::app);
642  file << configPbMap.hue_threshold << " " << rejectHistH_T << " "
643  << acceptHistH_T << " " << rejectHistH_F << " " << acceptHistH_F
644  << endl;
645  file.close();
646 #endif
647 }
648 
649 void PbMapMaker::detectPlanesCloud(
650  pcl::PointCloud<PointT>::Ptr& pointCloudPtr_arg, Eigen::Matrix4f& poseKF,
651  double distThreshold, double angleThreshold, double minInliersF)
652 {
653  boost::mutex::scoped_lock updateLock(mtx_pbmap_busy);
654 
655  unsigned minInliers = minInliersF * pointCloudPtr_arg->size();
656 
657 #ifdef _VERBOSE
658  cout << "detectPlanes in a cloud with " << pointCloudPtr_arg->size()
659  << " points " << minInliers << " minInliers\n";
660 #endif
661 
662  pcl::PointCloud<PointT>::Ptr pointCloudPtr_arg2(
663  new pcl::PointCloud<PointT>);
664  pcl::copyPointCloud(*pointCloudPtr_arg, *pointCloudPtr_arg2);
665 
666  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedCloudPtr(
667  new pcl::PointCloud<pcl::PointXYZRGBA>);
668  pcl::transformPointCloud(*pointCloudPtr_arg, *alignedCloudPtr, poseKF);
669 
670  {
671  std::lock_guard<std::mutex> csl(CS_visualize);
672  *mPbMap.globalMapPtr += *alignedCloudPtr;
673  // Downsample voxel map's point cloud
674  static pcl::VoxelGrid<pcl::PointXYZRGBA> grid;
675  grid.setLeafSize(0.02, 0.02, 0.02);
676  pcl::PointCloud<pcl::PointXYZRGBA> globalMap;
677  grid.setInputCloud(mPbMap.globalMapPtr);
678  grid.filter(globalMap);
679  mPbMap.globalMapPtr->clear();
680  *mPbMap.globalMapPtr = globalMap;
681  } // End CS
682 
683  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
684  ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
685  ne.setMaxDepthChangeFactor(0.02f); // For VGA: 0.02f, 10.0f
686  ne.setNormalSmoothingSize(10.0f);
687  ne.setDepthDependentSmoothing(true);
688 
689  pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
690  mps.setMinInliers(minInliers);
691  cout << "Params " << minInliers << " " << angleThreshold << " "
692  << distThreshold << endl;
693  mps.setAngularThreshold(angleThreshold); // (0.017453 * 2.0) // 3 degrees
694  mps.setDistanceThreshold(distThreshold); // 2cm
695 
696  pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(
697  new pcl::PointCloud<pcl::Normal>);
698  ne.setInputCloud(pointCloudPtr_arg2);
699  ne.compute(*normal_cloud);
700 
701 #ifdef _VERBOSE
702  double plane_extract_start = pcl::getTime();
703 #endif
704  mps.setInputNormals(normal_cloud);
705  mps.setInputCloud(pointCloudPtr_arg2);
706 
707  std::vector<pcl::PlanarRegion<PointT>,
708  aligned_allocator<pcl::PlanarRegion<PointT>>>
709  regions;
710  std::vector<pcl::ModelCoefficients> model_coefficients;
711  std::vector<pcl::PointIndices> inlier_indices;
712  pcl::PointCloud<pcl::Label>::Ptr labels(new pcl::PointCloud<pcl::Label>);
713  std::vector<pcl::PointIndices> label_indices;
714  std::vector<pcl::PointIndices> boundary_indices;
715  mps.segmentAndRefine(
716  regions, model_coefficients, inlier_indices, labels, label_indices,
717  boundary_indices);
718 
719 #ifdef _VERBOSE
720  double plane_extract_end = pcl::getTime();
721  std::cout << "Plane extraction took "
722  << double(plane_extract_end - plane_extract_start) << std::endl;
723  // std::cout << "Frame took " << double (plane_extract_end -
724  // normal_start) << std::endl;
725  cout << regions.size() << " planes detected\n";
726 #endif
727 
728  // Create a vector with the planes detected in this keyframe, and calculate
729  // their parameters (normal, center, pointclouds, etc.)
730  // in the global reference
731  vector<Plane> detectedPlanes;
732  for (size_t i = 0; i < regions.size(); i++)
733  {
734  Plane plane;
735 
736  Vector3f centroid = regions[i].getCentroid();
737  plane.v3center = compose(poseKF, centroid);
738  plane.v3normal =
739  poseKF.block(0, 0, 3, 3) * Vector3f(
740  model_coefficients[i].values[0],
741  model_coefficients[i].values[1],
742  model_coefficients[i].values[2]);
743  // plane.curvature = regions[i].getCurvature();
744  // assert(plane.v3normal*plane.v3center.transpose() <= 0);
745  // if(plane.v3normal*plane.v3center.transpose() <= 0)
746  // plane.v3normal *= -1;
747 
748  // Extract the planar inliers from the input cloud
749  pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
750  extract.setInputCloud(pointCloudPtr_arg2);
751  extract.setIndices(
752  boost::make_shared<const pcl::PointIndices>(inlier_indices[i]));
753  extract.setNegative(false);
754  extract.filter(
755  *plane.planePointCloudPtr); // Write the planar point cloud
756 
757  static pcl::VoxelGrid<pcl::PointXYZRGBA> plane_grid;
758  plane_grid.setLeafSize(0.05, 0.05, 0.05);
759  pcl::PointCloud<pcl::PointXYZRGBA> planeCloud;
760  plane_grid.setInputCloud(plane.planePointCloudPtr);
761  plane_grid.filter(planeCloud);
762  plane.planePointCloudPtr->clear();
763  pcl::transformPointCloud(planeCloud, *plane.planePointCloudPtr, poseKF);
764 
765  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contourPtr(
766  new pcl::PointCloud<pcl::PointXYZRGBA>);
767  contourPtr->points = regions[i].getContour();
768  plane_grid.setLeafSize(0.1, 0.1, 0.1);
769  plane_grid.setInputCloud(contourPtr);
770  plane_grid.filter(*plane.polygonContourPtr);
771  // plane.contourPtr->points = regions[i].getContour();
772  // pcl::transformPointCloud(*plane.contourPtr,*plane.polygonContourPtr,poseKF);
773  pcl::transformPointCloud(*plane.polygonContourPtr, *contourPtr, poseKF);
774  plane.calcConvexHull(contourPtr);
775  plane.computeMassCenterAndArea();
776  plane.areaVoxels = plane.planePointCloudPtr->size() * 0.0025;
777 
778 #ifdef _VERBOSE
779  cout << "Area plane region " << plane.areaVoxels << " of Chull "
780  << plane.areaHull << " of polygon "
781  << plane.compute2DPolygonalArea() << endl;
782 #endif
783 
784  // Check whether this region correspond to the same plane as a previous
785  // one (this situation may happen when there exists a small
786  // discontinuity in the observation)
787  bool isSamePlane = false;
788  for (size_t j = 0; j < detectedPlanes.size(); j++)
789  if (areSamePlane(
790  detectedPlanes[j], plane, configPbMap.max_cos_normal,
791  configPbMap.max_dist_center_plane,
792  configPbMap.proximity_threshold)) // The planes are merged
793  // if they are the same
794  {
795  isSamePlane = true;
796 
797  mergePlanes(detectedPlanes[j], plane);
798 
799 #ifdef _VERBOSE
800  cout << "\tTwo regions support the same plane in the same "
801  "KeyFrame\n";
802 #endif
803 
804  break;
805  }
806  if (!isSamePlane) detectedPlanes.push_back(plane);
807  }
808 
809 #ifdef _VERBOSE
810  cout << detectedPlanes.size() << " Planes detected\n";
811 #endif
812 
813  // Merge detected planes with previous ones if they are the same
814  size_t numPrevPlanes = mPbMap.vPlanes.size();
815  // set<unsigned> observedPlanes;
816  observedPlanes.clear();
817  {
818  std::lock_guard<std::mutex> csl(CS_visualize);
819  for (size_t i = 0; i < detectedPlanes.size(); i++)
820  {
821  // Check similarity with previous planes detected
822  bool isSamePlane = false;
823  vector<Plane>::iterator itPlane = mPbMap.vPlanes.begin();
824  // if(frameQueue.size() != 12)
825  for (size_t j = 0; j < numPrevPlanes;
826  j++, itPlane++) // numPrevPlanes
827  {
828  if (areSamePlane(
829  mPbMap.vPlanes[j], detectedPlanes[i],
830  configPbMap.max_cos_normal,
831  configPbMap.max_dist_center_plane,
832  configPbMap.proximity_threshold)) // The planes are
833  // merged if they are
834  // the same
835  {
836  // if (j==2 && frameQueue.size() == 12)
837  // {
838  // cout << "Same plane\n";
839  //
840  //// ofstream pbm;
841  //// pbm.open("comparePlanes.txt");
842  // {
843  // cout << " ID " << mPbMap.vPlanes[j].id << "
844  // obs " << mPbMap.vPlanes[j].numObservations;
845  // cout << " areaVoxels " <<
846  // mPbMap.vPlanes[j].areaVoxels << " areaVoxels "
847  // << mPbMap.vPlanes[j].areaHull;
848  // cout << " ratioXY " <<
849  // mPbMap.vPlanes[j].elongation << " structure "
850  // << mPbMap.vPlanes[j].bFromStructure << " label
851  // " << mPbMap.vPlanes[j].label;
852  // cout << "\n normal\n" <<
853  // mPbMap.vPlanes[j].v3normal << "\n center\n" <<
854  // mPbMap.vPlanes[j].v3center;
855  // cout << "\n PpalComp\n" <<
856  // mPbMap.vPlanes[j].v3PpalDir << "\n RGB\n" <<
857  // mPbMap.vPlanes[j].v3colorNrgb;
858  // cout << "\n Neighbors (" <<
859  // mPbMap.vPlanes[j].neighborPlanes.size() << "):
860  // ";
861  // for(map<unsigned,unsigned>::iterator
862  // it=mPbMap.vPlanes[j].neighborPlanes.begin();
863  // it != mPbMap.vPlanes[j].neighborPlanes.end();
864  // it++)
865  // cout << it->first << " ";
866  // cout << "\n CommonObservations: ";
867  // for(map<unsigned,unsigned>::iterator
868  // it=mPbMap.vPlanes[j].neighborPlanes.begin();
869  // it != mPbMap.vPlanes[j].neighborPlanes.end();
870  // it++)
871  // cout << it->second << " ";
872  // cout << "\n ConvexHull (" <<
873  // mPbMap.vPlanes[j].polygonContourPtr->size() <<
874  // "): \n";
875  // for(unsigned jj=0; jj <
876  // mPbMap.vPlanes[j].polygonContourPtr->size();
877  // jj++)
878  // cout << "\t" <<
879  // mPbMap.vPlanes[j].polygonContourPtr->points[jj].x
880  // << " " <<
881  // mPbMap.vPlanes[j].polygonContourPtr->points[jj].y
882  // << " " <<
883  // mPbMap.vPlanes[j].polygonContourPtr->points[jj].z
884  // << endl;
885  // cout << endl;
886  // }
887  // {
888  //// cout << " ID " << detectedPlanes[i].id << "
889  /// obs " << detectedPlanes[i].numObservations;
890  //// cout << " areaVoxels " <<
891  /// detectedPlanes[i].areaVoxels << " areaVoxels " <<
892  /// detectedPlanes[i].areaHull;
893  //// cout << " ratioXY " <<
894  /// detectedPlanes[i].elongation << " structure " <<
895  /// detectedPlanes[i].bFromStructure << " label " <<
896  /// detectedPlanes[i].label;
897  // cout << "\n normal\n" <<
898  // detectedPlanes[i].v3normal << "\n center\n" <<
899  // detectedPlanes[i].v3center;
900  //// cout << "\n PpalComp\n" <<
901  /// detectedPlanes[i].v3PpalDir << "\n RGB\n" <<
902  /// detectedPlanes[i].v3colorNrgb;
903  //// cout << "\n Neighbors (" <<
904  /// detectedPlanes[i].neighborPlanes.size() << "): ";
905  //// for(map<unsigned,unsigned>::iterator
906  /// it=detectedPlanes[i].neighborPlanes.begin(); it !=
907  /// detectedPlanes[i].neighborPlanes.end(); it++)
908  //// cout << it->first << " ";
909  //// cout << "\n CommonObservations: ";
910  //// for(map<unsigned,unsigned>::iterator
911  /// it=detectedPlanes[i].neighborPlanes.begin(); it !=
912  /// detectedPlanes[i].neighborPlanes.end(); it++)
913  //// cout << it->second << " ";
914  // cout << "\n ConvexHull (" <<
915  // detectedPlanes[i].polygonContourPtr->size() <<
916  // "): \n";
917  // for(unsigned jj=0; jj <
918  // detectedPlanes[i].polygonContourPtr->size();
919  // jj++)
920  // cout << "\t" <<
921  // detectedPlanes[i].polygonContourPtr->points[jj].x
922  // << " " <<
923  // detectedPlanes[i].polygonContourPtr->points[jj].y
924  // << " " <<
925  // detectedPlanes[i].polygonContourPtr->points[jj].z
926  // << endl;
927  // cout << endl;
928  // }
929  //// pbm.close();
930  // }
931 
932  isSamePlane = true;
933 
934  mergePlanes(mPbMap.vPlanes[j], detectedPlanes[i]);
935 
936  // Update proximity graph
937  checkProximity(
938  mPbMap.vPlanes[j],
939  configPbMap
940  .proximity_neighbor_planes); // Detect neighbors
941 
942 #ifdef _VERBOSE
943  cout << "Previous plane " << mPbMap.vPlanes[j].id
944  << " area " << mPbMap.vPlanes[j].areaVoxels
945  << " of polygon "
946  << mPbMap.vPlanes[j].compute2DPolygonalArea() << endl;
947 #endif
948 
949  if (observedPlanes.count(mPbMap.vPlanes[j].id) ==
950  0) // If this plane has already been observed through a
951  // previous partial plane in this same keyframe,
952  // then we must not account twice in the observation
953  // count
954  {
955  mPbMap.vPlanes[j].numObservations++;
956 
957  // Update co-visibility graph
958  for (set<unsigned>::iterator it =
959  observedPlanes.begin();
960  it != observedPlanes.end(); it++)
961  if (mPbMap.vPlanes[j].neighborPlanes.count(*it))
962  {
963  mPbMap.vPlanes[j].neighborPlanes[*it]++;
964  mPbMap.vPlanes[*it].neighborPlanes
965  [mPbMap.vPlanes[j]
966  .id]++; // j = mPbMap.vPlanes[j]
967  }
968  else
969  {
970  mPbMap.vPlanes[j].neighborPlanes[*it] = 1;
971  mPbMap.vPlanes[*it]
972  .neighborPlanes[mPbMap.vPlanes[j].id] = 1;
973  }
974 
975  observedPlanes.insert(mPbMap.vPlanes[j].id);
976  }
977 
978 #ifdef _VERBOSE
979  cout << "Same plane\n";
980 #endif
981 
982  itPlane++;
983  for (size_t k = j + 1; k < numPrevPlanes;
984  k++, itPlane++) // numPrevPlanes
985  if (areSamePlane(
986  mPbMap.vPlanes[j], mPbMap.vPlanes[k],
987  configPbMap.max_cos_normal,
988  configPbMap.max_dist_center_plane,
989  configPbMap.proximity_threshold)) // The planes
990  // are merged
991  // if they
992  // are the
993  // same
994  {
995  mergePlanes(mPbMap.vPlanes[j], mPbMap.vPlanes[k]);
996 
997  mPbMap.vPlanes[j].numObservations +=
998  mPbMap.vPlanes[k].numObservations;
999 
1000  for (set<unsigned>::iterator it =
1001  mPbMap.vPlanes[k].nearbyPlanes.begin();
1002  it != mPbMap.vPlanes[k].nearbyPlanes.end();
1003  it++)
1004  mPbMap.vPlanes[*it].nearbyPlanes.erase(
1005  mPbMap.vPlanes[k].id);
1006 
1007  for (map<unsigned, unsigned>::iterator it =
1008  mPbMap.vPlanes[k].neighborPlanes.begin();
1009  it != mPbMap.vPlanes[k].neighborPlanes.end();
1010  it++)
1011  mPbMap.vPlanes[it->first].neighborPlanes.erase(
1012  mPbMap.vPlanes[k].id);
1013 
1014  // Update plane index
1015  for (size_t h = k + 1; h < numPrevPlanes; h++)
1016  --mPbMap.vPlanes[h].id;
1017 
1018  for (size_t h = 0; h < numPrevPlanes; h++)
1019  {
1020  if (k == h) continue;
1021 
1022  for (set<unsigned>::iterator it =
1023  mPbMap.vPlanes[h].nearbyPlanes.begin();
1024  it != mPbMap.vPlanes[h].nearbyPlanes.end();
1025  it++)
1026  if (*it > mPbMap.vPlanes[k].id)
1027  {
1028  mPbMap.vPlanes[h].nearbyPlanes.insert(
1029  *it - 1);
1030  mPbMap.vPlanes[h].nearbyPlanes.erase(
1031  *it);
1032  }
1033 
1034  for (map<unsigned, unsigned>::iterator it =
1035  mPbMap.vPlanes[h]
1036  .neighborPlanes.begin();
1037  it !=
1038  mPbMap.vPlanes[h].neighborPlanes.end();
1039  it++)
1040  if (it->first > mPbMap.vPlanes[k].id)
1041  {
1042  mPbMap.vPlanes[h]
1043  .neighborPlanes[it->first - 1] =
1044  it->second;
1045  mPbMap.vPlanes[h].neighborPlanes.erase(
1046  it);
1047  }
1048  }
1049 
1050  mPbMap.vPlanes.erase(itPlane);
1051  --numPrevPlanes;
1052 
1053 #ifdef _VERBOSE
1054  cout << "MERGE TWO PREVIOUS PLANES WHEREBY THE "
1055  "INCORPORATION OF A NEW REGION \n";
1056 #endif
1057  }
1058 
1059  break;
1060  }
1061  }
1062  if (!isSamePlane)
1063  {
1064  detectedPlanes[i].id = mPbMap.vPlanes.size();
1065  detectedPlanes[i].numObservations = 1;
1066  detectedPlanes[i].bFullExtent = false;
1067  detectedPlanes[i].nFramesAreaIsStable = 0;
1068  // detectedPlanes[i].calcMainColor(calcMainColor();
1069  if (configPbMap.makeClusters)
1070  {
1071  detectedPlanes[i].semanticGroup =
1072  clusterize->currentSemanticGroup;
1073  clusterize->groups[clusterize->currentSemanticGroup]
1074  .push_back(detectedPlanes[i].id);
1075  }
1076 
1077 #ifdef _VERBOSE
1078  cout << "New plane " << detectedPlanes[i].id << " area "
1079  << detectedPlanes[i].areaVoxels << " of polygon "
1080  << detectedPlanes[i].areaHull << endl;
1081 #endif
1082 
1083  // Update proximity graph
1084  checkProximity(
1085  detectedPlanes[i],
1086  configPbMap.proximity_neighbor_planes); // Detect neighbors
1087  // with max
1088  // separation of
1089  // 1.0 meters
1090 
1091  // Update co-visibility graph
1092  for (set<unsigned>::iterator it = observedPlanes.begin();
1093  it != observedPlanes.end(); it++)
1094  {
1095  detectedPlanes[i].neighborPlanes[*it] = 1;
1096  mPbMap.vPlanes[*it].neighborPlanes[detectedPlanes[i].id] =
1097  1;
1098  }
1099 
1100  observedPlanes.insert(detectedPlanes[i].id);
1101 
1102  mPbMap.vPlanes.push_back(detectedPlanes[i]);
1103  }
1104  }
1105  }
1106 
1107 // if(frameQueue.size() == 12)
1108 // cout << "Same plane? " << areSamePlane(mPbMap.vPlanes[2],
1109 // mPbMap.vPlanes[9], configPbMap.max_cos_normal,
1110 // configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) <<
1111 // endl;
1112 
1113 #ifdef _VERBOSE
1114  cout << "\n\tobservedPlanes: ";
1115  cout << observedPlanes.size() << " Planes observed\n";
1116  for (set<unsigned>::iterator it = observedPlanes.begin();
1117  it != observedPlanes.end(); it++)
1118  cout << *it << " ";
1119  cout << endl;
1120 #endif
1121 
1122  // For all observed planes
1123  for (set<unsigned>::iterator it = observedPlanes.begin();
1124  it != observedPlanes.end(); it++)
1125  {
1126  Plane& observedPlane = mPbMap.vPlanes[*it];
1127 
1128  // Calculate principal direction
1129  observedPlane.calcElongationAndPpalDir();
1130 
1131  ////cout << "Update color\n";
1132  // Update color
1133  observedPlane.calcMainColor();
1134 
1135 #ifdef _VERBOSE
1136  cout << "Plane " << observedPlane.id << " color\n"
1137  << observedPlane.v3colorNrgb << endl;
1138 #endif
1139 
1140  // Infer knowledge from the planes (e.g. do these planes represent the
1141  // floor, walls, etc.)
1142  if (configPbMap.inferStructure)
1143  mpPlaneInferInfo->searchTheFloor(poseKF, observedPlane);
1144  } // End for obsevedPlanes
1145  // cout << "Updated planes\n";
1146 
1147  // for(set<unsigned>::iterator it = observedPlanes.begin(); it !=
1148  // observedPlanes.end(); it++)
1149  // {
1150  // Plane &observedPlane = mPbMap.vPlanes[*it];
1151  // watchProperties(observedPlanes, observedPlane); // Color paper
1152  // }
1153 
1154  // Search the floor plane
1155  if (mPbMap.FloorPlane !=
1156  -1) // Verify that the observed planes centers are above the floor
1157  {
1158 #ifdef _VERBOSE
1159  cout << "Verify that the observed planes centers are above the floor\n";
1160 #endif
1161 
1162  for (set<unsigned>::reverse_iterator it = observedPlanes.rbegin();
1163  it != observedPlanes.rend(); it++)
1164  {
1165  if (static_cast<int>(*it) == mPbMap.FloorPlane) continue;
1166  if (mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(
1167  mPbMap.vPlanes[*it].v3center -
1168  mPbMap.vPlanes[mPbMap.FloorPlane].v3center) < -0.1)
1169  {
1170  if (mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(
1171  mPbMap.vPlanes[*it].v3normal) >
1172  0.99) //(cos 8.1º = 0.99)
1173  {
1174  mPbMap.vPlanes[*it].label = "Floor";
1175  mPbMap.vPlanes[mPbMap.FloorPlane].label = "";
1176  mPbMap.FloorPlane = *it;
1177  }
1178  else
1179  {
1180  // assert(false);
1181  mPbMap.vPlanes[mPbMap.FloorPlane].label = "";
1182  mPbMap.FloorPlane = -1;
1183  break;
1184  }
1185  }
1186  }
1187  }
1188 
1189  if (configPbMap.detect_loopClosure)
1190  for (set<unsigned>::iterator it = observedPlanes.begin();
1191  it != observedPlanes.end(); it++)
1192  {
1193  // cout << "insert planes\n";
1194  if (mpPbMapLocaliser->vQueueObservedPlanes.size() < 10)
1195  mpPbMapLocaliser->vQueueObservedPlanes.push_back(*it);
1196  }
1197 
1198 #ifdef _VERBOSE
1199  cout << "DetectedPlanesCloud finished\n";
1200 #endif
1201 
1202  updateLock.unlock();
1203 }
1204 
1205 bool graphRepresentation = false;
1206 void keyboardEventOccurred(
1207  const pcl::visualization::KeyboardEvent& event, void* viewer_void)
1208 {
1209  if ((event.getKeySym() == "r" || event.getKeySym() == "R") &&
1210  event.keyDown())
1211  {
1212  graphRepresentation = !graphRepresentation;
1213  }
1214 }
1215 
1216 /*!Check if the the input plane is the same than this plane for some given angle
1217  * and distance thresholds.
1218  * If the planes are the same they are merged in this and the function returns
1219  * true. Otherwise it returns false.*/
1220 bool PbMapMaker::areSamePlane(
1221  Plane& plane1, Plane& plane2, const float& cosAngleThreshold,
1222  const float& distThreshold, const float& proxThreshold)
1223 {
1224  // Check that both planes have similar orientation
1225  if (plane1.v3normal.dot(plane2.v3normal) < cosAngleThreshold) return false;
1226  // if(plane1.id == 2)
1227  // cout << "normal " << plane1.v3normal.dot(plane2.v3normal) << " " <<
1228  // cosAngleThreshold << endl;
1229 
1230  // Check the normal distance of the planes centers using their average
1231  // normal
1232  float dist_normal = plane1.v3normal.dot(plane2.v3center - plane1.v3center);
1233  // if(fabs(dist_normal) > distThreshold ) // Avoid matching different
1234  // parallel planes
1235  // return false;
1236  float thres_max_dist =
1237  max(distThreshold,
1238  distThreshold * 2 * (plane2.v3center - plane1.v3center).norm());
1239  if (fabs(dist_normal) >
1240  thres_max_dist) // Avoid matching different parallel planes
1241  return false;
1242  // if(plane1.id == 2)
1243  // {
1244  // cout << "dist_normal " << dist_normal << " " << thres_max_dist <<
1245  // endl;
1246  // if(arePlanesNearby(plane1, plane2, proxThreshold))
1247  // cout << "planes rearby" << endl;
1248  // }
1249 
1250  // Once we know that the planes are almost coincident (parallelism and
1251  // position)
1252  // we check that the distance between the planes is not too big
1253  return arePlanesNearby(plane1, plane2, proxThreshold);
1254 }
1255 
1256 void PbMapMaker::mergePlanes(Plane& updatePlane, Plane& discardPlane)
1257 {
1258  // Update normal and center
1259  updatePlane.v3normal = updatePlane.areaVoxels * updatePlane.v3normal +
1260  discardPlane.areaVoxels * discardPlane.v3normal;
1261  updatePlane.v3normal = updatePlane.v3normal / (updatePlane.v3normal).norm();
1262  // Update point inliers
1263  // *updatePlane.polygonContourPtr += *discardPlane.polygonContourPtr; //
1264  // Merge polygon points
1265  *updatePlane.planePointCloudPtr +=
1266  *discardPlane.planePointCloudPtr; // Add the points of the new
1267  // detection and perform a voxel grid
1268 
1269  // Filter the points of the patch with a voxel-grid. This points are used
1270  // only for visualization
1271  static pcl::VoxelGrid<pcl::PointXYZRGBA> merge_grid;
1272  merge_grid.setLeafSize(0.05, 0.05, 0.05);
1273  pcl::PointCloud<pcl::PointXYZRGBA> mergeCloud;
1274  merge_grid.setInputCloud(updatePlane.planePointCloudPtr);
1275  merge_grid.filter(mergeCloud);
1276  updatePlane.planePointCloudPtr->clear();
1277  *updatePlane.planePointCloudPtr = mergeCloud;
1278 
1279  // if(configPbMap.use_color)
1280  // updatePlane.calcMainColor();
1281 
1282  *discardPlane.polygonContourPtr += *updatePlane.polygonContourPtr;
1283  updatePlane.calcConvexHull(discardPlane.polygonContourPtr);
1284  updatePlane.computeMassCenterAndArea();
1285 
1286  // Move the points to fulfill the plane equation
1287  updatePlane.forcePtsLayOnPlane();
1288 
1289  // Update area
1290  double area_recalc = updatePlane.planePointCloudPtr->size() * 0.0025;
1291  mpPlaneInferInfo->isFullExtent(updatePlane, area_recalc);
1292  updatePlane.areaVoxels = updatePlane.planePointCloudPtr->size() * 0.0025;
1293 }
1294 
1295 // Color = (red[i], grn[i], blu[i])
1296 // The color order is: red, green, blue, yellow, pink, turquoise, orange,
1297 // purple, dark green, beige
1298 unsigned char red[10] = {255, 0, 0, 255, 255, 0, 255, 204, 0, 255};
1299 unsigned char grn[10] = {0, 255, 0, 255, 0, 255, 160, 51, 128, 222};
1300 unsigned char blu[10] = {0, 0, 255, 0, 255, 255, 0, 204, 0, 173};
1301 
1302 double ared[10] = {1.0, 0, 0, 1.0, 1.0, 0, 1.0, 0.8, 0, 1.0};
1303 double agrn[10] = {0, 1.0, 0, 1.0, 0, 1.0, 0.6, 0.2, 0.5, 0.9};
1304 double ablu[10] = {0, 0, 1.0, 0, 1.0, 1.0, 0, 0.8, 0, 0.7};
1305 
1306 void PbMapMaker::viz_cb(pcl::visualization::PCLVisualizer& viz)
1307 {
1308  if (mPbMap.globalMapPtr->empty())
1309  {
1310  std::this_thread::sleep_for(10ms);
1311  return;
1312  }
1313 
1314  {
1315  std::lock_guard<std::mutex> csl(CS_visualize);
1316 
1317  // Render the data
1318  {
1319  viz.removeAllShapes();
1320  viz.removeAllPointClouds();
1321 
1322  char name[1024];
1323 
1324  // if(graphRepresentation)
1325  // {
1326  // for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
1327  // {
1328  // pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0],
1329  // 2*mPbMap.vPlanes[i].v3center[1],
1330  // 2*mPbMap.vPlanes[i].v3center[2]);
1331  // double radius = 0.1 *
1332  // sqrt(mPbMap.vPlanes[i].areaVoxels);
1333  // sprintf (name, "sphere%u", static_cast<unsigned>(i));
1334  // viz.addSphere (center, radius, ared[i%10], agrn[i%10],
1335  // ablu[i%10], name);
1336  //
1337  // if( !mPbMap.vPlanes[i].label.empty() )
1338  // viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1,
1339  // ared[i%10], agrn[i%10], ablu[i%10],
1340  // mPbMap.vPlanes[i].label);
1341  // else
1342  // {
1343  // sprintf (name, "P%u", static_cast<unsigned>(i));
1344  // viz.addText3D (name, center, 0.1, ared[i%10],
1345  // agrn[i%10], ablu[i%10], name);
1346  // }
1347  //
1348  // // Draw edges
1349  // if(!configPbMap.graph_mode) // Nearby neighbors
1350  // for(set<unsigned>::iterator it =
1351  // mPbMap.vPlanes[i].nearbyPlanes.begin(); it !=
1352  // mPbMap.vPlanes[i].nearbyPlanes.end(); it++)
1353  // {
1354  // if(*it > mPbMap.vPlanes[i].id)
1355  // break;
1356  //
1357  // sprintf (name, "commonObs%u_%u",
1358  // static_cast<unsigned>(i),
1359  // static_cast<unsigned>(*it));
1360  // pcl::PointXYZ
1361  // center_it(2*mPbMap.vPlanes[*it].v3center[0],
1362  // 2*mPbMap.vPlanes[*it].v3center[1],
1363  // 2*mPbMap.vPlanes[*it].v3center[2]);
1364  // viz.addLine (center, center_it, ared[i%10],
1365  // agrn[i%10], ablu[i%10], name);
1366  // }
1367  // else
1368  // for(map<unsigned,unsigned>::iterator it =
1369  // mPbMap.vPlanes[i].neighborPlanes.begin(); it !=
1370  // mPbMap.vPlanes[i].neighborPlanes.end(); it++)
1371  // {
1372  // if(it->first > mPbMap.vPlanes[i].id)
1373  // break;
1374  //
1375  // sprintf (name, "commonObs%u_%u",
1376  // static_cast<unsigned>(i),
1377  // static_cast<unsigned>(it->first));
1378  // pcl::PointXYZ
1379  // center_it(2*mPbMap.vPlanes[it->first].v3center[0],
1380  // 2*mPbMap.vPlanes[it->first].v3center[1],
1381  // 2*mPbMap.vPlanes[it->first].v3center[2]);
1382  // viz.addLine (center, center_it, ared[i%10],
1383  // agrn[i%10], ablu[i%10], name);
1384  //
1385  // sprintf (name, "edge%u_%u",
1386  // static_cast<unsigned>(i),
1387  // static_cast<unsigned>(it->first));
1388  // char commonObs[8];
1389  // sprintf (commonObs, "%u", it->second);
1390  // pcl::PointXYZ half_edge( (center_it.x+center.x)/2,
1391  // (center_it.y+center.y)/2, (center_it.z+center.z)/2
1392  // );
1393  // viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0,
1394  // 1.0, name);
1395  // }
1396  //
1397  // }
1398  // }
1399  // else
1400  { // Regular representation
1401 
1402  if (graphRepresentation)
1403  {
1404  if (!viz.updatePointCloud(mPbMap.globalMapPtr, "cloud"))
1405  viz.addPointCloud(mPbMap.globalMapPtr, "cloud");
1406  return;
1407  }
1408 
1409  if (mpPbMapLocaliser != nullptr)
1410  if (mpPbMapLocaliser->alignedModelPtr)
1411  {
1412  if (!viz.updatePointCloud(
1413  mpPbMapLocaliser->alignedModelPtr, "model"))
1414  viz.addPointCloud(
1415  mpPbMapLocaliser->alignedModelPtr, "model");
1416  }
1417 
1418  sprintf(
1419  name, "PointCloud size %u",
1420  static_cast<unsigned>(mPbMap.globalMapPtr->size()));
1421  viz.addText(name, 10, 20);
1422 
1423  // pcl::ModelCoefficients plane_coefs;
1424  // plane_coefs.values[0] = mPbMap.vPlanes[0].v3normal[0];
1425  // plane_coefs.values[1] = mPbMap.vPlanes[0].v3normal[1];
1426  // plane_coefs.values[2] = mPbMap.vPlanes[0].v3normal[2];
1427  // plane_coefs.values[3] = -(mPbMap.vPlanes[0].v3normal .dot
1428  // (mPbMap.vPlanes[0].v3center) );
1429  // viz.addPlane (plane_coefs);
1430 
1431  for (size_t i = 0; i < mPbMap.vPlanes.size(); i++)
1432  {
1433  Plane& plane_i = mPbMap.vPlanes[i];
1434  sprintf(name, "normal_%u", static_cast<unsigned>(i));
1435  pcl::PointXYZ pt1, pt2; // Begin and end points of normal's
1436  // arrow for visualization
1437  pt1 = pcl::PointXYZ(
1438  plane_i.v3center[0], plane_i.v3center[1],
1439  plane_i.v3center[2]);
1440  pt2 = pcl::PointXYZ(
1441  plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
1442  plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
1443  plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
1444  viz.addArrow(
1445  pt2, pt1, ared[i % 10], agrn[i % 10], ablu[i % 10],
1446  false, name);
1447 
1448  // Draw Ppal diretion
1449  // if( plane_i.elongation > 1.3 )
1450  // {
1451  // sprintf (name, "ppalComp_%u",
1452  // static_cast<unsigned>(i));
1453  // pcl::PointXYZ pt3 = pcl::PointXYZ (
1454  // plane_i.v3center[0] + (0.2f *
1455  // plane_i.v3PpalDir[0]),
1456  // plane_i.v3center[1]
1457  // + (0.2f *
1458  // plane_i.v3PpalDir[1]),
1459  // plane_i.v3center[2]
1460  // + (0.2f *
1461  // plane_i.v3PpalDir[2]));
1462  // viz.addArrow (pt3, plane_i.pt1, ared[i%10],
1463  // agrn[i%10], ablu[i%10], false, name);
1464  // }
1465 
1466  // if( !plane_i.label.empty() )
1467  // viz.addText3D (plane_i.label, pt2, 0.1,
1468  // ared[i%10], agrn[i%10], ablu[i%10],
1469  // plane_i.label);
1470  // else
1471  {
1472  sprintf(name, "n%u", static_cast<unsigned>(i));
1473  // sprintf (name, "n%u_%u",
1474  // static_cast<unsigned>(i),
1475  // static_cast<unsigned>(plane_i.semanticGroup));
1476  viz.addText3D(
1477  name, pt2, 0.1, ared[i % 10], agrn[i % 10],
1478  ablu[i % 10], name);
1479  }
1480 
1481  // sprintf (name, "planeRaw_%02u",
1482  // static_cast<unsigned>(i));
1483  // viz.addPointCloud
1484  // (plane_i.planeRawPointCloudPtr, name);//
1485  // contourPtr, planePointCloudPtr,
1486  // polygonContourPtr
1487 
1488  // if(!configPbMap.makeClusters)
1489  // {
1490  sprintf(name, "plane_%02u", static_cast<unsigned>(i));
1491  // if(plane_i.bDominantColor)
1492  {
1493  // pcl::visualization::PointCloudColorHandlerCustom
1494  // <PointT> color (plane_i.planePointCloudPtr,
1495  // red[i%10], grn[i%10], blu[i%10]);
1496  ////
1497  /// pcl::visualization::PointCloudColorHandlerCustom
1498  ///< PointT> color (plane_i.planePointCloudPtr,
1499  /// red[plane_i.semanticGroup%10],
1500  /// grn[plane_i.semanticGroup%10],
1501  /// blu[plane_i.semanticGroup%10]);
1502  // viz.addPointCloud
1503  // (plane_i.planePointCloudPtr, color, name);
1504  // viz.setPointCloudRenderingProperties
1505  // (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1506  // 2, name);
1507  // }
1508  // else
1509  // {
1510  // sprintf (name, "plane_%02u",
1511  // static_cast<unsigned>(i));
1512  // pcl::visualization::PointCloudColorHandlerCustom
1513  // <PointT> color
1514  // (plane_i.planePointCloudPtr,
1515  // red[plane_i.semanticGroup%10],
1516  // grn[plane_i.semanticGroup%10],
1517  // blu[plane_i.semanticGroup%10]);
1518 
1519  double illum = 0;
1520  if (fabs(plane_i.v3colorNrgb[0] - 0.33) < 0.03 &&
1521  fabs(plane_i.v3colorNrgb[1] - 0.33) < 0.03 &&
1522  fabs(plane_i.v3colorNrgb[2] - 0.33) < 0.03 &&
1523  plane_i.dominantIntensity > 400)
1524  illum = 0.5;
1525 
1526  pcl::visualization::PointCloudColorHandlerCustom<PointT>
1527  color(
1528  plane_i.planePointCloudPtr,
1529  plane_i.v3colorNrgb[0] *
1530  (plane_i.dominantIntensity +
1531  (755 - plane_i.dominantIntensity) * illum),
1532  plane_i.v3colorNrgb[1] *
1533  (plane_i.dominantIntensity +
1534  (755 - plane_i.dominantIntensity) * illum),
1535  plane_i.v3colorNrgb[2] *
1536  (plane_i.dominantIntensity +
1537  (755 - plane_i.dominantIntensity) *
1538  illum));
1539  // pcl::visualization::PointCloudColorHandlerCustom
1540  // <PointT> color
1541  // (plane_i.planePointCloudPtr,
1542  // plane_i.v3colorNrgb[0] * plane_i.dominantIntensity,
1543  // plane_i.v3colorNrgb[1] * plane_i.dominantIntensity,
1544  // plane_i.v3colorNrgb[2] * plane_i.dominantIntensity);
1545  viz.addPointCloud(
1546  plane_i.planePointCloudPtr, color, name);
1547  viz.setPointCloudRenderingProperties(
1548  pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10,
1549  name);
1550  // }
1551  }
1552  // else
1553  // viz.addPointCloud (plane_i.planePointCloudPtr,
1554  // name);// contourPtr, planePointCloudPtr,
1555  // polygonContourPtr
1556 
1557  // sprintf (name, "planeBorder_%02u",
1558  // static_cast<unsigned>(i));
1559  // pcl::visualization::PointCloudColorHandlerCustom
1560  // <PointT> color2 (plane_i.contourPtr, 255, 255,
1561  // 255);
1562  // viz.addPointCloud (plane_i.contourPtr, color2,
1563  // name);// contourPtr, planePointCloudPtr,
1564  // polygonContourPtr
1565 
1566  // //Edges
1567  // if(mPbMap.edgeCloudPtr->size() > 0)
1568  // {
1569  // sprintf (name, "planeEdge_%02u",
1570  // static_cast<unsigned>(i));
1571  // pcl::visualization::PointCloudColorHandlerCustom
1572  // <PointT> color4 (mPbMap.edgeCloudPtr, 255,
1573  // 255, 0);
1574  // viz.addPointCloud (mPbMap.edgeCloudPtr,
1575  // color4, name);// contourPtr,
1576  // planePointCloudPtr, polygonContourPtr
1577  // viz.setPointCloudRenderingProperties
1578  // (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1579  // 5, name);
1580  //
1581  // sprintf (name, "edge%u",
1582  // static_cast<unsigned>(i));
1583  // viz.addLine
1584  // (mPbMap.edgeCloudPtr->points.front(),
1585  // mPbMap.edgeCloudPtr->points.back(), ared[3],
1586  // agrn[3], ablu[3], name);
1587  // }
1588 
1589  sprintf(name, "approx_plane_%02d", int(i));
1590  viz.addPolygon<PointT>(
1591  plane_i.polygonContourPtr, 0.5 * red[i % 10],
1592  0.5 * grn[i % 10], 0.5 * blu[i % 10], name);
1593  }
1594 
1595  // if(configPbMap.makeClusters)
1596  // for(map<unsigned, std::vector<unsigned> >::iterator
1597  // it=clusterize->groups.begin(); it !=
1598  // clusterize->groups.end(); it++)
1599  // for(size_t i=0; i < it->second.size(); i++)
1600  // {
1601  // unsigned planeID = it->second[i];
1602  // Plane &plane_i = mPbMap.vPlanes[planeID];
1603  // sprintf (name, "plane_%02u",
1604  // static_cast<unsigned>(planeID));
1605  // pcl::visualization::PointCloudColorHandlerCustom
1606  // <PointT> color (plane_i.planePointCloudPtr,
1607  // red[planeID%10], grn[planeID%10], blu[planeID%10]);
1608  // viz.addPointCloud (plane_i.planePointCloudPtr, color,
1609  // name);// contourPtr, planePointCloudPtr,
1610  // polygonContourPtr
1611  // viz.setPointCloudRenderingProperties
1612  // (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3,
1613  // name);
1614  // }
1615 
1616  // Draw recognized plane labels
1617  if (mpPbMapLocaliser != nullptr)
1618  for (map<string, pcl::PointXYZ>::iterator it =
1619  mpPbMapLocaliser->foundPlaces.begin();
1620  it != mpPbMapLocaliser->foundPlaces.end(); it++)
1621  viz.addText3D(
1622  it->first, it->second, 0.3, 0.9, 0.9, 0.9,
1623  it->first);
1624  }
1625  }
1626  }
1627 }
1628 
1629 void PbMapMaker::run()
1630 {
1631  cloudViewer.runOnVisualizationThread(
1632  boost::bind(&PbMapMaker::viz_cb, this, _1), "viz_cb");
1633  cloudViewer.registerKeyboardCallback(keyboardEventOccurred);
1634 
1635  size_t numPrevKFs = 0;
1636  size_t minGrowPlanes = 5;
1637  while (!m_pbmaker_must_stop) // Stop loop if PbMapMaker
1638  {
1639  if (numPrevKFs == frameQueue.size())
1640  {
1641  std::this_thread::sleep_for(10ms);
1642  }
1643  else
1644  {
1645  // Assign pointCloud of last KF to the global map
1646  detectPlanesCloud(
1647  frameQueue.back().cloudPtr, frameQueue.back().pose,
1648  configPbMap.dist_threshold, configPbMap.angle_threshold,
1649  configPbMap.minInliersRate);
1650 
1651  if (configPbMap.makeClusters)
1652  {
1653  if (mPbMap.vPlanes.size() > minGrowPlanes)
1654  {
1655  // Evaluate the partition of the current groups with minNcut
1656  int size_partition =
1657  clusterize->evalPartition(observedPlanes);
1658  cout << "PARTITION SIZE " << size_partition << endl;
1659  assert(size_partition < 2);
1660 
1661  minGrowPlanes += 2;
1662  }
1663  }
1664 
1665  ++numPrevKFs;
1666  }
1667  }
1668 
1669  // saveInfoFiles(); // save watch statistics
1670 
1671  m_pbmaker_finished = true;
1672 }
1673 
1674 void PbMapMaker::serializePbMap(string path)
1675 {
1676  boost::mutex::scoped_lock updateLock(mtx_pbmap_busy);
1677 
1678  mPbMap.savePbMap(path);
1679 
1680  updateLock.unlock();
1681 }
1682 
1683 bool PbMapMaker::stop_pbMapMaker()
1684 {
1685  m_pbmaker_must_stop = true;
1686  while (!m_pbmaker_finished) std::this_thread::sleep_for(1ms);
1687  cout << "Waiting for PbMapMaker thread to die.." << endl;
1688 
1689  pbmaker_hd.join();
1690 
1691  return true;
1692 }
1693 
1694 PbMapMaker::~PbMapMaker()
1695 {
1696  cout << "\n\n\nPbMapMaker destructor called -> Save color information to "
1697  "file\n";
1698  saveInfoFiles();
1699 
1700  delete mpPlaneInferInfo;
1701  delete mpPbMapLocaliser;
1702 
1703  stop_pbMapMaker();
1704 
1705  cout << " .. PbMapMaker has died." << endl;
1706 }
1707 
1708 #endif
graphRepresentation
bool graphRepresentation
Definition: PbMapMaker.cpp:1205
configPbMap
struct config_pbmap configPbMap
config_pbmap::path_save_pbmap
std::string path_save_pbmap
Definition: PbMapMaker.cpp:112
mrpt::pbmap::Plane::compute2DPolygonalArea
float compute2DPolygonalArea()
Compute the area of a 2D planar polygon patch - using a given normal.
Definition: Plane.cpp:249
mrpt::pbmap::PbMapMaker::rejectHistH_T
float rejectHistH_T
Definition: PbMapMaker.h:175
mrpt::pbmap::PbMapMaker::saveInfoFiles
void saveInfoFiles()
Definition: PbMapMaker.cpp:575
config_pbmap::detect_loopClosure
bool detect_loopClosure
Definition: PbMapMaker.cpp:107
mrpt::pbmap::PbMapMaker::pbmaker_hd
std::thread pbmaker_hd
Definition: PbMapMaker.h:152
mrpt::pbmap::Plane::calcMainColor
void calcMainColor()
Definition: Plane.cpp:603
mrpt::pbmap::PbMapMaker::acceptNrgb_F
float acceptNrgb_F
Definition: PbMapMaker.h:171
blu
unsigned char blu[10]
Definition: PbMapMaker.cpp:1300
mrpt::pbmap::PbMapMaker::acceptC1C2C3_F
float acceptC1C2C3_F
Definition: PbMapMaker.h:170
mrpt::pbmap::PbMap::vPlanes
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:57
Eigen
Definition: eigen_frwds.h:15
mrpt::pbmap::PbMapMaker::acceptIntensity_T
float acceptIntensity_T
Definition: PbMapMaker.h:173
mrpt::pbmap::PbMap::savePbMap
void savePbMap(std::string filePath)
Definition: PbMap.cpp:85
mrpt::pbmap::Plane::dominantIntensity
float dominantIntensity
Definition: Plane.h:160
red
unsigned char red[10]
Definition: PbMapMaker.cpp:1298
mrpt::pbmap::PbMapMaker::acceptAreaT
float acceptAreaT
Definition: PbMapMaker.h:168
ablu
double ablu[10]
Definition: PbMapMaker.cpp:1304
mrpt::pbmap::PbMapLocaliser
Definition: PbMapLocaliser.h:40
mrpt::pbmap::Plane::hist_H
std::vector< float > hist_H
Definition: Plane.h:165
mrpt::pbmap::PbMapMaker::watchProperties
void watchProperties(std::set< unsigned > &observedPlanes, Plane &observedPlane)
double dist_THRESHOLDFull = 1.2; double dist_THRESHOLDFull_inv = 1/dist_THRESHOLDFull;
Definition: PbMapMaker.cpp:356
mrpt::pbmap::Plane::areaVoxels
float areaVoxels
Definition: Plane.h:150
mrpt::pbmap::PbMapMaker::acceptHistH_F
float acceptHistH_F
Definition: PbMapMaker.h:175
mrpt::pbmap::PbMapMaker::m_pbmaker_finished
bool m_pbmaker_finished
Definition: PbMapMaker.h:161
PbMapMaker.h
mrpt::pbmap::Plane::prog_hist_H
std::vector< std::vector< float > > prog_hist_H
Definition: Plane.h:175
config_pbmap::rawlog_path
std::string rawlog_path
Definition: PbMapMaker.cpp:69
keyboardEventOccurred
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *viewer_void)
Definition: PbMapMaker.cpp:1206
config_pbmap::save_registered_cloud
bool save_registered_cloud
Definition: PbMapMaker.cpp:113
mrpt::pbmap::PbMapMaker::mergePlanes
void mergePlanes(Plane &updatePlane, Plane &discardPlane)
Definition: PbMapMaker.cpp:1256
mrpt::pbmap::PbMapMaker::acceptC1C2C3_T
float acceptC1C2C3_T
Definition: PbMapMaker.h:170
mrpt::pbmap::Segment
Definition: Miscellaneous.h:92
mrpt::pbmap::PbMapMaker::rejectNrgb_T
float rejectNrgb_T
Definition: PbMapMaker.h:171
CConfigFile.h
config_pbmap::proximity_neighbor_planes
float proximity_neighbor_planes
Definition: PbMapMaker.cpp:84
mrpt::pbmap::PbMapMaker::rejectIntensity_T
float rejectIntensity_T
Definition: PbMapMaker.h:172
mrpt::pbmap::PbMapMaker::rejectNrgb_F
float rejectNrgb_F
Definition: PbMapMaker.h:171
config_pbmap::color_threshold
float color_threshold
Definition: PbMapMaker.cpp:71
mrpt::pbmap::PbMapMaker::run
void run()
Definition: PbMapMaker.cpp:1629
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:150
mrpt::pbmap::PbMapMaker::acceptAreaF
float acceptAreaF
Definition: PbMapMaker.h:168
mrpt::pbmap::PbMapMaker::rejectHistH_F
float rejectHistH_F
Definition: PbMapMaker.h:175
mrpt::pbmap::Plane::v3colorNrgb
Eigen::Vector3f v3colorNrgb
! Radiometric description
Definition: Plane.h:159
config_pbmap::use_color
bool use_color
Definition: PbMapMaker.cpp:83
mrpt::pbmap::PbMapMaker::observedPlanes
std::set< unsigned > observedPlanes
Definition: PbMapMaker.h:140
mrpt::pbmap::PbMapMaker::acceptColor_F
float acceptColor_F
Definition: PbMapMaker.h:174
mrpt::pbmap::PbMapMaker::rejectElongT
float rejectElongT
Definition: PbMapMaker.h:169
mrpt::pbmap::SemanticClustering::groups
std::map< unsigned, std::vector< unsigned > > groups
Definition: SemanticClustering.h:57
pbmap-precomp.h
config_pbmap::angle_threshold
float angle_threshold
Definition: PbMapMaker.cpp:78
grn
unsigned char grn[10]
Definition: PbMapMaker.cpp:1299
mrpt::pbmap::PlaneInferredInfo::isFullExtent
void isFullExtent(Plane &plane, double newArea)
! Check if the area of input plane is stable and bounded (e.g < 1 m2) along the last keyframes that o...
Definition: PlaneInferredInfo.cpp:239
mrpt::pbmap::Plane::forcePtsLayOnPlane
void forcePtsLayOnPlane()
Definition: Plane.cpp:220
mrpt::pbmap::Plane::areaHull
float areaHull
Definition: Plane.h:151
mrpt::pbmap::Plane::computeMassCenterAndArea
void computeMassCenterAndArea()
Compute the patch's convex-hull area and mass center.
Definition: Plane.cpp:285
ared
double ared[10]
Definition: PbMapMaker.cpp:1302
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:169
config_pbmap::proximity_threshold
float proximity_threshold
Definition: PbMapMaker.cpp:94
name
GLuint const GLchar * name
Definition: glext.h:4054
mrpt::pbmap::PbMapMaker::checkProximity
void checkProximity(Plane &plane, float proximity)
Definition: PbMapMaker.cpp:317
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:125
mrpt::pbmap::Plane::numObservations
unsigned numObservations
Definition: Plane.h:128
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:134
mrpt::pbmap::Plane::prog_area
std::vector< double > prog_area
Definition: Plane.h:168
mrpt::system::os::sprintf
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:189
mrpt::pbmap::PbMap::globalMapPtr
pcl::PointCloud< PointT >::Ptr globalMapPtr
Definition: pbmap/PbMap.h:67
mrpt::pbmap::PbMapMaker::stop_pbMapMaker
bool stop_pbMapMaker()
Definition: PbMapMaker.cpp:1683
mrpt::pbmap::PbMapMaker::mtx_pbmap_busy
boost::mutex mtx_pbmap_busy
Definition: PbMapMaker.h:133
mrpt::pbmap::Plane::v3center
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:141
mrpt::pbmap::SemanticClustering::currentSemanticGroup
unsigned currentSemanticGroup
Definition: SemanticClustering.h:51
config_pbmap::hue_threshold
float hue_threshold
Definition: PbMapMaker.cpp:73
config_pbmap::dist_threshold
float dist_threshold
Definition: PbMapMaker.cpp:76
config_pbmap::makeClusters
bool makeClusters
Definition: PbMapMaker.cpp:103
mrpt::pbmap::PbMapMaker::rejectC1C2C3_F
float rejectC1C2C3_F
Definition: PbMapMaker.h:170
mrpt::pbmap::dist3D_Segment_to_Segment2
float dist3D_Segment_to_Segment2(Segment S1, Segment S2)
Definition: Miscellaneous.cpp:25
mrpt::pbmap::Plane::elongation
float elongation
Definition: Plane.h:148
mrpt::pbmap::Plane
A class used to store a planar feature (Plane for short).
Definition: Plane.h:46
mrpt::pbmap::PbMapMaker::acceptColor_T
float acceptColor_T
Definition: PbMapMaker.h:174
config_pbmap::config_localiser
string config_localiser
Definition: PbMapMaker.cpp:109
mrpt::pbmap::PbMapMaker::acceptElongT
float acceptElongT
Definition: PbMapMaker.h:169
values
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3582
config_pbmap::intensity_threshold
float intensity_threshold
Definition: PbMapMaker.cpp:72
mrpt::pbmap::PbMapMaker::mpPlaneInferInfo
PlaneInferredInfo * mpPlaneInferInfo
Definition: PbMapMaker.h:143
config_pbmap::max_dist_center_plane
float max_dist_center_plane
Definition: PbMapMaker.cpp:91
readConfigFile
void readConfigFile(const string &config_file_name)
Definition: PbMapMaker.cpp:118
mrpt::pbmap::Plane::prog_Nrgb
std::vector< Eigen::Vector3f > prog_Nrgb
Definition: Plane.h:173
color
GLuint color
Definition: glext.h:8300
mrpt::pbmap::Plane::polygonContourPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:182
mrpt::pbmap::Plane::prog_C1C2C3
std::vector< Eigen::Vector3f > prog_C1C2C3
Definition: Plane.h:172
mrpt::pbmap::PbMapMaker::rejectColor_T
float rejectColor_T
Definition: PbMapMaker.h:174
mrpt::pbmap::PbMapLocaliser::foundPlaces
std::map< std::string, pcl::PointXYZ > foundPlaces
Definition: PbMapLocaliser.h:65
config_pbmap::input_from_rawlog
bool input_from_rawlog
Definition: PbMapMaker.cpp:68
mrpt::pbmap::PbMapMaker::areSamePlane
bool areSamePlane(Plane &plane1, Plane &plane2, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
Definition: PbMapMaker.cpp:1220
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:113
mrpt::pbmap::PbMapMaker::serializePbMap
void serializePbMap(std::string path)
Definition: PbMapMaker.cpp:1674
mrpt::pbmap::PointT
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:36
agrn
double agrn[10]
Definition: PbMapMaker.cpp:1303
mrpt::pbmap::PbMapMaker::rejectAreaF
float rejectAreaF
Definition: PbMapMaker.h:168
mrpt::pbmap::isInHull
bool isInHull(PointT &point3D, pcl::PointCloud< PointT >::Ptr hull3D)
Definition: Miscellaneous.cpp:103
mrpt::pbmap::PbMapMaker::m_pbmaker_must_stop
bool m_pbmaker_must_stop
Definition: PbMapMaker.h:158
mrpt::pbmap::PbMapMaker::acceptNrgb_T
float acceptNrgb_T
Definition: PbMapMaker.h:171
config_pbmap::max_cos_normal
float max_cos_normal
Definition: PbMapMaker.cpp:90
mrpt::pbmap::PbMapMaker::acceptHistH_T
float acceptHistH_T
Definition: PbMapMaker.h:175
mrpt::pbmap::PbMapMaker::rejectIntensity_F
float rejectIntensity_F
Definition: PbMapMaker.h:172
mrpt::pbmap
Definition: ConsistencyTest.h:27
mrpt::pbmap::PbMapMaker::rejectColor_F
float rejectColor_F
Definition: PbMapMaker.h:174
mrpt::pbmap::Plane::planePointCloudPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:186
config_pbmap::graph_mode
int graph_mode
Definition: PbMapMaker.cpp:96
mrpt::pbmap::PbMapMaker::mPbMap
PbMap mPbMap
Definition: PbMapMaker.h:137
config_pbmap::inferStructure
bool inferStructure
Definition: PbMapMaker.cpp:101
mrpt::pbmap::Plane::prog_intensity
std::vector< float > prog_intensity
Definition: Plane.h:174
config_pbmap::path_save_registered_cloud
std::string path_save_registered_cloud
Definition: PbMapMaker.cpp:114
BhattacharyyaDist
double BhattacharyyaDist(std::vector< float > &hist1, std::vector< float > &hist2)
Definition: PbMapMaker.cpp:51
mrpt::pbmap::Plane::calcConvexHull
void calcConvexHull(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &pointCloud, std::vector< size_t > &indices=DEFAULT_VECTOR)
! Calculate the plane's convex hull with the monotone chain algorithm.
Definition: Plane.cpp:761
mrpt::pbmap::Plane::v3normal
Eigen::Vector3f v3normal
Definition: Plane.h:142
mrpt::pbmap::Plane::prog_elongation
std::vector< double > prog_elongation
Definition: Plane.h:169
mrpt::pbmap::SemanticClustering
Definition: SemanticClustering.h:48
mrpt::pbmap::PbMapMaker::clusterize
SemanticClustering * clusterize
Definition: PbMapMaker.h:131
CS_visualize
std::mutex CS_visualize
Definition: PbMapMaker.cpp:48
config_pbmap::minInliersRate
float minInliersRate
Definition: PbMapMaker.cpp:80
config_pbmap::record_rawlog
bool record_rawlog
Definition: PbMapMaker.cpp:70
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:33
mrpt::pbmap::PbMapMaker::rejectAreaT
float rejectAreaT
Definition: PbMapMaker.h:168
mrpt::pbmap::PbMapMaker::cloudViewer
pcl::visualization::CloudViewer cloudViewer
Definition: PbMapMaker.h:80
mrpt::pbmap::PbMapMaker::arePlanesNearby
bool arePlanesNearby(Plane &plane1, Plane &plane2, const float distThreshold)
Definition: PbMapMaker.cpp:214
mrpt::pbmap::Plane::id
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:127
mrpt::pbmap::PbMapMaker::detectPlanesCloud
void detectPlanesCloud(pcl::PointCloud< PointT >::Ptr &pointCloudPtr_arg, Eigen::Matrix4f &poseKF, double distThreshold, double angleThreshold, double minInliersF)
Definition: PbMapMaker.cpp:649
mrpt::pbmap::PbMapMaker::acceptIntensity_F
float acceptIntensity_F
Definition: PbMapMaker.h:172
mrpt::pbmap::PbMapMaker::~PbMapMaker
~PbMapMaker()
Definition: PbMapMaker.cpp:1694
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::pbmap::PlaneInferredInfo::searchTheFloor
bool searchTheFloor(Eigen::Matrix4f &poseSensor, Plane &plane)
! Check if the input plane correpond to the floor.
Definition: PlaneInferredInfo.cpp:35
config_pbmap
Definition: PbMapMaker.cpp:65
mrpt::pbmap::PbMapMaker::acceptElongF
float acceptElongF
Definition: PbMapMaker.h:169
mrpt::pbmap::PbMap::FloorPlane
int FloorPlane
Definition: pbmap/PbMap.h:63
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::pbmap::SemanticClustering::evalPartition
int evalPartition(std::set< unsigned > &observedPlanes)
Definition: SemanticClustering.h:379
mrpt::pbmap::PbMapMaker::rejectElongF
float rejectElongF
Definition: PbMapMaker.h:169
mrpt::pbmap::PbMapMaker::rejectC1C2C3_T
float rejectC1C2C3_T
Definition: PbMapMaker.h:170
mrpt::pbmap::getVector3fromPointXYZ
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:40
mrpt::pbmap::PbMapMaker::frameQueue
std::vector< frameRGBDandPose > frameQueue
Definition: PbMapMaker.h:74
ASSERT_FILE_EXISTS_
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
mrpt::pbmap::Plane::v3colorC1C2C3
Eigen::Vector3f v3colorC1C2C3
Definition: Plane.h:164
types_math.h
mrpt::pbmap::PbMapMaker::mpPbMapLocaliser
PbMapLocaliser * mpPbMapLocaliser
Definition: PbMapMaker.h:128
mrpt::pbmap::Plane::nearbyPlanes
std::set< unsigned > nearbyPlanes
Definition: Plane.h:130
mrpt::pbmap::compose
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:57
mrpt::pbmap::PlaneInferredInfo
A class used to infer some semantic meaning to the planes of a PbMap.
Definition: PlaneInferredInfo.h:38
mrpt::pbmap::Plane::calcElongationAndPpalDir
void calcElongationAndPpalDir()
Definition: Plane.cpp:331
mrpt::pbmap::PbMapLocaliser::vQueueObservedPlanes
std::vector< unsigned > vQueueObservedPlanes
Definition: PbMapLocaliser.h:62
mrpt::pbmap::diffPoints
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:46
mrpt::pbmap::PbMapMaker::viz_cb
void viz_cb(pcl::visualization::PCLVisualizer &viz)
Definition: PbMapMaker.cpp:1306
mrpt::pbmap::PbMapLocaliser::alignedModelPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr alignedModelPtr
Definition: PbMapLocaliser.h:68


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST