Пример #1
0
int GraphicEnd::readimage()
{
    cout<<"loading image "<<_index<<endl;
    //读取灰度图,深度图和点云
    ss<<_rgbPath<<_index<<".png";
    _currRGB = imread(ss.str(), 0);
    ss.str("");
    ss.clear();

    ss<<_depPath<<_index<<".png";
    _currDep = imread(ss.str(), -1);
    ss.str("");
    ss.clear();
    
    ss<<_pclPath<<_index<<".pcd";
    pcl::io::loadPCDFile(ss.str(), *_currCloud);
    static pcl::PassThrough<PointT> pass;
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 6.0);
    
    PointCloud::Ptr tmp( new PointCloud() );
    pass.setInputCloud(_currCloud);
    pass.filter( *tmp );
    
    _currCloud->swap( *tmp );
    ss.str("");
    ss.clear();
    
    cout<<"load ok."<<endl;
    return 0;
}
Пример #2
0
void mario::filterA( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr & dst)
{
	static double OUT_FILTER_LOWER = FileIO::getConst("OUT_FILTER_LOWER");
	static double OUT_FILTER_UPPER = FileIO::getConst("OUT_FILTER_UPPER");
	static double DOWN_FILTER_LEAF = FileIO::getConst("DOWN_FILTER_LEAF"); // 大きいほど除去する
	static int const filterNum = 3;
	static pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudPtrs[filterNum+1];
	// フィルタリング
	if( cloud ){
		static pcl::PassThrough<pcl::PointXYZRGBA> pass; // 外れ値除去フィルタ
		static pcl::VoxelGrid< pcl::PointXYZRGBA > sor; // ダウンサンプリングフィルタ
		static bool isInitDone = false;
		if( isInitDone == false ){
			// 外れ値除去フィルタの設定
			pass.setFilterFieldName ("z");
			pass.setFilterLimits (OUT_FILTER_LOWER, OUT_FILTER_UPPER);
			// ダウンサンプリングフィルタの設定
			if( DOWN_FILTER_LEAF > 0 ){
				sor.setLeafSize (DOWN_FILTER_LEAF,DOWN_FILTER_LEAF, DOWN_FILTER_LEAF);
			}
			for( int i=0; i<filterNum; i++ ){
				cloudPtrs[i] = ( pcl::PointCloud<pcl::PointXYZRGBA>::Ptr )(new pcl::PointCloud<pcl::PointXYZRGBA>);
			}
			isInitDone = true;
		}

		int filterCount = 0;
		// はずれ値除去フィルタ
		if( OUT_FILTER_LOWER > 0 && OUT_FILTER_UPPER > 0 ){
			pass.setInputCloud ( cloud );
			///pass.setFilterLimitsNegative (true);
			filterCount++;
			pass.filter ( *cloudPtrs[filterCount] );

		}
		// ダウンサンプリングフィルタ
		if( DOWN_FILTER_LEAF > 0 ){
			if( filterCount > 0 ){
				sor.setInputCloud ( cloudPtrs[filterCount] );
			}else{
				sor.setInputCloud ( cloud );
			}
			filterCount++;
			sor.filter ( *cloudPtrs[filterCount] );
		}
		// 平面抽出
		//auto inliers = segmentate( cloud_down_filtered, 0.001 ); //大きいほどアバウトに検出
		//auto cloud_plane_filtered = filter( cloud_down_filtered, inliers, true );
		//inliers = segmentate( cloud_plane_filtered, 0.001 ); //大きいほどアバウトに検出
		// 格納されている順番に赤く着色
		//redIteration( *cloud_down_filtered );
		// 赤色を検出して緑色に変換
		//redDetection( *cloudPtrs[filterCount] );
		dst = cloudPtrs[filterCount]->makeShared();
	}
}
Пример #3
0
    void initialize(bool toFilter) {
        // A passthrough filter to remove spurious NaNs
        pass.setInputCloud(cloud);
        pass.filter(*cloud_filtered);
        std::cout << "PointCloud after pass through has: " <<
                cloud_filtered->points.size() << " data points." << std::endl;


        // Downsample the dataset using a leaf size of 1cm
        // After filtering the point cloud, all indices do not point to the
        // original points. Therefore disable this if called from initializeFromPointCLoud
        if (toFilter) {
            vg.setInputCloud(cloud_filtered);
            vg.filter(*cloud_filtered);
            std::cout << "PointCloud after filtering has: " <<
                    cloud_filtered->points.size() << " data points." << std::endl;
        }
        // Estimate point normals
        ne.setInputCloud(cloud_filtered);
        ne.compute(*cloud_normals);
        ROS_INFO("%lu normals estimated", cloud_normals->points.size());
    }
Пример #4
0
    void initialize(const std::string & pcd_name) {
        // Read in the cloud data
        reader.read(pcd_name, *cloud);
        std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl;

        // pcl::io::loadPCDFile(pcd_name, *cloud);

        // A passthrough filter to remove spurious NaNs
        pass.setInputCloud(cloud);
        pass.filter(*cloud_filtered);
        std::cout << "PointCloud after filtering has: " <<
                cloud_filtered->points.size() << " data points." << std::endl;

        // Downsample the dataset using a leaf size of 1cm
        vg.setInputCloud(cloud_filtered);
        vg.filter(*cloud_filtered);
        std::cout << "PointCloud after filtering has: " <<
                cloud_filtered->points.size() << " data points." << std::endl;

        // Estimate point normals
        ne.setInputCloud(cloud_filtered);
        ne.compute(*cloud_normals);
        ROS_INFO("%lu normals estimated", cloud_normals->points.size());
    }
/**
 * Filter cloud on z-axis. aka cut off cloud at given distance.
 * This method will use setKeepOrganized on the given PassThrough Filter.
 *
 * Return the filtered cloud.
 */
void 
 PointCloudOperations::filterZAxis(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out, pcl::PassThrough<pcl::PointXYZRGB> &pass,
    float zAxisFilterMin, float zAxisFilterMax)
{
  Logger logger("point_cloud_operations");

  if(cloud_in->points.size() == 0)
  {
    logger.logError("Could not filter on Z Axis. input cloud empty");
    return;
  }

  boost::posix_time::ptime s = boost::posix_time::microsec_clock::local_time();

  pass.setInputCloud(cloud_in);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(zAxisFilterMin, zAxisFilterMax);
  pass.setKeepOrganized(true);
  pass.filter(*cloud_out);

  boost::posix_time::ptime e = boost::posix_time::microsec_clock::local_time();
  logger.logTime(s, e, "filterZAxis()");
}
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);

}