void
compute (ConstCloudPtr &input, Cloud &output, int max_window_size, float slope, float max_distance, float initial_distance, float cell_size, float base, bool exponential)
{
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
std::vector<int> ground;
ProgressiveMorphologicalFilter<PointType> pmf;
pmf.setInputCloud (input);
pmf.setMaxWindowSize (max_window_size);
pmf.setSlope (slope);
pmf.setMaxDistance (max_distance);
pmf.setInitialDistance (initial_distance);
pmf.setCellSize (cell_size);
pmf.setBase (base);
pmf.setExponential (exponential);
pmf.extract (ground);
PointIndicesPtr idx (new PointIndices);
idx->indices = ground;
ExtractIndices<PointType> extract;
extract.setInputCloud (input);
extract.setIndices (idx);
extract.setNegative (false);
extract.filter (output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
/**
Toma nube de puntos con gripper y objeto tomado, y los separa, entregando dos nubes diferentes.
La forma de separarlos es considerando al gripper como un paralelepípedo y luego:
- Obtener todos los puntos dentro del paralelepípedo
- Acumular sus índices
- Retornar puntos dentro y fuera.
@param cloud_in: Nube con scaneo
@param gripper_out: Referencia de nube donde retornar gripper aislado
@param object_out: Referencia de nube donde retornar objeto aislado
@return: Nada.
*/
void isolateObject(PointCloud<PointXYZ>::Ptr cloud_in, PointCloud<PointXYZ>::Ptr &gripper_out, PointCloud<PointXYZ>::Ptr &object_out){
// Capturar índices de puntos dentro de paralelepípedo
PointIndices::Ptr inliers (new PointIndices());
for (int i=0; i < cloud_in->points.size(); i++){
for (int j=0; j < gripper_boxes.size(); j++){
if (isPointInsideBox(cloud_in->points[i], gripper_boxes[j])){
inliers->indices.push_back(i);
break;
}
}
}
// Extraer índices de cada figura
ExtractIndices<PointXYZ> extractor;
extractor.setInputCloud(cloud_in);
extractor.setIndices(inliers);
extractor.setNegative(false);
extractor.filter(*gripper_out);
extractor.setNegative(true);
extractor.filter(*object_out);
}
PointCloud<PointXYZ>::Ptr PCLTools::segmentPlanar(PointCloud<PointXYZ>::Ptr cloud, bool negative) {
// Create the segmentation object for the planar model and set all the parameters
PointCloud<PointXYZ>::Ptr cloud_f(new PointCloud<PointXYZ>);
SACSegmentation<PointXYZ> seg;
PointIndices::Ptr inliers(new PointIndices);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ> ());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size () == 0) {
cerr << "Could not estimate a planar model for the given dataset." << endl;
return cloud_f;
}
// Extract the planar inliers from the input cloud
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative(negative);
extract.filter(*cloud_f);
return cloud_f;
}
// Subsample cloud for faster matching and processing, while filling in normals.
void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const
{
PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced;
PointCloud<Normal> normals;
PointCloud<PointXYZRGBNormal> cloud_normals;
std::vector<int> indices;
// Filter out nans.
removeNaNFromPointCloud(input, cloud_nan_filtered, indices);
indices.clear();
// Filter out everything outside a [200x200x200] box.
Eigen::Vector4f min_pt(-100, -100, -100, -100);
Eigen::Vector4f max_pt(100, 100, 100, 100);
getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices);
ExtractIndices<PointXYZRGB> boxfilter;
boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered));
boxfilter.setIndices (boost::make_shared<vector<int> > (indices));
boxfilter.filter(cloud_box_filtered);
// Reduce pointcloud
VoxelGrid<PointXYZRGB> voxelfilter;
voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered));
voxelfilter.setLeafSize (0.05, 0.05, 0.05);
// voxelfilter.setLeafSize (0.1, 0.1, 0.1);
voxelfilter.filter (cloud_voxel_reduced);
// Compute normals
NormalEstimation<PointXYZRGB, Normal> normalest;
normalest.setViewPoint(0, 0, 0);
normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ());
//normalest.setKSearch (10);
normalest.setRadiusSearch (0.25);
// normalest.setRadiusSearch (0.4);
normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced));
normalest.compute(normals);
pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals);
// Filter based on curvature
PassThrough<PointXYZRGBNormal> normalfilter;
normalfilter.setFilterFieldName("curvature");
// normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals));
normalfilter.filter(output);
}
void getCylClusters (boost::shared_ptr<PointCloud<T> > sceneCloud, vector<boost::shared_ptr<PointCloud<T> > > &outVector) {
typedef typename pcl::search::KdTree<T>::Ptr my_KdTreePtr;
typedef typename pcl::PointCloud<T>::Ptr my_PointCloudPtr;
NormalEstimation<T, Normal> ne;
SACSegmentationFromNormals<T, Normal> seg;
my_KdTreePtr tree (new pcl::search::KdTree<T> ());
PointCloud<Normal>::Ptr cloud_normals (new PointCloud<pcl::Normal>);
ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
PointIndices::Ptr inliers_cylinder (new pcl::PointIndices);
ExtractIndices<T> extract;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (sceneCloud);
ne.setKSearch (50);
ne.compute (*cloud_normals);
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_CYLINDER);
seg.setMethodType (SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.05);
seg.setRadiusLimits (0, 0.1);
seg.setInputCloud (sceneCloud);
seg.setInputNormals (cloud_normals);
// Obtain the cylinder inliers and coefficients
seg.segment (*inliers_cylinder, *coefficients_cylinder);
cout << " Number of inliers vs total number of points " << inliers_cylinder->indices.size() << " vs " << sceneCloud->size() << endl;
// Write the cylinder inliers to disk
extract.setInputCloud (sceneCloud);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
my_PointCloudPtr cloud_cylinder (new PointCloud<T> ());
extract.filter (*cloud_cylinder);
outVector.push_back(cloud_cylinder);
}
void
segment (const PointT &picked_point,
int picked_idx,
PlanarRegion<PointT> ®ion,
typename PointCloud<PointT>::Ptr &object)
{
object.reset ();
// Segment out all planes
vector<ModelCoefficients> model_coefficients;
vector<PointIndices> inlier_indices, boundary_indices;
vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
// Prefer a faster method if the cloud is organized, over RANSAC
if (cloud_->isOrganized ())
{
print_highlight (stderr, "Estimating normals ");
TicToc tt; tt.tic ();
// Estimate normals
PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
estimateNormals (cloud_, *normal_cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");
OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
mps.setMinInliers (1000);
mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
mps.setDistanceThreshold (0.03); // 2 cm
mps.setMaximumCurvature (0.001); // a small curvature
mps.setProjectPoints (true);
mps.setComparator (plane_comparator_);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
// Use one of the overloaded segmentAndRefine calls to get all the information that we want out
PointCloud<Label>::Ptr labels (new PointCloud<Label>);
vector<PointIndices> label_indices;
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
else
{
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.005);
// Copy XYZ and Normals to a new cloud
typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);
ModelCoefficients coefficients;
ExtractIndices<PointT> extract;
PointIndices::Ptr inliers (new PointIndices ());
// Up until 30% of the original cloud is left
int i = 1;
while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
{
seg.setInputCloud (cloud_segmented);
print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
TicToc tt; tt.tic ();
seg.segment (*inliers, coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
// No datasets could be found anymore
if (inliers->indices.empty ())
break;
// Save this plane
PlanarRegion<PointT> region;
region.setCoefficients (coefficients);
regions.push_back (region);
inlier_indices.push_back (*inliers);
model_coefficients.push_back (coefficients);
// Extract the outliers
extract.setInputCloud (cloud_segmented);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_remaining);
cloud_segmented.swap (cloud_remaining);
}
}
print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());
double max_dist = numeric_limits<double>::max ();
// Compute the distances from all the planar regions to the picked point, and select the closest region
int idx = -1;
for (size_t i = 0; i < regions.size (); ++i)
{
double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
if (dist < max_dist)
{
max_dist = dist;
idx = static_cast<int> (i);
}
}
// Get the plane that holds the object of interest
if (idx != -1)
{
plane_indices_.reset (new PointIndices (inlier_indices[idx]));
if (cloud_->isOrganized ())
{
approximatePolygon (regions[idx], region, 0.01f, false, true);
print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
}
else
{
// Save the current region
region = regions[idx];
print_highlight (stderr, "Obtaining the boundary points for the region ");
TicToc tt; tt.tic ();
// Project the inliers to obtain a better hull
typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
ProjectInliers<PointT> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_);
proj.setIndices (plane_indices_);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Compute the boundary points as a ConvexHull
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud_projected);
PointCloud<PointT> plane_hull;
chull.reconstruct (plane_hull);
region.setContour (plane_hull);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
}
}
// Segment the object of interest
if (plane_indices_ && !plane_indices_->indices.empty ())
{
plane_.reset (new PointCloud<PointT>);
copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
object.reset (new PointCloud<PointT>);
segmentObject (picked_idx, cloud_, plane_indices_, *object);
}
}
/** \brief Given a plane, and the set of inlier indices representing it,
* segment out the object of intererest supported by it.
*
* \param[in] picked_idx the index of a point on the object
* \param[in] cloud the full point cloud dataset
* \param[in] plane_indices a set of indices representing the plane supporting the object of interest
* \param[out] object the segmented resultant object
*/
void
segmentObject (int picked_idx,
const typename PointCloud<PointT>::ConstPtr &cloud,
const PointIndices::Ptr &plane_indices,
PointCloud<PointT> &object)
{
typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud);
chull.setIndices (plane_indices);
chull.reconstruct (*plane_hull);
// Remove the plane indices from the data
typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
ExtractIndices<PointT> extract (true);
extract.setInputCloud (cloud);
extract.setIndices (plane_indices);
extract.setNegative (false);
extract.filter (*plane);
PointIndices::Ptr indices_but_the_plane (new PointIndices);
extract.getRemovedIndices (*indices_but_the_plane);
// Extract all clusters above the hull
PointIndices::Ptr points_above_plane (new PointIndices);
ExtractPolygonalPrismData<PointT> exppd;
exppd.setInputCloud (cloud);
exppd.setIndices (indices_but_the_plane);
exppd.setInputPlanarHull (plane_hull);
exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
exppd.setHeightLimits (0.001, 0.5); // up to half a meter
exppd.segment (*points_above_plane);
vector<PointIndices> euclidean_label_indices;
// Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
if (cloud_->isOrganized ())
{
// Use an organized clustering segmentation to extract the individual clusters
typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
euclidean_cluster_comparator->setInputCloud (cloud);
euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
// Set the entire scene to false, and the inliers of the objects located on top of the plane to true
Label l; l.label = 0;
PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
// Mask the objects that we want to split into clusters
for (const int &index : points_above_plane->indices)
scene->points[index].label = 1;
euclidean_cluster_comparator->setLabels (scene);
boost::shared_ptr<std::set<uint32_t> > exclude_labels = boost::make_shared<std::set<uint32_t> > ();
exclude_labels->insert (0);
euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (cloud);
PointCloud<Label> euclidean_labels;
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
}
else
{
print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
TicToc tt; tt.tic ();
EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setSearchMethod (search_);
ec.setInputCloud (cloud);
ec.setIndices (points_above_plane);
ec.extract (euclidean_label_indices);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
}
// For each cluster found
bool cluster_found = false;
for (const auto &euclidean_label_index : euclidean_label_indices)
{
if (cluster_found)
break;
// Check if the point that we picked belongs to it
for (size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
{
if (picked_idx != euclidean_label_index.indices[j])
continue;
copyPointCloud (*cloud, euclidean_label_index.indices, object);
cluster_found = true;
break;
}
}
}
int main(int argc, char** argv) {
sensor_msgs::PointCloud2 cloud_blob, cloud_tmp;
sensor_msgs::PointCloud cloud_blob2;
pcl::PointCloud<PointT> cloud;
ofstream labelfile, featfile;
labelfile.open ("labels.txt");
featfile.open ("feats.txt");
// read the pcd file
if (pcl::io::loadPCDFile(argv[1], cloud_blob) == -1) {
ROS_ERROR("Couldn't read file test_pcd.pcd");
return (-1);
}
ROS_INFO("Loaded %d data points from test_pcd.pcd with the following fields: %s", (int) (cloud_blob.width * cloud_blob.height), pcl::getFieldsList(cloud_blob).c_str());
// convert to templated message type
pcl::fromROSMsg(cloud_blob, cloud);
pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud));
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT > ());
pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ());
//pcl::PointCloud<PointXYZI>::Ptr cloud_seg (new pcl::PointCloud<PointXYZI> ());
pcl::PointIndices::Ptr segment_indices(new pcl::PointIndices());
// get segments
// find the max segment number
int max_segment_num = 0;
for (size_t i = 0; i < cloud.points.size(); ++i) {
if (max_segment_num < cloud.points[i].segment) {
max_segment_num = (int) cloud.points[i].segment;
}
}
ExtractIndices<PointT> extract;
Eigen::Vector4f min_p;
Eigen::Vector4f max_p;
for (int seg = 1; seg <= max_segment_num; seg++) {
vector<float> features;
int label;
segment_indices->indices.clear();
for (size_t i = 0; i < cloud.points.size(); ++i) {
if (cloud.points[i].segment == seg) {
segment_indices->indices.push_back(i);
label = cloud.points[i].label;
}
}
extract.setInputCloud(cloud_ptr);
extract.setIndices(segment_indices);
extract.setNegative(false);
extract.filter(*cloud_seg);
// std::cerr << seg << ". Cloud size after extracting : " << cloud_seg->points.size() << std::endl;
if (cloud_seg->points[0].label != 0) {
SpectralAnalysis sa(0.05);
SpinImageNormal spin_image(0.025, 0.025, 5, 4, false, sa);
ShapeSpectral shape_spectral(sa);
OrientationNormal o_normal(0, 0, 1, sa);
OrientationTangent o_tangent(0, 0, 1, sa);
Position position;
BoundingBoxSpectral bbox_spectral(1.0, sa);
// histogram feats
vector<Descriptor3D*> descriptors_3d;
descriptors_3d.push_back(&shape_spectral);
descriptors_3d.push_back(&o_normal);
//descriptors_3d.push_back(&o_tangent);
//descriptors_3d.push_back(&position);
//descriptors_3d.push_back(&bbox_spectral);
pcl::toROSMsg(*cloud_seg, cloud_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(cloud_tmp, cloud_blob2);
cloud_kdtree::KdTreeANN pt_cloud_kdtree(cloud_blob2);
vector<const geometry_msgs::Point32*> interest_pts;
if (cloud_seg->points.size() < 200) {
interest_pts.resize(cloud_blob2.points.size());
for (size_t i = 0; i < cloud_blob2.points.size(); i++) {
interest_pts[i] = &(cloud_blob2.points[i]);
}
} else {
interest_pts.resize(100);
map<int, int> in;
int count = 0;
while (count < 100) {
int a = rand() % cloud_blob2.points.size();
if (in.find(a) == in.end()) {
in[a] = 1;
interest_pts[count] = &(cloud_blob2.points[a]);
}
count++;
}
}
// vector<vector<float> > descriptor_results;
// spin_image.compute(cloud_blob2, pt_cloud_kdtree, interest_pts, descriptor_results);
unsigned int nbr_descriptors = descriptors_3d.size();
vector<vector<vector<float> > > all_descriptor_results(nbr_descriptors);
vector<vector<vector<float> > > hist_feats(nbr_descriptors);
for (unsigned int i = 0; i < nbr_descriptors; i++) {
std::cerr << "hist featnum: " << i << "\n" ;
descriptors_3d[i]->compute(cloud_blob2, pt_cloud_kdtree, interest_pts, all_descriptor_results[i]);
std::cerr << "feature computed" << "\n" ;
get_feat_histogram(all_descriptor_results[i], hist_feats[i]);
concat_feats(features,hist_feats[i]);
}
// average feats
vector<Descriptor3D*> descriptors_3d_avg;
descriptors_3d_avg.push_back(&spin_image);
unsigned int nbr_descriptors_avg = descriptors_3d_avg.size();
vector<vector<vector<float> > > all_avg_descriptor_results(nbr_descriptors);
vector<vector<float> > avg_feats(nbr_descriptors_avg);
for (unsigned int i = 0; i < nbr_descriptors_avg; i++) {
std::cerr << "avg featnum: " << i << "\n" ;
descriptors_3d_avg[i]->compute(cloud_blob2, pt_cloud_kdtree, interest_pts, all_avg_descriptor_results[i]);
get_avg_feats(all_avg_descriptor_results[i], avg_feats[i]);
concat_feats(features,avg_feats[i]);
}
getMinMax(*cloud_ptr, *segment_indices, min_p, max_p);
// ROS_INFO("minp : %f,%f,%f\t maxp : %f,%f,%f", min_p[0], min_p[1], min_p[2], max_p[0], max_p[1], max_p[2]);
// ROS_INFO("size of all_descriptor_results : %d", all_descriptor_results[1].size());
// add bounding box features
features.push_back(max_p[0]- min_p[0]);
features.push_back(max_p[1]- min_p[1]);
features.push_back(max_p[2]- min_p[2]);
// write label to file
labelfile << cloud_seg->points[0].label <<"\n";
// write features to file
for (vector<float>::iterator it = features.begin(); it < features.end(); it++) {
featfile << *it <<"\t";
}
featfile <<"\n";
}
}
// print out the features to a file
labelfile.close();
featfile.close();
}
// Returns the points that are above the floor, but not the floor itself
CloudT get_object_points(CloudT cloud)
{
// PHASE 1: DATA CLEANUP
////////////////////////////////////////////////////////
// Filter out NaNs from data (this is necessary now) ...
PassThrough<PointXYZRGB> nan_remover;
nan_remover.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
nan_remover.setFilterFieldName("z");
nan_remover.setFilterLimits(0.0, 10.0);
nan_remover.filter(cloud);
// Filter out statistical outliers
StatisticalOutlierRemoval<PointXYZRGB> statistical_filter;
statistical_filter.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0);
statistical_filter.filter(cloud);
// Downsample to 1cm Voxel Grid
pcl::VoxelGrid<PointT> downsampler;
downsampler.setInputCloud(boost::make_shared<CloudT>(cloud));
downsampler.setLeafSize(0.005, 0.005, 0.005);
downsampler.filter(cloud);
ROS_INFO("PointCloud after filtering: %d data points.", cloud.width * cloud.height);
// PHASE 2: FIND THE GROUND PLANE
/////////////////////////////////////////////////////////
// Step 3: Find the ground plane using RANSAC
ModelCoefficients ground_coefficients;
PointIndices ground_indices;
SACSegmentation<PointXYZRGB> ground_finder;
ground_finder.setOptimizeCoefficients(true);
ground_finder.setModelType(SACMODEL_PLANE);
ground_finder.setMethodType(SAC_RANSAC);
ground_finder.setDistanceThreshold(0.015);
ground_finder.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
ground_finder.segment(ground_indices, ground_coefficients);
// PHASE 3: EXTRACT ONLY POINTS ABOVE THE GROUND PLANE
/////////////////////////////////////////////////////////
// Step 3a. Extract the ground plane inliers
CloudT ground_points;
ExtractIndices<PointXYZRGB> extractor;
extractor.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
extractor.setIndices(boost::make_shared<PointIndices>(ground_indices));
extractor.filter(ground_points);
// Step 3b. Extract the ground plane outliers
CloudT object_points;
ExtractIndices<PointXYZRGB> outlier_extractor;
outlier_extractor.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
outlier_extractor.setIndices(boost::make_shared<PointIndices>(ground_indices));
outlier_extractor.setNegative(true);
outlier_extractor.filter(object_points);
// Step 3c. Project the ground inliers
PointCloud<PointXYZRGB> cloud_projected;
ProjectInliers<PointXYZRGB> proj;
proj.setModelType(SACMODEL_PLANE);
proj.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(ground_points));
proj.setModelCoefficients(boost::make_shared<ModelCoefficients>(ground_coefficients));
proj.filter(cloud_projected);
// Step 3d. Create a Convex Hull representation of the projected inliers
PointCloud<PointXYZRGB> ground_hull;
ConvexHull2D<PointXYZRGB, PointXYZRGB> chull;
chull.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud_projected));
chull.reconstruct(ground_hull);
ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull.points.size ());
hull_pub.publish(ground_hull);
// Step 3e. Extract only those outliers that lie above the ground plane's convex hull
PointIndices object_indices;
ExtractPolygonalPrismData<PointT> hull_limiter;
hull_limiter.setInputCloud(boost::make_shared<CloudT>(object_points));
hull_limiter.setInputPlanarHull(boost::make_shared<CloudT>(ground_hull));
hull_limiter.setHeightLimits(0, 0.3);
hull_limiter.segment(object_indices);
ExtractIndices<PointT> object_extractor;
object_extractor.setInputCloud(boost::make_shared<CloudT>(object_points));
object_extractor.setIndices(boost::make_shared<PointIndices>(object_indices));
object_extractor.filter(object_points);
return object_points;
}
/**
* This function tries to find all planes in a point cloud by applying
* a model-based RANSAC algorithm.
*
* Heavily inspired by
* http://www.pointclouds.org/documentation/tutorials/extract_indices.php
*/
void filterPlanes(
TheiaCloudPtr inCloud,
TheiaCloudPtr outPlanes,
TheiaCloudPtr outObjects
){
TheiaCloudPtr workingCloudPtr(new TheiaCloud(*inCloud));
size_t origCloudSize = workingCloudPtr->points.size();
size_t minPlanePoints = config.minPercentage * origCloudSize;
if(origCloudSize < 3){
ROS_INFO("Input cloud for findPlanes is too small");
ROS_INFO("Check cropping and rescaling parameters");
return;
}
TheiaCloudPtr allPlanesCloudPtr(new TheiaCloud());
PointIndices::Ptr inliers(new PointIndices());
ModelCoefficients::Ptr coefficients(new ModelCoefficients());
// prepare plane segmentation
SACSegmentation<TheiaPoint> seg;
seg.setModelType(SACMODEL_PLANE);
seg.setMethodType(SAC_RANSAC);
seg.setMaxIterations(config.numbIterations);
seg.setDistanceThreshold(config.planeDistThresh);
seg.setOptimizeCoefficients(config.planeOptimize);
// prepare extractor
ExtractIndices<TheiaPoint> extract;
while(workingCloudPtr->points.size() >= 4){
seg.setInputCloud(workingCloudPtr);
seg.segment(*inliers, *coefficients);
/**
* Stop the search for planes when the found plane
* is too small (in terms of points).
*/
size_t numbInliers = inliers->indices.size();
if(!numbInliers) break;
if(numbInliers < minPlanePoints) break;
// extract plane points
TheiaCloudPtr planeCloudPtr(new TheiaCloud());
extract.setInputCloud(workingCloudPtr);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*planeCloudPtr);
*allPlanesCloudPtr += *planeCloudPtr;
/**
* Remove plane from input cloud
*/
TheiaCloudPtr remainingCloudPtr(new TheiaCloud());
extract.setNegative(true);
extract.filter(*remainingCloudPtr);
/**
* Continue search
* But only use remaining point cloud
*/
workingCloudPtr->swap(*remainingCloudPtr);
}
*outPlanes = TheiaCloud(*allPlanesCloudPtr);
*outObjects = TheiaCloud(*workingCloudPtr);
}
int main(int argc, char** argv)
{
if (argc < 2)
{
cout << "Input a PCD file name...\n";
return 0;
}
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>);
PCDReader reader;
reader.read(argv[1], *cloud);
cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n";
PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>);
VoxelGrid<PointXYZ> vg;
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n";
SACSegmentation<PointXYZ> seg;
PointIndices::Ptr inliers(new PointIndices);
PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
seg.setOptimizeCoefficients(true);
seg.setModelType(SACMODEL_PLANE);
seg.setMethodType(SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
int i=0, nr_points = (int)cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
cout << "Coud not estimate a planar model for the given dataset.\n";
break;
}
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_plane);
cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n";
extract.setNegative(true);
extract.filter(*cloud_f);
cloud_filtered->swap(*cloud_f);
}
search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
kdtree->setInputCloud(cloud_filtered);
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ece;
ece.setClusterTolerance(0.02);
ece.setMinClusterSize(100);
ece.setMaxClusterSize(25000);
ece.setSearchMethod(kdtree);
ece.setInputCloud(cloud_filtered);
ece.extract(cluster_indices);
PCDWriter writer;
int j = 0;
for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>);
for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit)
cluster_cloud->points.push_back(cloud_filtered->points[*pit]);
cluster_cloud->width = cluster_cloud->points.size();
cluster_cloud->height = 1;
cluster_cloud->is_dense = true;
cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n";
stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<PointXYZ>(ss.str(), *cluster_cloud, false);
j++;
}
return 0;
}