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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019