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

colored clusters

Browse Source
This commit is contained in:
caroline 2022-05-02 14:07:50 +02:00
parent 1a8ccab6da
commit 8f4ec0bf9f
2 changed files with 57 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cfg/config.cfg
View File

@ -15,7 +15,7 @@ gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping", 5, -5, 5)
gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5) gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5)
gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis", 0, 0, 180) gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis", 0, 0, 180)
gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 45, 0, 180) gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 0, 0, 180)
gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis", 0, 0, 180) gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis", 0, 0, 180)
gen.add("movex", double_t, 0, "Move along X - Axis", 0, -10, 10) gen.add("movex", double_t, 0, "Move along X - Axis", 0, -10, 10)

107
src/initial_processing.cpp
View File

@ -40,6 +40,7 @@ ros::Publisher pub3;
ros::Publisher marker_pub; ros::Publisher marker_pub;
ros::Publisher marker_pub2; ros::Publisher marker_pub2;
ros::Publisher visual; ros::Publisher visual;
ros::Publisher steer;
float xbpos = 5.0; float xbpos = 5.0;
float xbneg = 0.0; float xbneg = 0.0;
@ -157,7 +158,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
line_list.scale.x = 0.05; line_list.scale.x = 0.05;
line_list.color.g = 1.0; line_list.color.g = 1.0;
line_list.color.a = 1.0; line_list.color.a = 1.0;
line_list.header.frame_id = "zed_base_link"; line_list.header.frame_id = "base_link";
line_list.action = visualization_msgs::Marker::ADD; line_list.action = visualization_msgs::Marker::ADD;
line_list.pose.orientation.w = 1.0; line_list.pose.orientation.w = 1.0;
geometry_msgs::Point p1; geometry_msgs::Point p2; geometry_msgs::Point p3; geometry_msgs::Point p4; geometry_msgs::Point p1; geometry_msgs::Point p2; geometry_msgs::Point p3; geometry_msgs::Point p4;
@ -246,25 +247,13 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
ec.setMaxClusterSize (25000); ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree); ec.setSearchMethod (tree);
ec.setInputCloud (cloud); ec.setInputCloud (cloud);
ec.extract (cluster_indices); // indices are saved in cluster_indices ec.extract (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;
// 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; int count = 0;
pcl::PointCloud<PointT> cluster_vector[cluster_indices.size()];
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ 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<pcl::PointCloud<PointT>> single_cluster;
for (const auto& idx : it->indices){ for (const auto& idx : it->indices){
cloud_cluster->push_back ((*cloud)[idx]); cloud_cluster->push_back ((*cloud)[idx]);
cloud_cluster->width = cloud_cluster->size (); cloud_cluster->width = cloud_cluster->size ();
@ -274,49 +263,64 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
} }
count++; count++;
} }
//std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl; visualization_msgs::Marker center_point;
//std::cout << "size of first cluster that has been found " << cluster_indices.size() << std::endl; uint32_t shape = visualization_msgs::Marker::SPHERE_LIST;
std::cout << "size of first cluster that has been found " << sizeof(cluster_vector)/sizeof(cluster_vector[0]) << std::endl; center_point.header.stamp = ros::Time::now();
//std::cout << "size of first cluster that has been found " << cluster_vector.size() << std::endl; center_point.type = shape;
int color_id = 1; center_point.action = visualization_msgs::Marker::ADD;
for (int i = 0; i < cluster_indices.size(); i++){
if (color_id == 1){ std::cout << "number of clusters that have been found: " << sizeof(cluster_vector)/sizeof(cluster_vector[0]) << std::endl;
for (int l = 0; l < cluster_vector[i].points.size(); l++){ //std::cout << "size of all cluster points: " << sizeof(cluster_vector) << std::endl;
cluster_vector[i].points[l].r = 200; //std::cout << "size of 2nd cluster: " << sizeof(cluster_vector[1]) << endl;
cluster_vector[i].points[l].g = 0; int color_id = 0;
cluster_vector[i].points[l].b = 0; //geometry_msgs::Point centroids[sizeof(cluster_vector)/sizeof(cluster_vector[0])]; //sizeof(cluster_vector)/sizeof(cluster_vector[0])
} //std::vector<geometry_msgs::Point> centroids;
Eigen::Vector4f centroid;
//colored stored in rgb_colors: red, blue, green, yellow, magenta, orange, light blue, black, purple, grey, pink, light red
int rgb_colors[12][3] = {{200,0,0},{0,0,200},{0,200,0},{255,255,0},{255,0,255},{255,165,0},{102,255,255},{0,0,0},{102,0,102},{192,192,192},{255,102,178},{255,51,51}};
for (int i = 0; i < sizeof(cluster_vector)/sizeof(cluster_vector[0]); i++){
for (int l = 0; l < cluster_vector[i].points.size(); l++){ //sizeof(cluster_vector[i])
cluster_vector[i].points[l].r = rgb_colors[color_id][0];
cluster_vector[i].points[l].g = rgb_colors[color_id][1];
cluster_vector[i].points[l].b = rgb_colors[color_id][2];
} }
if (color_id == 2){ pcl::compute3DCentroid (cluster_vector[i], centroid);
for (int l = 0; l < cluster_vector[i].points.size(); l++){ geometry_msgs::Point point;
cluster_vector[i].points[l].r = 0; point.x = centroid[0];
cluster_vector[i].points[l].g = 0; point.y = centroid[1];
cluster_vector[i].points[l].b = 200; point.z = centroid[2];
} center_point.points.push_back(point);
}
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++; color_id++;
if (color_id > sizeof(rgb_colors)/sizeof(rgb_colors[0])){
color_id = 0;
}
} }
center_point.pose.orientation.w = 1.0;
center_point.scale.x = 0.1;
center_point.scale.y = 0.1;
center_point.scale.z = 0.1;
center_point.color.r = 1.0;
center_point.color.g = 1.0;
center_point.color.b = 1.0;
center_point.color.a = 1.0;
center_point.lifetime = ros::Duration();
center_point.header.frame_id = "zed2i_base_link";
steer.publish (center_point);
pcl::PointCloud<PointT> clusters; pcl::PointCloud<PointT> clusters;
for (int j = 0; j < cluster_indices.size(); j++){ for (int j = 0; j < sizeof(cluster_vector)/sizeof(cluster_vector[0]); j++){
clusters += cluster_vector[j]; clusters += cluster_vector[j];
} }
sensor_msgs::PointCloud2 clustered_cloud; sensor_msgs::PointCloud2 clustered_cloud;
pcl::toROSMsg(clusters, clustered_cloud); // publish all points in all rows pcl::toROSMsg(clusters, clustered_cloud);
//pcl::toROSMsg(*cloud_cluster, clustered_cloud);
clustered_cloud.header.frame_id = "zed2i_base_link"; clustered_cloud.header.frame_id = "zed2i_base_link";
pub3.publish(clustered_cloud); pub3.publish(clustered_cloud);
} }
// convert to ROS data type // convert to ROS data type
@ -347,6 +351,7 @@ int main(int argc, char **argv){
pub3 = n.advertise<sensor_msgs::PointCloud2>("clustered_cloud", 1); pub3 = n.advertise<sensor_msgs::PointCloud2>("clustered_cloud", 1);
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10); marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10); marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);
steer = n.advertise<visualization_msgs::Marker>("centroid_marker", 1);
// create publisher for visualising marker // create publisher for visualising marker
visual = n.advertise<visualization_msgs::Marker> ("marker", 1); visual = n.advertise<visualization_msgs::Marker> ("marker", 1);