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]
}
/**
* 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);
}