Esempio n. 1
0
	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);
}
Esempio n. 3
0
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
}