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

in progress: differently colored batches

Browse Source
This commit is contained in:
caroline 2022-04-27 16:45:01 +02:00
parent 4f9d5d2721
commit 1a8ccab6da
2 changed files with 65 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
pcl_methods.odt
View File

Binary file not shown.

81
src/initial_processing.cpp
View File

@ -26,7 +26,7 @@
#include <pcl/kdtree/kdtree_flann.h> // to use the kdtree search in pcl
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
/*
@ -62,12 +62,13 @@ float passz_max = 5.0;
float voxel_size = 0.02;
float Pseg_dist = 0.025;
int green_thresh = 150;
bool cluster_on = false;
bool cluster_on = true;
typedef pcl::PointXYZRGB PointT;
void callback(pcl_tutorial::configConfig &config, uint32_t level)
{
std::cout << "dynamic parameters have been updated";
xbpos = config.xbpos;
xbneg = config.xbneg;
ybpos = config.ybpos;
@ -113,7 +114,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
transform.rotate(Eigen::AngleAxisf((roty*M_PI) / 180, Eigen::Vector3f::UnitY()));
transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud(*cloud, *cloud, transform);
// Create filtering objects for all 3 coordinates
pcl::PassThrough<PointT> passx;
passx.setInputCloud (cloud);
@ -240,7 +241,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
// 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.setClusterTolerance (0.2); // 2cm => choose right value: too small => 1 objects appears as multiple objects
ec.setMinClusterSize (50);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
@ -249,31 +250,79 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
// 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>);
// std::vector<std::vector<pcl::PointCloud<PointT>>> cluster_vector(2, std::vector<pcl::PointCloud<PointT>>());
//std::vector<pcl::PointCloud<PointT>> cluster_vector{{}};
pcl::PointCloud<PointT> cluster_vector[cluster_indices.size ()];
//std::vector<pcl::PointCloud<PointT>> myRow(cluster_indices.size ());
//std::cout << "größe von cluster_vector: " << cluster_vector.size ();
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
int count = 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 ();
std::vector<pcl::PointCloud<PointT>> single_cluster;
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;
cloud_cluster->is_dense = true;
cluster_vector[count] = *cloud_cluster;
}
count++;
}
//std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl;
//std::cout << "size of first cluster that has been found " << cluster_indices.size() << std::endl;
std::cout << "size of first cluster that has been found " << sizeof(cluster_vector)/sizeof(cluster_vector[0]) << std::endl;
//std::cout << "size of first cluster that has been found " << cluster_vector.size() << std::endl;
int color_id = 1;
for (int i = 0; i < cluster_indices.size(); i++){
if (color_id == 1){
for (int l = 0; l < cluster_vector[i].points.size(); l++){
cluster_vector[i].points[l].r = 200;
cluster_vector[i].points[l].g = 0;
cluster_vector[i].points[l].b = 0;
}
}
if (color_id == 2){
for (int l = 0; l < cluster_vector[i].points.size(); l++){
cluster_vector[i].points[l].r = 0;
cluster_vector[i].points[l].g = 0;
cluster_vector[i].points[l].b = 200;
}
}
if (color_id == 3){
for (int l = 0; l < cluster_vector[i].points.size(); l++){
cluster_vector[i].points[l].r = 0;
cluster_vector[i].points[l].g = 200;
cluster_vector[i].points[l].b = 0;
color_id = 0;
}
}
std::cout << color_id;
color_id++;
}
pcl::PointCloud<PointT> clusters;
for (int j = 0; j < cluster_indices.size(); j++){
clusters += cluster_vector[j];
}
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);
pcl::toROSMsg(clusters, clustered_cloud); // publish all points in all rows
//pcl::toROSMsg(*cloud_cluster, clustered_cloud);
clustered_cloud.header.frame_id = "zed2i_base_link";
pub3.publish(clustered_cloud);
//}
}
// convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud, output);
pub2.publish(output);
}
int main(int argc, char **argv){