void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_full (std::vector<CloudPtr> & clusters)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
CloudPtr cloud_filtered_ (new Cloud ());
CloudPtr cloud_downsampled_ (new Cloud ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
CloudPtr table_projected_ (new Cloud ());
CloudPtr table_hull_ (new Cloud ());
CloudPtr cloud_objects_ (new Cloud ());
CloudPtr cluster_object_ (new Cloud ());
typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
clusters_tree_->setEpsilon (1);
// Normal estimation parameters
n3d_.setKSearch (10);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (sac_distance_threshold_);
seg_.setMaxIterations (2000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_MSAC);
seg_.setProbability (0.98);
proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
prism_.setHeightLimits (object_min_height_, object_max_height_);
// Clustering parameters
cluster_.setClusterTolerance (object_cluster_tolerance_);
cluster_.setMinClusterSize (object_cluster_min_size_);
cluster_.setSearchMethod (clusters_tree_);
// ---[ PassThroughFilter
pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
pass_.setFilterFieldName ("z");
pass_.setInputCloud (input_);
pass_.filter (*cloud_filtered_);
if (int (cloud_filtered_->points.size ()) < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
cloud_filtered_->points.size ());
return;
}
// Downsample the point cloud
grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid_.setDownsampleAllData (false);
grid_.setInputCloud (cloud_filtered_);
grid_.filter (*cloud_downsampled_);
PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %zu out of %zu\n",
min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ());
// ---[ Estimate the point normals
n3d_.setInputCloud (cloud_downsampled_);
n3d_.compute (*cloud_normals_);
PCL_INFO ("[DominantPlaneSegmentation] %zu normals estimated. \n", cloud_normals_->points.size ());
// ---[ Perform segmentation
seg_.setInputCloud (cloud_downsampled_);
seg_.setInputNormals (cloud_normals_);
seg_.segment (*table_inliers_, *table_coefficients_);
if (table_inliers_->indices.size () == 0)
{
PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
proj_.setInputCloud (cloud_downsampled_);
proj_.setIndices (table_inliers_);
proj_.setModelCoefficients (table_coefficients_);
proj_.filter (*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
CloudPtr table_hull (new Cloud ());
hull_.setInputCloud (table_projected_);
hull_.reconstruct (*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
model_coefficients[0] = table_coefficients_->values[0];
model_coefficients[1] = table_coefficients_->values[1];
model_coefficients[2] = table_coefficients_->values[2];
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (0, 0, 0, 0);
// See if we need to flip any plane normals
vp -= table_hull->points[0].getVector4fMap ();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (model_coefficients);
// Flip the plane normal
if (cos_theta < 0)
{
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
}
//Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
prism_.setInputCloud (cloud_filtered_);
prism_.setInputPlanarHull (table_hull);
prism_.segment (cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
extract_object_indices.setInputCloud (cloud_downsampled_);
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (*cloud_objects_);
if (cloud_objects_->points.size () == 0)
return;
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clusters2;
cluster_.setInputCloud (cloud_filtered_);
cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
cluster_.extract (clusters2);
PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %zu.\n",
clusters2.size ());
clusters.resize (clusters2.size ());
for (size_t i = 0; i < clusters2.size (); ++i)
{
clusters[i] = CloudPtr (new Cloud ());
pcl::copyPointCloud (*cloud_filtered_, clusters2[i].indices, *clusters[i]);
}
}
template<typename PointType> void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast (std::vector<CloudPtr> & clusters)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
// Is the input dataset organized?
if (input_->is_dense)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used with organized point clouds!\n");
return;
}
CloudConstPtr cloud_;
CloudPtr cloud_filtered_ (new Cloud ());
CloudPtr cloud_downsampled_ (new Cloud ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
CloudPtr table_projected_ (new Cloud ());
CloudPtr table_hull_ (new Cloud ());
CloudPtr cloud_objects_ (new Cloud ());
CloudPtr cluster_object_ (new Cloud ());
typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
clusters_tree_->setEpsilon (1);
// Normal estimation parameters
n3d_.setKSearch (k_);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (sac_distance_threshold_);
seg_.setMaxIterations (2000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_RANSAC);
seg_.setProbability (0.99);
proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
prism_.setHeightLimits (object_min_height_, object_max_height_);
// Clustering parameters
cluster_.setClusterTolerance (object_cluster_tolerance_);
cluster_.setMinClusterSize (object_cluster_min_size_);
cluster_.setSearchMethod (clusters_tree_);
// ---[ PassThroughFilter
pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
pass_.setFilterFieldName ("z");
pass_.setInputCloud (input_);
pass_.filter (*cloud_filtered_);
if (int (cloud_filtered_->points.size ()) < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
cloud_filtered_->points.size ());
return;
}
// Downsample the point cloud
grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid_.setDownsampleAllData (false);
grid_.setInputCloud (cloud_filtered_);
grid_.filter (*cloud_downsampled_);
// ---[ Estimate the point normals
n3d_.setInputCloud (cloud_downsampled_);
n3d_.compute (*cloud_normals_);
// ---[ Perform segmentation
seg_.setInputCloud (cloud_downsampled_);
seg_.setInputNormals (cloud_normals_);
seg_.segment (*table_inliers_, *table_coefficients_);
if (table_inliers_->indices.size () == 0)
{
PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
proj_.setInputCloud (cloud_downsampled_);
proj_.setIndices (table_inliers_);
proj_.setModelCoefficients (table_coefficients_);
proj_.filter (*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
CloudPtr table_hull (new Cloud ());
hull_.setInputCloud (table_projected_);
hull_.reconstruct (*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
model_coefficients[0] = table_coefficients_->values[0];
model_coefficients[1] = table_coefficients_->values[1];
model_coefficients[2] = table_coefficients_->values[2];
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (0, 0, 0, 0);
// See if we need to flip any plane normals
vp -= table_hull->points[0].getVector4fMap ();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (model_coefficients);
// Flip the plane normal
if (cos_theta < 0)
{
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
}
//Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
prism_.setInputCloud (input_);
prism_.setInputPlanarHull (table_hull);
prism_.segment (cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
extract_object_indices.setInputCloud (input_);
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (*cloud_objects_);
//create new binary pointcloud with intensity values (0 and 1), 0 for non-object pixels and 1 otherwise
pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud (new pcl::PointCloud<pcl::PointXYZI>);
{
binary_cloud->width = input_->width;
binary_cloud->height = input_->height;
binary_cloud->points.resize (input_->points.size ());
binary_cloud->is_dense = input_->is_dense;
size_t idx;
for (size_t i = 0; i < cloud_object_indices.indices.size (); ++i)
{
idx = cloud_object_indices.indices[i];
binary_cloud->points[idx].getVector4fMap () = input_->points[idx].getVector4fMap ();
binary_cloud->points[idx].intensity = 1.0;
}
}
//connected components on the binary image
std::map<float, float> connected_labels;
float c_intensity = 0.1f;
float intensity_incr = 0.1f;
{
int wsize = wsize_;
for (int i = 0; i < int (binary_cloud->width); i++)
{
for (int j = 0; j < int (binary_cloud->height); j++)
{
if (binary_cloud->at (i, j).intensity != 0)
{
//check neighboring pixels, first left and then top
//be aware of margins
if ((i - 1) < 0 && (j - 1) < 0)
{
//top-left pixel
(*binary_cloud) (i, j).intensity = c_intensity;
}
else
{
if ((j - 1) < 0)
{
//top-row, check on the left of pixel to assign a new label or not
int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left)
{
//Nothing found on the left, check bigger window
bool found = false;
for (int kk = 2; kk < wsize && !found; kk++)
{
if ((i - kk) < 0)
continue;
int left = check ((*binary_cloud) (i - kk, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left == 0)
{
found = true;
}
}
if (!found)
{
c_intensity += intensity_incr;
(*binary_cloud) (i, j).intensity = c_intensity;
}
}
}
else
{
if ((i - 1) == 0)
{
//check only top
int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (top)
{
bool found = false;
for (int kk = 2; kk < wsize && !found; kk++)
{
if ((j - kk) < 0)
continue;
int top = check ((*binary_cloud) (i, j - kk), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (top == 0)
{
found = true;
}
}
if (!found)
{
c_intensity += intensity_incr;
(*binary_cloud) (i, j).intensity = c_intensity;
}
}
}
else
{
//check left and top
int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left == 0 && top == 0)
{
//both top and left had labels, check if they are different
//if they are, take the smallest one and mark labels to be connected..
if ((*binary_cloud) (i - 1, j).intensity != (*binary_cloud) (i, j - 1).intensity)
{
float smaller_intensity = (*binary_cloud) (i - 1, j).intensity;
float bigger_intensity = (*binary_cloud) (i, j - 1).intensity;
if ((*binary_cloud) (i - 1, j).intensity > (*binary_cloud) (i, j - 1).intensity)
{
smaller_intensity = (*binary_cloud) (i, j - 1).intensity;
bigger_intensity = (*binary_cloud) (i - 1, j).intensity;
}
connected_labels[bigger_intensity] = smaller_intensity;
(*binary_cloud) (i, j).intensity = smaller_intensity;
}
}
if (left == 1 && top == 1)
{
//if none had labels, increment c_intensity
//search first on bigger window
bool found = false;
for (int dist = 2; dist < wsize && !found; dist++)
{
if (((i - dist) < 0) || ((j - dist) < 0))
continue;
int left = check ((*binary_cloud) (i - dist, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
int top = check ((*binary_cloud) (i, j - dist), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left == 0 && top == 0)
{
if ((*binary_cloud) (i - dist, j).intensity != (*binary_cloud) (i, j - dist).intensity)
{
float smaller_intensity = (*binary_cloud) (i - dist, j).intensity;
float bigger_intensity = (*binary_cloud) (i, j - dist).intensity;
if ((*binary_cloud) (i - dist, j).intensity > (*binary_cloud) (i, j - dist).intensity)
{
smaller_intensity = (*binary_cloud) (i, j - dist).intensity;
bigger_intensity = (*binary_cloud) (i - dist, j).intensity;
}
connected_labels[bigger_intensity] = smaller_intensity;
(*binary_cloud) (i, j).intensity = smaller_intensity;
found = true;
}
}
else if (left == 0 || top == 0)
{
//one had label
found = true;
}
}
if (!found)
{
//none had label in the bigger window
c_intensity += intensity_incr;
(*binary_cloud) (i, j).intensity = c_intensity;
}
}
}
}
}
}
}
}
}
std::map<float, pcl::PointIndices> clusters_map;
{
std::map<float, float>::iterator it;
for (int i = 0; i < int (binary_cloud->width); i++)
{
for (int j = 0; j < int (binary_cloud->height); j++)
{
if (binary_cloud->at (i, j).intensity != 0)
{
//check if this is a root label...
it = connected_labels.find (binary_cloud->at (i, j).intensity);
while (it != connected_labels.end ())
{
//the label is on the list, change pixel intensity until it has a root label
(*binary_cloud) (i, j).intensity = (*it).second;
it = connected_labels.find (binary_cloud->at (i, j).intensity);
}
std::map<float, pcl::PointIndices>::iterator it_indices;
it_indices = clusters_map.find (binary_cloud->at (i, j).intensity);
if (it_indices == clusters_map.end ())
{
pcl::PointIndices indices;
clusters_map[binary_cloud->at (i, j).intensity] = indices;
}
clusters_map[binary_cloud->at (i, j).intensity].indices.push_back (static_cast<int> (j * binary_cloud->width + i));
}
}
}
}
clusters.resize (clusters_map.size ());
std::map<float, pcl::PointIndices>::iterator it_indices;
int k = 0;
for (it_indices = clusters_map.begin (); it_indices != clusters_map.end (); it_indices++)
{
if (int ((*it_indices).second.indices.size ()) >= object_cluster_min_size_)
{
clusters[k] = CloudPtr (new Cloud ());
pcl::copyPointCloud (*input_, (*it_indices).second.indices, *clusters[k]);
k++;
indices_clusters_.push_back((*it_indices).second);
}
}
clusters.resize (k);
}