void ColorHistogramMatcher::feature(
const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
const jsk_pcl_ros::ClusterPointIndices::ConstPtr& input_indices)
{
boost::mutex::scoped_lock(mutex_);
if (!reference_set_) {
NODELET_WARN("reference histogram is not available yet");
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*input_cloud, *pcl_cloud);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud (new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloudXYZRGBtoXYZHSV(*pcl_cloud, *hsv_cloud);
// compute histograms first
std::vector<std::vector<float> > histograms;
histograms.resize(input_indices->cluster_indices.size());
pcl::ExtractIndices<pcl::PointXYZHSV> extract;
extract.setInputCloud(hsv_cloud);
// for debug
jsk_pcl_ros::ColorHistogramArray histogram_array;
histogram_array.header = input_cloud->header;
for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
pcl::IndicesPtr indices (new std::vector<int>(input_indices->cluster_indices[i].indices));
extract.setIndices(indices);
pcl::PointCloud<pcl::PointXYZHSV> segmented_cloud;
extract.filter(segmented_cloud);
std::vector<float> histogram;
computeHistogram(segmented_cloud, histogram, policy_);
histograms[i] = histogram;
ColorHistogram ros_histogram;
ros_histogram.header = input_cloud->header;
ros_histogram.histogram = histogram;
histogram_array.histograms.push_back(ros_histogram);
}
all_histogram_pub_.publish(histogram_array);
// compare histograms
jsk_pcl_ros::ClusterPointIndices result;
result.header = input_indices->header;
for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
const double coefficient = bhattacharyyaCoefficient(histograms[i], reference_histogram_);
NODELET_DEBUG_STREAM("coefficient: " << i << "::" << coefficient);
if (coefficient > coefficient_thr_) {
result.cluster_indices.push_back(input_indices->cluster_indices[i]);
}
}
result_pub_.publish(result);
}
int main (int argc, char** argv)
{
//---------------------------------------------------------------------------------------------------
//-- Initialization stuff
//---------------------------------------------------------------------------------------------------
//-- Command-line arguments
float ransac_threshold = 0.02;
float hsv_s_threshold = 0.30;
float hsv_v_threshold = 0.35;
//-- Show usage
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
{
show_usage(argv[0]);
return 0;
}
if (pcl::console::find_switch(argc, argv, "--ransac-threshold"))
pcl::console::parse_argument(argc, argv, "--ransac-threshold", ransac_threshold);
else
{
std::cerr << "RANSAC theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-s-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-s-threshold", hsv_s_threshold);
else
{
std::cerr << "Saturation theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-v-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-v-threshold", hsv_v_threshold);
else
{
std::cerr << "Value theshold not specified, using default value..." << std::endl;
}
//-- Get point cloud file from arguments
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
if (filenames.size() != 1)
{
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames.size() != 1)
{
show_usage(argv[0]);
return -1;
}
file_is_pcd = true;
}
//-- Load point cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Load point cloud data (with color)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Print arguments to user
std::cout << "Selected arguments: " << std::endl
<< "\tRANSAC threshold: " << ransac_threshold << std::endl
<< "\tColor point threshold: " << hsv_s_threshold << std::endl
<< "\tColor region threshold: " << hsv_v_threshold << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//--------------------------------------------------------------------------------------------------------
//-- Program does actual work from here
//--------------------------------------------------------------------------------------------------------
Debug debug;
debug.setAutoShow(false);
debug.setEnabled(false);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(source_cloud_color, Debug::COLOR_ORIGINAL);
debug.show("Original with color");
//-- Downsample the dataset prior to plane detection (using a leaf size of 1cm)
//-----------------------------------------------------------------------------------
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(source_cloud);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f);
voxel_grid.filter(*cloud_filtered);
std::cout << "Initially PointCloud has: " << source_cloud->points.size () << " data points." << std::endl;
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
//-- Detect all possible planes
//-----------------------------------------------------------------------------------
std::vector<pcl::ModelCoefficientsPtr> all_planes;
pcl::SACSegmentation<pcl::PointXYZ> ransac_segmentation;
ransac_segmentation.setOptimizeCoefficients(true);
ransac_segmentation.setModelType(pcl::SACMODEL_PLANE);
ransac_segmentation.setMethodType(pcl::SAC_RANSAC);
ransac_segmentation.setDistanceThreshold(ransac_threshold);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr current_plane(new pcl::ModelCoefficients);
int i=0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
ransac_segmentation.setInputCloud(cloud_filtered);
ransac_segmentation.segment(*inliers, *current_plane);
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::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
//-- Save plane
pcl::ModelCoefficients::Ptr copy_current_plane(new pcl::ModelCoefficients);
*copy_current_plane = *current_plane;
all_planes.push_back(copy_current_plane);
//-- Debug stuff
debug.setEnabled(false);
debug.plotPlane(*current_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(cloud_filtered, Debug::COLOR_RED);
debug.show("Plane segmentation");
}
//-- Filter planes to obtain garment plane
//-----------------------------------------------------------------------------------
pcl::ModelCoefficients::Ptr garment_plane(new pcl::ModelCoefficients);
float min_height = FLT_MAX;
pcl::PointXYZ garment_projected_center;
for(int i = 0; i < all_planes.size(); i++)
{
//-- Check orientation
Eigen::Vector3f normal_vector(all_planes[i]->values[0],
all_planes[i]->values[1],
all_planes[i]->values[2]);
normal_vector.normalize();
Eigen::Vector3f good_orientation(0, -1, -1);
good_orientation.normalize();
std::cout << "Checking vector with dot product: " << std::abs(normal_vector.dot(good_orientation)) << std::endl;
if (std::abs(normal_vector.dot(good_orientation)) >= 0.9)
{
//-- Check "height" (height is defined in the local frame of reference in the yz direction)
//-- With this frame, it is approximately equal to the norm of the vector OO' (being O the
//-- center of the old frame and O' the projection of that center onto the plane).
//-- Project center point onto given plane:
pcl::PointCloud<pcl::PointXYZ>::Ptr center_to_be_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
center_to_be_projected_cloud->points.push_back(pcl::PointXYZ(0,0,0));
pcl::PointCloud<pcl::PointXYZ>::Ptr center_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> project_inliners;
project_inliners.setModelType(pcl::SACMODEL_PLANE);
project_inliners.setInputCloud(center_to_be_projected_cloud);
project_inliners.setModelCoefficients(all_planes[i]);
project_inliners.filter(*center_projected_cloud);
pcl::PointXYZ projected_center = center_projected_cloud->points[0];
Eigen::Vector3f projected_center_vector(projected_center.x, projected_center.y, projected_center.z);
float height = projected_center_vector.norm();
if (height < min_height)
{
min_height = height;
*garment_plane = *all_planes[i];
garment_projected_center = projected_center;
}
}
}
if (!(min_height < FLT_MAX))
{
std::cerr << "Garment plane not found!" << std::endl;
return -3;
}
else
{
std::cout << "Found closest plane with h=" << min_height << std::endl;
//-- Debug stuff
debug.setEnabled(true);
debug.plotPlane(*garment_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(source_cloud, Debug::COLOR_RED);
debug.show("Garment plane");
}
//-- Reorient cloud to origin (with color point cloud)
//-----------------------------------------------------------------------------------
//-- Translating to center
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f translation_transform = Eigen::Affine3f::Identity();
translation_transform.translation() << -garment_projected_center.x, -garment_projected_center.y, -garment_projected_center.z;
//pcl::transformPointCloud(*source_cloud_color, *centered_cloud, translation_transform);
//-- Orient using the plane normal
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f normal_vector(garment_plane->values[0], garment_plane->values[1], garment_plane->values[2]);
//-- Check normal vector orientation
if (normal_vector.dot(Eigen::Vector3f::UnitZ()) >= 0 && normal_vector.dot(Eigen::Vector3f::UnitY()) >= 0)
normal_vector = -normal_vector;
Eigen::Quaternionf rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(normal_vector, Eigen::Vector3f::UnitZ());
//pcl::transformPointCloud(*centered_cloud, *oriented_cloud, Eigen::Vector3f(0,0,0), rotation_quaternion);
Eigen::Transform<float, 3, Eigen::Affine> t(rotation_quaternion * translation_transform);
pcl::transformPointCloud(*source_cloud_color, *oriented_cloud, t);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform1.txt"), translation_transform, rotation_quaternion);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_cloud, Debug::COLOR_GREEN);
debug.show("Oriented");
//-- Filter points under the garment table
//-----------------------------------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr garment_table_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> passthrough_filter;
passthrough_filter.setInputCloud(oriented_cloud);
passthrough_filter.setFilterFieldName("z");
passthrough_filter.setFilterLimits(-ransac_threshold/2.0f, FLT_MAX);
passthrough_filter.setFilterLimitsNegative(false);
passthrough_filter.filter(*garment_table_cloud);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(garment_table_cloud, Debug::COLOR_GREEN);
debug.show("Table cloud (filtered)");
//-- Color segmentation of the garment
//-----------------------------------------------------------------------------------
//-- HSV thresholding
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloudXYZRGBtoXYZHSV(*garment_table_cloud, *hsv_cloud);
for (int i = 0; i < hsv_cloud->points.size(); i++)
{
if (isnan(hsv_cloud->points[i].x) || isnan(hsv_cloud->points[i].y || isnan(hsv_cloud->points[i].z)))
continue;
if (hsv_cloud->points[i].s > hsv_s_threshold && hsv_cloud->points[i].v > hsv_v_threshold)
filtered_garment_cloud->push_back(garment_table_cloud->points[i]);
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(filtered_garment_cloud, Debug::COLOR_GREEN);
debug.show("Garment cloud");
//-- Euclidean Clustering of the resultant cloud
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(filtered_garment_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> euclidean_custering;
euclidean_custering.setClusterTolerance(0.005);
euclidean_custering.setMinClusterSize(100);
euclidean_custering.setSearchMethod(tree);
euclidean_custering.setInputCloud(filtered_garment_cloud);
euclidean_custering.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr largest_color_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
int largest_cluster_size = 0;
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back(filtered_garment_cloud->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "Found cluster of " << cloud_cluster->points.size() << " points." << std::endl;
if (cloud_cluster->points.size() > largest_cluster_size)
{
largest_cluster_size = cloud_cluster->points.size();
*largest_color_cluster = *cloud_cluster;
}
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(largest_color_cluster, Debug::COLOR_GREEN);
debug.show("Filtered garment cloud");
//-- Centering the point cloud before saving it
//-----------------------------------------------------------------------------------
//-- Find bounding box
pcl::MomentOfInertiaEstimation<pcl::PointXYZRGB> feature_extractor;
pcl::PointXYZRGB min_point_AABB, max_point_AABB;
pcl::PointXYZRGB min_point_OBB, max_point_OBB;
pcl::PointXYZRGB position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.setInputCloud(largest_color_cluster);
feature_extractor.compute();
feature_extractor.getAABB(min_point_AABB, max_point_AABB);
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
//-- Translating to center
pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f garment_translation_transform = Eigen::Affine3f::Identity();
garment_translation_transform.translation() << -position_OBB.x, -position_OBB.y, -position_OBB.z;
pcl::transformPointCloud(*largest_color_cluster, *centered_garment_cloud, garment_translation_transform);
//-- Orient using the principal axes of the bounding box
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f principal_axis_x(max_point_OBB.x - min_point_OBB.x, 0, 0);
Eigen::Quaternionf garment_rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(principal_axis_x, Eigen::Vector3f::UnitX()); //-- This transformation is wrong (I guess)
Eigen::Transform<float, 3, Eigen::Affine> t2 = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
t2.rotate(rotational_matrix_OBB.inverse());
//pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, Eigen::Vector3f(0,0,0), garment_rotation_quaternion);
pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, t2);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform2.txt"), garment_translation_transform, Eigen::Quaternionf(t2.rotation()));
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_garment_cloud, Debug::COLOR_GREEN);
debug.plotBoundingBox(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB, Debug::COLOR_YELLOW);
debug.show("Oriented garment patch");
//-- Save point cloud in file to process it in Python
pcl::io::savePCDFileBinary(argv[filenames[0]]+std::string("-output.pcd"), *oriented_garment_cloud);
return 0;
}
//####################################################################################
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
if(flag)
{
flag = false;
std::cout << "/original_pointcloud received..." << std::endl;
}
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PCLPointCloud2 cloud_filtered;
// convert from pointcloud2 to PCL
// pcl_conversions::toPCL(*cloud_msg,pcl_pc2);
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::fromPCLPointCloud2(*cloud,*hsv_cloud);
pcl::fromPCLPointCloud2(*cloud,*cloud_filtered);
std::cout << "/* point cloud size */" << cloud_filtered->size() << std::endl;
//#################################################################################### converting rgb to hsv -> xyz
double min, max, delta, r, g, b, h, s, v, x, y, z, dis_r, dis_g, dis_b, dis_w;
for (int pit = 0; pit < hsv_cloud->size() ; pit++)
{
// std::cout << "point " << pit << " : " << cloud_filtered->points[pit] << std::endl
// h=0;
// s=0;
// v=0;
r = hsv_cloud->points[pit].r;
g = hsv_cloud->points[pit].g;
b = hsv_cloud->points[pit].b;
min = r < g ? r : g;
min = min < b ? min : b;
max = r > g ? r : g;
max = max > b ? max : b;
v = max/255.0; // v
delta = max - min;
if (delta < 0.00001)
{
s = 0;
h = 0; // undefined, maybe nan?
}
if( max > 0.0 ) { // NOTE: if Max is == 0, this divide would cause a crash
s = (delta / max); // s
} else {
// if max is 0, then r = g = b = 0
// s = 0, v is undefined
s = 0.0;
h = 0.0; //NAN; // its now undefined
}
if( r >= max ) // > is bogus, just keeps compilor happy
h = ( g - b ) / delta; // between yellow & magenta
else
if( g >= max )
h = 2.0 + ( b - r ) / delta; // between cyan & yellow
else
h = 4.0 + ( r - g ) / delta; // between magenta & cyan
h *= 60.0; // degrees
if( h < 0.0 )
h += 360.0;
if (r==0)
if (g==0)
if (b==0)
h = s = v = 0.0;
if (r==254)
if (g==254)
if (b==254)
{
h = s = 0.0;
v = 1.0;
}
x = s*cos(h*PI/180.0);
y = s*sin(h*PI/180.0);
z = v;
hsv_cloud->points[pit].x = x;
hsv_cloud->points[pit].y = y;
hsv_cloud->points[pit].z = z;
// hsv_cloud->points[pit].r = (x+1.0)*255/2;
// hsv_cloud->points[pit].g = (y+1.0)*255/2;
// hsv_cloud->points[pit].b = z*255;
// the actual test
dis_r = sqrt(pow(x-0.38177621,2.0) + pow(y-0.10490212,2.0) + pow(z-0.58518914,2.0)); //red
dis_g = sqrt(pow(x+0.13,2.0) + pow(y-0.44343874,2.0) + pow(z-0.3631243,2.0)); //green
dis_b = sqrt(pow(x+.13,2.0) + pow(y+0.44343874,2.0) + pow(z-.3631243,2.0)); //blue
dis_w = sqrt(pow(x-0.0,2.0) + pow(y-0.0,2.0) + pow(z-0.7358909,2.0)); //white
// mydist = {dis_r,dis_g,dis_b,dis_w};
// std::cout << "/* message */" << mydist << std::endl;
if (dis_r<dis_b && dis_r<dis_g && dis_r<dis_w)
{
cloud_filtered->points[pit].r = 255;
cloud_filtered->points[pit].g = 0;
cloud_filtered->points[pit].b = 0;
}
else if(dis_b<dis_r && dis_b<dis_g && dis_b<dis_w)
{
cloud_filtered->points[pit].r = 0;
cloud_filtered->points[pit].g = 0;
cloud_filtered->points[pit].b = 255;
}
else if(dis_w<dis_r && dis_w<dis_g && dis_w<dis_b)
{
cloud_filtered->points[pit].r = 255;
cloud_filtered->points[pit].g = 255;
cloud_filtered->points[pit].b = 255;
}
else if(dis_g<dis_r && dis_g<dis_w && dis_g<dis_b)
{
cloud_filtered->points[pit].r = 0;
cloud_filtered->points[pit].g = 255;
cloud_filtered->points[pit].b = 0;
}
}
//#################################################################################### remove outlayers
// pcl::toPCLPointCloud2(*hsv_cloud,*cloud);
// // // Perform the actual filtering Remove outlayer, very slow !
// pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> outlayer2;
// outlayer2.setInputCloud (cloudPtr);
// outlayer2.setMeanK (8);
// outlayer2.setStddevMulThresh (.1);
// outlayer2.filter (*cloud);
// pcl::fromPCLPointCloud2(*cloud,*hsv_cloud);
//#################################################################################### add cylinders
if (cylinder_flag)
{
// Fill in the cloud data
cylinder.width = 500;
cylinder.height = 1;
cylinder.is_dense = false;
cylinder.points.resize (cylinder.width * cylinder.height);
for (size_t i = 0; i < cylinder.points.size (); ++i)
{
cylinder.points[i].x = 1.0*cos(2*PI*i/cylinder.points.size());
cylinder.points[i].y = 1.0*sin(2*PI*i/cylinder.points.size());
cylinder.points[i].z = -.02;
cylinder.points[i].r = 255;
cylinder.points[i].g = 0;
cylinder.points[i].b = 0;
}
// Fill in the cloud data
cylinder2.width = 500;
cylinder2.height = 1;
cylinder2.is_dense = false;
cylinder2.points.resize (cylinder2.width * cylinder2.height);
for (size_t i = 0; i < cylinder2.points.size (); ++i)
{
cylinder2.points[i].x = 1.0*cos(2*PI*i/cylinder2.points.size());
cylinder2.points[i].y = 1.0*sin(2*PI*i/cylinder2.points.size());
cylinder2.points[i].z = 1.02;
cylinder2.points[i].r = 0;
cylinder2.points[i].g = 0;
cylinder2.points[i].b = 255;
}
cylinder_flag = false;
}
*hsv_cloud += cylinder;
*hsv_cloud += cylinder2;
//#################################################################################### publishing results
pcl::toPCLPointCloud2(*hsv_cloud,*cloud);
// Convert to ROS data type
pcl_conversions::fromPCL(*cloud, output);
// Publish the data
pub.publish (output);
pcl::toPCLPointCloud2(*cloud_filtered,*cloud);
// Convert to ROS data type
pcl_conversions::fromPCL(*cloud, output);
// Publish the data
pub2.publish (output);
// pub.publish (output);
}
