pcl::PCLPointCloud2Ptr PlanSegmentor::passthrough_filter(pcl::PCLPointCloud2Ptr p_input,
double p_min_distance,
double p_max_distance)
{
pcl::PassThrough<pcl::PCLPointCloud2> pt_filter;
pt_filter.setFilterFieldName ("z");
pt_filter.setFilterLimits (p_min_distance, p_max_distance);
pt_filter.setKeepOrganized (false);
pt_filter.setInputCloud (p_input);
pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2);
pt_filter.filter (*cloud_filtered);
//added by JeanJean
pt_filter.setInputCloud(cloud_filtered);
pt_filter.setFilterFieldName("x");
pt_filter.setFilterLimits(-1.0, 1.0);
pcl::PCLPointCloud2::Ptr ptr_cloud_filtered_x(new pcl::PCLPointCloud2);
pt_filter.filter(*ptr_cloud_filtered_x);
pt_filter.setInputCloud(ptr_cloud_filtered_x);
pt_filter.setFilterFieldName("y");
pt_filter.setFilterLimits(-1.0, 1.0);
pcl::PCLPointCloud2::Ptr ptr_cloud_filtered_y(new pcl::PCLPointCloud2);
pt_filter.filter(*ptr_cloud_filtered_y);
/////////////////////////////////////////////////
return ptr_cloud_filtered_y;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
// Replace the path below with the path where you saved your file
reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);
std::cerr << "Cloud before filtering: " << std::endl;
std::cerr << *cloud << std::endl;
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
std::cerr << "Cloud after filtering: " << std::endl;
std::cerr << *cloud_filtered << std::endl;
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);
sor.setNegative (true);
sor.filter (*cloud_filtered);
writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);
return (0);
}
Eigen::Vector3d estimate_plane_normals(PointCloud::Ptr cloud_f)
{
PointCloud::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_f_aux (new pcl::PointCloud<pcl::PointXYZRGB>);
PointCloud::Ptr cloud_filtered_while (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_f);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.1);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_filtered);
sor.setMeanK (50);
sor.setStddevMulThresh (2);
sor.filter (*cloud_filtered);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
cloud_filtered_while =cloud_filtered;
int i = 0, nr_points = (int) cloud_filtered_while->points.size ();
// While 30% of the original cloud is still there
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
while (cloud_filtered_while->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered_while);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered_while);
extract.setIndices (inliers);
// Remove the planar inliers, extract the rest
extract.setNegative (false);
extract.filter (*cloud_plane);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f_aux);
cloud_filtered_while.swap (cloud_f_aux);
i++;
}
Eigen::Vector3d plane_normal_vector ;
for(int i=0;i<3;i++)
plane_normal_vector(i) = coefficients->values[i];
return plane_normal_vector;
}
int
main (int argc, char** argv)
{
if (argc != 3)
{
std::cerr << "please provide filename followed by leaf size (e.g. cloud.pcd 0.01)" << std::endl;
exit(0);
}
sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2 ());
sensor_msgs::PointCloud2::Ptr cloud_filtered (new sensor_msgs::PointCloud2 ());
float leafSize = atof(argv[2]);
//Fill in the cloud data
pcl::PCDReader reader;
// Replace the path below with the path where you saved your file
reader.read (argv[1], *cloud); // Remember to download the file first!
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ").";
// Create the filtering object
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
pcl::PCDWriter writer;
writer.write ("filter_out.pcd", *cloud_filtered,
Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
return (0);
}
// CALLBACKS
void pointcloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Create the filtering objects
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName (fieldName_);
if(numBands_ <= 1){
// Just center it to be nice
pass.setFilterLimits ((startPoint_+endPoint_)/2.0, (startPoint_+endPoint_)/2.0+bandWidth_);
pass.filter(*cloud_filtered);
*output_cloud = *cloud_filtered;
} else {
// Do the first one manually so that certain parameters like frame_id always match
pass.setFilterLimits (startPoint_, startPoint_+bandWidth_);
pass.filter(*cloud_filtered);
*output_cloud = *cloud_filtered;
float dL = endPoint_-startPoint_;
for(int i = 1; i < numBands_; i++){
float sLine = startPoint_ + i*dL/(float)(numBands_-1);
float eLine = sLine + bandWidth_;
pass.setFilterLimits (sLine, eLine);
pass.filter(*cloud_filtered);
*output_cloud += *cloud_filtered;
}
}
cloudout_pub_.publish(*output_cloud);
}
void voxelGridFilter( pcl::PointCloud<PointType>::Ptr cloud )
{
// フィルター前の点群の数を表示する
pcl::console::print_info( "before point clouds : %d\n",
cloud->size() );
// フィルターする範囲
// Kinect FusionはKinectのカメラ座標系で記録されるのでメートル単位
// 0.01の場合は1cm単位でフィルターする
float leaf = 0.01f;
pcl::VoxelGrid<PointType> grid;
// フィルターする範囲を設定
grid.setLeafSize( leaf, leaf, leaf );
// フィルターする点群を設定
grid.setInputCloud( cloud );
// フィルターする(新しい点群に保存する)
pcl::PointCloud<PointType>::Ptr
cloud_filtered( new pcl::PointCloud<PointType> );
grid.filter( *cloud_filtered );
// 点群を戻す
pcl::copyPointCloud( *cloud_filtered, *cloud );
// フィルター後の点群の数を表示する
pcl::console::print_info( "after point clouds : %d\n",
cloud->size() );
}
void PassThrough::filter_xyz() {
CLOG(LTRACE) <<"filter_xyz()";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_cloud_xyz.read();
if (!pass_through) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (xa, xb);
pass.setFilterLimitsNegative (negative_x);
pass.filter (*cloud_filtered);
// Set resulting cloud as input.
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (ya, yb);
pass.setFilterLimitsNegative (negative_y);
pass.filter (*cloud_filtered);
pass.setFilterFieldName ("z");
pass.setFilterLimits (za, zb);
pass.setFilterLimitsNegative (negative_z);
pass.filter (*cloud_filtered);
out_cloud_xyz.write(cloud_filtered);
} else
out_cloud_xyz.write(cloud);
}
int PCLWrapper::filter(unsigned short *depth_data, float *point_cloud_data){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(
new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
cloud->width = IMAGE_WIDTH;
cloud->height = IMAGE_HEIGHT;
cloud->points.resize(cloud->width * cloud->height);
//copy the data...? slow but for testing now.
int i=0;
const float fx_d = 1.0/5.5879981950414015e+02;
const float fy_d = 1.0/5.5874227168094478e+02;
const float cx_d = 3.1844162327317980e+02;
const float cy_d = 2.4574257294583529e+02;
unsigned short *my_depth_data = depth_data;
float *my_point_cloud_data = point_cloud_data;
double sum=0;
for(int y=0; y<IMAGE_HEIGHT; y++){
for(int x=0; x<IMAGE_WIDTH; x++){
//copy the data to the point cloud struc object.
unsigned short d_val = *my_depth_data;
float my_z = d_val*0.001f;
sum=sum+my_z;
float my_x = my_z * (x-cx_d) * fx_d;
float my_y = my_z * (y-cy_d) * fy_d;
cloud->points[i].x = my_x;
cloud->points[i].y = my_y;
cloud->points[i].z = my_z;
my_depth_data++;
i++;
}
}
// Create the filtering object
pcl::PassThrough < pcl::PointXYZ > pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 100.0);
// //pass.setFilterLimitsNegative (true);
pass.filter(*cloud_filtered);
size_t new_size = cloud_filtered->width * cloud_filtered->height;
// char buf[1024];
// sprintf(buf, "Original: %d, Filtered: %d, Ratio: %f, Sum %f \n", IMAGE_WIDTH*IMAGE_HEIGHT, new_size, ((float)new_size)/(IMAGE_WIDTH*IMAGE_HEIGHT), sum);
// __android_log_write(ANDROID_LOG_INFO, "PCL FILTER TESTING:", buf);
//save the results to the pointer
for(int i=0; i<cloud_filtered->width*cloud_filtered->height;i++){
float x = cloud_filtered->points[i].x;
float y = cloud_filtered->points[i].y;
float z = cloud_filtered->points[i].z;
*my_point_cloud_data=x;
*(my_point_cloud_data+1)=y;
*(my_point_cloud_data+2)=z;
my_point_cloud_data+=3;
}
return new_size;
}
bool PointCloudFunctions::statisticalOutlierRemovalAndSave(const cv::SparseMat &vmt, string fileName, int meanK, double stdDevMulThreshold)
{
// "Our sparse outlier removal is based on the computation of the distribution of point to neighbors distances in the input dataset.
// For each point, we compute the mean distance from it to all its neighbors. By assuming that the resulted distribution is
// Gaussian with a mean and a standard deviation, all points whose mean distances are outside an interval defined by the global
// distances mean and standard deviation can be considered as outliers and trimmed from the dataset."
PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);
// Create the filtering object
StatisticalOutlierRemoval<PointXYZI> sor;
sor.setInputCloud (cloud);
sor.setMeanK (meanK);
sor.setStddevMulThresh (stdDevMulThreshold);
sor.filter (*cloud_filtered);
cloud->clear();
cloud.reset();
bool resultRet = (pcl::io::savePCDFileASCII (fileName, *cloud_filtered) >= 0); //FIXME: what is the return value? it's not mentioned in http://docs.pointclouds.org/1.6.0/group__io.html#ga5e406a5854fa8ad026cad85158fef266
cloud_filtered->clear();
cloud_filtered.reset();
return resultRet;
}
bool PointCloudFunctions::radiusOutlierRemovalAndSave(const cv::SparseMat &vmt, string fileName, double radius, int minNeighbors)
{
// "The user specifies a number of neighbors which every indice must have within a specified radius to remain in the PointCloud."
PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);
// Create the filtering object
RadiusOutlierRemoval <PointXYZI> ror;
ror.setInputCloud(cloud);
ror.setRadiusSearch (radius);
ror.setMinNeighborsInRadius (minNeighbors);
ror.setNegative (true);
ror.filter (*cloud_filtered);
cloud->clear();
cloud.reset();
bool resultRet = (pcl::io::savePCDFileASCII (fileName, *cloud_filtered) >= 0); //FIXME: what is the return value? it's not mentioned in http://docs.pointclouds.org/1.6.0/group__io.html#ga5e406a5854fa8ad026cad85158fef266
cloud_filtered->clear();
cloud_filtered.reset();
return resultRet;
}
int
main (int argc, char** argv)
{
pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
// Fill in the cloud data
pcl::PCDReader reader;
// Replace the path below with the path where you saved your file
reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ").";
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
pcl::PCDWriter writer;
writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered,
Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
return (0);
}
int main (int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
if (argc != 2) {
PCL_ERROR ("Syntax: %s input.pcd\n", argv[0]);
return -1;
}
pcl::io::loadPCDFile (argv[1], *cloud);
std::string inputfile = argv[1];
std::cout << "Loaded "
<< cloud->width * cloud->height
<< " data points from " << inputfile << "."
<< std::endl;
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud);
sor.setMeanK (100);
sor.setStddevMulThresh(1.0);
sor.filter (*cloud_filtered);
std::string delimiter = ".pcd";
std::string outfile_inliers = "inliersCloud" + inputfile.substr(inputfile.find(delimiter) - 1, inputfile.find('\0'));
pcl::io::savePCDFileASCII (outfile_inliers, *cloud_filtered);
std::cerr << "Saved "
<< cloud_filtered->width * cloud_filtered->height
<< " data points to " << outfile_inliers << "."
<< std::endl;
return (0);
}
int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
// Replace the path below with the path where you saved your file
reader.read<pcl::PointXYZ> (argv[1], *cloud);
std::cerr << "Cloud before filtering: " << std::endl;
std::cerr << *cloud << std::endl;
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setMeanK (400);
sor.setStddevMulThresh (3.5);
sor.filter (*cloud_filtered);
std::cerr << "Cloud after filtering: " << std::endl;
std::cerr << *cloud_filtered << std::endl;
//pcl::PCDWriter writer;
//writer.write<pcl::PointXYZ> (argv[2], *cloud_filtered, false);
pcl::io::savePCDFileBinary(argv[2], *cloud_filtered);
return (0);
}
int main (int argc, char** argv){
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_removed (new pcl::PointCloud<pcl::PointXYZ> ());
// Fill in the cloud data
cloud->width = 5;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
}
std::cerr << "Cloud before filtering: " << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
std::cerr << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl;
// Create the filtering object
pcl::PassThrough<pcl::PointXYZ> pass(true);
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
pcl::IndicesConstPtr indices = pass.getRemovedIndices();
// std::cerr<<"n_indices: "<<indices->size()<<std::endl;
// std::cerr<<"indices: ";
// for(unsigned int i=0;i<indices->size();i++)
// std::cerr<<(*indices)[i]<<"\t";
// std::cerr<<std::endl;
pcl::PointIndices::Ptr removed_indices (new pcl::PointIndices());
removed_indices->indices = *indices;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices (removed_indices);
extract.setNegative (false);
extract.filter (*cloud_removed);
std::cerr <<std::endl<< "Cloud after filtering: " << std::endl;
for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
std::cerr << cloud_filtered->points[i].x << " "
<< cloud_filtered->points[i].y << " "
<< cloud_filtered->points[i].z << std::endl;
std::cerr <<std::endl<< "Cloud being removed: " << std::endl;
for (size_t i = 0; i < cloud_removed->points.size (); ++i)
std::cerr << cloud_removed->points[i].x << " "
<< cloud_removed->points[i].y << " "
<< cloud_removed->points[i].z << std::endl;
return (0);
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
// Build a filter to remove spurious NaNs
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.1);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: "
<< cloud_filtered->points.size () << " data points." << std::endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// 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_filtered);
seg.segment (*inliers, *coefficients);
std::cerr << "PointCloud after segmentation has: "
<< inliers->indices.size () << " inliers." << std::endl;
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::cerr << "PointCloud after projection has: "
<< cloud_projected->points.size () << " data points." << std::endl;
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (cloud_projected);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
std::cerr << "Concave hull has: " << cloud_hull->points.size ()
<< " data points." << std::endl;
pcl::PCDWriter writer;
writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);
return (0);
}
QList <pcl::cloud_composer::CloudComposerItem*>
pcl::cloud_composer::VoxelGridDownsampleTool::performAction (ConstItemList input_data, PointTypeFlags::PointType)
{
QList <CloudComposerItem*> output;
const CloudComposerItem* input_item;
// Check input data length
if ( input_data.size () == 0)
{
qCritical () << "Empty input in VoxelGridDownsampleTool!";
return output;
}
else if ( input_data.size () > 1)
{
qWarning () << "Input vector has more than one item in VoxelGridDownsampleTool";
}
input_item = input_data.value (0);
if (input_item->type () == CloudComposerItem::CLOUD_ITEM)
{
double leaf_x = parameter_model_->getProperty("Leaf Size x").toDouble ();
double leaf_y = parameter_model_->getProperty("Leaf Size y").toDouble ();
double leaf_z = parameter_model_->getProperty("Leaf Size z").toDouble ();
pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> ();
//////////////// THE WORK - FILTERING OUTLIERS ///////////////////
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> vox_grid;
vox_grid.setInputCloud (input_cloud);
vox_grid.setLeafSize (float (leaf_x), float (leaf_y), float (leaf_z));
//Create output cloud
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2);
//Filter!
vox_grid.filter (*cloud_filtered);
//////////////////////////////////////////////////////////////////
//Get copies of the original origin and orientation
Eigen::Vector4f source_origin = input_item->data (ItemDataRole::ORIGIN).value<Eigen::Vector4f> ();
Eigen::Quaternionf source_orientation = input_item->data (ItemDataRole::ORIENTATION).value<Eigen::Quaternionf> ();
//Put the modified cloud into an item, stick in output
CloudItem* cloud_item = new CloudItem (input_item->text () + tr (" vox ds")
, cloud_filtered
, source_origin
, source_orientation);
output.append (cloud_item);
}
else
{
qDebug () << "Input item in VoxelGridDownsampleTool is not a cloud!!!";
}
return output;
}
//capture cloud data and process filtering
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) //this is a subfunction from a sub object under main function
{
pcl::PassThrough<pcl::PointXYZ> pass; //generate the object which contains the function of pass filtering
pcl::VoxelGrid<pcl::PointXYZ> sor; //generate the object which contains the function of voxel filtering
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); //generate the container of filtered clouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredv (new pcl::PointCloud<pcl::PointXYZ>); //generate the container of filtered voxel clouds
if (!viewer.wasStopped())
//modify pointclouds here, it's assumed it's captured and used as parameters in the function
{
pass.setInputCloud (cloud); //pcl::PassThrough<pcl::PointXYZ>
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
sor.setInputCloud (cloud_filtered);
sor.setLeafSize (0.01f, 0.01f, 0.01f);// set the density of the voxel grid
sor.filter (*cloud_filteredv);
///////////for normals
//~ std::vector<int> indices (floor (cloud_filteredv->points.size () / 10));
//~ for (size_t i = 0; indices.size (); ++i) indices[i] = i;
//~
//~ // Create the normal estimation class, and pass the input dataset to it
//~ pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
//~ ne.setInputCloud (cloud_filteredv);
//~
//~ // Pass the indices
//~ boost::shared_ptr<std::vector<int> > indicesptr (new std::vector<int> (indices));
//~ ne.setIndices (indicesptr);
//~
//~ // Create an empty kdtree representation, and pass it to the normal estimation object.
//~ // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
//~ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
//~ ne.setSearchMethod (tree);
//~
//~ // Output datasets
//~ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
//~
//~ // Use all neighbors in a sphere of radius 3cm
//~ ne.setRadiusSearch (0.03);
//~
//~ // Compute the features
//~ ne.compute (*cloud_normals);
///////////for normals
viewer.showCloud (cloud_filteredv);
}
}
pcl::PCLPointCloud2Ptr PlanSegmentor::voxelgrid_filter(const pcl::PCLPointCloud2ConstPtr p_cloud,
float p_leaf_size)
{
pcl::VoxelGrid<pcl::PCLPointCloud2> vx_grid;
vx_grid.setInputCloud (p_cloud);
vx_grid.setLeafSize (p_leaf_size, p_leaf_size, p_leaf_size);
pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2);
vx_grid.filter (*cloud_filtered);
return cloud_filtered;
}
PointCloudConstPtr ICPWrapper::downsampleCloud (PointCloudConstPtr input)
{
const double voxel_size = 0.01;
PointCloudPtr cloud_filtered (new PointCloud);
pcl17::VoxelGrid<PointType> downsampler;
downsampler.setInputCloud (input);
downsampler.setLeafSize (voxel_size, voxel_size, voxel_size);
downsampler.filter (*cloud_filtered);
return cloud_filtered;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr VoxelGrid_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::VoxelGrid<pcl::PointXYZRGB> sorvg;
sorvg.setInputCloud (point_cloud_ptr);
sorvg.setLeafSize (0.01f, 0.01f, 0.01f);
sorvg.filter(*cloud_filtered);
return cloud_filtered;
}
void cloud_cb (sensor_msgs::PointCloud2ConstPtr input) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg (*input, *cloud_filtered);
//std::cout << "Input pointCloud has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (ModelType);//(pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (MaxIterations);//(100);
seg.setDistanceThreshold (DistanceThreshold);//(0.02);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > RatioLimit * nr_points)//0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
// extract.setNegative (false);
// // Write the planar inliers to disk
// extract.filter (*cloud_plane);
// std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (ExtractNegative);
extract.filter (*cloud_f);
cloud_filtered = cloud_f;
}
sensor_msgs::PointCloud2 output;
pcl::toROSMsg (*cloud_filtered , output);
output.header.stamp = ros::Time::now();
output.header.frame_id = "/camera_rgb_optical_frame";
// Publish the data
cloud_pub.publish (output);
}
void greedy_proj () {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::io::loadPCDFile ("input.pcd", *cloud_blob);
pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);
cloud_blob = cloud_filtered;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (20);
n.compute (*normals);
// Concatenate the XYZ and normal fields
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
// Create search tree
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (1);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (10);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
// Get result
gp3.setInputCloud (cloud_with_normals);
gp3.setSearchMethod (tree2);
gp3.reconstruct (triangles);
// Additional vertex information
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
pcl::io::saveVTKFile("output.vtk", triangles);
pcl::io::savePolygonFileSTL("output.stl", triangles);
}
pcl::PointCloud<pcl::PointXYZRGB> Remove_outliers(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud);//外れ値を除去する点群を入力
sor.setMeanK(50);//MeanKを設定
sor.setStddevMulThresh(0.1);
sor.setNegative(false);//外れ値を出力する場合はtrueにする
sor.filter(*cloud_filtered);//出力
return *cloud_filtered;
}
/**
* Downsample by converting a point cloud to a voxel grid.
*/
PointCloud::Ptr makeVoxelGrid(PointCloud::Ptr cloud){
PointCloud::Ptr cloud_filtered (new PointCloud);
pcl::VoxelGrid<Point> vox;
vox.setInputCloud (cloud);
vox.setLeafSize (0.01f, 0.01f, 0.01f);
vox.filter (*cloud_filtered);
//std::cout<<"inPointCloud["<<cloud->points.size()<<"] VoxelGrid["<<cloud_filtered->points.size()<<"]"<<std::endl;
return cloud_filtered;
}
bool pass_through_filter(pcl::PointCloud<PointType>::Ptr cloud, float min_depth , float max_depth, float min_x, float max_x, float min_y, float max_y) {
// Create the filtering object
pcl::PointCloud<PointType>::Ptr cloud_filtered(new pcl::PointCloud<PointType>);
pcl::PassThrough<PointType> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (min_depth, max_depth);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
*cloud = *cloud_filtered;
return true;
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
bool update=false;
int sockfd;
boost::thread serverthread(server,&sockfd,&update, &viewer);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB> ());
float yaw, pitch,roll,posx,posy,posz,averagez;
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr = readFile("output12.csv",&averagez,&posx,&posy,&posz,&yaw,&pitch,&roll);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud_ptr);
//viewerinitial->addPointCloud<pcl::PointXYZRGB> (point_cloud_ptr, rgb, "sample cloud");
//show_device_direction(yaw, pitch, roll);
//cloud_filtered=VoxelGrid_filter(point_cloud_ptr);
//chose_command("collision",cloud_filtered);
//chose_command("identification",cloud_filtered);
while (1)
{
if(viewer->wasStopped () || viewerinitial->wasStopped ())break;
if(update){
std::cout << "cout refresh"<< std::endl;
update=false;
}
viewer->spinOnce (100);
viewerinitial->spinOnce(100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
pthread_cancel(serverthread.native_handle());
close(sockfd);
return 0;
}
void PassThrough::filter_xyzsift() {
LOG(LTRACE) <<"filter_xyzsift()";
pcl::PointCloud<PointXYZSIFT>::Ptr cloud = in_cloud_xyzsift.read();
if (!pass_through) {
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_filtered (new pcl::PointCloud<PointXYZSIFT>);
applyFilter(cloud, *cloud_filtered, "x", xa, xb, negative_x);
applyFilter(cloud_filtered, *cloud_filtered, "y", ya, yb, negative_y);
applyFilter(cloud_filtered, *cloud_filtered, "z", za, zb, negative_z);
out_cloud_xyzsift.write(cloud_filtered);
} else
out_cloud_xyzsift.write(cloud);
}
/**
* Filters a point cloud to remove noise. For every point, it samples 50 (sor.setMeanK()) number
* of nearest neighbors and removes any points that are more than 1.0 std dev (sor.setStddevMulThresh())
* away from a point.
*/
PointCloud::Ptr statisticalFilter(PointCloud::Ptr cloud){
PointCloud::Ptr cloud_filtered (new PointCloud);
// Create the filtering object
pcl::StatisticalOutlierRemoval<Point> sor;
sor.setInputCloud (cloud);
sor.setMeanK (10);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
//std::cout<<"inPointCloud["<<cloud->points.size()<<"] StatisticalFilter["<<cloud_filtered->points.size()<<"]"<<std::endl;
return cloud_filtered;
}
PCPointT::Ptr PlanSegmentor::radius_outlier_removal_filter(PCPointT::Ptr p_input,
double p_radius,
int p_minNN)
{
//std::cout << "#of points before : " << cloud_input->size() << std::endl;
pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> outrem;
outrem.setInputCloud(p_input);
outrem.setRadiusSearch(p_radius);
outrem.setMinNeighborsInRadius (p_minNN);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
outrem.filter (*cloud_filtered);
// std::cout << "#of points after : " << cloud_filtered->size() << std::endl;
return cloud_filtered;
}
void addToGlobalCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in) {
/*
// Initialize global cloud if not already done so
if (!globalCloudPtr){
globalCloudPtr = cloud_in;
return;
}
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
transformCam2Robot (*cloud_in, *transformed_cloud);
// Initialize global cloud if not already done so
if (!globalCloudPtr) {
globalCloudPtr = transformed_cloud;
return;
}
/*
// Preform ICP
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputCloud(transformed_cloud);
icp.setInputTarget(globalCloudPtr);
pcl::PointCloud<pcl::PointXYZRGB> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
*globalCloudPtr += Final;
*/
//globalCloudPtr = transformed_cloud;
//return;
*globalCloudPtr += *transformed_cloud;
// Perform voxelgrid filtering
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (globalCloudPtr);
sor.setLeafSize (res, res, res);
sor.filter (*cloud_filtered);
globalCloudPtr = cloud_filtered;
}
