diff --git a/pcl_methods.odt b/pcl_methods.odt index b00a985..951ea26 100644 Binary files a/pcl_methods.odt and b/pcl_methods.odt differ diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index a83595e..d137d95 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -26,7 +26,7 @@ #include // to use the kdtree search in pcl #include #include - +#include /* @@ -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 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 cluster_indices; pcl::EuclideanClusterExtraction 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 // => 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::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ - std::vector::const_iterator it = cluster_indices.begin (); - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - + + + // std::vector>> cluster_vector(2, std::vector>()); + //std::vector> cluster_vector{{}}; + pcl::PointCloud cluster_vector[cluster_indices.size ()]; + //std::vector> myRow(cluster_indices.size ()); + //std::cout << "größe von cluster_vector: " << cluster_vector.size (); + + + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + int count = 0; + + for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ + //std::vector::const_iterator it = cluster_indices.begin (); + std::vector> 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 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){