andrew/pcl_tutorial
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

clustering in progress

Browse Source
This commit is contained in:
caroline 2022-04-26 17:26:32 +02:00
parent ff7823c83f
commit 4f9d5d2721
2 changed files with 59 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cfg/config.cfg
View File

@ -35,4 +35,6 @@ gen.add("Pseg_dist", double_t, 0, "Point distance for planar segmentation", 0
gen.add("green_thresh", int_t, 0, "Green threshold for color filtering", 150, 0 , 255)
gen.add("cluster_on", bool_t, 0, "determines if Euclidean Clustering is applied", False)
exit(gen.generate(PACKAGE, "pcl_tutorial", "config"))

123
src/initial_processing.cpp
View File

@ -16,7 +16,6 @@
#include <pcl/filters/passthrough.h> // used for passthrough filtering
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h> // for plane segmentation
@ -26,6 +25,7 @@
#include <pcl/filters/conditional_removal.h> // for color filtering
#include <pcl/kdtree/kdtree_flann.h> // to use the kdtree search in pcl
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/normal_3d.h>
@ -36,6 +36,7 @@ TBD: get to know typedef such as stringstream
// define publisher
ros::Publisher pub;
ros::Publisher pub2;
ros::Publisher pub3;
ros::Publisher marker_pub;
ros::Publisher marker_pub2;
ros::Publisher visual;
@ -61,6 +62,7 @@ float passz_max = 5.0;
float voxel_size = 0.02;
float Pseg_dist = 0.025;
int green_thresh = 150;
bool cluster_on = false;
typedef pcl::PointXYZRGB PointT;
@ -87,6 +89,7 @@ void callback(pcl_tutorial::configConfig &config, uint32_t level)
voxel_size = config.voxel_size;
Pseg_dist = config.Pseg_dist;
green_thresh = config.green_thresh;
cluster_on = config.cluster_on;
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
@ -180,40 +183,41 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
line_list.points.push_back(p8); line_list.points.push_back(p7);
marker_pub2.publish(line_list);
pcl::VoxelGrid<PointT> vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well
vox.setInputCloud (cloud); // Set input cloud
vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel [m]
// Create the filtering object: downsample data by using a predefined leaf size
pcl::VoxelGrid<PointT> vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well
vox.setInputCloud (cloud); // Set input cloud
vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel [m]
vox.filter (*cloud);
// Planar segmentation
// coefficients: used to show the contents of inliers set together with estimated plane parameters
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// coefficients: used to show the contents of inliers set together with estimated plane parameters
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory: set model and method type
// distance threshold defines how close a point has to be to the model to be considered an inlier
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC); // ransac is a simple and robust method
// seg.setMaxIterations (100);
seg.setDistanceThreshold (Pseg_dist);
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0){
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
std::cout << "Could not estimate a planar model";
//return (-1);
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
}
//Extraction step of the inliers
// Extraction step of the inliers from input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false); // double check if this is the correct value
// Get points associated with the planar model
extract.filter(*cloud);
// Remove planar inliers & extract the rest
extract.setNegative (true);
//extract.filter(*cloud);
pcl::PointCloud<PointT> cloudF;
extract.filter(cloudF);
cloud->swap(cloudF);
extract.filter(*cloud);
// color filtering
// Building the condition for points to remain in cloud
@ -227,62 +231,42 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
// apply the filter
color_filter.filter(*cloud);
if (cluster_on == true){
// Creating the KdTree object for the search method of the extraction
// https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud);
// Vector of point indices containing 1 instance of point indices for each detected cluster
// cluster_indices[0] contains all indices of the 1st cluster in the point cloud
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm => choose right value: too small => 1 objects appears as multiple objects
ec.setMinClusterSize (50);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices); // indices are saved in cluster_indices
// seperating each cluster out of vector<PointIndices>
// => iterate through cluster_indices, create new PointCloud for each entry and write all points of respective cluster into that PointCloud
//int j = 0;
//for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
/* // Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.2); // 2cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
ROS_INFO_STREAM(cluster_indices.size()<<" clusters extracted ");
Eigen::Vector4f centroidL;
Eigen::Vector4f minL;
Eigen::Vector4f maxL;
Eigen::Vector4f centroidR;
Eigen::Vector4f minR;
Eigen::Vector4f maxR;
bool first_Lcentroid_detected = false;
bool first_Rcentroid_detected = false;
Eigen::Vector4f centroidCENT;
Eigen::Vector4f minCENT;
Eigen::Vector4f maxCENT;
bool first_centroid_detected = false;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<PointT>::Ptr cloud_clusterL (new pcl::PointCloud<PointT>);
for (const auto& idx : it->indices){
cloud_clusterL->push_back ((*cloud)[idx]); //*
cloud_clusterL->width = cloud_clusterL->size ();
cloud_clusterL->height = 1;
cloud_clusterL->is_dense = true;
Eigen::Vector4f centroid;
Eigen::Vector4f min;
Eigen::Vector4f max;
pcl::compute3DCentroid (*cloud_clusterL, centroid);
pcl::getMinMax3D (*cloud_clusterL, min, max);
}
if (first_centroid_detected){
if (abs(centroid[1]) < abs(centroidCENT[1])){
centroidCENT = centroid;
minCENT = min;
maxCENT = max;
for (const auto& idx : it->indices){
cloud_cluster->push_back ((*cloud)[idx]);
cloud_cluster->width = cloud_cluster->size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
}
}
else{
centroidCENT = centroid;
minCENT = min;
maxCENT = max;
first_centroid_detected = true;
}
} */
std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl;
sensor_msgs::PointCloud2 clustered_cloud;
//clustered_cloud.header.frame_id = 1;
pcl::toROSMsg(*cloud_cluster, clustered_cloud);
pub3.publish(clustered_cloud);
//}
}
// convert to ROS data type
sensor_msgs::PointCloud2 output;
@ -311,6 +295,7 @@ int main(int argc, char **argv){
// create publisher for output processed point cloud
pub = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
pub2 = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
pub3 = n.advertise<sensor_msgs::PointCloud2>("clustered_cloud", 1);
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);