ExtractPolygonalPrismData and ConcaveHull bugfix (PointCloudLibrary#5168

) * ExtractPolygonalPrismData and ConcaveHull bugfix PointCloudLibrary#5163 (comment) * ExtractPolygonalPrismData and ConcaveHull bugfix PointCloudLibrary#5163 (comment) * test by generating the concave hull manually * sin and cos for floats * azure test compilation errors * minor changes per code review * minor changes per review * Update CMakeLists.txt * Update test_concave_prism.cpp * Update extract_polygonal_prism_data.hpp * Rename * Rename * Rename * Update extract_polygonal_prism_data.hpp --------- Co-authored-by: Yoav Miller <yoavm@magnusmetal.com> Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com>
ctbinzi · Jun 3, 2024 · f63fcf4 · f63fcf4
1 parent 8f2f865
commit f63fcf4
Show file tree

Hide file tree

Showing 4 changed files with 131 additions and 1 deletion.
diff --git a/segmentation/include/pcl/segmentation/extract_polygonal_prism_data.h b/segmentation/include/pcl/segmentation/extract_polygonal_prism_data.h
@@ -40,6 +40,7 @@
 #include <limits>
 
 #include <pcl/pcl_base.h>
+#include <pcl/Vertices.h> // for Vertices
 
 namespace pcl
 {
@@ -124,6 +125,16 @@ namespace pcl
       inline void 
       setInputPlanarHull (const PointCloudConstPtr &hull) { planar_hull_ = hull; }
 
+       /** \brief Provide a vector of the concave polygons indices, as recieved from ConcaveHull::polygons.
+       * \note This is only needed when using ConcaveHull that has more than one polygon.
+       * \param[in] polygons - see ConcaveHull::polygons
+       */
+      inline void
+      setPolygons(const std::vector<pcl::Vertices>& polygons)
+      {
+        polygons_ = polygons;
+      }
+
       /** \brief Get a pointer the input planar hull dataset. */
       inline PointCloudConstPtr 
       getInputPlanarHull () const { return (planar_hull_); }
@@ -185,6 +196,9 @@ namespace pcl
       /** \brief A pointer to the input planar hull dataset. */
       PointCloudConstPtr planar_hull_{nullptr};
 
+      /** \brief polygons indices vectors, as recieved from ConcaveHull */
+      std::vector<pcl::Vertices> polygons_;
+
       /** \brief The minimum number of points needed on the convex hull. */
       int min_pts_hull_{3};
 

diff --git a/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp b/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp
@@ -227,6 +227,20 @@ pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
   PointT pt_xy;
   pt_xy.z = 0;
 
+  std::vector<pcl::PointCloud<PointT>> polygons(polygons_.size());
+  if (polygons_.empty()) {
+    polygons.push_back(polygon);
+  }
+  else { // incase of concave hull, prepare separate polygons
+    for (size_t i = 0; i < polygons_.size(); i++) {
+      const auto& polygon_i = polygons_[i];
+      polygons[i].reserve(polygon_i.vertices.size());
+      for (const auto& pointIdx : polygon_i.vertices) {
+        polygons[i].points.push_back(polygon[pointIdx]);
+      }
+    }
+  }
+
   output.indices.resize (indices_->size ());
   int l = 0;
   for (std::size_t i = 0; i < projected_points.size (); ++i)
@@ -243,8 +257,14 @@ pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
     pt_xy.x = pt[k1];
     pt_xy.y = pt[k2];
 
-    if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
+    bool in_poly = false;
+    for (const auto& poly : polygons) {
+      in_poly ^= pcl::isXYPointIn2DXYPolygon(pt_xy, poly);
+    }
+
+    if (!in_poly) {
       continue;
+    }
 
     output.indices[l++] = (*indices_)[i];
   }

diff --git a/test/segmentation/CMakeLists.txt b/test/segmentation/CMakeLists.txt
@@ -20,6 +20,10 @@ PCL_ADD_TEST(a_segmentation_test test_segmentation
              LINK_WITH pcl_gtest pcl_io pcl_segmentation pcl_features pcl_kdtree pcl_search pcl_common
              ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/car6.pcd" "${PCL_SOURCE_DIR}/test/colored_cloud.pcd")
 
+PCL_ADD_TEST(a_prism_test test_concave_prism
+             FILES test_concave_prism.cpp
+             LINK_WITH pcl_gtest pcl_segmentation pcl_common)
+
 PCL_ADD_TEST(test_non_linear test_non_linear
              FILES test_non_linear.cpp
              LINK_WITH pcl_gtest pcl_common pcl_io pcl_sample_consensus pcl_segmentation pcl_kdtree pcl_search

diff --git a/test/segmentation/test_concave_prism.cpp b/test/segmentation/test_concave_prism.cpp
@@ -0,0 +1,92 @@
+#include <pcl/test/gtest.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+
+#include <random>
+
+using namespace pcl;
+using std::vector;
+
+TEST(ExtractPolygonalPrism, two_rings)
+{
+  float rMin = 0.1, rMax = 0.25f;
+  float dx = 0.5f; // shift the rings from [0,0,0] to [+/-dx, 0, 0]
+
+  // prepare 2 rings
+  PointCloud<PointXYZ>::Ptr ring(new PointCloud<PointXYZ>);
+  { // use random
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    std::uniform_real_distribution<float> radiusDist(rMin, rMax);
+    std::uniform_real_distribution<float> radianDist(-M_PI, M_PI);
+    std::uniform_real_distribution<float> zDist(-0.01f, 0.01f);
+    for (size_t i = 0; i < 1000; i++) {
+      float radius = radiusDist(gen);
+      float angle = radianDist(gen);
+      float z = zDist(gen);
+      PointXYZ point(cosf(angle) * radius, sinf(angle) * radius, z);
+      ring->push_back(point);
+    }
+  }
+
+  PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+  cloud->reserve(ring->size() * 2);
+  for (auto& point : ring->points) {
+    auto left = point;
+    auto right = point;
+    left.x -= dx;
+    right.x += dx;
+    cloud->push_back(left);
+    cloud->push_back(right);
+  }
+
+  // create hull
+  PointCloud<PointXYZ>::Ptr hullCloud(new PointCloud<PointXYZ>);
+  vector<Vertices> rings(4);
+  float radiuses[] = {rMin - 0.01f, rMax + 0.01f, rMin - 0.01f, rMax + 0.01f};
+  float centers[] = {-dx, -dx, +dx, +dx};
+  for (size_t i = 0; i < rings.size(); i++) {
+    auto r = radiuses[i];
+    auto xCenter = centers[i];
+    for (float a = -M_PI; a < M_PI; a += 0.05f) {
+      rings[i].vertices.push_back(hullCloud->size());
+      PointXYZ point(xCenter + r * cosf(a), r * sinf(a), 0);
+      hullCloud->push_back(point);
+    }
+  }
+
+  // add more points before using prism
+  size_t ringsPointCount = cloud->size();
+  cloud->push_back(PointXYZ(0, 0, 0));
+  for (float a = -M_PI; a < M_PI; a += 0.05f) {
+    float r = 4 * rMax;
+    PointXYZ point(r * cosf(a), r * sinf(a), 0);
+    cloud->push_back(point);
+  }
+
+  // do prism
+  PointIndices::Ptr inliers(new PointIndices);
+  ExtractPolygonalPrismData<PointXYZ> ex;
+  {
+    ex.setInputCloud(cloud);
+    ex.setInputPlanarHull(hullCloud);
+    ex.setHeightLimits(-1, 1);
+    ex.setPolygons(rings);
+    ex.segment(*inliers);
+  }
+
+  // check that all of the rings are in the prism.
+  EXPECT_EQ(inliers->indices.size(), ringsPointCount);
+  for(std::size_t i=0; i<inliers->indices.size(); ++i) {
+    EXPECT_EQ(inliers->indices[i], i);
+  }
+}
+
+int
+main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return (RUN_ALL_TESTS());
+}