void geonDetector::initialize()
{
totalCount = 0;
pcl::PointCloud<PointT>::Ptr cloud1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered21 (new pcl::PointCloud<PointT>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals21 (new pcl::PointCloud<pcl::Normal>());
pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr geonIndices1 (new pcl::PointIndices());
pcl::search::KdTree<PointT>::Ptr tree2 (new pcl::search::KdTree<PointT>());
this->cloud = cloud1;
this->cloud_filtered = cloud_filtered1;
this->cloud_normals = cloud_normals1;
this->cloud_filtered2 = cloud_filtered21;
this->cloud_normals2 = cloud_normals21;
this->coefficients = coefficients1;
this->geonIndices = geonIndices1;
this->tree = tree2;
}
void pcl_process::plane_fitting(const PointCloudRGB::ConstPtr& pcl_in)
{
PointCloudRGB cloud_input = *pcl_in;
ROS_INFO("total points: %d", cloud_input.points.size());
//perform downsampling for the input pointcloud;
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
PointCloudRGB::Ptr input_msg_filtered (new PointCloudRGB ());
float downsample_size_ = 0.05;
sor.setInputCloud(cloud_input.makeShared());
sor.setLeafSize (downsample_size_, downsample_size_, downsample_size_);
sor.filter (*input_msg_filtered);
cloud_input = * input_msg_filtered;
ROS_INFO("remained points: %d", cloud_input.points.size());
//at most 3 planes in the pool;
pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers1 (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients2 (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers2 (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients3 (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers3 (new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane1 (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane2 (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane3 (new pcl::PointCloud<pcl::PointXYZRGB> ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.1);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
seg.setInputCloud (cloud_input.makeShared ());
seg.segment (*inliers1, *coefficients1);
extract.setInputCloud (cloud_input.makeShared());
extract.setIndices (inliers1);
extract.setNegative (false);
extract.filter (*surface_plane1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr remained_points (new pcl::PointCloud<pcl::PointXYZRGB> ());
extract.setInputCloud (cloud_input.makeShared());
extract.setIndices (inliers1);
extract.setNegative (true);
extract.filter (*remained_points);
double remained_ratio = ((double)remained_points->points.size())/((double)cloud_input.points.size());
if(remained_ratio > 0.2)
{
seg.setInputCloud (remained_points->makeShared ());
seg.segment (*inliers2, *coefficients2);
extract.setInputCloud (remained_points->makeShared ());
extract.setIndices (inliers2);
extract.setNegative (false);
extract.filter (*surface_plane2);
extract.setInputCloud (remained_points->makeShared ());
extract.setIndices (inliers2);
extract.setNegative (true);
extract.filter (*remained_points);
remained_ratio = ((double)remained_points->points.size())/((double)cloud_input.points.size());
if(remained_ratio > 0.2)
{
seg.setInputCloud (remained_points->makeShared ());
seg.segment (*inliers3, *coefficients3);
extract.setInputCloud (remained_points->makeShared ());
extract.setIndices (inliers3);
extract.setNegative (false);
extract.filter (*surface_plane3);
extract.setInputCloud (remained_points->makeShared ());
extract.setIndices (inliers3);
extract.setNegative (true);
extract.filter (*remained_points);
}
}
int plane1_type, plane2_type, plane3_type;
plane1_type = plane_classication(*inliers1, *coefficients1, cloud_input);
plane2_type = plane_classication(*inliers2, *coefficients2, cloud_input);
plane3_type = plane_classication(*inliers3, *coefficients3, cloud_input);
if(plane1_type ==1) pcl_vis_pub_.publish(*surface_plane1);
else if(plane2_type ==1) pcl_vis_pub_.publish(*surface_plane2);
else if(plane3_type ==1) pcl_vis_pub_.publish(*surface_plane3);
}
