CloudPtr
get ()
{
//lock while we swap our cloud and reset it.
boost::mutex::scoped_lock lock (mtx_);
//float asdf = seg_.getProbability();
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_final (new pcl::PointIndices ());
CloudPtr residual_cloud (new Cloud);
CloudPtr plane_cloud (new Cloud);
CloudPtr temp_cloud1 (new Cloud);
CloudPtr temp_cloud2 (new Cloud);
grid_.setInputCloud (cloud_);
grid_.filter (*residual_cloud);
int numpointsstart = (int)residual_cloud->points.size();
seg_.setInputCloud (residual_cloud);
extract_.setInputCloud (residual_cloud);
extract_.setNegative(true);
int i = 0;
do{
seg_.segment (*inliers, *coefficients);
if (inliers->indices.size())
inliers_final->indices.insert(inliers_final->indices.end(), inliers->indices.begin(), inliers->indices.end());
extract_.setIndices (inliers);
extract_.filter (*(i?temp_cloud1:temp_cloud2));
seg_.setInputCloud (i?temp_cloud1:temp_cloud2);
extract_.setInputCloud (i?temp_cloud1:temp_cloud2);
i = (i + 1) % 2;
} while (inliers->indices.size() > 0.1 * numpointsstart);
extract_.setInputCloud (residual_cloud);
extract_.setIndices (inliers_final);
extract_.setNegative(false);
extract_.filter (*plane_cloud);
return (plane_cloud);
}
void RansacDominantPlaneExtractor::extract(PlanarPolygon& planar_polygon)
{
// Part 1: find inliers and coefficients for the dominant plane
pcl::PointIndices::Ptr plane_inliers_(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr plane_coefficients_(new pcl::ModelCoefficients);
PointCloud::Ptr downsampled_cloud(new PointCloud);
PointCloudN::Ptr downsampled_normals(new PointCloudN);
// Downsample input
vg_.setInputCloud(input_);
vg_.filter(*downsampled_cloud);
// Estimate normals for the downsampled cloud
ne_.setInputCloud(downsampled_cloud);
ne_.compute(*downsampled_normals);
// Segment plane
ssfn_.setInputCloud(downsampled_cloud);
ssfn_.setInputNormals(downsampled_normals);
ssfn_.segment(*plane_inliers_, *plane_coefficients_);
if (plane_inliers_->indices.size() == 0)
{
ROS_WARN("[RansacDominantPlaneExtractor::extract] No planar regions found! Aborting.");
return;
}
// Part 2: create corresponding PlanarPolygon
PointCloud::Ptr plane_cloud(new PointCloud);
PointCloud::Ptr plane_hull(new PointCloud);
// Project inliers onto the plane
pi_.setInputCloud(downsampled_cloud);
pi_.setIndices(plane_inliers_);
pi_.setModelCoefficients(plane_coefficients_);
pi_.filter(*plane_cloud);
// Calculate convex hull for inliers
pcl::ConvexHull<PointT> convex_hull;
convex_hull.setInputCloud(plane_cloud);
convex_hull.reconstruct(*plane_hull);
// Create PlanarPolygon
Eigen::Vector4f coefficients;
coefficients << plane_coefficients_->values[0],
plane_coefficients_->values[1],
plane_coefficients_->values[2],
plane_coefficients_->values[3];
shrinkPointCloud(plane_hull, shrink_plane_polygon_ratio_);
planar_polygon = PlanarPolygon(plane_hull->points, coefficients);
}
void pcCallback(const sensor_msgs::PointCloud2::Ptr &msg)
{
//sensor_msgs to point cloud
sensor_msgs::PointCloud2::Ptr cloud = msg;
rgbcloudPtr convert_cloud(new rgbcloud);
pcl::fromROSMsg(*cloud, *convert_cloud);
//点群削減
rgbcloudPtr vf_cloud(new rgbcloud);
vf_cloud = voxelGrid(convert_cloud);
rgbcloudPtr bf_cloud(new rgbcloud);
// if(vf_cloud->empty()){}
// else{
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr flann(new pcl::search::KdTree<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::Normal>::Ptr normals_cloud(new pcl::PointCloud<pcl::Normal>);
// pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> norm_est;
// norm_est.setSearchMethod(flann);
// norm_est.setInputCloud(vf_cloud);
// norm_est.setRadiusSearch(0.02);
// norm_est.compute(*normals_cloud);
//
// pcl::BoundaryEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::Boundary> boundary_est;
// pcl::BoundaryEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::Boundary>::PointCloudOut boundary;
// boundary_est.setRadiusSearch(0.02);
// boundary_est.setInputNormals(normals_cloud);
//
// boundary_est.setSearchMethod(flann);
// boundary_est.setInputCloud(vf_cloud);
// boundary_est.setAngleThreshold(M_PI/6);
// boundary_est.compute(boundary);
//
// for(int i = 0; i < boundary.size(); i++){
// if(boundary.points[i].boundary_point == 0){
// bf_cloud->push_back(vf_cloud->points.at(i));
// }else{
// pcl::PointXYZRGB pt = vf_cloud->points.at(i);
// pt.x = NAN;
// pt.y = NAN;
// pt.z = NAN;
// bf_cloud->push_back(pt);
// }
// }
// }
//平面検出＋平面除去
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
rgbcloudPtr extract_cloud(new rgbcloud);
rgbcloudPtr plane_cloud(new rgbcloud);
extract_cloud = extractindices(vf_cloud, inliers, coefficients, true);
for(int i= 0; i < 7; i++){
planeSegmentation(vf_cloud, inliers, coefficients);
extract_cloud = extractindices(extract_cloud, inliers, coefficients, true);
//plane_cloud = extractindices(vf_cloud, inliers, coefficients, false);
}
//その他の点の除去
// rgbcloudPtr removed_outlier_cloud(new rgbcloud);
// removed_outlier_cloud = removedCloud(extract_cloud, inliers);
//クラスタリング
std::vector<pcl::PointIndices> clusters;
//clusters = regiongrowing(extract_cloud);
clusters = euclideanCluster(extract_cloud);
//std::cout<<"clusters : "<<clusters.size()<<std::endl;
//std::cout<<"Point Size :"<<extract_cloud->size()<<std::endl;
rgbcloudPtr tempcloud(new rgbcloud);
int i = 0;
for(std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it){
for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit){
tempcloud->points.push_back(extract_cloud->points[*pit]);
if(tempcloud->points.size() > 300){
pcl::PointXYZRGB pt = extract_cloud->points.at(i);
pt.x = NAN;
pt.y = NAN;
pt.z = NAN;
tempcloud->push_back(pt);
}
tempcloud->width = tempcloud->size();
tempcloud->height = 1;
tempcloud->is_dense = true;
}
}
//重心計算
extract_cloud->header.frame_id = msg->header.frame_id;
Eigen::Vector4f centroid;
//centroid = centroidCompute(tempcloud, clusters);
geometry_msgs::PoseArray objectpoint;
geometry_msgs::Pose objectpose;
objectpoint.header.frame_id = msg->header.frame_id;
objectpoint.header.stamp = ros::Time::now();
for(int i =0; i < clusters.size(); i++){
pcl::compute3DCentroid(*extract_cloud, clusters[i].indices, centroid);
std::cout<<"error no"<<std::endl;
objectpose.position.x = centroid[0];
objectpose.position.y = centroid[1];
objectpose.position.z = centroid[2];
std::cout<<"[x y z] = "<<" ["<<i<<" ] "<<"["<<objectpose.position.x<< " , "<<objectpose.position.y <<" , "<<objectpose.position.z <<"]"<<std::endl;
objectpoint.poses.push_back(objectpose);
point_pub.publish(objectpoint);
}
//ビジュアライズ用
for(size_t i = 0; i < clusters.size(); i++){
mark_cluster(extract_cloud, centroid, i);
}
std::cout<<"tempcloud"<<tempcloud->width<<" : "<<tempcloud->height<<std::endl;
std::cout<<"extract_cloud"<<extract_cloud->width<<" : "<<extract_cloud->height<<std::endl;
#ifdef TEST
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*tempcloud, output);
std::cout<<"stop"<<std::endl;
output.header.frame_id = msg->header.frame_id;
pub.publish(output);
#endif
}