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; }
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(); } }
Extractor() { tree.reset(new pcl::KdTreeFLANN<Point > ()); cloud.reset(new pcl::PointCloud<Point>); cloud_filtered.reset(new pcl::PointCloud<Point>); cloud_normals.reset(new pcl::PointCloud<pcl::Normal>); coefficients_plane_.reset(new pcl::ModelCoefficients); coefficients_cylinder_.reset(new pcl::ModelCoefficients); inliers_plane_.reset(new pcl::PointIndices); inliers_cylinder_.reset(new pcl::PointIndices); // Filter Pass pass.setFilterFieldName("z"); pass.setFilterLimits(-100, 100); // VoxelGrid for Downsampling LEAFSIZE = 0.01f; vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE); // Any object < CUT_THRESHOLD will be abandoned. //CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700 CUT_THRESHOLD = 700; //for nonfiltering // Clustering cluster_.setClusterTolerance(0.06 * UNIT); cluster_.setMinClusterSize(50.0); cluster_.setSearchMethod(clusters_tree_); // Normals ne.setSearchMethod(tree); ne.setKSearch(50); // 50 by default // plane SAC seg_plane.setOptimizeCoefficients(true); seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE); seg_plane.setNormalDistanceWeight(0.1); // 0.1 seg_plane.setMethodType(pcl::SAC_RANSAC); seg_plane.setMaxIterations(1000); // 10000 seg_plane.setDistanceThreshold(0.05); // 0.03 // cylinder SAC seg_cylinder.setOptimizeCoefficients(true); seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER); seg_cylinder.setMethodType(pcl::SAC_RANSAC); seg_cylinder.setNormalDistanceWeight(0.1); seg_cylinder.setMaxIterations(10000); seg_cylinder.setDistanceThreshold(0.02); // 0.05 seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1] }
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()); }
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 ObjectDetector::processCloud(const CloudConstPtr& cloud) { cloud_pass_through_.reset(new Cloud); // Computation goes here pass_through_.setInputCloud(cloud); pass.filter(*cloud_pass_through_); // Estimate 3d convex hull pcl::ConvexHull<PointType> hr; hr.setInputCloud(cloud_pass_through_); cloud_hull_.reset(new Cloud); hr.reconstruct(*cloud_hull_, vertices_); cloud_ = cloud; new_cloud_ = true; }
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); }