#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/MarkerArray.h>
#include "adaptive_clustering/ClusterArray.h"//自定义消息类型
#include<adaptive_clustering/distance.h>
// PCL
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>//体素化滤波，对点云进行下采样，减少点云的数量数据，并保存点云的形状特征。设置滤波的参数，栅格大小。
#include <pcl/filters/passthrough.h>//直通滤波
#include <pcl/segmentation/extract_clusters.h>//根据欧式距离对点云进行分割聚类
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>//计算中心
//#define LOG
ros::Publisher cluster_array_pub_;
ros::Publisher cloud_filtered_pub_;
ros::Publisher pose_array_pub_;
ros::Publisher marker_array_pub_;
ros::Publisher distance_pub;
// ros::Publisher CluseterArray_center_y;
// adaptive_clustering::ClusterArray CluseterArray_center_x_;
// adaptive_clustering::ClusterArray CluseterArray_center_y_;
bool print_fps_;
float z_axis_min_;
float z_axis_max_;
int cluster_size_min_;
int cluster_size_max_;
const int region_max_ = 10; // 扫描的区域，Change this value to match how far you want to detect.
int regions_[100];
int frames;
clock_t start_time;
bool reset = true;//fps
void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& ros_pc2_in)
{
if(print_fps_)if(reset)
{
frames=0;
start_time=clock();//把当前时刻赋值给start time
reset=false;
}//fps FPS是图像领域中的定义，是指画面每秒传输帧数，通俗来讲就是指动画或视频的画面数。FPS是测量用于保存、显示动态视频的信息数量。
/*** Convert ROS message to PCL ***/
pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pc_in(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in);//fromROSMsg，将ros pointcloud2类型的消息转换成PCL pointcloud类型，ROS和PCL间进行点云数据的交互。
/*** Remove ground and ceiling ***/
pcl::IndicesPtr pc_indices(new std::vector<int>);// pcl::IndicesPtr 用于存储点云的索引，用于指定要操作的点云子集。
pcl::PassThrough<pcl::PointXYZI> pt;//设置直通滤波。
pt.setInputCloud(pcl_pc_in);
pt.setFilterFieldName("z");//滤除Z轴上，不在范围的点云。
pt.setFilterLimits(z_axis_min_, z_axis_max_);
pt.filter(*pc_indices);
/*** Divide the point cloud into nested circular regions *将点云划分为嵌套的圆形区域**/
boost::array<std::vector<int>, region_max_> indices_array;
//indices_array是一个数组，它用来存储每个区域的索引。它的类型是boost::array<std::vector<int>, region_max_>，
//其中region_max_是指定的最大区域数，每个索引是一个整数向量，它存储了该区域中的点的索引。用于存储每个区域的索引
for(int i = 0; i < pc_indices->size(); i++)
{
float range = 0.0;
for(int j = 0; j < region_max_; j++) // 扫描的区域
{
float d2 = pcl_pc_in->points[(*pc_indices)[i]].x * pcl_pc_in->points[(*pc_indices)[i]].x +
pcl_pc_in->points[(*pc_indices)[i]].y * pcl_pc_in->points[(*pc_indices)[i]].y;
//x2+y2 平方 cloud.points[i].x;
if(d2 > range * range && d2 <= (range+regions_[j]) * (range+regions_[j])) //如果距离点大于range的平方，小于regions_[j]的平方
{
indices_array[j].push_back((*pc_indices)[i]);
break;//跳出当前循环
//这段代码的作用是检查每个点的距离是否在指定的范围内，
//如果在指定的范围内，则将该点的索引添加到indices_array数组中。
//具体来说，range表示之前计算出的距离范围，regions_[j]表示当前正在检查的区域的半径，
//如果距离大于range* range并且小于(range + regions_[j])* (range + regions_[j])，
//则说明该点属于当前正在检查的区域，将该点的索引添加到indices_array数组中，然后退出循环，开始检查下一个点。
}
range += regions_[j];
}
}
//这段代码的作用是根据给定的点云，使用分割算法将点云分割为多个不同的区域。
//具体来说，代码中的regions_数组表示每个区域的半径，它们构成一个等差数列，
//每个点都会被分配到最适合它的区域，然后indices_array数组用来存储每个区域的索引。
/*** Euclidean clustering *欧式聚类**/
float tolerance = 0.0;
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr, Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZI>::Ptr > > clusters;
//模板参数表达式，告诉编译器vector容器 clusters，模板参数类型是std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr
// 内存分配器是 Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZI>::Ptr
///用于存储<pcl::PointCloud<pcl::PointXYZI>类型的指针的容器，用于存储将点云分割为聚类的结果。
/* aligned_allocator内存分配器，eigen中的固定大小的类使用STL容器的时候， 标准模板库（Standard Template Library，STL，
如果直接使用会出错，所谓固定大小（fixed-size）的类是指在编译过程中就已经分配好内存空间的类，
为了提高运算速度，对于SSE或者AltiVec指令集，向量化必须要求向量是以16字节即128bit对齐的方式分配内存空间，
所以针对这个问题，容器需要使用eigen自己定义的内存分配器，即aligned_allocator*/
for(int i = 0; i < region_max_; i++) {
tolerance += 0.1;
if(indices_array[i].size() > cluster_size_min_)
{
boost::shared_ptr<std::vector<int> > indices_array_ptr(new std::vector<int>(indices_array[i]));
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
tree->setInputCloud(pcl_pc_in, indices_array_ptr);
std::vector<pcl::PointIndices> cluster_indices;//获取聚类的结果
pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
//欧几里得分类，两点间的绝对距离，直线距离。
//EuclideanClusterExtraction模板类，根据不同的字段，如坐标，法向量，颜色，强度等，来计算中心，
// 并使用KDtree方法寻找邻近点，用这个类来设置滤波的参数，用extract（）来获取聚类的结果
ec.setClusterTolerance(tolerance);//设置近邻搜索的搜索半径
ec.setMinClusterSize(cluster_size_min_);//设置一个聚类需要的最少点数
ec.setMaxClusterSize(cluster_size_max_);//设置一个聚类需要的最大点数目
ec.setSearchMethod(tree); //设置点云的搜索机制
ec.setInputCloud(pcl_pc_in);//设置原始点云
ec.setIndices(indices_array_ptr);
ec.extract(cluster_indices);//从点云中提取聚类
for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++)
{//cluster_indices为提取聚类后的点云
//该循环作用：将输入点云(pcl_pc_in)中的点根据聚类指标(cluster_indices)分割成多个聚类，并将每个聚类存储到容器clusters中。
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZI>);
for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
{//pit是一个指向std::vector<int>::const_iterator类型的迭代器，用于遍历it->indices容器中的索引值。
cluster->points.push_back(pcl_pc_in->points[*pit]);
}//pcl_pc_in->points[*pit]的作用是获取输入点云(pcl_pc_in)中索引值为*pit的点，并将其存储到cluster容器中。
cluster->width = cluster->size();
cluster->height = 1;//（1） 对于无组织点云数据集，高度设置为1；（2）对于一个有组织的点云数据集，高度等于行数。
cluster->is_dense = true;//指定点云中的所有数据是有效的则为true，否则为false
clusters.push_back(cluster);
}
}
}
/*** Output ***/
if(cloud_filtered_pub_.getNumSubscribers() > 0) {
pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pc_out(new pcl::PointCloud<pcl::PointXYZI>);
sensor_msgs::PointCloud2 ros_pc2_out;
pcl::copyPointCloud(*pcl_pc_in, *pc_indices, *pcl_pc_out);
pcl::toROSMsg(*pcl_pc_out, ros_pc2_out);
cloud_filtered_pub_.publish(ros_pc2_out);
}//作用是将输入点云(pcl_pc_in)中的索引值为pc_indices的点云存储到pcl_pc_out容器中，并将其发布到话题cloud_filtered_pub_中。
adaptive_clustering::ClusterArray cluster_array;
geometry_msgs::PoseArray pose_array;
visualization_msgs::MarkerArray marker_array;
for(int i = 0; i < clusters.size(); i++) {
//该循环作用是计算容器clusters中每个聚类的质心，并将其存储到pose_array.poses容器中，最后将其发布到话题pose_array_pub_中。
if(cluster_array_pub_.getNumSubscribers() > 0) {
sensor_msgs::PointCloud2 ros_pc2_out;
pcl::toROSMsg(*clusters[i], ros_pc2_out);
cluster_array.clusters.push_back(ros_pc2_out);
}
if(pose_array_pub_.getNumSubscribers() > 0) {
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*clusters[i], centroid);
//compute3DCentroid的作用是计算输入点云(clusters[i])中每个聚类的质心，并将其存储到centroid变量中。
geometry_msgs::Pose pose;
pose.position.x = centroid[0];
pose.position.y = centroid[1];
pose.position.z = centroid[2];
pose.orientation.w = 1;//pose.orientation.w = 1的作用是设置pose变量的四元数表示的方向姿态，w为1表示姿态方向不变。
pose_array.poses.push_back(pose);
//首先计算容器clusters中每个聚类的质心centroid，
//然后将其存储到变量pose中，最后将pose变量存储到pose_array.poses容器中，并将其发布到话题pose_array_pub_中。
#ifdef LOG
Eigen::Vector4f min, max;
pcl::getMinMax3D(*clusters[i], min, max);
//getMinMax3D的作用是计算输入点云(clusters[i])中每个聚类的最小点和最大点，并将其存储到min和max变量中。
std::cerr << ros_pc2_in->header.seq << " "
<< ros_pc2_in->header.stamp << " "
<< min[0] << " "
<< min[1] << " "
<< min[2] << " "
<< max[0] << " "
<< max[1] << " "
<< max[2] << " "
<< std::endl;
//#ifdef LOG表示定义了LOG宏，
//此处的作用是将容器clusters中每个聚类的最小点和最大点的信息(min和max)
//以及输入点云的信息(ros_pc2_in->header.seq, ros_pc2_in->header.stamp)输出到标准错误输出流中(std::cerr)。
#endif
}
Eigen::Vector4f min, max,min2,max2;
pcl::getMinMax3D(*clusters[0], min, max);
pcl::getMinMax3D(*clusters[1], min2, max2);
// std::cerr<<(max[0]+min[0])/2.0<<(max[1]+min[1])/2.0<<std::endl;//y的最大小，0是x,1是Y，
adaptive_clustering::distance distance_; //这里是自定义消息。
distance_.name="distance";
distance_.distance_x=(max[0]+min[0])/2.0;
distance_.distance_y=(max[1]+min[1])/2.0;
distance_.distance_x2=(max2[0]+min2[0])/2.0;
distance_.distance_y2=(max2[1]+min2[1])/2.0;
distance_pub.publish(distance_);
visualization_msgs::Marker marker;
marker.header = ros_pc2_in->header;
marker.ns = "adaptive_clustering";
marker.id = i;
marker.type = visualization_msgs::Marker::LINE_LIST;
// geometry_msgs::Point p[24];
// p[0].x = max[0]; p[0].y = max[1]; p[0].z = max[2];
// p[1].x = min[0]; p[1].y = max[1]; p[1].z = max[2];
// p[2].x = max[0]; p[2].y = max[1]; p[2].z = max[2];
// p[3].x = max[0]; p[3].y = min[1]; p[3].z = max[2];
// p[4].x = max[0]; p[4].y = max[1]; p[4].z = max[2];
// p[5].x = max[0]; p[5].y = max[1]; p[5].z = min[2];
// p[6].x = min[0]; p[6].y = min[1]; p[6].z = min[2];
// p[7].x = max[0]; p[7].y = min[1]; p[7].z = min[2];
// p[8].x = min[0]; p[8].y = min[1]; p[8].z = min[2];
// p[9].x = min[0]; p[9].y = max[1]; p[9].z = min[2];
// p[10].x = min[0]; p[10].y = min[1]; p[10].z = min[2];
// p[11].x = min[0]; p[11].y = min[1]; p[11].z = max[2];
// p[12].x = min[0]; p[12].y = max[1]; p[12].z = max[2];
// p[13].x = min[0]; p[13].y = max[1]; p[13].z = min[2];
// p[14].x = min[0]; p[14].y = max[1]; p[14].z = max[2];
// p[15].x = min[0]; p[15].y = min[1]; p[15].z = max[2];
// p[16].x = max[0]; p[16].y = min[1]; p[16].z = max[2];
// p[17].x = max[0]; p[17].y = min[1]; p[17].z = min[2];
// p[18].x = max[0]; p[18].y = min[1]; p[18].z = max[2];
// p[19].x = min[0]; p[19].y = min[1]; p[19].z = max[2];
// p[20].x = max[0]; p[20].y = max[1]; p[20].z = min[2];
// p[21].x = min[0]; p[21].y = max[1]; p[21].z = min[2];
// p[22].x = max[0]; p[22].y = max[1]; p[22].z = min[2];
// p[23].x = max[0]; p[23].y = min[1]; p[23].z = min[2];
// for(int i = 0; i < 24; i++) {
// marker.points.push_back(p[i]);
// }
geometry_msgs::Point p[4];
p[0].x = max[0]; p[0].y = max[1]; p[0].z = 0;
p[1].x = min[0]; p[1].y = max[1]; p[1].z = 0;
p[2].x = min[0]; p[2].y = min[1]; p[2].z = 0;
p[3].x = max[0]; p[3].y = min[1]; p[3].z = 0;
for(int i = 0; i < 4; i++) {
marker.points.push_back(p[i]); }
//}0123分别代表什么？
//
//0123分别代表四个点：左上、右上、左下、右下。
// 这段代码是用来绘制立方体的，它从min和max数组中获取立方体的最大最小点，然后将这些点添加到marker的points变量中，
// 以便在RViz中绘制立方体。因为立方体的边数是6，每条边需要4个点，所以总共需要24个点。
marker.scale.x = 0.02;
marker.color.a = 1.0;//alpha通道,表示透明度,取值范围[0,1]
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.5;
marker.lifetime = ros::Duration(0.1);
marker_array.markers.push_back(marker);
}
if(cluster_array.clusters.size()) {
cluster_array.header = ros_pc2_in->header;
cluster_array_pub_.publish(cluster_array);
}
if(pose_array.poses.size()) {
pose_array.header = ros_pc2_in->header;
pose_array_pub_.publish(pose_array);
}
if(marker_array.markers.size()) {
marker_array_pub_.publish(marker_array);
}
if(print_fps_)if(++frames>10)
{
std::cerr<<"[adaptive_clustering] fps = "<<float(frames)/(float(clock()-start_time)/CLOCKS_PER_SEC)
<<", timestamp = "<<clock()/CLOCKS_PER_SEC<<std::endl;reset = true;
}//fps
}
/******************************************************************************************************************************************************/
int main(int argc, char **argv)
{
ros::init(argc, argv, "adaptive_clustering");
/*** Subscribers ***/
ros::NodeHandle nh;
ros::Subscriber point_cloud_sub = nh.subscribe<sensor_msgs::PointCloud2>("/points_out", 100, pointCloudCallback);
distance_pub=nh.advertise<adaptive_clustering::distance>("/distance",100);
/*** Publishers ***/
ros::NodeHandle private_nh("~");
cluster_array_pub_ = private_nh.advertise<adaptive_clustering::ClusterArray>("clusters", 100);
cloud_filtered_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("cloud_filtered", 100);
pose_array_pub_ = private_nh.advertise<geometry_msgs::PoseArray>("poses", 100);
marker_array_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("markers", 100);
/*** Parameters ***/
std::string sensor_model;
private_nh.param<std::string>("sensor_model", sensor_model, "VLP-16"); // VLP-16, HDL-32E, HDL-64E
private_nh.param<bool>("print_fps", print_fps_, false);
private_nh.param<float>("z_axis_min", z_axis_min_, -0.8);
private_nh.param<float>("z_axis_max", z_axis_max_, 2.0);
private_nh.param<int>("cluster_size_min", cluster_size_min_, 3);
private_nh.param<int>("cluster_size_max", cluster_size_max_, 2200000);
// Divide the point cloud into nested circular regions centred at the sensor.
// For more details, see our IROS-17 paper "Online learning for human classification in 3D LiDAR-based tracking"
if(sensor_model.compare("VLP-16") == 0)
{
regions_[0] = 2; regions_[1] = 3; regions_[2] = 3; regions_[3] = 3; regions_[4] = 3;
regions_[5] = 3; regions_[6] = 3; regions_[7] = 2; regions_[8] = 3; regions_[9] = 3;
regions_[10]= 3; regions_[11]= 3; regions_[12]= 3; regions_[13]= 3;
} else if (sensor_model.compare("HDL-32E") == 0) {
regions_[0] = 4; regions_[1] = 5; regions_[2] = 4; regions_[3] = 5; regions_[4] = 4;
regions_[5] = 5; regions_[6] = 5; regions_[7] = 4; regions_[8] = 5; regions_[9] = 4;
regions_[10]= 5; regions_[11]= 5; regions_[12]= 4; regions_[13]= 5;
} else if (sensor_model.compare("HDL-64E") == 0) {
regions_[0] = 14; regions_[1] = 14; regions_[2] = 14; regions_[3] = 15; regions_[4] = 14;
} else if (sensor_model.compare("Single-Line") == 0) {
regions_[0] = 2;
}else {
ROS_FATAL("Unknown sensor model!");
}
ros::spin();
return 0;
}
09-21
1773
1773
“相关推荐”对你有帮助么？
-
非常没帮助 -
没帮助 -
一般 -
有帮助 -
非常有帮助
提交
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
