MRPT  1.9.9
PbMapMaker.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
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>
25 #include <mrpt/pbmap/PbMapMaker.h>
26 #include <pcl/ModelCoefficients.h>
27 #include <pcl/common/time.h>
28 #include <pcl/common/transforms.h>
29 #include <pcl/features/integral_image_normal.h>
30 #include <pcl/features/normal_3d.h>
31 #include <pcl/filters/extract_indices.h>
32 #include <pcl/filters/voxel_grid.h>
33 #include <pcl/segmentation/organized_connected_component_segmentation.h>
34 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
35 
36 #include <iostream>
37 #include <mutex>
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 
291  pcl::PointCloud<PointT>::Ptr& pointCloudPtr_arg, Eigen::Matrix4f& poseKF,
292  double distThreshold, double angleThreshold, double minInliersF)
293 {
294  boost::mutex::scoped_lock updateLock(mtx_pbmap_busy);
295 
296  unsigned minInliers = minInliersF * pointCloudPtr_arg->size();
297 
298 #ifdef _VERBOSE
299  cout << "detectPlanes in a cloud with " << pointCloudPtr_arg->size()
300  << " points " << minInliers << " minInliers\n";
301 #endif
302 
303  pcl::PointCloud<PointT>::Ptr pointCloudPtr_arg2(
304  new pcl::PointCloud<PointT>);
305  pcl::copyPointCloud(*pointCloudPtr_arg, *pointCloudPtr_arg2);
306 
307  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedCloudPtr(
308  new pcl::PointCloud<pcl::PointXYZRGBA>);
309  pcl::transformPointCloud(*pointCloudPtr_arg, *alignedCloudPtr, poseKF);
310 
311  {
312  std::lock_guard<std::mutex> csl(CS_visualize);
313  *mPbMap.globalMapPtr += *alignedCloudPtr;
314  // Downsample voxel map's point cloud
315  static pcl::VoxelGrid<pcl::PointXYZRGBA> grid;
316  grid.setLeafSize(0.02, 0.02, 0.02);
317  pcl::PointCloud<pcl::PointXYZRGBA> globalMap;
318  grid.setInputCloud(mPbMap.globalMapPtr);
319  grid.filter(globalMap);
320  mPbMap.globalMapPtr->clear();
321  *mPbMap.globalMapPtr = globalMap;
322  } // End CS
323 
324  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
325  ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
326  ne.setMaxDepthChangeFactor(0.02f); // For VGA: 0.02f, 10.0f
327  ne.setNormalSmoothingSize(10.0f);
328  ne.setDepthDependentSmoothing(true);
329 
330  pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
331  mps.setMinInliers(minInliers);
332  cout << "Params " << minInliers << " " << angleThreshold << " "
333  << distThreshold << endl;
334  mps.setAngularThreshold(angleThreshold); // (0.017453 * 2.0) // 3 degrees
335  mps.setDistanceThreshold(distThreshold); // 2cm
336 
337  pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(
338  new pcl::PointCloud<pcl::Normal>);
339  ne.setInputCloud(pointCloudPtr_arg2);
340  ne.compute(*normal_cloud);
341 
342 #ifdef _VERBOSE
343  double plane_extract_start = pcl::getTime();
344 #endif
345  mps.setInputNormals(normal_cloud);
346  mps.setInputCloud(pointCloudPtr_arg2);
347 
348  std::vector<
349  pcl::PlanarRegion<PointT>, aligned_allocator<pcl::PlanarRegion<PointT>>>
350  regions;
351  std::vector<pcl::ModelCoefficients> model_coefficients;
352  std::vector<pcl::PointIndices> inlier_indices;
353  pcl::PointCloud<pcl::Label>::Ptr labels(new pcl::PointCloud<pcl::Label>);
354  std::vector<pcl::PointIndices> label_indices;
355  std::vector<pcl::PointIndices> boundary_indices;
356  mps.segmentAndRefine(
357  regions, model_coefficients, inlier_indices, labels, label_indices,
358  boundary_indices);
359 
360 #ifdef _VERBOSE
361  double plane_extract_end = pcl::getTime();
362  std::cout << "Plane extraction took "
363  << double(plane_extract_end - plane_extract_start) << std::endl;
364  // std::cout << "Frame took " << double (plane_extract_end -
365  // normal_start) << std::endl;
366  cout << regions.size() << " planes detected\n";
367 #endif
368 
369  // Create a vector with the planes detected in this keyframe, and calculate
370  // their parameters (normal, center, pointclouds, etc.)
371  // in the global reference
372  vector<Plane> detectedPlanes;
373  for (size_t i = 0; i < regions.size(); i++)
374  {
375  Plane plane;
376 
377  Vector3f centroid = regions[i].getCentroid();
378  plane.v3center = compose(poseKF, centroid);
379  plane.v3normal =
380  poseKF.block(0, 0, 3, 3) * Vector3f(
381  model_coefficients[i].values[0],
382  model_coefficients[i].values[1],
383  model_coefficients[i].values[2]);
384  // plane.curvature = regions[i].getCurvature();
385  // assert(plane.v3normal*plane.v3center.transpose() <= 0);
386  // if(plane.v3normal*plane.v3center.transpose() <= 0)
387  // plane.v3normal *= -1;
388 
389  // Extract the planar inliers from the input cloud
390  pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
391  extract.setInputCloud(pointCloudPtr_arg2);
392  extract.setIndices(
393  boost::make_shared<const pcl::PointIndices>(inlier_indices[i]));
394  extract.setNegative(false);
395  extract.filter(
396  *plane.planePointCloudPtr); // Write the planar point cloud
397 
398  static pcl::VoxelGrid<pcl::PointXYZRGBA> plane_grid;
399  plane_grid.setLeafSize(0.05, 0.05, 0.05);
400  pcl::PointCloud<pcl::PointXYZRGBA> planeCloud;
401  plane_grid.setInputCloud(plane.planePointCloudPtr);
402  plane_grid.filter(planeCloud);
403  plane.planePointCloudPtr->clear();
404  pcl::transformPointCloud(planeCloud, *plane.planePointCloudPtr, poseKF);
405 
406  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contourPtr(
407  new pcl::PointCloud<pcl::PointXYZRGBA>);
408  contourPtr->points = regions[i].getContour();
409  plane_grid.setLeafSize(0.1, 0.1, 0.1);
410  plane_grid.setInputCloud(contourPtr);
411  plane_grid.filter(*plane.polygonContourPtr);
412  // plane.contourPtr->points = regions[i].getContour();
413  // pcl::transformPointCloud(*plane.contourPtr,*plane.polygonContourPtr,poseKF);
414  pcl::transformPointCloud(*plane.polygonContourPtr, *contourPtr, poseKF);
415  plane.calcConvexHull(contourPtr);
416  plane.computeMassCenterAndArea();
417  plane.areaVoxels = plane.planePointCloudPtr->size() * 0.0025;
418 
419 #ifdef _VERBOSE
420  cout << "Area plane region " << plane.areaVoxels << " of Chull "
421  << plane.areaHull << " of polygon "
422  << plane.compute2DPolygonalArea() << endl;
423 #endif
424 
425  // Check whether this region correspond to the same plane as a previous
426  // one (this situation may happen when there exists a small
427  // discontinuity in the observation)
428  bool isSamePlane = false;
429  for (size_t j = 0; j < detectedPlanes.size(); j++)
430  if (areSamePlane(
431  detectedPlanes[j], plane, configPbMap.max_cos_normal,
433  configPbMap.proximity_threshold)) // The planes are merged
434  // if they are the same
435  {
436  isSamePlane = true;
437 
438  mergePlanes(detectedPlanes[j], plane);
439 
440 #ifdef _VERBOSE
441  cout << "\tTwo regions support the same plane in the same "
442  "KeyFrame\n";
443 #endif
444 
445  break;
446  }
447  if (!isSamePlane) detectedPlanes.push_back(plane);
448  }
449 
450 #ifdef _VERBOSE
451  cout << detectedPlanes.size() << " Planes detected\n";
452 #endif
453 
454  // Merge detected planes with previous ones if they are the same
455  size_t numPrevPlanes = mPbMap.vPlanes.size();
456  // set<unsigned> observedPlanes;
457  observedPlanes.clear();
458  {
459  std::lock_guard<std::mutex> csl(CS_visualize);
460  for (size_t i = 0; i < detectedPlanes.size(); i++)
461  {
462  // Check similarity with previous planes detected
463  bool isSamePlane = false;
464  auto itPlane = mPbMap.vPlanes.begin();
465  // if(frameQueue.size() != 12)
466  for (size_t j = 0; j < numPrevPlanes;
467  j++, itPlane++) // numPrevPlanes
468  {
469  // The planes are merged if they are the same
470  if (areSamePlane(
471  mPbMap.vPlanes[j], detectedPlanes[i],
475  {
476  isSamePlane = true;
477  mergePlanes(mPbMap.vPlanes[j], detectedPlanes[i]);
478 
479  // Update proximity graph
481  mPbMap.vPlanes[j],
483  .proximity_neighbor_planes); // Detect neighbors
484 
485 #ifdef _VERBOSE
486  cout << "Previous plane " << mPbMap.vPlanes[j].id
487  << " area " << mPbMap.vPlanes[j].areaVoxels
488  << " of polygon "
489  << mPbMap.vPlanes[j].compute2DPolygonalArea() << endl;
490 #endif
491 
492  if (observedPlanes.count(mPbMap.vPlanes[j].id) ==
493  0) // If this plane has already been observed through a
494  // previous partial plane in this same keyframe,
495  // then we must not account twice in the observation
496  // count
497  {
498  mPbMap.vPlanes[j].numObservations++;
499 
500  // Update co-visibility graph
501  for (auto it = observedPlanes.begin();
502  it != observedPlanes.end(); it++)
503  if (mPbMap.vPlanes[j].neighborPlanes.count(*it))
504  {
505  mPbMap.vPlanes[j].neighborPlanes[*it]++;
506  mPbMap.vPlanes[*it].neighborPlanes
507  [mPbMap.vPlanes[j]
508  .id]++; // j = mPbMap.vPlanes[j]
509  }
510  else
511  {
512  mPbMap.vPlanes[j].neighborPlanes[*it] = 1;
513  mPbMap.vPlanes[*it]
514  .neighborPlanes[mPbMap.vPlanes[j].id] = 1;
515  }
516 
517  observedPlanes.insert(mPbMap.vPlanes[j].id);
518  }
519 
520 #ifdef _VERBOSE
521  cout << "Same plane\n";
522 #endif
523 
524  itPlane++;
525  for (size_t k = j + 1; k < numPrevPlanes;
526  k++, itPlane++) // numPrevPlanes
527  if (areSamePlane(
528  mPbMap.vPlanes[j], mPbMap.vPlanes[k],
532  {
533  // The planes are merged if they are the same
535 
536  mPbMap.vPlanes[j].numObservations +=
537  mPbMap.vPlanes[k].numObservations;
538 
539  for (auto it =
540  mPbMap.vPlanes[k].nearbyPlanes.begin();
541  it != mPbMap.vPlanes[k].nearbyPlanes.end();
542  it++)
543  mPbMap.vPlanes[*it].nearbyPlanes.erase(
544  mPbMap.vPlanes[k].id);
545 
546  for (auto it =
547  mPbMap.vPlanes[k].neighborPlanes.begin();
548  it != mPbMap.vPlanes[k].neighborPlanes.end();
549  it++)
550  mPbMap.vPlanes[it->first].neighborPlanes.erase(
551  mPbMap.vPlanes[k].id);
552 
553  // Update plane index
554  for (size_t h = k + 1; h < numPrevPlanes; h++)
555  --mPbMap.vPlanes[h].id;
556 
557  for (size_t h = 0; h < numPrevPlanes; h++)
558  {
559  if (k == h) continue;
560 
561  for (auto it =
562  mPbMap.vPlanes[h].nearbyPlanes.begin();
563  it != mPbMap.vPlanes[h].nearbyPlanes.end();
564  it++)
565  if (*it > mPbMap.vPlanes[k].id)
566  {
567  mPbMap.vPlanes[h].nearbyPlanes.insert(
568  *it - 1);
569  mPbMap.vPlanes[h].nearbyPlanes.erase(
570  *it);
571  }
572 
573  for (auto it = mPbMap.vPlanes[h]
574  .neighborPlanes.begin();
575  it !=
576  mPbMap.vPlanes[h].neighborPlanes.end();
577  it++)
578  if (it->first > mPbMap.vPlanes[k].id)
579  {
580  mPbMap.vPlanes[h]
581  .neighborPlanes[it->first - 1] =
582  it->second;
583  mPbMap.vPlanes[h].neighborPlanes.erase(
584  it);
585  }
586  }
587 
588  mPbMap.vPlanes.erase(itPlane);
589  --numPrevPlanes;
590 
591 #ifdef _VERBOSE
592  cout << "MERGE TWO PREVIOUS PLANES WHEREBY THE "
593  "INCORPORATION OF A NEW REGION \n";
594 #endif
595  }
596 
597  break;
598  }
599  }
600  if (!isSamePlane)
601  {
602  detectedPlanes[i].id = mPbMap.vPlanes.size();
603  detectedPlanes[i].numObservations = 1;
604  detectedPlanes[i].bFullExtent = false;
605  detectedPlanes[i].nFramesAreaIsStable = 0;
606  // detectedPlanes[i].calcMainColor(calcMainColor();
608  {
609  detectedPlanes[i].semanticGroup =
612  .push_back(detectedPlanes[i].id);
613  }
614 
615 #ifdef _VERBOSE
616  cout << "New plane " << detectedPlanes[i].id << " area "
617  << detectedPlanes[i].areaVoxels << " of polygon "
618  << detectedPlanes[i].areaHull << endl;
619 #endif
620 
621  // Update proximity graph
623  detectedPlanes[i],
624  configPbMap.proximity_neighbor_planes); // Detect neighbors
625  // with max
626  // separation of
627  // 1.0 meters
628 
629  // Update co-visibility graph
630  for (auto it = observedPlanes.begin();
631  it != observedPlanes.end(); it++)
632  {
633  detectedPlanes[i].neighborPlanes[*it] = 1;
634  mPbMap.vPlanes[*it].neighborPlanes[detectedPlanes[i].id] =
635  1;
636  }
637 
638  observedPlanes.insert(detectedPlanes[i].id);
639 
640  mPbMap.vPlanes.push_back(detectedPlanes[i]);
641  }
642  }
643  }
644 
645 #ifdef _VERBOSE
646  cout << "\n\tobservedPlanes: ";
647  cout << observedPlanes.size() << " Planes observed\n";
648  for (set<unsigned>::iterator it = observedPlanes.begin();
649  it != observedPlanes.end(); it++)
650  cout << *it << " ";
651  cout << endl;
652 #endif
653 
654  // For all observed planes
655  for (auto it = observedPlanes.begin(); it != observedPlanes.end(); it++)
656  {
657  Plane& observedPlane = mPbMap.vPlanes[*it];
658 
659  // Calculate principal direction
660  observedPlane.calcElongationAndPpalDir();
661 
662  ////cout << "Update color\n";
663  // Update color
664  observedPlane.calcMainColor();
665 
666 #ifdef _VERBOSE
667  cout << "Plane " << observedPlane.id << " color\n"
668  << observedPlane.v3colorNrgb << endl;
669 #endif
670 
671  // Infer knowledge from the planes (e.g. do these planes represent the
672  // floor, walls, etc.)
674  mpPlaneInferInfo->searchTheFloor(poseKF, observedPlane);
675  } // End for obsevedPlanes
676 
677  // Search the floor plane
678  if (mPbMap.FloorPlane !=
679  -1) // Verify that the observed planes centers are above the floor
680  {
681 #ifdef _VERBOSE
682  cout << "Verify that the observed planes centers are above the floor\n";
683 #endif
684 
685  for (auto it = observedPlanes.rbegin(); it != observedPlanes.rend();
686  it++)
687  {
688  if (static_cast<int>(*it) == mPbMap.FloorPlane) continue;
689  if (mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(
690  mPbMap.vPlanes[*it].v3center -
691  mPbMap.vPlanes[mPbMap.FloorPlane].v3center) < -0.1)
692  {
693  if (mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(
694  mPbMap.vPlanes[*it].v3normal) >
695  0.99) //(cos 8.1º = 0.99)
696  {
697  mPbMap.vPlanes[*it].label = "Floor";
698  mPbMap.vPlanes[mPbMap.FloorPlane].label = "";
699  mPbMap.FloorPlane = *it;
700  }
701  else
702  {
703  // assert(false);
704  mPbMap.vPlanes[mPbMap.FloorPlane].label = "";
705  mPbMap.FloorPlane = -1;
706  break;
707  }
708  }
709  }
710  }
711 
713  for (auto it = observedPlanes.begin(); it != observedPlanes.end(); it++)
714  {
715  // cout << "insert planes\n";
716  if (mpPbMapLocaliser->vQueueObservedPlanes.size() < 10)
717  mpPbMapLocaliser->vQueueObservedPlanes.push_back(*it);
718  }
719 
720 #ifdef _VERBOSE
721  cout << "DetectedPlanesCloud finished\n";
722 #endif
723 
724  updateLock.unlock();
725 }
726 
727 bool graphRepresentation = false;
729  const pcl::visualization::KeyboardEvent& event, void* viewer_void)
730 {
731  if ((event.getKeySym() == "r" || event.getKeySym() == "R") &&
732  event.keyDown())
733  {
735  }
736 }
737 
738 /*!Check if the the input plane is the same than this plane for some given angle
739  * and distance thresholds.
740  * If the planes are the same they are merged in this and the function returns
741  * true. Otherwise it returns false.*/
743  Plane& plane1, Plane& plane2, const float& cosAngleThreshold,
744  const float& distThreshold, const float& proxThreshold)
745 {
746  // Check that both planes have similar orientation
747  if (plane1.v3normal.dot(plane2.v3normal) < cosAngleThreshold) return false;
748 
749  // Check the normal distance of the planes centers using their average
750  // normal
751  float dist_normal = plane1.v3normal.dot(plane2.v3center - plane1.v3center);
752  // if(fabs(dist_normal) > distThreshold ) // Avoid matching different
753  // parallel planes
754  // return false;
755  float thres_max_dist =
756  max(distThreshold,
757  distThreshold * 2 * (plane2.v3center - plane1.v3center).norm());
758  if (fabs(dist_normal) >
759  thres_max_dist) // Avoid matching different parallel planes
760  return false;
761 
762  // Once we know that the planes are almost coincident (parallelism and
763  // position)
764  // we check that the distance between the planes is not too big
765  return arePlanesNearby(plane1, plane2, proxThreshold);
766 }
767 
768 void PbMapMaker::mergePlanes(Plane& updatePlane, Plane& discardPlane)
769 {
770  // Update normal and center
771  updatePlane.v3normal = updatePlane.areaVoxels * updatePlane.v3normal +
772  discardPlane.areaVoxels * discardPlane.v3normal;
773  updatePlane.v3normal = updatePlane.v3normal / (updatePlane.v3normal).norm();
774  // Update point inliers
775  // *updatePlane.polygonContourPtr += *discardPlane.polygonContourPtr; //
776  // Merge polygon points
777  *updatePlane.planePointCloudPtr +=
778  *discardPlane.planePointCloudPtr; // Add the points of the new
779  // detection and perform a voxel grid
780 
781  // Filter the points of the patch with a voxel-grid. This points are used
782  // only for visualization
783  static pcl::VoxelGrid<pcl::PointXYZRGBA> merge_grid;
784  merge_grid.setLeafSize(0.05, 0.05, 0.05);
785  pcl::PointCloud<pcl::PointXYZRGBA> mergeCloud;
786  merge_grid.setInputCloud(updatePlane.planePointCloudPtr);
787  merge_grid.filter(mergeCloud);
788  updatePlane.planePointCloudPtr->clear();
789  *updatePlane.planePointCloudPtr = mergeCloud;
790 
791  // if(configPbMap.use_color)
792  // updatePlane.calcMainColor();
793 
794  *discardPlane.polygonContourPtr += *updatePlane.polygonContourPtr;
795  updatePlane.calcConvexHull(discardPlane.polygonContourPtr);
796  updatePlane.computeMassCenterAndArea();
797 
798  // Move the points to fulfill the plane equation
799  updatePlane.forcePtsLayOnPlane();
800 
801  // Update area
802  double area_recalc = updatePlane.planePointCloudPtr->size() * 0.0025;
803  mpPlaneInferInfo->isFullExtent(updatePlane, area_recalc);
804  updatePlane.areaVoxels = updatePlane.planePointCloudPtr->size() * 0.0025;
805 }
806 
807 // Color = (red[i], grn[i], blu[i])
808 // The color order is: red, green, blue, yellow, pink, turquoise, orange,
809 // purple, dark green, beige
810 unsigned char red[10] = {255, 0, 0, 255, 255, 0, 255, 204, 0, 255};
811 unsigned char grn[10] = {0, 255, 0, 255, 0, 255, 160, 51, 128, 222};
812 unsigned char blu[10] = {0, 0, 255, 0, 255, 255, 0, 204, 0, 173};
813 
814 double ared[10] = {1.0, 0, 0, 1.0, 1.0, 0, 1.0, 0.8, 0, 1.0};
815 double agrn[10] = {0, 1.0, 0, 1.0, 0, 1.0, 0.6, 0.2, 0.5, 0.9};
816 double ablu[10] = {0, 0, 1.0, 0, 1.0, 1.0, 0, 0.8, 0, 0.7};
817 
818 void PbMapMaker::viz_cb(pcl::visualization::PCLVisualizer& viz)
819 {
820  if (mPbMap.globalMapPtr->empty())
821  {
822  std::this_thread::sleep_for(10ms);
823  return;
824  }
825 
826  {
827  std::lock_guard<std::mutex> csl(CS_visualize);
828 
829  // Render the data
830  {
831  viz.removeAllShapes();
832  viz.removeAllPointClouds();
833 
834  char name[1024];
835 
836  // if(graphRepresentation)
837  // {
838  // for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
839  // {
840  // pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0],
841  // 2*mPbMap.vPlanes[i].v3center[1],
842  // 2*mPbMap.vPlanes[i].v3center[2]);
843  // double radius = 0.1 *
844  // sqrt(mPbMap.vPlanes[i].areaVoxels);
845  // sprintf (name, "sphere%u", static_cast<unsigned>(i));
846  // viz.addSphere (center, radius, ared[i%10], agrn[i%10],
847  // ablu[i%10], name);
848  //
849  // if( !mPbMap.vPlanes[i].label.empty() )
850  // viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1,
851  // ared[i%10], agrn[i%10], ablu[i%10],
852  // mPbMap.vPlanes[i].label);
853  // else
854  // {
855  // sprintf (name, "P%u", static_cast<unsigned>(i));
856  // viz.addText3D (name, center, 0.1, ared[i%10],
857  // agrn[i%10], ablu[i%10], name);
858  // }
859  //
860  // // Draw edges
861  // if(!configPbMap.graph_mode) // Nearby neighbors
862  // for(set<unsigned>::iterator it =
863  // mPbMap.vPlanes[i].nearbyPlanes.begin(); it !=
864  // mPbMap.vPlanes[i].nearbyPlanes.end(); it++)
865  // {
866  // if(*it > mPbMap.vPlanes[i].id)
867  // break;
868  //
869  // sprintf (name, "commonObs%u_%u",
870  // static_cast<unsigned>(i),
871  // static_cast<unsigned>(*it));
872  // pcl::PointXYZ
873  // center_it(2*mPbMap.vPlanes[*it].v3center[0],
874  // 2*mPbMap.vPlanes[*it].v3center[1],
875  // 2*mPbMap.vPlanes[*it].v3center[2]);
876  // viz.addLine (center, center_it, ared[i%10],
877  // agrn[i%10], ablu[i%10], name);
878  // }
879  // else
880  // for(map<unsigned,unsigned>::iterator it =
881  // mPbMap.vPlanes[i].neighborPlanes.begin(); it !=
882  // mPbMap.vPlanes[i].neighborPlanes.end(); it++)
883  // {
884  // if(it->first > mPbMap.vPlanes[i].id)
885  // break;
886  //
887  // sprintf (name, "commonObs%u_%u",
888  // static_cast<unsigned>(i),
889  // static_cast<unsigned>(it->first));
890  // pcl::PointXYZ
891  // center_it(2*mPbMap.vPlanes[it->first].v3center[0],
892  // 2*mPbMap.vPlanes[it->first].v3center[1],
893  // 2*mPbMap.vPlanes[it->first].v3center[2]);
894  // viz.addLine (center, center_it, ared[i%10],
895  // agrn[i%10], ablu[i%10], name);
896  //
897  // sprintf (name, "edge%u_%u",
898  // static_cast<unsigned>(i),
899  // static_cast<unsigned>(it->first));
900  // char commonObs[8];
901  // sprintf (commonObs, "%u", it->second);
902  // pcl::PointXYZ half_edge( (center_it.x+center.x)/2,
903  // (center_it.y+center.y)/2, (center_it.z+center.z)/2
904  // );
905  // viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0,
906  // 1.0, name);
907  // }
908  //
909  // }
910  // }
911  // else
912  { // Regular representation
913 
915  {
916  if (!viz.updatePointCloud(mPbMap.globalMapPtr, "cloud"))
917  viz.addPointCloud(mPbMap.globalMapPtr, "cloud");
918  return;
919  }
920 
921  if (mpPbMapLocaliser != nullptr)
923  {
924  if (!viz.updatePointCloud(
926  viz.addPointCloud(
928  }
929 
930  sprintf(
931  name, "PointCloud size %u",
932  static_cast<unsigned>(mPbMap.globalMapPtr->size()));
933  viz.addText(name, 10, 20);
934 
935  // pcl::ModelCoefficients plane_coefs;
936  // plane_coefs.values[0] = mPbMap.vPlanes[0].v3normal[0];
937  // plane_coefs.values[1] = mPbMap.vPlanes[0].v3normal[1];
938  // plane_coefs.values[2] = mPbMap.vPlanes[0].v3normal[2];
939  // plane_coefs.values[3] = -(mPbMap.vPlanes[0].v3normal .dot
940  // (mPbMap.vPlanes[0].v3center) );
941  // viz.addPlane (plane_coefs);
942 
943  for (size_t i = 0; i < mPbMap.vPlanes.size(); i++)
944  {
945  Plane& plane_i = mPbMap.vPlanes[i];
946  sprintf(name, "normal_%u", static_cast<unsigned>(i));
947  pcl::PointXYZ pt1, pt2; // Begin and end points of normal's
948  // arrow for visualization
949  pt1 = pcl::PointXYZ(
950  plane_i.v3center[0], plane_i.v3center[1],
951  plane_i.v3center[2]);
952  pt2 = pcl::PointXYZ(
953  plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
954  plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
955  plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
956  viz.addArrow(
957  pt2, pt1, ared[i % 10], agrn[i % 10], ablu[i % 10],
958  false, name);
959 
960  // Draw Ppal diretion
961  // if( plane_i.elongation > 1.3 )
962  // {
963  // sprintf (name, "ppalComp_%u",
964  // static_cast<unsigned>(i));
965  // pcl::PointXYZ pt3 = pcl::PointXYZ (
966  // plane_i.v3center[0] + (0.2f *
967  // plane_i.v3PpalDir[0]),
968  // plane_i.v3center[1]
969  // + (0.2f *
970  // plane_i.v3PpalDir[1]),
971  // plane_i.v3center[2]
972  // + (0.2f *
973  // plane_i.v3PpalDir[2]));
974  // viz.addArrow (pt3, plane_i.pt1, ared[i%10],
975  // agrn[i%10], ablu[i%10], false, name);
976  // }
977 
978  // if( !plane_i.label.empty() )
979  // viz.addText3D (plane_i.label, pt2, 0.1,
980  // ared[i%10], agrn[i%10], ablu[i%10],
981  // plane_i.label);
982  // else
983  {
984  sprintf(name, "n%u", static_cast<unsigned>(i));
985  // sprintf (name, "n%u_%u",
986  // static_cast<unsigned>(i),
987  // static_cast<unsigned>(plane_i.semanticGroup));
988  viz.addText3D(
989  name, pt2, 0.1, ared[i % 10], agrn[i % 10],
990  ablu[i % 10], name);
991  }
992 
993  // sprintf (name, "planeRaw_%02u",
994  // static_cast<unsigned>(i));
995  // viz.addPointCloud
996  // (plane_i.planeRawPointCloudPtr, name);//
997  // contourPtr, planePointCloudPtr,
998  // polygonContourPtr
999 
1000  // if(!configPbMap.makeClusters)
1001  // {
1002  sprintf(name, "plane_%02u", static_cast<unsigned>(i));
1003  // if(plane_i.bDominantColor)
1004  {
1005  // pcl::visualization::PointCloudColorHandlerCustom
1006  // <PointT> color (plane_i.planePointCloudPtr,
1007  // red[i%10], grn[i%10], blu[i%10]);
1008  ////
1009  /// pcl::visualization::PointCloudColorHandlerCustom
1010  ///< PointT> color (plane_i.planePointCloudPtr,
1011  /// red[plane_i.semanticGroup%10],
1012  /// grn[plane_i.semanticGroup%10],
1013  /// blu[plane_i.semanticGroup%10]);
1014  // viz.addPointCloud
1015  // (plane_i.planePointCloudPtr, color, name);
1016  // viz.setPointCloudRenderingProperties
1017  // (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1018  // 2, name);
1019  // }
1020  // else
1021  // {
1022  // sprintf (name, "plane_%02u",
1023  // static_cast<unsigned>(i));
1024  // pcl::visualization::PointCloudColorHandlerCustom
1025  // <PointT> color
1026  // (plane_i.planePointCloudPtr,
1027  // red[plane_i.semanticGroup%10],
1028  // grn[plane_i.semanticGroup%10],
1029  // blu[plane_i.semanticGroup%10]);
1030 
1031  double illum = 0;
1032  if (fabs(plane_i.v3colorNrgb[0] - 0.33) < 0.03 &&
1033  fabs(plane_i.v3colorNrgb[1] - 0.33) < 0.03 &&
1034  fabs(plane_i.v3colorNrgb[2] - 0.33) < 0.03 &&
1035  plane_i.dominantIntensity > 400)
1036  illum = 0.5;
1037 
1038  pcl::visualization::PointCloudColorHandlerCustom<PointT>
1039  color(
1040  plane_i.planePointCloudPtr,
1041  plane_i.v3colorNrgb[0] *
1042  (plane_i.dominantIntensity +
1043  (755 - plane_i.dominantIntensity) * illum),
1044  plane_i.v3colorNrgb[1] *
1045  (plane_i.dominantIntensity +
1046  (755 - plane_i.dominantIntensity) * illum),
1047  plane_i.v3colorNrgb[2] *
1048  (plane_i.dominantIntensity +
1049  (755 - plane_i.dominantIntensity) *
1050  illum));
1051  // pcl::visualization::PointCloudColorHandlerCustom
1052  // <PointT> color
1053  // (plane_i.planePointCloudPtr,
1054  // plane_i.v3colorNrgb[0] * plane_i.dominantIntensity,
1055  // plane_i.v3colorNrgb[1] * plane_i.dominantIntensity,
1056  // plane_i.v3colorNrgb[2] * plane_i.dominantIntensity);
1057  viz.addPointCloud(
1058  plane_i.planePointCloudPtr, color, name);
1059  viz.setPointCloudRenderingProperties(
1060  pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10,
1061  name);
1062  // }
1063  }
1064  // else
1065  // viz.addPointCloud (plane_i.planePointCloudPtr,
1066  // name);// contourPtr, planePointCloudPtr,
1067  // polygonContourPtr
1068 
1069  // sprintf (name, "planeBorder_%02u",
1070  // static_cast<unsigned>(i));
1071  // pcl::visualization::PointCloudColorHandlerCustom
1072  // <PointT> color2 (plane_i.contourPtr, 255, 255,
1073  // 255);
1074  // viz.addPointCloud (plane_i.contourPtr, color2,
1075  // name);// contourPtr, planePointCloudPtr,
1076  // polygonContourPtr
1077 
1078  // //Edges
1079  // if(mPbMap.edgeCloudPtr->size() > 0)
1080  // {
1081  // sprintf (name, "planeEdge_%02u",
1082  // static_cast<unsigned>(i));
1083  // pcl::visualization::PointCloudColorHandlerCustom
1084  // <PointT> color4 (mPbMap.edgeCloudPtr, 255,
1085  // 255, 0);
1086  // viz.addPointCloud (mPbMap.edgeCloudPtr,
1087  // color4, name);// contourPtr,
1088  // planePointCloudPtr, polygonContourPtr
1089  // viz.setPointCloudRenderingProperties
1090  // (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1091  // 5, name);
1092  //
1093  // sprintf (name, "edge%u",
1094  // static_cast<unsigned>(i));
1095  // viz.addLine
1096  // (mPbMap.edgeCloudPtr->points.front(),
1097  // mPbMap.edgeCloudPtr->points.back(), ared[3],
1098  // agrn[3], ablu[3], name);
1099  // }
1100 
1101  sprintf(name, "approx_plane_%02d", int(i));
1102  viz.addPolygon<PointT>(
1103  plane_i.polygonContourPtr, 0.5 * red[i % 10],
1104  0.5 * grn[i % 10], 0.5 * blu[i % 10], name);
1105  }
1106 
1107  // if(configPbMap.makeClusters)
1108  // for(map<unsigned, std::vector<unsigned> >::iterator
1109  // it=clusterize->groups.begin(); it !=
1110  // clusterize->groups.end(); it++)
1111  // for(size_t i=0; i < it->second.size(); i++)
1112  // {
1113  // unsigned planeID = it->second[i];
1114  // Plane &plane_i = mPbMap.vPlanes[planeID];
1115  // sprintf (name, "plane_%02u",
1116  // static_cast<unsigned>(planeID));
1117  // pcl::visualization::PointCloudColorHandlerCustom
1118  // <PointT> color (plane_i.planePointCloudPtr,
1119  // red[planeID%10], grn[planeID%10], blu[planeID%10]);
1120  // viz.addPointCloud (plane_i.planePointCloudPtr, color,
1121  // name);// contourPtr, planePointCloudPtr,
1122  // polygonContourPtr
1123  // viz.setPointCloudRenderingProperties
1124  // (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3,
1125  // name);
1126  // }
1127 
1128  // Draw recognized plane labels
1129  if (mpPbMapLocaliser != nullptr)
1130  for (auto it = mpPbMapLocaliser->foundPlaces.begin();
1131  it != mpPbMapLocaliser->foundPlaces.end(); it++)
1132  viz.addText3D(
1133  it->first, it->second, 0.3, 0.9, 0.9, 0.9,
1134  it->first);
1135  }
1136  }
1137  }
1138 }
1139 
1141 {
1142  cloudViewer.runOnVisualizationThread(
1143  boost::bind(&PbMapMaker::viz_cb, this, _1), "viz_cb");
1144  cloudViewer.registerKeyboardCallback(keyboardEventOccurred);
1145 
1146  size_t numPrevKFs = 0;
1147  size_t minGrowPlanes = 5;
1148  while (!m_pbmaker_must_stop) // Stop loop if PbMapMaker
1149  {
1150  if (numPrevKFs == frameQueue.size())
1151  {
1152  std::this_thread::sleep_for(10ms);
1153  }
1154  else
1155  {
1156  // Assign pointCloud of last KF to the global map
1158  frameQueue.back().cloudPtr, frameQueue.back().pose,
1161 
1163  {
1164  if (mPbMap.vPlanes.size() > minGrowPlanes)
1165  {
1166  // Evaluate the partition of the current groups with minNcut
1167  int size_partition =
1169  cout << "PARTITION SIZE " << size_partition << endl;
1170  assert(size_partition < 2);
1171 
1172  minGrowPlanes += 2;
1173  }
1174  }
1175 
1176  ++numPrevKFs;
1177  }
1178  }
1179 
1180  // saveInfoFiles(); // save watch statistics
1181 
1182  m_pbmaker_finished = true;
1183 }
1184 
1186 {
1187  boost::mutex::scoped_lock updateLock(mtx_pbmap_busy);
1188 
1189  mPbMap.savePbMap(path);
1190 
1191  updateLock.unlock();
1192 }
1193 
1195 {
1196  m_pbmaker_must_stop = true;
1197  while (!m_pbmaker_finished) std::this_thread::sleep_for(1ms);
1198  cout << "Waiting for PbMapMaker thread to die.." << endl;
1199 
1200  pbmaker_hd.join();
1201 
1202  return true;
1203 }
1204 
1206 {
1207  delete mpPlaneInferInfo;
1208  delete mpPbMapLocaliser;
1209 
1210  stop_pbMapMaker();
1211 
1212  cout << " .. PbMapMaker has died." << endl;
1213 }
1214 
1215 #endif
Scalar * iterator
Definition: eigen_plugins.h:26
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:43
PlaneInferredInfo * mpPlaneInferInfo
Definition: PbMapMaker.h:140
string config_localiser
Definition: PbMapMaker.cpp:106
double ared[10]
Definition: PbMapMaker.cpp:814
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:810
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:179
std::set< unsigned > observedPlanes
Definition: PbMapMaker.h:137
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3586
A class used to store a planar feature (Plane for short).
Definition: Plane.h:43
float dist_threshold
Definition: PbMapMaker.cpp:73
PbMapLocaliser * mpPbMapLocaliser
Definition: PbMapMaker.h:125
bool areSamePlane(Plane &plane1, Plane &plane2, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
Definition: PbMapMaker.cpp:742
float dominantIntensity
Definition: Plane.h:157
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:728
boost::mutex mtx_pbmap_busy
Definition: PbMapMaker.h:130
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:818
float max_cos_normal
Definition: PbMapMaker.cpp:87
Eigen::Vector3f v3colorNrgb
! Radiometric description
Definition: Plane.h:156
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:8459
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:768
double agrn[10]
Definition: PbMapMaker.cpp:815
float areaVoxels
Definition: Plane.h:147
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:139
std::string path_save_pbmap
Definition: PbMapMaker.cpp:109
std::set< unsigned > nearbyPlanes
Definition: Plane.h:127
unsigned char blu[10]
Definition: PbMapMaker.cpp:812
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:183
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:148
void forcePtsLayOnPlane()
Definition: Plane.cpp:220
SemanticClustering * clusterize
Definition: PbMapMaker.h:128
for(int w=2;w< 12;w++)
unsigned char grn[10]
Definition: PbMapMaker.cpp:811
float proximity_threshold
Definition: PbMapMaker.cpp:91
GLsizei const GLchar ** string
Definition: glext.h:4116
std::thread pbmaker_hd
Definition: PbMapMaker.h:149
std::vector< unsigned > vQueueObservedPlanes
pcl::PointCloud< PointT >::Ptr globalMapPtr
Definition: pbmap/PbMap.h:64
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:290
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:124
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:4068
float hue_threshold
Definition: PbMapMaker.cpp:70
std::vector< frameRGBDandPose > frameQueue
Definition: PbMapMaker.h:71
std::string rawlog_path
Definition: PbMapMaker.cpp:66
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:33
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:138
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:727
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:54
float color_threshold
Definition: PbMapMaker.cpp:68
float max_dist_center_plane
Definition: PbMapMaker.cpp:88
double ablu[10]
Definition: PbMapMaker.cpp:816
#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:191
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:77
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:37
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:54
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: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019