void
cloud_cb (const sensor_msgs::PointCloud2 input)
{
// Create a container for the data.
sensor_msgs::PointCloud2 output;
pcl::PCLPointCloud2 pcl_pc;
pcl_conversions::toPCL(input, pcl_pc);
// Do data processing here...
//output = *input;
pcl::PointCloud<pcl::PointXYZI> conv_input;
pcl::fromPCLPointCloud2(pcl_pc, conv_input);
pcln::ConditionalEuclideanClustering<pcl::PointXYZI> cec (true);
cec.setInputCloud (conv_input);
cec.setConditionFunction (&enforceIntensitySimilarity);
// Points within this distance from one another are going to need to validate the enforceIntensitySimilarity function to be part of the same cluster:
cec.setClusterTolerance (0.09f);
// Size constraints for the clusters:
cec.setMinClusterSize (5);
cec.setMaxClusterSize (30);
// The resulting clusters (an array of pointindices):
cec.segment (*clusters);
// The clusters that are too small or too large in size can also be extracted separately:
cec.getRemovedClusters (small_clusters, large_clusters);
output=*input;
// Publish the data.
pub.publish (output);
}
void RegionGrowingMultiplePlaneSegmentation::segment(
const sensor_msgs::PointCloud2::ConstPtr& msg,
const sensor_msgs::PointCloud2::ConstPtr& normal_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!done_initialization_) {
return;
}
vital_checker_->poke();
{
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*msg, *cloud);
pcl::fromROSMsg(*normal_msg, *normal);
// concatenate fields
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normal, *all_cloud);
pcl::PointIndices::Ptr indices (new pcl::PointIndices);
for (size_t i = 0; i < all_cloud->points.size(); i++) {
if (!isnan(all_cloud->points[i].x) &&
!isnan(all_cloud->points[i].y) &&
!isnan(all_cloud->points[i].z) &&
!isnan(all_cloud->points[i].normal_x) &&
!isnan(all_cloud->points[i].normal_y) &&
!isnan(all_cloud->points[i].normal_z) &&
!isnan(all_cloud->points[i].curvature)) {
if (all_cloud->points[i].curvature < max_curvature_) {
indices->indices.push_back(i);
}
}
}
pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true);
// vector of pcl::PointIndices
pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters);
cec.setInputCloud (all_cloud);
cec.setIndices(indices);
cec.setClusterTolerance(cluster_tolerance_);
cec.setMinClusterSize(min_size_);
//cec.setMaxClusterSize(max_size_);
{
boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex);
setCondifionFunctionParameter(angular_threshold_, distance_threshold_);
cec.setConditionFunction(
&RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction);
//ros::Time before = ros::Time::now();
cec.segment (*clusters);
// ros::Time end = ros::Time::now();
// ROS_INFO("segment took %f sec", (before - end).toSec());
}
// Publish raw cluster information
// pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices>
jsk_recognition_msgs::ClusterPointIndices ros_clustering_result;
ros_clustering_result.cluster_indices
= pcl_conversions::convertToROSPointIndices(*clusters, msg->header);
ros_clustering_result.header = msg->header;
pub_clustering_result_.publish(ros_clustering_result);
// estimate planes
std::vector<pcl::PointIndices::Ptr> all_inliers;
std::vector<pcl::ModelCoefficients::Ptr> all_coefficients;
jsk_recognition_msgs::PolygonArray ros_polygon;
ros_polygon.header = msg->header;
for (size_t i = 0; i < clusters->size(); i++) {
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]);
ransacEstimation(cloud, cluster,
*plane_inliers, *plane_coefficients);
if (plane_inliers->indices.size() > 0) {
jsk_recognition_utils::ConvexPolygon::Ptr convex
= jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>(
cloud, plane_inliers, plane_coefficients);
if (convex) {
if (min_area_ > convex->area() || convex->area() > max_area_) {
continue;
}
{
// check direction consistency of coefficients and convex
Eigen::Vector3f coefficient_normal(plane_coefficients->values[0],
plane_coefficients->values[1],
plane_coefficients->values[2]);
if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) {
convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
}
}
// Normal should direct to origin
{
Eigen::Vector3f p = convex->getPointOnPlane();
Eigen::Vector3f n = convex->getNormal();
if (p.dot(n) > 0) {
convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
Eigen::Vector3f new_normal = convex->getNormal();
plane_coefficients->values[0] = new_normal[0];
plane_coefficients->values[1] = new_normal[1];
plane_coefficients->values[2] = new_normal[2];
plane_coefficients->values[3] = convex->getD();
}
}
geometry_msgs::PolygonStamped polygon;
polygon.polygon = convex->toROSMsg();
polygon.header = msg->header;
ros_polygon.polygons.push_back(polygon);
all_inliers.push_back(plane_inliers);
all_coefficients.push_back(plane_coefficients);
}
}
}
jsk_recognition_msgs::ClusterPointIndices ros_indices;
ros_indices.cluster_indices =
pcl_conversions::convertToROSPointIndices(all_inliers, msg->header);
ros_indices.header = msg->header;
pub_inliers_.publish(ros_indices);
jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
ros_coefficients.header = msg->header;
ros_coefficients.coefficients =
pcl_conversions::convertToROSModelCoefficients(
all_coefficients, msg->header);
pub_coefficients_.publish(ros_coefficients);
pub_polygons_.publish(ros_polygon);
}
}
int
main (int argc, char** argv)
{
// Data containers used
pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);
pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);
pcl::console::TicToc tt;
// Load the input point cloud
std::cerr << "Loading...\n", tt.tic ();
pcl::io::loadPCDFile (argv[1], *cloud_in);
std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n";
// Downsample the cloud using a Voxel Grid class
std::cerr << "Downsampling...\n", tt.tic ();
pcl::VoxelGrid<PointTypeIO> vg;
vg.setInputCloud (cloud_in);
vg.setLeafSize (80.0, 80.0, 80.0);
vg.setDownsampleAllData (true);
vg.filter (*cloud_out);
std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n";
// Set up a Normal Estimation class and merge data in cloud_with_normals
std::cerr << "Computing normals...\n", tt.tic ();
pcl::copyPointCloud (*cloud_out, *cloud_with_normals);
pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
ne.setInputCloud (cloud_out);
ne.setSearchMethod (search_tree);
ne.setRadiusSearch (300.0);
ne.compute (*cloud_with_normals);
std::cerr << ">> Done: " << tt.toc () << " ms\n";
// Set up a Conditional Euclidean Clustering class
std::cerr << "Segmenting to clusters...\n", tt.tic ();
pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);
cec.setInputCloud (cloud_with_normals);
cec.setConditionFunction (&customRegionGrowing);
cec.setClusterTolerance (500.0);
cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);
cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);
cec.segment (*clusters);
cec.getRemovedClusters (small_clusters, large_clusters);
std::cerr << ">> Done: " << tt.toc () << " ms\n";
// Using the intensity channel for lazy visualization of the output
for (int i = 0; i < small_clusters->size (); ++i)
for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0;
for (int i = 0; i < large_clusters->size (); ++i)
for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0;
for (int i = 0; i < clusters->size (); ++i)
{
int label = rand () % 8;
for (int j = 0; j < (*clusters)[i].indices.size (); ++j)
cloud_out->points[(*clusters)[i].indices[j]].intensity = label;
}
// Save the output point cloud
std::cerr << "Saving...\n", tt.tic ();
pcl::io::savePCDFile ("output.pcd", *cloud_out);
std::cerr << ">> Done: " << tt.toc () << " ms\n";
return (0);
}
