Main MRPT website > C++ reference for MRPT 1.5.6
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 href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
13  */
14 
15 #include "pbmap-precomp.h" // precomp. hdr
16 
17 #if MRPT_HAS_PCL
18 
19 
20 #include <mrpt/utils/types_math.h> // Eigen
21 #include <mrpt/system/threads.h>
22 
23 //#include <pcl/io/io.h>
24 //#include <pcl/io/pcd_io.h>
25 #include <pcl/features/integral_image_normal.h>
26 #include <pcl/features/normal_3d.h>
27 #include <pcl/ModelCoefficients.h>
28 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
29 #include <pcl/segmentation/organized_connected_component_segmentation.h>
30 #include <pcl/filters/extract_indices.h>
31 #include <pcl/filters/voxel_grid.h>
32 #include <pcl/common/transforms.h>
33 #include <pcl/common/time.h>
35 #include <mrpt/utils/CConfigFile.h>
36 #include <mrpt/pbmap/PbMapMaker.h>
37 
38 #include <iostream>
39 
40 //#define _VERBOSE 0
41 #define WATCH_UNARY 0
42 #define WATCH_BINARY 0
43 #define WATCH_COLOR 1
44 
45 using namespace std;
46 using namespace Eigen;
47 using namespace mrpt::pbmap;
48 
50 
51 // Bhattacharyya histogram distance function
52 double BhattacharyyaDist(std::vector<float> &hist1, std::vector<float> &hist2)
53 {
54 assert(hist1.size() == hist2.size());
55 double BhattachDist;
56 double BhattachDist_aux = 0.0;
57 for(unsigned i=0; i < hist1.size(); i++)
58  BhattachDist_aux += sqrt(hist1[i]*hist2[i]);
59 
60 BhattachDist = sqrt(1 - BhattachDist_aux);
61 
62 return BhattachDist;
63 }
64 
65 /*!Some parameters to specify input/output and some thresholds.*/
67 {
68  // [global]
75 
76  // [plane_segmentation]
77  float dist_threshold; // Maximum distance to the plane between neighbor 3D-points
78  float angle_threshold; // = 0.017453 * 4.0 // Maximum angle between 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 neighbors when the closest distance between them is under proximity_neighbor_planes
84 // float max_angle_normals; // (10º) Two planar patches that represent the same surface must have similar normals // QUITAR
86  float max_dist_center_plane; // Two planar patches that represent the same surface must have their center in the same plane
87  float proximity_threshold; // Two planar patches that represent the same surface must overlap or be nearby
88  int graph_mode; // This var selects the condition to create edges in the graph, either proximity of planar patches or co-visibility in a single frame
89 
90  // [semantics]
91  bool inferStructure; // Infer if the planes correspond to the floor, ceiling or walls
92  bool makeClusters; // Should the PbMapMaker cluster the planes according to their co-visibility
93 
94  // [localisation]
95  bool detect_loopClosure; // Run PbMapLocaliser in a different threads to detect loop closures or preloaded PbMaps
97 
98  // [serialize]
102 
103 } configPbMap;
104 
105 void readConfigFile(const string &config_file_name)
106 {
107  mrpt::utils::CConfigFile config_file(config_file_name);
108 
109  // Plane segmentation
110  configPbMap.dist_threshold = config_file.read_float("plane_segmentation","dist_threshold",0.04,true);
111  configPbMap.angle_threshold = config_file.read_float("plane_segmentation","angle_threshold",0.069812,true);
112  configPbMap.minInliersRate = config_file.read_float("plane_segmentation","minInliersRate",0.01,true);
113 
114  // map_construction
115  configPbMap.use_color = config_file.read_bool("map_construction","use_color",false);
116  configPbMap.color_threshold = config_file.read_float("map_construction","color_threshold",0.09);
117  configPbMap.intensity_threshold = config_file.read_float("map_construction","intensity_threshold",255.0);
118  configPbMap.hue_threshold = config_file.read_float("unary","hue_threshold",0.25);
119  configPbMap.proximity_neighbor_planes = config_file.read_float("map_construction","proximity_neighbor_planes",1.0);
120  configPbMap.graph_mode = config_file.read_int("map_construction","graph_mode",1);
121  configPbMap.max_cos_normal = config_file.read_float("map_construction","max_cos_normal",0.9848,true);
122  configPbMap.max_dist_center_plane = config_file.read_float("map_construction","max_dist_center_plane",0.1,true);
123  configPbMap.proximity_threshold = config_file.read_float("map_construction","proximity_threshold",0.15);
124 
125  // [semantics]
126  configPbMap.inferStructure = config_file.read_bool("semantics","inferStructure",true);
127  configPbMap.makeClusters = config_file.read_bool("semantics","makeCovisibilityClusters",false);
128 // configPbMap.path_prev_pbmap = config_file.read_string("localisation","path_prev_pbmap","",true);
129 
130  // [localisation]
131  configPbMap.detect_loopClosure = config_file.read_bool("localisation","detect_loopClosure",true);
133  configPbMap.config_localiser = config_file.read_string("localisation","config_localiser","",true);
134 
135  // serialize
136  configPbMap.path_save_pbmap = config_file.read_string("serialize","path_save_pbmap","map");
137  configPbMap.save_registered_cloud = config_file.read_bool("serialize","save_registered_cloud",true);
138  configPbMap.path_save_registered_cloud = config_file.read_string("serialize","path_save_registered_cloud","/home/edu/Projects/PbMaps/PbMaps.txt");
139 //cout << "path_save_registered_cloud " << configPbMap.path_save_registered_cloud << endl;
140 
141  #ifdef _VERBOSE
142  cout << "readConfigFile configPbMap.ini dist_threshold " << configPbMap.dist_threshold << endl;
143  #endif
144 }
145 
146 PbMapMaker::PbMapMaker(const string &config_file) :
147  cloudViewer("Cloud Viewer"),
148  mpPbMapLocaliser(NULL),
149  m_pbmaker_must_stop(false),
150  m_pbmaker_finished(false)
151 {
152  // Load parameters
153  ASSERT_FILE_EXISTS_(config_file)
154  readConfigFile(config_file);
155 
157 
160 
163 
165 
166 
167  // Unary
168  rejectAreaF = 0, acceptAreaF = 0, rejectAreaT = 0, acceptAreaT = 0;
175 }
176 
177 
178 bool PbMapMaker::arePlanesNearby(Plane &plane1, Plane &plane2, const float distThreshold)
179 {
180  float distThres2 = distThreshold * distThreshold;
181 
182  // First we check distances between centroids and vertex to accelerate this check
183  if( (plane1.v3center - plane2.v3center).squaredNorm() < distThres2 )
184  return true;
185 
186  for(unsigned i=1; i < plane1.polygonContourPtr->size(); i++)
187  if( (getVector3fromPointXYZ(plane1.polygonContourPtr->points[i]) - plane2.v3center).squaredNorm() < distThres2 )
188  return true;
189 
190  for(unsigned j=1; j < plane2.polygonContourPtr->size(); j++)
191  if( (plane1.v3center - getVector3fromPointXYZ(plane2.polygonContourPtr->points[j]) ).squaredNorm() < distThres2 )
192  return true;
193 
194  for(unsigned i=1; i < plane1.polygonContourPtr->size(); i++)
195  for(unsigned j=1; j < plane2.polygonContourPtr->size(); j++)
196  if( (diffPoints(plane1.polygonContourPtr->points[i], plane2.polygonContourPtr->points[j]) ).squaredNorm() < distThres2 )
197  return true;
198 
199  //If not found yet, search properly by checking distances:
200  // a) Between an edge and a vertex
201  // b) Between two edges (imagine two polygons on perpendicular planes)
202  // c) Between a vertex and the inside of the polygon
203  // d) Or the polygons intersect
204 
205  // a) & b)
206  for(unsigned i=1; i < plane1.polygonContourPtr->size(); i++)
207  for(unsigned j=1; j < plane2.polygonContourPtr->size(); j++)
208  if(dist3D_Segment_to_Segment2(Segment(plane1.polygonContourPtr->points[i],plane1.polygonContourPtr->points[i-1]), Segment(plane2.polygonContourPtr->points[j],plane2.polygonContourPtr->points[j-1])) < distThres2)
209  return true;
210 
211  // c)
212  for(unsigned i=1; i < plane1.polygonContourPtr->size(); i++)
213  if( plane2.v3normal.dot(getVector3fromPointXYZ(plane1.polygonContourPtr->points[i]) - plane2.v3center) < distThreshold )
214  if(isInHull(plane1.polygonContourPtr->points[i], plane2.polygonContourPtr) )
215  return true;
216 
217  for(unsigned j=1; j < plane2.polygonContourPtr->size(); j++)
218  if( plane1.v3normal.dot(getVector3fromPointXYZ(plane2.polygonContourPtr->points[j]) - plane1.v3center) < distThreshold )
219  if(isInHull(plane2.polygonContourPtr->points[j], plane1.polygonContourPtr) )
220  return true;
221 
222 //cout << "Polygons intersect?\n";
223  // d)
224 // for(unsigned i=1; i < plane1.polygonContourPtr->size(); i++)
225 // if( plane2.v3normal.dot(getVector3fromPointXYZ(plane1.polygonContourPtr->points[i]) - plane2.v3center) < distThreshold )
226 // {
227 //// cout << "Elements\n" << plane2.v3normal << "\n xyz " << plane1.polygonContourPtr->points[i].x << " " << plane1.polygonContourPtr->points[i].y << " " << plane1.polygonContourPtr->points[i].z
228 //// << " xyz2 " << plane1.polygonContourPtr->points[i-1].x << " " << plane1.polygonContourPtr->points[i-1].y << " " << plane1.polygonContourPtr->points[i-1].z << endl;
229 //// assert( plane2.v3normal.dot(diffPoints(plane1.polygonContourPtr->points[i], plane1.polygonContourPtr->points[i-1]) ) != 0 );
230 // float d = (plane2.v3normal.dot(plane2.v3center - getVector3fromPointXYZ(plane1.polygonContourPtr->points[i-1]) ) ) / (plane2.v3normal.dot(diffPoints(plane1.polygonContourPtr->points[i], plane1.polygonContourPtr->points[i-1]) ) );
231 // PointT intersection;
232 // intersection.x = d * plane1.polygonContourPtr->points[i].x + (1-d) * plane1.polygonContourPtr->points[i-1].x;
233 // intersection.y = d * plane1.polygonContourPtr->points[i].y + (1-d) * plane1.polygonContourPtr->points[i-1].y;
234 // intersection.z = d * plane1.polygonContourPtr->points[i].z + (1-d) * plane1.polygonContourPtr->points[i-1].z;
235 // if(isInHull(intersection, plane2.polygonContourPtr) )
236 // return true;
237 // }
238 //if (plane1.id==2 && frameQueue.size() == 12)
239 //cout << "pasaFinal\n";
240  return false;
241 }
242 
243 void PbMapMaker::checkProximity(Plane &plane, float proximity)
244 {
245  for(unsigned i=0; i < mPbMap.vPlanes.size(); i++ )
246  {
247  if(plane.id == mPbMap.vPlanes[i].id)
248  continue;
249 
250  if(plane.nearbyPlanes.count(mPbMap.vPlanes[i].id))
251  continue;
252 
253  if(arePlanesNearby(plane, mPbMap.vPlanes[i], proximity) ) // If the planes are closer than proximity (in meters), then mark them as neighbors
254  {
255  plane.nearbyPlanes.insert(mPbMap.vPlanes[i].id);
256  mPbMap.vPlanes[i].nearbyPlanes.insert(plane.id);
257  }
258  }
259 }
260 
261 //// double area_THRESHOLD=10.0;
262 // double area_THRESHOLD_inv=1/configPbMap.area_THRESHOLD;
263 //// double elongation_THRESHOLD=1.15;
264 // double elongation_THRESHOLD_inv=1/configPbMap.elongation_THRESHOLD;
265 //// double color_threshold=0.3;
266 //double colorDev_THRESHOLD;
267 // double dist_THRESHOLD = 1.6; double dist_THRESHOLD_inv = 1/dist_THRESHOLD;
268 //// double dist_THRESHOLDFull = 1.2; double dist_THRESHOLDFull_inv = 1/dist_THRESHOLDFull;
269 // double height_THRESHOLD=0.5;
270 // double height_THRESHOLD_parallel=0.03;
271 // double normal_THRESHOLD=1.0;
272 // double ppaldir_normal_THRESHOLD=8;
273 //
274 // double singleC_THRESHOLD = 0.2;
275 // double elong_rely_ppal_THRESHOLD=1.5;
276 // double elong_rely_ppal_THRESHOLD_tight=1.3;
277 
278 void PbMapMaker::watchProperties(set<unsigned> &observedPlanes, Plane &observedPlane)
279 {
280 cout << "PbMapMaker::watchProperties..." << configPbMap.color_threshold << " " << configPbMap.intensity_threshold << " " << configPbMap.hue_threshold << "\n";
281  // Watch properties
282 // double tCondition;
283 // mrpt::utils::CTicTac clock;
284 // colorDev_THRESHOLD=configPbMap.color_threshold/10;
285 
286  if(observedPlane.numObservations > 2)
287  {
288  // Previous instanes of same plane
289  for(size_t i = 0; i < observedPlane.prog_Nrgb.size(); i++)
290  {
291  #if WATCH_UNARY
292 // tCondition = pcl::getTime();
293 // clock.Tic();
294  // Check area unary
295  double rel_areas = observedPlane.area / observedPlane.prog_area[i];
296  if( rel_areas < area_THRESHOLD_inv || rel_areas > configPbMap.area_THRESHOLD )
297  rejectAreaT++;
298  else
299  acceptAreaT++;
300 // timeCompareArea.push_back( (pcl::getTime() - tCondition)*1e6 );
301 // timeCompareArea.push_back( clock.Tac()*1e6 );
302 
303 // tCondition = pcl::getTime();
304  // Check ratio unary
305  double rel_ratios = observedPlane.elongation / observedPlane.prog_elongation[i];
306  if( rel_ratios < elongation_THRESHOLD_inv || rel_ratios > configPbMap.elongation_THRESHOLD )
307  rejectElongT++;
308  else
309  acceptElongT++;
310 // timeCompareElong.push_back( (pcl::getTime() - tCondition)*1e6 );
311  #endif
312 
313  #if WATCH_COLOR
314 // tCondition = pcl::getTime();
315  // Check colorC1C2C3 unary
316  double dif_C1C2C3 = (observedPlane.v3colorC1C2C3 - observedPlane.prog_C1C2C3[i]).norm();
317  // cout << "color1 " << observedPlane.v3colorC1C2C3 << " color2 " << prevPlane.v3colorC1C2C3 << endl;
318  if( dif_C1C2C3 > configPbMap.color_threshold )
319  rejectC1C2C3_T++;
320  else
321  acceptC1C2C3_T++;
322 // timeCompareC1C2C3.push_back( (pcl::getTime() - tCondition)*1e6 );
323 
324  double dif_Nrgb = (observedPlane.v3colorNrgb - observedPlane.prog_Nrgb[i]).norm();
325  // cout << "color1 " << observedPlane.v3colorNrgb << " color2 " << prevPlane.v3colorNrgb << endl;
326 // if( dif_Nrgb > configPbMap.color_threshold || fabs(observedPlane.dominantIntensity - observedPlane.prog_intensity[i]) > 255)
327  if( dif_Nrgb > configPbMap.color_threshold)
328  rejectNrgb_T++;
329  else
330  acceptNrgb_T++;
331 // timeCompareNrgb.push_back( (pcl::getTime() - tCondition)*1e6 );
332 
333  if( fabs(observedPlane.dominantIntensity - observedPlane.prog_intensity[i]) > configPbMap.intensity_threshold)
335  else
337 
338  if( fabs(observedPlane.dominantIntensity - observedPlane.prog_intensity[i]) > configPbMap.intensity_threshold ||
339  dif_Nrgb > configPbMap.color_threshold)
340  rejectColor_T++;
341  else
342  acceptColor_T++;
343 
344  // Hue histogram
345  double hist_dist = BhattacharyyaDist(observedPlane.hist_H, observedPlane.prog_hist_H[i]);
346  if( hist_dist > configPbMap.hue_threshold )
347  rejectHistH_T++;
348  else
349  acceptHistH_T++;
350  #endif
351  }
352 
353 //cout << "Watch rejection. Planes " << mPbMap.vPlanes.size() << "\n";
354  // Get unary rejection rate
355  for(size_t i = 0; i < mPbMap.vPlanes.size(); i++)
356  {
357  if(i == observedPlane.id)
358  continue;
359 
360  Plane &prevPlane = mPbMap.vPlanes[i];
361 // cout << "color prev plane " << prevPlane.v3colorNrgb.transpose() << " " << prevPlane.dominantIntensity << " " << prevPlane.bDominantColor << endl;
362 
363  #if WATCH_UNARY
364 // tCondition = pcl::getTime();
365 // clock.Tic();
366  double rel_areas = observedPlane.area / prevPlane.area;
367  if( rel_areas < area_THRESHOLD_inv || rel_areas > configPbMap.area_THRESHOLD )
368  rejectAreaF++;
369  else
370  acceptAreaF++;
371 // timeCompareArea.push_back( (pcl::getTime() - tCondition)*1e6 );
372 // timeCompareArea.push_back( clock.Tac()*1e6 );
373 
374 // tCondition = pcl::getTime();
375  double rel_ratios = observedPlane.elongation / prevPlane.elongation;
376  if( rel_ratios < elongation_THRESHOLD_inv || rel_ratios > configPbMap.elongation_THRESHOLD )
377  rejectElongF++;
378  else
379  acceptElongF++;
380 // timeCompareElong.push_back( (pcl::getTime() - tCondition)*1e6 );
381  #endif
382 
383  #if WATCH_COLOR
384 // tCondition = pcl::getTime();
385  double dif_C1C2C3 = (observedPlane.v3colorC1C2C3 - prevPlane.v3colorC1C2C3).norm();
386 // cout << "color1 " << observedPlane.v3colorC1C2C3 << " color2 " << prevPlane.v3colorC1C2C3 << endl;
387  if( dif_C1C2C3 > configPbMap.color_threshold )
388  rejectC1C2C3_F++;
389  else
390  acceptC1C2C3_F++;
391 // timeCompareC1C2C3.push_back( (pcl::getTime() - tCondition)*1e6 );
392 
393 // // Nrgb
394  double dif_Nrgb = (observedPlane.v3colorNrgb - prevPlane.v3colorNrgb).norm();
395 // cout << "color1 " << observedPlane.v3colorNrgb << " color2 " << prevPlane.v3colorNrgb << endl;
396 // if( dif_Nrgb > configPbMap.color_threshold || fabs(observedPlane.dominantIntensity - prevPlane.dominantIntensity) > 255)
397  if( dif_Nrgb > configPbMap.color_threshold)
398  rejectNrgb_F++;
399  else
400  acceptNrgb_F++;
401 // timeCompareNrgb.push_back( (pcl::getTime() - tCondition)*1e6 );
402 
403 //cout << "intensity conditions\n";
404 //cout << " elements " << observedPlane.dominantIntensity <<" " << prevPlane.dominantIntensity << " " << configPbMap.intensity_threshold;
405 //cout << " " << rejectIntensity_F << " " << acceptIntensity_F << endl;
406 
407  if( fabs(observedPlane.dominantIntensity - prevPlane.dominantIntensity) > configPbMap.intensity_threshold)
409  else
411 
412  if( fabs(observedPlane.dominantIntensity - prevPlane.dominantIntensity) > configPbMap.intensity_threshold ||
413  dif_Nrgb > configPbMap.color_threshold)
414  rejectColor_F++;
415  else
416  acceptColor_F++;
417 //cout << "rejectColor_F " << rejectColor_F << "acceptColor_F " << acceptColor_F << endl;
418 
419  // Hue histogram
420  double hist_dist = BhattacharyyaDist(observedPlane.hist_H, prevPlane.hist_H);
421  if( hist_dist > configPbMap.hue_threshold )
422  rejectHistH_F++;
423  else
424  acceptHistH_F++;
425 //cout << "finish reject conditions\n";
426 
427  #endif
428  }
429 
430  #if WATCH_UNARY
431  observedPlane.prog_area.push_back(observedPlane.area);
432 // observedPlane.prog_v3center.push_back(observedPlane.v3center);
433 // observedPlane.prog_v3normal.push_back(observedPlane.v3normal);
434  observedPlane.prog_elongation.push_back(observedPlane.elongation);
435 // observedPlane.prog_v3PpalDir.push_back(observedPlane.v3PpalDir);
436  #endif
437 
438 //cout << "Update progression\n";
439 
440  #if WATCH_COLOR
441  observedPlane.prog_C1C2C3.push_back(observedPlane.v3colorC1C2C3);
442  observedPlane.prog_Nrgb.push_back(observedPlane.v3colorNrgb);
443  observedPlane.prog_intensity.push_back(observedPlane.dominantIntensity);
444  observedPlane.prog_hist_H.push_back(observedPlane.hist_H);
445  #endif
446  }
447 cout << " ...Watching finished\n";
448 }
449 
451 {
452 cout << "PbMapMaker::saveInfoFiles(...)\n";
453 
454 // cout << "DiscAreaF rate " << rejectAreaF/(rejectAreaF+acceptAreaF) << " meas " << rejectAreaF+acceptAreaF << endl;
455 // cout << "DiscElongF rate " << rejectElongF/(rejectElongF+acceptElongF) << " meas " << rejectElongF+acceptElongF << endl;
456 // cout << "DiscC1C2C3_F rate " << rejectC1C2C3_F/(rejectC1C2C3_F+acceptC1C2C3_F) << " meas " << rejectC1C2C3_F+acceptC1C2C3_F << endl;
457 //
458 // cout << "DiscAreaT rate " << rejectAreaT/(rejectAreaT+acceptAreaT) << " meas " << rejectAreaT+acceptAreaT << endl;
459 // cout << "DiscElongT rate " << rejectElongT/(rejectElongT+acceptElongT) << " meas " << rejectElongT+acceptElongT << endl;
460 // cout << "DiscC1C2C3_T rate " << rejectC1C2C3_T/(rejectC1C2C3_T+acceptC1C2C3_T) << " meas " << rejectC1C2C3_T+acceptC1C2C3_T << endl;
461 
462  string results_file;
463  ofstream file;
464 
465 #if WATCH_UNARY
466  results_file = "results/areaRestriction.txt";
467  file.open(results_file.c_str(), ios::app);
468  file << configPbMap.area_THRESHOLD << " " << rejectAreaT << " " << acceptAreaT << " " << rejectAreaF << " " << acceptAreaF << endl;
469  file.close();
470 
471  results_file = "results/elongRestriction.txt";
472  file.open(results_file.c_str(), ios::app);
473  file << configPbMap.elongation_THRESHOLD << " " << rejectElongT << " " << acceptElongT << " " << rejectElongF << " " << acceptElongF << endl;
474  file.close();
475 #endif
476 
477 #if WATCH_COLOR
478  results_file = "results/c1c2c3Restriction.txt";
479  file.open(results_file.c_str(), ios::app);
480  file << configPbMap.color_threshold << " " << rejectC1C2C3_T << " " << acceptC1C2C3_T << " " << rejectC1C2C3_F << " " << acceptC1C2C3_F << endl;
481  file.close();
482 
483  results_file = "results/NrgbRestriction.txt";
484  file.open(results_file.c_str(), ios::app);
485  file << configPbMap.color_threshold << " " << rejectNrgb_T << " " << acceptNrgb_T << " " << rejectNrgb_F << " " << acceptNrgb_F << endl;
486  file.close();
487 
488  results_file = "results/IntensityRestriction.txt";
489  file.open(results_file.c_str(), ios::app);
490  file << configPbMap.intensity_threshold << " " << rejectIntensity_T << " " << acceptIntensity_T << " " << rejectIntensity_F << " " << acceptIntensity_F << endl;
491  file.close();
492 
493  results_file = "results/ColorRestriction.txt";
494  file.open(results_file.c_str(), ios::app);
495  file << configPbMap.intensity_threshold << " " << rejectColor_T << " " << acceptColor_T << " " << rejectColor_F << " " << acceptColor_F << endl;
496  file.close();
497 
498  results_file = "results/HueRestriction.txt";
499  file.open(results_file.c_str(), ios::app);
500  file << configPbMap.hue_threshold << " " << rejectHistH_T << " " << acceptHistH_T << " " << rejectHistH_F << " " << acceptHistH_F << endl;
501  file.close();
502 #endif
503 }
504 
505 void PbMapMaker::detectPlanesCloud( pcl::PointCloud<PointT>::Ptr &pointCloudPtr_arg,
506  Eigen::Matrix4f &poseKF,
507  double distThreshold, double angleThreshold, double minInliersF)
508 {
509  boost::mutex::scoped_lock updateLock(mtx_pbmap_busy);
510 
511  unsigned minInliers = minInliersF * pointCloudPtr_arg->size();
512 
513  #ifdef _VERBOSE
514  cout << "detectPlanes in a cloud with " << pointCloudPtr_arg->size() << " points " << minInliers << " minInliers\n";
515  #endif
516 
517  pcl::PointCloud<PointT>::Ptr pointCloudPtr_arg2(new pcl::PointCloud<PointT>);
518  pcl::copyPointCloud(*pointCloudPtr_arg,*pointCloudPtr_arg2);
519 
520  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>);
521  pcl::transformPointCloud(*pointCloudPtr_arg,*alignedCloudPtr,poseKF);
522 
524  *mPbMap.globalMapPtr += *alignedCloudPtr;
525  // Downsample voxel map's point cloud
526  static pcl::VoxelGrid<pcl::PointXYZRGBA> grid;
527  grid.setLeafSize(0.02,0.02,0.02);
528  pcl::PointCloud<pcl::PointXYZRGBA> globalMap;
529  grid.setInputCloud (mPbMap.globalMapPtr);
530  grid.filter (globalMap);
531  mPbMap.globalMapPtr->clear();
532  *mPbMap.globalMapPtr = globalMap;
533  } // End CS
534 
535  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
536  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
537  ne.setMaxDepthChangeFactor (0.02f); // For VGA: 0.02f, 10.0f
538  ne.setNormalSmoothingSize (10.0f);
539  ne.setDepthDependentSmoothing (true);
540 
541  pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
542  mps.setMinInliers (minInliers); cout << "Params " << minInliers << " " << angleThreshold << " " << distThreshold << endl;
543  mps.setAngularThreshold (angleThreshold); // (0.017453 * 2.0) // 3 degrees
544  mps.setDistanceThreshold (distThreshold); //2cm
545 
546  pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
547  ne.setInputCloud (pointCloudPtr_arg2);
548  ne.compute (*normal_cloud);
549 
550 #ifdef _VERBOSE
551  double plane_extract_start = pcl::getTime ();
552 #endif
553  mps.setInputNormals (normal_cloud);
554  mps.setInputCloud (pointCloudPtr_arg2);
555 
556  std::vector<pcl::PlanarRegion<PointT>, aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
557  std::vector<pcl::ModelCoefficients> model_coefficients;
558  std::vector<pcl::PointIndices> inlier_indices;
559  pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
560  std::vector<pcl::PointIndices> label_indices;
561  std::vector<pcl::PointIndices> boundary_indices;
562  mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
563 
564  #ifdef _VERBOSE
565  double plane_extract_end = pcl::getTime();
566  std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
567 // std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
568  cout << regions.size() << " planes detected\n";
569  #endif
570 
571  // Create a vector with the planes detected in this keyframe, and calculate their parameters (normal, center, pointclouds, etc.)
572  // in the global reference
573  vector<Plane> detectedPlanes;
574  for (size_t i = 0; i < regions.size (); i++)
575  {
576  Plane plane;
577 
578  Vector3f centroid = regions[i].getCentroid ();
579  plane.v3center = compose(poseKF, centroid);
580  plane.v3normal = poseKF.block(0,0,3,3) * Vector3f(model_coefficients[i].values[0], model_coefficients[i].values[1], model_coefficients[i].values[2]);
581 // plane.curvature = regions[i].getCurvature();
582 // assert(plane.v3normal*plane.v3center.transpose() <= 0);
583 // if(plane.v3normal*plane.v3center.transpose() <= 0)
584 // plane.v3normal *= -1;
585 
586  // Extract the planar inliers from the input cloud
587  pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
588  extract.setInputCloud (pointCloudPtr_arg2);
589  extract.setIndices ( boost::make_shared<const pcl::PointIndices> (inlier_indices[i]) );
590  extract.setNegative (false);
591  extract.filter (*plane.planePointCloudPtr); // Write the planar point cloud
592 
593  static pcl::VoxelGrid<pcl::PointXYZRGBA> plane_grid;
594  plane_grid.setLeafSize(0.05,0.05,0.05);
595  pcl::PointCloud<pcl::PointXYZRGBA> planeCloud;
596  plane_grid.setInputCloud (plane.planePointCloudPtr);
597  plane_grid.filter (planeCloud);
598  plane.planePointCloudPtr->clear();
599  pcl::transformPointCloud(planeCloud,*plane.planePointCloudPtr,poseKF);
600 
601  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>);
602  contourPtr->points = regions[i].getContour();
603  plane_grid.setLeafSize(0.1,0.1,0.1);
604  plane_grid.setInputCloud (contourPtr);
605  plane_grid.filter (*plane.polygonContourPtr);
606 // plane.contourPtr->points = regions[i].getContour();
607 // pcl::transformPointCloud(*plane.contourPtr,*plane.polygonContourPtr,poseKF);
608  pcl::transformPointCloud(*plane.polygonContourPtr,*contourPtr,poseKF);
609  plane.calcConvexHull(contourPtr);
610  plane.computeMassCenterAndArea();
611  plane.areaVoxels= plane.planePointCloudPtr->size() * 0.0025;
612 
613  #ifdef _VERBOSE
614  cout << "Area plane region " << plane.areaVoxels<< " of Chull " << plane.areaHull << " of polygon " << plane.compute2DPolygonalArea() << endl;
615  #endif
616 
617  // Check whether this region correspond to the same plane as a previous one (this situation may happen when there exists a small discontinuity in the observation)
618  bool isSamePlane = false;
619  for (size_t j = 0; j < detectedPlanes.size(); j++)
620  if( areSamePlane(detectedPlanes[j], plane, configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) ) // The planes are merged if they are the same
621  {
622  isSamePlane = true;
623 
624  mergePlanes(detectedPlanes[j], plane);
625 
626  #ifdef _VERBOSE
627  cout << "\tTwo regions support the same plane in the same KeyFrame\n";
628  #endif
629 
630  break;
631  }
632  if(!isSamePlane)
633  detectedPlanes.push_back(plane);
634  }
635 
636  #ifdef _VERBOSE
637  cout << detectedPlanes.size () << " Planes detected\n";
638  #endif
639 
640  // Merge detected planes with previous ones if they are the same
641  size_t numPrevPlanes = mPbMap.vPlanes.size();
642 // set<unsigned> observedPlanes;
643  observedPlanes.clear();
645  for (size_t i = 0; i < detectedPlanes.size (); i++)
646  {
647  // Check similarity with previous planes detected
648  bool isSamePlane = false;
649  vector<Plane>::iterator itPlane = mPbMap.vPlanes.begin();
650 // if(frameQueue.size() != 12)
651  for(size_t j = 0; j < numPrevPlanes; j++, itPlane++) // numPrevPlanes
652  {
653  if( areSamePlane(mPbMap.vPlanes[j], detectedPlanes[i], configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) ) // The planes are merged if they are the same
654  {
655 // if (j==2 && frameQueue.size() == 12)
656 // {
657 // cout << "Same plane\n";
658 //
659 //// ofstream pbm;
660 //// pbm.open("comparePlanes.txt");
661 // {
662 // cout << " ID " << mPbMap.vPlanes[j].id << " obs " << mPbMap.vPlanes[j].numObservations;
663 // cout << " areaVoxels " << mPbMap.vPlanes[j].areaVoxels << " areaVoxels " << mPbMap.vPlanes[j].areaHull;
664 // cout << " ratioXY " << mPbMap.vPlanes[j].elongation << " structure " << mPbMap.vPlanes[j].bFromStructure << " label " << mPbMap.vPlanes[j].label;
665 // cout << "\n normal\n" << mPbMap.vPlanes[j].v3normal << "\n center\n" << mPbMap.vPlanes[j].v3center;
666 // cout << "\n PpalComp\n" << mPbMap.vPlanes[j].v3PpalDir << "\n RGB\n" << mPbMap.vPlanes[j].v3colorNrgb;
667 // cout << "\n Neighbors (" << mPbMap.vPlanes[j].neighborPlanes.size() << "): ";
668 // for(map<unsigned,unsigned>::iterator it=mPbMap.vPlanes[j].neighborPlanes.begin(); it != mPbMap.vPlanes[j].neighborPlanes.end(); it++)
669 // cout << it->first << " ";
670 // cout << "\n CommonObservations: ";
671 // for(map<unsigned,unsigned>::iterator it=mPbMap.vPlanes[j].neighborPlanes.begin(); it != mPbMap.vPlanes[j].neighborPlanes.end(); it++)
672 // cout << it->second << " ";
673 // cout << "\n ConvexHull (" << mPbMap.vPlanes[j].polygonContourPtr->size() << "): \n";
674 // for(unsigned jj=0; jj < mPbMap.vPlanes[j].polygonContourPtr->size(); jj++)
675 // cout << "\t" << mPbMap.vPlanes[j].polygonContourPtr->points[jj].x << " " << mPbMap.vPlanes[j].polygonContourPtr->points[jj].y << " " << mPbMap.vPlanes[j].polygonContourPtr->points[jj].z << endl;
676 // cout << endl;
677 // }
678 // {
679 //// cout << " ID " << detectedPlanes[i].id << " obs " << detectedPlanes[i].numObservations;
680 //// cout << " areaVoxels " << detectedPlanes[i].areaVoxels << " areaVoxels " << detectedPlanes[i].areaHull;
681 //// cout << " ratioXY " << detectedPlanes[i].elongation << " structure " << detectedPlanes[i].bFromStructure << " label " << detectedPlanes[i].label;
682 // cout << "\n normal\n" << detectedPlanes[i].v3normal << "\n center\n" << detectedPlanes[i].v3center;
683 //// cout << "\n PpalComp\n" << detectedPlanes[i].v3PpalDir << "\n RGB\n" << detectedPlanes[i].v3colorNrgb;
684 //// cout << "\n Neighbors (" << detectedPlanes[i].neighborPlanes.size() << "): ";
685 //// for(map<unsigned,unsigned>::iterator it=detectedPlanes[i].neighborPlanes.begin(); it != detectedPlanes[i].neighborPlanes.end(); it++)
686 //// cout << it->first << " ";
687 //// cout << "\n CommonObservations: ";
688 //// for(map<unsigned,unsigned>::iterator it=detectedPlanes[i].neighborPlanes.begin(); it != detectedPlanes[i].neighborPlanes.end(); it++)
689 //// cout << it->second << " ";
690 // cout << "\n ConvexHull (" << detectedPlanes[i].polygonContourPtr->size() << "): \n";
691 // for(unsigned jj=0; jj < detectedPlanes[i].polygonContourPtr->size(); jj++)
692 // cout << "\t" << detectedPlanes[i].polygonContourPtr->points[jj].x << " " << detectedPlanes[i].polygonContourPtr->points[jj].y << " " << detectedPlanes[i].polygonContourPtr->points[jj].z << endl;
693 // cout << endl;
694 // }
695 //// pbm.close();
696 // }
697 
698  isSamePlane = true;
699 
700  mergePlanes(mPbMap.vPlanes[j], detectedPlanes[i]);
701 
702  // Update proximity graph
704 
705  #ifdef _VERBOSE
706  cout << "Previous plane " << mPbMap.vPlanes[j].id << " area " << mPbMap.vPlanes[j].areaVoxels<< " of polygon " << mPbMap.vPlanes[j].compute2DPolygonalArea() << endl;
707  #endif
708 
709  if( observedPlanes.count(mPbMap.vPlanes[j].id) == 0 ) // If this plane has already been observed through a previous partial plane in this same keyframe, then we must not account twice in the observation count
710  {
711  mPbMap.vPlanes[j].numObservations++;
712 
713  // Update co-visibility graph
714  for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++)
715  if(mPbMap.vPlanes[j].neighborPlanes.count(*it))
716  {
717  mPbMap.vPlanes[j].neighborPlanes[*it]++;
718  mPbMap.vPlanes[*it].neighborPlanes[mPbMap.vPlanes[j].id]++; // j = mPbMap.vPlanes[j]
719  }
720  else
721  {
722  mPbMap.vPlanes[j].neighborPlanes[*it] = 1;
723  mPbMap.vPlanes[*it].neighborPlanes[mPbMap.vPlanes[j].id] = 1;
724  }
725 
726  observedPlanes.insert(mPbMap.vPlanes[j].id);
727  }
728 
729  #ifdef _VERBOSE
730  cout << "Same plane\n";
731  #endif
732 
733  itPlane++;
734  for(size_t k = j+1; k < numPrevPlanes; k++, itPlane++) // numPrevPlanes
736  {
738 
739  mPbMap.vPlanes[j].numObservations += mPbMap.vPlanes[k].numObservations;
740 
741  for(set<unsigned>::iterator it = mPbMap.vPlanes[k].nearbyPlanes.begin(); it != mPbMap.vPlanes[k].nearbyPlanes.end(); it++)
742  mPbMap.vPlanes[*it].nearbyPlanes.erase(mPbMap.vPlanes[k].id);
743 
744  for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[k].neighborPlanes.begin(); it != mPbMap.vPlanes[k].neighborPlanes.end(); it++)
745  mPbMap.vPlanes[it->first].neighborPlanes.erase(mPbMap.vPlanes[k].id);
746 
747  // Update plane index
748  for(size_t h = k+1; h < numPrevPlanes; h++)
749  --mPbMap.vPlanes[h].id;
750 
751  for(size_t h = 0; h < numPrevPlanes; h++)
752  {
753  if(k==h)
754  continue;
755 
756  for(set<unsigned>::iterator it = mPbMap.vPlanes[h].nearbyPlanes.begin(); it != mPbMap.vPlanes[h].nearbyPlanes.end(); it++)
757  if(*it > mPbMap.vPlanes[k].id)
758  {
759  mPbMap.vPlanes[h].nearbyPlanes.insert(*it-1);
760  mPbMap.vPlanes[h].nearbyPlanes.erase(*it);
761  }
762 
763  for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[h].neighborPlanes.begin(); it != mPbMap.vPlanes[h].neighborPlanes.end(); it++)
764  if(it->first > mPbMap.vPlanes[k].id)
765  {
766  mPbMap.vPlanes[h].neighborPlanes[it->first-1] = it->second;
767  mPbMap.vPlanes[h].neighborPlanes.erase(it);
768  }
769  }
770 
771  mPbMap.vPlanes.erase(itPlane);
772  --numPrevPlanes;
773 
774  #ifdef _VERBOSE
775  cout << "MERGE TWO PREVIOUS PLANES WHEREBY THE INCORPORATION OF A NEW REGION \n";
776  #endif
777  }
778 
779  break;
780  }
781  }
782  if(!isSamePlane)
783  {
784  detectedPlanes[i].id = mPbMap.vPlanes.size();
785  detectedPlanes[i].numObservations = 1;
786  detectedPlanes[i].bFullExtent = false;
787  detectedPlanes[i].nFramesAreaIsStable = 0;
788 // detectedPlanes[i].calcMainColor(calcMainColor();
790  {
791  detectedPlanes[i].semanticGroup = clusterize->currentSemanticGroup;
792  clusterize->groups[clusterize->currentSemanticGroup].push_back(detectedPlanes[i].id);
793  }
794 
795  #ifdef _VERBOSE
796  cout << "New plane " << detectedPlanes[i].id << " area " << detectedPlanes[i].areaVoxels<< " of polygon " << detectedPlanes[i].areaHull << endl;
797  #endif
798 
799  // Update proximity graph
800  checkProximity(detectedPlanes[i], configPbMap.proximity_neighbor_planes); // Detect neighbors with max separation of 1.0 meters
801 
802  // Update co-visibility graph
803  for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++)
804  {
805  detectedPlanes[i].neighborPlanes[*it] = 1;
806  mPbMap.vPlanes[*it].neighborPlanes[detectedPlanes[i].id] = 1;
807  }
808 
809  observedPlanes.insert(detectedPlanes[i].id);
810 
811  mPbMap.vPlanes.push_back(detectedPlanes[i]);
812  }
813  }
814  }
815 
816 // if(frameQueue.size() == 12)
817 // cout << "Same plane? " << areSamePlane(mPbMap.vPlanes[2], mPbMap.vPlanes[9], configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) << endl;
818 
819  #ifdef _VERBOSE
820  cout << "\n\tobservedPlanes: ";
821  cout << observedPlanes.size () << " Planes observed\n";
822  for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++)
823  cout << *it << " ";
824  cout << endl;
825  #endif
826 
827  // For all observed planes
828  for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++)
829  {
830  Plane &observedPlane = mPbMap.vPlanes[*it];
831 
832  // Calculate principal direction
833  observedPlane.calcElongationAndPpalDir();
834 
835 ////cout << "Update color\n";
836  // Update color
837  observedPlane.calcMainColor();
838 
839  #ifdef _VERBOSE
840  cout << "Plane " << observedPlane.id << " color\n" << observedPlane.v3colorNrgb << endl;
841  #endif
842 
843  // Infer knowledge from the planes (e.g. do these planes represent the floor, walls, etc.)
845  mpPlaneInferInfo->searchTheFloor(poseKF, observedPlane);
846  } // End for obsevedPlanes
847 //cout << "Updated planes\n";
848 
849 // for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++)
850 // {
851 // Plane &observedPlane = mPbMap.vPlanes[*it];
852 // watchProperties(observedPlanes, observedPlane); // Color paper
853 // }
854 
855  // Search the floor plane
856  if(mPbMap.FloorPlane != -1) // Verify that the observed planes centers are above the floor
857  {
858  #ifdef _VERBOSE
859  cout << "Verify that the observed planes centers are above the floor\n";
860  #endif
861 
862  for(set<unsigned>::reverse_iterator it = observedPlanes.rbegin(); it != observedPlanes.rend(); it++)
863  {
864  if(static_cast<int>(*it) == mPbMap.FloorPlane)
865  continue;
866  if( mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(mPbMap.vPlanes[*it].v3center - mPbMap.vPlanes[mPbMap.FloorPlane].v3center) < -0.1 )
867  {
868  if(mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(mPbMap.vPlanes[*it].v3normal) > 0.99) //(cos 8.1º = 0.99)
869  {
870  mPbMap.vPlanes[*it].label = "Floor";
871  mPbMap.vPlanes[mPbMap.FloorPlane].label = "";
872  mPbMap.FloorPlane = *it;
873  }
874  else
875  {
876 // assert(false);
877  mPbMap.vPlanes[mPbMap.FloorPlane].label = "";
878  mPbMap.FloorPlane = -1;
879  break;
880  }
881  }
882  }
883  }
884 
886  for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++)
887  {
888 // cout << "insert planes\n";
889  if(mpPbMapLocaliser->vQueueObservedPlanes.size() < 10)
890  mpPbMapLocaliser->vQueueObservedPlanes.push_back(*it);
891  }
892 
893  #ifdef _VERBOSE
894  cout << "DetectedPlanesCloud finished\n";
895  #endif
896 
897  updateLock.unlock();
898 }
899 
900 
901 bool graphRepresentation = false;
902 void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void* viewer_void)
903 {
904  if ( (event.getKeySym () == "r" || event.getKeySym () == "R") && event.keyDown ())
905  {
907  }
908 }
909 
910 /*!Check if the the input plane is the same than this plane for some given angle and distance thresholds.
911  * If the planes are the same they are merged in this and the function returns true. Otherwise it returns false.*/
912 bool PbMapMaker::areSamePlane(Plane &plane1, Plane &plane2, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
913 {
914  // Check that both planes have similar orientation
915  if( plane1.v3normal.dot(plane2.v3normal) < cosAngleThreshold )
916  return false;
917 // if(plane1.id == 2)
918 // cout << "normal " << plane1.v3normal.dot(plane2.v3normal) << " " << cosAngleThreshold << endl;
919 
920  // Check the normal distance of the planes centers using their average normal
921  float dist_normal = plane1.v3normal.dot(plane2.v3center - plane1.v3center);
922 // if(fabs(dist_normal) > distThreshold ) // Avoid matching different parallel planes
923 // return false;
924  float thres_max_dist = max(distThreshold, distThreshold*2*(plane2.v3center - plane1.v3center).norm());
925  if(fabs(dist_normal) > thres_max_dist ) // Avoid matching different parallel planes
926  return false;
927 // if(plane1.id == 2)
928 // {
929 // cout << "dist_normal " << dist_normal << " " << thres_max_dist << endl;
930 // if(arePlanesNearby(plane1, plane2, proxThreshold))
931 // cout << "planes rearby" << endl;
932 // }
933 
934  // Once we know that the planes are almost coincident (parallelism and position)
935  // we check that the distance between the planes is not too big
936  return arePlanesNearby(plane1, plane2, proxThreshold);
937 }
938 
939 void PbMapMaker::mergePlanes(Plane &updatePlane, Plane &discardPlane)
940 {
941  // Update normal and center
942  updatePlane.v3normal = updatePlane.areaVoxels*updatePlane.v3normal + discardPlane.areaVoxels*discardPlane.v3normal;
943  updatePlane.v3normal = updatePlane.v3normal / (updatePlane.v3normal).norm();
944  // Update point inliers
945 // *updatePlane.polygonContourPtr += *discardPlane.polygonContourPtr; // Merge polygon points
946  *updatePlane.planePointCloudPtr += *discardPlane.planePointCloudPtr; // Add the points of the new detection and perform a voxel grid
947 
948  // Filter the points of the patch with a voxel-grid. This points are used only for visualization
949  static pcl::VoxelGrid<pcl::PointXYZRGBA> merge_grid;
950  merge_grid.setLeafSize(0.05,0.05,0.05);
951  pcl::PointCloud<pcl::PointXYZRGBA> mergeCloud;
952  merge_grid.setInputCloud (updatePlane.planePointCloudPtr);
953  merge_grid.filter (mergeCloud);
954  updatePlane.planePointCloudPtr->clear();
955  *updatePlane.planePointCloudPtr = mergeCloud;
956 
957 // if(configPbMap.use_color)
958 // updatePlane.calcMainColor();
959 
960  *discardPlane.polygonContourPtr += *updatePlane.polygonContourPtr;
961  updatePlane.calcConvexHull(discardPlane.polygonContourPtr);
962  updatePlane.computeMassCenterAndArea();
963 
964  // Move the points to fulfill the plane equation
965  updatePlane.forcePtsLayOnPlane();
966 
967  // Update area
968  double area_recalc = updatePlane.planePointCloudPtr->size() * 0.0025;
969  mpPlaneInferInfo->isFullExtent(updatePlane, area_recalc);
970  updatePlane.areaVoxels= updatePlane.planePointCloudPtr->size() * 0.0025;
971 
972 }
973 
974 // Color = (red[i], grn[i], blu[i])
975 // The color order is: red, green, blue, yellow, pink, turquoise, orange, purple, dark green, beige
976 unsigned char red [10] = {255, 0, 0, 255, 255, 0, 255, 204, 0, 255};
977 unsigned char grn [10] = { 0, 255, 0, 255, 0, 255, 160, 51, 128, 222};
978 unsigned char blu [10] = { 0, 0, 255, 0, 255, 255, 0 , 204, 0, 173};
979 
980 double ared [10] = {1.0, 0, 0, 1.0, 1.0, 0, 1.0, 0.8, 0, 1.0};
981 double agrn [10] = { 0, 1.0, 0, 1.0, 0, 1.0, 0.6, 0.2, 0.5, 0.9};
982 double ablu [10] = { 0, 0, 1.0, 0, 1.0, 1.0, 0, 0.8, 0, 0.7};
983 
984 void PbMapMaker::viz_cb (pcl::visualization::PCLVisualizer& viz)
985 {
986  if (mPbMap.globalMapPtr->empty())
987  {
989  return;
990  }
991 
993 
994  // Render the data
995  {
996  viz.removeAllShapes();
997  viz.removeAllPointClouds();
998 
999  char name[1024];
1000 
1001 // if(graphRepresentation)
1002 // {
1003 // for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
1004 // {
1005 // pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0], 2*mPbMap.vPlanes[i].v3center[1], 2*mPbMap.vPlanes[i].v3center[2]);
1006 // double radius = 0.1 * sqrt(mPbMap.vPlanes[i].areaVoxels);
1007 // sprintf (name, "sphere%u", static_cast<unsigned>(i));
1008 // viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);
1009 //
1010 // if( !mPbMap.vPlanes[i].label.empty() )
1011 // viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], mPbMap.vPlanes[i].label);
1012 // else
1013 // {
1014 // sprintf (name, "P%u", static_cast<unsigned>(i));
1015 // viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
1016 // }
1017 //
1018 // // Draw edges
1019 // if(!configPbMap.graph_mode) // Nearby neighbors
1020 // for(set<unsigned>::iterator it = mPbMap.vPlanes[i].nearbyPlanes.begin(); it != mPbMap.vPlanes[i].nearbyPlanes.end(); it++)
1021 // {
1022 // if(*it > mPbMap.vPlanes[i].id)
1023 // break;
1024 //
1025 // sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
1026 // pcl::PointXYZ center_it(2*mPbMap.vPlanes[*it].v3center[0], 2*mPbMap.vPlanes[*it].v3center[1], 2*mPbMap.vPlanes[*it].v3center[2]);
1027 // viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
1028 // }
1029 // else
1030 // for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[i].neighborPlanes.begin(); it != mPbMap.vPlanes[i].neighborPlanes.end(); it++)
1031 // {
1032 // if(it->first > mPbMap.vPlanes[i].id)
1033 // break;
1034 //
1035 // sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
1036 // pcl::PointXYZ center_it(2*mPbMap.vPlanes[it->first].v3center[0], 2*mPbMap.vPlanes[it->first].v3center[1], 2*mPbMap.vPlanes[it->first].v3center[2]);
1037 // viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
1038 //
1039 // sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
1040 // char commonObs[8];
1041 // sprintf (commonObs, "%u", it->second);
1042 // pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
1043 // viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
1044 // }
1045 //
1046 // }
1047 // }
1048 // else
1049  { // Regular representation
1050 
1052  {
1053  if (!viz.updatePointCloud (mPbMap.globalMapPtr, "cloud"))
1054  viz.addPointCloud (mPbMap.globalMapPtr, "cloud");
1055  return;
1056  }
1057 
1058 
1059  if(mpPbMapLocaliser != NULL)
1061  if (!viz.updatePointCloud (mpPbMapLocaliser->alignedModelPtr, "model"))
1062  viz.addPointCloud (mpPbMapLocaliser->alignedModelPtr, "model");}
1063 
1064  sprintf (name, "PointCloud size %u", static_cast<unsigned>( mPbMap.globalMapPtr->size() ) );
1065  viz.addText(name, 10, 20);
1066 
1067 //pcl::ModelCoefficients plane_coefs;
1068 //plane_coefs.values[0] = mPbMap.vPlanes[0].v3normal[0];
1069 //plane_coefs.values[1] = mPbMap.vPlanes[0].v3normal[1];
1070 //plane_coefs.values[2] = mPbMap.vPlanes[0].v3normal[2];
1071 //plane_coefs.values[3] = -(mPbMap.vPlanes[0].v3normal .dot (mPbMap.vPlanes[0].v3center) );
1072 //viz.addPlane (plane_coefs);
1073 
1074  for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
1075  {
1076  Plane &plane_i = mPbMap.vPlanes[i];
1077  sprintf (name, "normal_%u", static_cast<unsigned>(i));
1078  pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
1079  pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
1080  pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
1081  plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
1082  plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
1083  viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
1084 
1085  // Draw Ppal diretion
1086 // if( plane_i.elongation > 1.3 )
1087 // {
1088 // sprintf (name, "ppalComp_%u", static_cast<unsigned>(i));
1089 // pcl::PointXYZ pt3 = pcl::PointXYZ ( plane_i.v3center[0] + (0.2f * plane_i.v3PpalDir[0]),
1090 // plane_i.v3center[1] + (0.2f * plane_i.v3PpalDir[1]),
1091 // plane_i.v3center[2] + (0.2f * plane_i.v3PpalDir[2]));
1092 // viz.addArrow (pt3, plane_i.pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
1093 // }
1094 
1095 // if( !plane_i.label.empty() )
1096 // viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
1097 // else
1098  {
1099  sprintf (name, "n%u", static_cast<unsigned>(i));
1100 // sprintf (name, "n%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(plane_i.semanticGroup));
1101  viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
1102  }
1103 
1104 // sprintf (name, "planeRaw_%02u", static_cast<unsigned>(i));
1105 // viz.addPointCloud (plane_i.planeRawPointCloudPtr, name);// contourPtr, planePointCloudPtr, polygonContourPtr
1106 
1107 // if(!configPbMap.makeClusters)
1108 // {
1109  sprintf (name, "plane_%02u", static_cast<unsigned>(i));
1110 // if(plane_i.bDominantColor)
1111  {
1112 // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[i%10], grn[i%10], blu[i%10]);
1113 //// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
1114 // viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
1115 // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
1116 // }
1117 // else
1118 // {
1119 // sprintf (name, "plane_%02u", static_cast<unsigned>(i));
1120 // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
1121 
1122  double illum = 0;
1123  if(fabs(plane_i.v3colorNrgb[0]-0.33) < 0.03 && fabs(plane_i.v3colorNrgb[1]-0.33) < 0.03 && fabs(plane_i.v3colorNrgb[2]-0.33) < 0.03 && plane_i.dominantIntensity > 400)
1124  illum = 0.5;
1125 
1126  pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr,
1127  plane_i.v3colorNrgb[0] * (plane_i.dominantIntensity+(755-plane_i.dominantIntensity)*illum),
1128  plane_i.v3colorNrgb[1] * (plane_i.dominantIntensity+(755-plane_i.dominantIntensity)*illum),
1129  plane_i.v3colorNrgb[2] * (plane_i.dominantIntensity+(755-plane_i.dominantIntensity)*illum));
1130 // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr,
1131 // plane_i.v3colorNrgb[0] * plane_i.dominantIntensity,
1132 // plane_i.v3colorNrgb[1] * plane_i.dominantIntensity,
1133 // plane_i.v3colorNrgb[2] * plane_i.dominantIntensity);
1134  viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
1135  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, name);
1136 // }
1137  }
1138 // else
1139 // viz.addPointCloud (plane_i.planePointCloudPtr, name);// contourPtr, planePointCloudPtr, polygonContourPtr
1140 
1141 // sprintf (name, "planeBorder_%02u", static_cast<unsigned>(i));
1142 // pcl::visualization::PointCloudColorHandlerCustom <PointT> color2 (plane_i.contourPtr, 255, 255, 255);
1143 // viz.addPointCloud (plane_i.contourPtr, color2, name);// contourPtr, planePointCloudPtr, polygonContourPtr
1144 
1145 // //Edges
1146 // if(mPbMap.edgeCloudPtr->size() > 0)
1147 // {
1148 // sprintf (name, "planeEdge_%02u", static_cast<unsigned>(i));
1149 // pcl::visualization::PointCloudColorHandlerCustom <PointT> color4 (mPbMap.edgeCloudPtr, 255, 255, 0);
1150 // viz.addPointCloud (mPbMap.edgeCloudPtr, color4, name);// contourPtr, planePointCloudPtr, polygonContourPtr
1151 // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
1152 //
1153 // sprintf (name, "edge%u", static_cast<unsigned>(i));
1154 // viz.addLine (mPbMap.edgeCloudPtr->points.front(), mPbMap.edgeCloudPtr->points.back(), ared[3], agrn[3], ablu[3], name);
1155 // }
1156 
1157  sprintf (name, "approx_plane_%02d", int (i));
1158  viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
1159  }
1160 
1161 //if(configPbMap.makeClusters)
1162 // for(map<unsigned, std::vector<unsigned> >::iterator it=clusterize->groups.begin(); it != clusterize->groups.end(); it++)
1163 // for(size_t i=0; i < it->second.size(); i++)
1164 // {
1165 // unsigned planeID = it->second[i];
1166 // Plane &plane_i = mPbMap.vPlanes[planeID];
1167 // sprintf (name, "plane_%02u", static_cast<unsigned>(planeID));
1168 // pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[planeID%10], grn[planeID%10], blu[planeID%10]);
1169 // viz.addPointCloud (plane_i.planePointCloudPtr, color, name);// contourPtr, planePointCloudPtr, polygonContourPtr
1170 // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, name);
1171 // }
1172 
1173  // Draw recognized plane labels
1174  if(mpPbMapLocaliser != NULL)
1176  viz.addText3D (it->first, it->second, 0.3, 0.9, 0.9, 0.9, it->first);
1177 
1178  }
1179  }
1180  }
1181 }
1182 
1184 {
1185  cloudViewer.runOnVisualizationThread (boost::bind(&PbMapMaker::viz_cb, this, _1), "viz_cb");
1186  cloudViewer.registerKeyboardCallback ( keyboardEventOccurred );
1187 
1188  size_t numPrevKFs = 0;
1189  size_t minGrowPlanes = 5;
1190  while(!m_pbmaker_must_stop) // Stop loop if PbMapMaker
1191  {
1192  if( numPrevKFs == frameQueue.size() )
1193  {
1194  mrpt::system::sleep(10);
1195  }
1196  else
1197  {
1198  // Assign pointCloud of last KF to the global map
1199  detectPlanesCloud( frameQueue.back().cloudPtr, frameQueue.back().pose,
1201 
1203  if(mPbMap.vPlanes.size() > minGrowPlanes)
1204  {
1205  // Evaluate the partition of the current groups with minNcut
1206  int size_partition = clusterize->evalPartition(observedPlanes);
1207  cout << "PARTITION SIZE " << size_partition << endl;
1208  assert(size_partition < 2);
1209 
1210  minGrowPlanes += 2;
1211  }
1212  }
1213 
1214  ++numPrevKFs;
1215  }
1216  }
1217 
1218 // saveInfoFiles(); // save watch statistics
1219 
1220  m_pbmaker_finished = true;
1221 }
1222 
1224 {
1225  boost::mutex::scoped_lock updateLock(mtx_pbmap_busy);
1226 
1227  mPbMap.savePbMap(path);
1228 
1229  updateLock.unlock();
1230 }
1231 
1233 {
1234  m_pbmaker_must_stop = true;
1235  while(!m_pbmaker_finished)
1237  cout << "Waiting for PbMapMaker thread to die.." << endl;
1238 
1240  pbmaker_hd.clear();
1241 
1242  return true;
1243 }
1244 
1246 {
1247  cout << "\n\n\nPbMapMaker destructor called -> Save color information to file\n";
1248  saveInfoFiles();
1249 
1250  delete mpPlaneInferInfo;
1251  delete mpPbMapLocaliser;
1252 
1253  stop_pbMapMaker();
1254 
1255  cout << " .. PbMapMaker has died." << endl;
1256 }
1257 
1258 #endif
This class provides simple critical sections functionality.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:44
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
PlaneInferredInfo * mpPlaneInferInfo
Definition: PbMapMaker.h:126
string config_localiser
Definition: PbMapMaker.cpp:96
double ared[10]
Definition: PbMapMaker.cpp:980
std::vector< double > prog_area
Definition: Plane.h:155
std::vector< Eigen::Vector3f > prog_Nrgb
Definition: Plane.h:158
float minInliersRate
Definition: PbMapMaker.cpp:79
std::map< std::string, pcl::PointXYZ > foundPlaces
unsigned char red[10]
Definition: PbMapMaker.cpp:976
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:167
std::set< unsigned > observedPlanes
Definition: PbMapMaker.h:123
A class used to store a planar feature (Plane for short).
Definition: Plane.h:48
float dist_threshold
Definition: PbMapMaker.cpp:77
mrpt::synch::CCriticalSection CS_visualize
Definition: PbMapMaker.cpp:49
PbMapLocaliser * mpPbMapLocaliser
Definition: PbMapMaker.h:110
bool areSamePlane(Plane &plane1, Plane &plane2, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
Definition: PbMapMaker.cpp:912
float dominantIntensity
Definition: Plane.h:148
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
void checkProximity(Plane &plane, float proximity)
Definition: PbMapMaker.cpp:243
void serializePbMap(std::string path)
GLboolean GLenum GLenum GLvoid * values
Definition: glew.h:2897
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *viewer_void)
Definition: PbMapMaker.cpp:902
Scalar * iterator
Definition: eigen_plugins.h:23
float elongation
Definition: Plane.h:137
boost::mutex mtx_pbmap_busy
Definition: PbMapMaker.h:115
bool inferStructure
Definition: PbMapMaker.cpp:91
void viz_cb(pcl::visualization::PCLVisualizer &viz)
Definition: PbMapMaker.cpp:984
float max_cos_normal
Definition: PbMapMaker.cpp:85
Eigen::Vector3f v3colorNrgb
! Radiometric description
Definition: Plane.h:147
float intensity_threshold
Definition: PbMapMaker.cpp:73
void watchProperties(std::set< unsigned > &observedPlanes, Plane &observedPlane)
Definition: PbMapMaker.cpp:278
struct config_pbmap configPbMap
void calcElongationAndPpalDir()
Definition: Plane.cpp:413
A class used to infer some semantic meaning to the planes of a PbMap.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::string path_save_registered_cloud
Definition: PbMapMaker.cpp:101
std::map< unsigned, std::vector< unsigned > > groups
mrpt::system::TThreadHandle pbmaker_hd
Definition: PbMapMaker.h:135
std::vector< Eigen::Vector3f > prog_C1C2C3
Definition: Plane.h:157
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr alignedModelPtr
std::vector< float > prog_intensity
Definition: Plane.h:159
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:55
void mergePlanes(Plane &updatePlane, Plane &discardPlane)
Definition: PbMapMaker.cpp:939
double agrn[10]
Definition: PbMapMaker.cpp:981
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
float areaVoxels
Definition: Plane.h:138
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...
Eigen::Vector3f v3normal
Definition: Plane.h:132
std::string path_save_pbmap
Definition: PbMapMaker.cpp:99
std::set< unsigned > nearbyPlanes
Definition: Plane.h:120
unsigned char blu[10]
Definition: PbMapMaker.cpp:978
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:169
bool record_rawlog
Definition: PbMapMaker.cpp:71
bool searchTheFloor(Eigen::Matrix4f &poseSensor, Plane &plane)
! Check if the input plane correpond to the floor.
GLuint const GLchar * name
Definition: glew.h:1721
float areaHull
Definition: Plane.h:139
void forcePtsLayOnPlane()
Definition: Plane.cpp:321
std::vector< double > prog_elongation
Definition: Plane.h:156
SemanticClustering * clusterize
Definition: PbMapMaker.h:113
unsigned char grn[10]
Definition: PbMapMaker.cpp:977
float proximity_threshold
Definition: PbMapMaker.cpp:87
GLuint color
Definition: glew.h:5778
std::vector< unsigned > vQueueObservedPlanes
pcl::PointCloud< PointT >::Ptr globalMapPtr
Definition: PbMap.h:64
bool arePlanesNearby(Plane &plane1, Plane &plane2, const float distThreshold)
Definition: PbMapMaker.cpp:178
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:216
bool detect_loopClosure
Definition: PbMapMaker.cpp:95
float proximity_neighbor_planes
Definition: PbMapMaker.cpp:83
bool save_registered_cloud
Definition: PbMapMaker.cpp:100
void calcMainColor()
Definition: Plane.cpp:642
float compute2DPolygonalArea()
Compute the area of a 2D planar polygon patch - using a given normal.
Definition: Plane.cpp:345
float PBMAP_IMPEXP dist3D_Segment_to_Segment2(Segment S1, Segment S2)
std::vector< std::vector< float > > prog_hist_H
Definition: Plane.h:160
GLsizei const GLcharARB ** string
Definition: glew.h:3293
void detectPlanesCloud(pcl::PointCloud< PointT >::Ptr &pointCloudPtr_arg, Eigen::Matrix4f &poseKF, double distThreshold, double angleThreshold, double minInliersF)
Definition: PbMapMaker.cpp:505
int evalPartition(std::set< unsigned > &observedPlanes)
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:117
float angle_threshold
Definition: PbMapMaker.cpp:78
bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud< PointT >::Ptr hull3D)
void computeMassCenterAndArea()
Compute the patch's convex-hull area and mass center.
Definition: Plane.cpp:375
std::vector< float > hist_H
Definition: Plane.h:153
float hue_threshold
Definition: PbMapMaker.cpp:74
std::vector< frameRGBDandPose > frameQueue
Definition: PbMapMaker.h:71
#define ASSERT_FILE_EXISTS_(FIL)
std::string rawlog_path
Definition: PbMapMaker.cpp:70
int BASE_IMPEXP sprintf(char *buf, size_t bufSize, const char *format,...) MRPT_NO_THROWS 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:191
bool makeClusters
Definition: PbMapMaker.cpp:92
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:131
void readConfigFile(const string &config_file_name)
Definition: PbMapMaker.cpp:105
void savePbMap(std::string filePath)
Definition: PbMap.cpp:102
unsigned numObservations
Definition: Plane.h:118
bool graphRepresentation
Definition: PbMapMaker.cpp:901
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::vector< Plane > vPlanes
Definition: PbMap.h:55
float color_threshold
Definition: PbMapMaker.cpp:72
float max_dist_center_plane
Definition: PbMapMaker.cpp:86
double ablu[10]
Definition: PbMapMaker.cpp:982
bool input_from_rawlog
Definition: PbMapMaker.cpp:69
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:34
Eigen::Vector3f v3colorC1C2C3
Definition: Plane.h:152
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:784
CONTAINER::Scalar norm(const CONTAINER &v)
pcl::visualization::CloudViewer cloudViewer
Definition: PbMapMaker.h:77
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:38
void BASE_IMPEXP joinThread(const TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:190
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
double BhattacharyyaDist(std::vector< float > &hist1, std::vector< float > &hist2)
Definition: PbMapMaker.cpp:52



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018