二维反光板聚类

#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;
}