PointCloudLibrary / pcl

Point Cloud Library (PCL)
https://pointclouds.org/
Other
9.65k stars 4.6k forks source link

[request] PCL Concave hull & Convex hull #6004

Closed nidhinsoni closed 2 months ago

nidhinsoni commented 2 months ago

Hello, I'm trying to create a boundary line using concave hull using the PCL library. I pre-downsampled, pre-segmented, pre-filtered the pcd file. From 2.3m data points, i came down to around 80 data points. Getting an error saying input cloud has no data. i'm new to Point clouds, so i was wondering whether it's the parameters or the limitation of the algorithm itself. I have uploaded the files here.

include

include <pcl/io/pcd_io.h>

include <pcl/point_types.h>

include <pcl/filters/passthrough.h>

include <pcl/segmentation/sac_segmentation.h>

include <pcl/segmentation/extract_clusters.h>

include <pcl/filters/voxel_grid.h>

include <pcl/filters/extract_indices.h>

include <pcl/features/normal_3d.h>

include <pcl/features/moment_of_inertia_estimation.h>

include <pcl/visualization/pcl_visualizer.h>

include <pcl/common/common.h>

include <pcl/keypoints/agast_2d.h>

include <pcl/filters/statistical_outlier_removal.h>

include <pcl/filters/passthrough.h>

include <pcl/surface/concave_hull.h>

int main() { // Load point cloud data pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::PCDReader reader; reader.read("holz1.pcd", *cloud);

// Apply any necessary preprocessing (optional)
// Downsample the cloud using a Voxel Grid class
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(30.0f, 30.0f, 30.0f); // Set the voxel grid leaf size
vg.filter(*cloud_filtered);
std::cerr << "Filtered cloud contains " << cloud_filtered->size() << " data points" << std::endl;

// Segmentation
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(12);
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);

// Object extraction
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(true); // Extract everything except the plane = TRUE
pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZ>);
extract.filter(*object_cloud);

// Statistical Outlier Removal
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(object_cloud);
sor.setMeanK(100); // Number of neighbors to analyze for each point ORIGINAL=50
sor.setStddevMulThresh(0.05); // Standard deviation multiplier threshold ORIGINAL=1.0
pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
sor.filter(*object_cloud_filtered);

// PASSTHROUGH FILTER
// Print min & max coordinate values for user
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(*object_cloud_filtered, min_pt, max_pt);
std::cout << "Min Z: " << min_pt.z << ", Max Z: " << max_pt.z << std::endl;
std::cout << "Min Y: " << min_pt.y << ", Max Y: " << max_pt.y << std::endl;
std::cout << "Min X: " << min_pt.x << ", Max X: " << max_pt.x << std::endl;

// Pass Through filter to remove points outside a specified range
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(object_cloud_filtered);
pass.setFilterFieldName("z"); // Filter along the Z axis
pass.setFilterLimits(1335.34, 1350.0); // Value is in mm. Please Note: Any changes to Z Value should be done on the Z max.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_PF(new pcl::PointCloud<pcl::PointXYZ>);
pass.filter(*cloud_filtered_PF);
std::cerr << "Filtered cloud contains " << cloud_filtered_PF->size() << " data points" << std::endl;

// Convex Hull
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud(cloud_filtered_PF);
chull.setAlpha(1);
chull.reconstruct(*cloud_hull);

// Save extracted object
pcl::PCDWriter writer;
writer.write("holz_PF.pcd", *cloud_filtered_PF, false);
std::cerr << "Object saved" << std::endl;

// Save convex hull object
writer.write("holz_PF_CH.pcd", *cloud_hull, false);
std::cerr << "Object with Convex hull saved" << std::endl;

return 0;

}

holz.zip

mvieth commented 2 months ago

@nidhinsoni I would say it is probably the parameter choice. Have you tested different values for alpha?

larshg commented 2 months ago

Hey You could alternatively try with: 1) Filter height (z=1350) (also X and Y if you want to remove the last noisy points) 2) Voxelfilter (I didn't do it, but could be done for faster processing / smoothing) 3) Project points to plane of objects - if you know that they are supposed to be flat. Some edges, seems to go up and downwards? If you have multiple layers, this option needs to be run after some clustering etc. 4) Normal estimation (k=50) (already here you can filter points by curvature) 5)Run Boundary_estimation (k=50) - it gave the following:

image

And from the side:

image

nidhinsoni commented 2 months ago

@larshg hey thanks a lot for your suggestion. Do you mind sharing the code here? I didn't quite get the third point you mentioned

larshg commented 2 months ago

I quickly just used the pcl_tools, so I don't have any code :smile:

larshg commented 2 months ago

A small tutorial for projecting points to a given plane can be found here https://pcl.readthedocs.io/projects/tutorials/en/latest/project_inliers.html

nidhinsoni commented 2 months ago

@nidhinsoni I would say it is probably the parameter choice. Have you tested different values for alpha?

Thanks this was it!