예제 #1
1
void init(const OpenROVmessages::LaserMsg msg){

	/********************* INITIALIZE CLOUDS *********************/
	// Mandatory: Init he sizes of the point clouds
	temp_cloud->width = msg.n_rois;
	temp_cloud->height = 2;
	temp_cloud->resize(temp_cloud->width * temp_cloud->height);

	nr_points = temp_cloud->points.size();

	/**************** INITIALIZE SEGMENTER ****************/
	// Mandatory: use perpendicular plane to use setAxis() function
	seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
	
//	seg.setOptimizeCoefficients (true);
	// Set method and threshold
	seg.setMethodType (pcl::SAC_RANSAC);
//	seg.setDistanceThreshold (msg.ransac_threshold);
	seg.setDistanceThreshold (5);
	
	// Set the axis for which to search for perpendicular planes
	Eigen::Vector3f axis = Eigen::Vector3f(1.0,1.0,0.0);
	
	// Set a allowed deviation angle from vector axis
	seg.setEpsAngle((60*CV_PI)/180);
	seg.setAxis(axis);
}
int main(int argc, char** argv) {
    ros::init(argc, argv, "downsample");
    ros::NodeHandle n;
    downsample_pub = n.advertise<sensor_msgs::PointCloud2>("/downsample", 10);
    depth_sub = n.subscribe("/camera/depth/points", 10, depthPointsCallback);
    ros::Rate loop_rate(10);

    // Set variables for filters

    vox.setLeafSize (0.01, 0.01, 0.01);

    sor.setMeanK (10);
    sor.setStddevMulThresh (0.1);

    ec.setClusterTolerance (0.02); // 2cm
    ec.setMinClusterSize (20);
    ec.setMaxClusterSize (25000);
    ec.setSearchMethod (tree);

    //seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.02);

    while(ros::ok()) {
        ros::spin();
    }
    return 0;
}
예제 #3
0
int find_planes(const OpenROVmessages::LaserMsg msg){
	ROS_INFO("Entered find_planes");
	// Fill cloud with data
	fill_cloud(msg);

	// Find planes in the cloud
	while(temp_cloud->points.size() > 0.1 * nr_points){

		seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);

		seg.setOptimizeCoefficients (true);
		// Set method and threshold
		seg.setMethodType (pcl::SAC_RANSAC);
		seg.setDistanceThreshold (msg.ransac_threshold);

		// Set the axis for which to search for perpendicular planes
		Eigen::Vector3f axis = Eigen::Vector3f(1.0,0.0,0.0);

		// Set a allowed deviation angle from vector axis
		seg.setEpsAngle((60*CV_PI)/180);
		seg.setAxis(axis);
		// Segment the planes and insert the coefficients and inliers in vectors
		seg.setInputCloud (temp_cloud);
		seg.segment (*inliers, *coefficients);

		// Check that we found a plane from the cloud
		if (inliers->indices.size () == 0)
		{
			std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}
		// push coeffients onto stack
		coef_vect.push_back(*coefficients);

		// Extract the inliers
		extract.setInputCloud (temp_cloud);
		extract.setIndices (inliers);

		// Extract found inliers from cloud
		extract.setNegative(true);
		extract.filter(*temp_cloud);
	}
	return 0;
}
예제 #4
0
int main(int argc, char **argv){
    if (argc < 2){
        printf("Error: Debe especificar un archivo .pcd\n");
        exit(1);
    }
    // Cargar nube de puntos
    PointCloud::Ptr cloud (new PointCloud);
    pcl::io::loadPCDFile(argv[1],*cloud);
    // Crear visualizador
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = simpleVis(cloud);
    // Segmentar
    segmentator.setOptimizeCoefficients(true);
    segmentator.setModelType(pcl::SACMODEL_PLANE);
    segmentator.setMethodType(pcl::SAC_RANSAC);
    segmentator.setDistanceThreshold(SEG_THRESHOLD);
    segmentator.setInputCloud(cloud);
    pcl::ModelCoefficients::Ptr coefs (new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices());
    segmentator.segment(*inliers,*coefs);
    // Ver qué se obtuvo
    printf("Se obtienen %d puntos\n",(int)inliers->indices.size());
    if (inliers->indices.size() == 0){
      printf("Segmentación no sirvió de nada.\n");
      exit(1);
    }
    viewer->addPlane(*coefs, "planito");
    // Obtener centroide y dibujarlo
    Eigen::Vector4f centroid;
    compute3DCentroid(*cloud,centroid);
    viewer->addSphere(pcl::PointXYZ(centroid(0),centroid(1),centroid(2)), 0.2, "centroide");
    // Dibujar un plano en el viewer para indicar más menos qué se obtuvo
    // http://docs.pointclouds.org/trunk/classpcl_1_1visualization_1_1_p_c_l_visualizer.html
    while (!viewer->wasStopped()){
        viewer->spinOnce(100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {

    // Instantiate various clouds
    pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
    pcl::PointCloud<pcl::PointXYZ> cloud;

    // Convert to PCL data type
    pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);

    // Apply Voxel Filter on PCLPointCloud2
    vox.setInputCloud (cloudPtr);
    vox.setInputCloud (cloudPtr);
    vox.filter (*cloud_intermediate);

    // Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
    pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();

    // Apply Passthrough Filter
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.3, 1);
    pass.setInputCloud (cloud_p);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    // Apply Passthrough Filter
    pass.setFilterFieldName ("x");
    pass.setFilterLimits (-0.2, 0.2);
    pass.setInputCloud (cloud_p);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.0, 3.0);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    // Apply Statistical Noise Filter
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);

    // Planar segmentation: Remove large planes? Or extract floor?
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    int nr_points = (int) cloud_p->points.size ();

    Eigen::Vector3f lol (0, 1, 0);
    seg.setEpsAngle(  30.0f * (3.14f/180.0f) );
    seg.setAxis(lol);
    //while(cloud_p->points.size () > 0.2 * nr_points) {
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
    seg.setInputCloud (cloud_p);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0)
    {
        //break;
    }
    else {
        /*std::cout << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << "\n";*/
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_p);
        extract.setIndices(inliers);
        extract.setNegative(true);
        extract.filter(*cloud_p);
    }
    //}

    Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
    seg.setAxis(lol_p);
    while(cloud_p->points.size () > 0.1 * nr_points) {

        seg.setInputCloud (cloud_p);
        seg.segment (*inliers, *coefficients);

        if (inliers->indices.size () == 0)
        {
            break;
        }
        else {
            /*std::cout << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << "\n";*/
            pcl::ExtractIndices<pcl::PointXYZ> extract;
            extract.setInputCloud(cloud_p);
            extract.setIndices(inliers);
            extract.setNegative(true);
            extract.filter(*cloud_p);
        }
    }

    // Apply Statistical Noise Filter
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);

    if(cloud_p->points.size() > 0) {
        std::vector<pcl::PointIndices> cluster_indices;
        tree->setInputCloud (cloud_p);
        ec.setInputCloud (cloud_p);
        ec.extract (cluster_indices);

        std::cout << "Clusters detected: " << cluster_indices.size() << "\n";
        // Convert to ROS data type
    }
    // Convert to ROS data type
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*cloud_p, output);


    // Publish the data
    downsample_pub.publish(output);

}