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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020