velodyne16点云特征分类

将velodyne16点云特征分为平面和角点，参考lio-sam

//*************************************************************************************************
//    点云特征分类 , 订阅： points_raw
//                 发布： cloud_IRTCN_surf   cloud_IRTCN_edge
//
//    source ~/catkin_ws/devel/setup.bash &&  rosrun my_lidar_cam_fusion cloud_extract
//
//****************************************************************************************************


#include <ros/ros.h>
#include <boost/thread.hpp>

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <sensor_msgs/PointCloud2.h>

using namespace std;
using namespace pcl;

struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint16_t, ring, ring) (float, time, time)
)

struct PointXYZIRDCN {
    PCL_ADD_POINT4D;
    float intensity;
    uint8_t ring;
    float distance; //距离
    uint16_t curvature; //曲率
    float number;     //-1为平面 ， 1为角点
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRDCN,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint8_t, ring, ring) (float, distance, distance)
    (uint16_t, curvature, curvature) (float,number, number)
)

using PointXYZIRT = VelodynePointXYZIRT;
//typedef pcl::PointXYZIRTD PointType;

class cloud_extract
{

public:

  cloud_extract()
  {
      cout<< "cloud_extract  pub :cloud_IRTCN_surf  cloud_IRTCN_edge "<<endl;
      cloud_pub_surf = n.advertise<sensor_msgs::PointCloud2>("cloud_IRTCN_surf", 1);//平面点云发布
      cloud_pub_edge = n.advertise<sensor_msgs::PointCloud2>("cloud_IRTCN_edge", 1);//角点点云发布

      sub1 = n.subscribe<sensor_msgs::PointCloud2>("points_raw", 10, &cloud_extract::chatterCallback,this);


  }
  ~cloud_extract()
  {
  }
  void chatterCallback(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloud_msg);

  void divide_cloud(pcl::PointCloud<PointXYZIRT>::Ptr cloud_tmp);

  void extracted_get(pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_ring);

  void classify_get(pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_class);

  void Publishe_surf_msg( pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_surf_msg );

  void Publishe_edge_msg( pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_edge_msg );

private:
   ros::NodeHandle n;

   ros::Time time;
   ros::Subscriber sub1;
   ros::Publisher cloud_pub_surf;
   ros::Publisher cloud_pub_edge;

   pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_IRTCN_surf;
   pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_IRTCN_edge;

};


void cloud_extract::chatterCallback(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloud_msg )
{
    time=cloud_msg->header.stamp;
    pcl::PointCloud<PointXYZIRT>::Ptr cloud_tmp(new pcl::PointCloud<PointXYZIRT>);//点云对象
    pcl::fromROSMsg (*cloud_msg, *cloud_tmp);

    cloud_IRTCN_surf.reset(new pcl::PointCloud<PointXYZIRDCN>());
    cloud_IRTCN_edge.reset(new pcl::PointCloud<PointXYZIRDCN>());


    divide_cloud(cloud_tmp);

}

void cloud_extract::divide_cloud(pcl::PointCloud<PointXYZIRT>::Ptr cloud_tmp)
{
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_0(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_1(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_2(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_3(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_4(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_5(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_6(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_7(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_8(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_9(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_10(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_11(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_12(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_13(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_14(new pcl::PointCloud<PointXYZIRDCN>);
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_15(new pcl::PointCloud<PointXYZIRDCN>);

    for (std::size_t i = 0; i < cloud_tmp->size(); i++)
    {
        if(-40<cloud_tmp->points[i].y && cloud_tmp->points[i].y<40)
        {
            PointXYZIRDCN thispoint;
            thispoint.x=cloud_tmp->points[i].x;
            thispoint.y=cloud_tmp->points[i].y;
            thispoint.z=cloud_tmp->points[i].z;
            thispoint.intensity=cloud_tmp->points[i].intensity;
            thispoint.ring=cloud_tmp->points[i].ring;
            thispoint.distance=sqrt(cloud_tmp->points[i].x*cloud_tmp->points[i].x
                                    + cloud_tmp->points[i].y*cloud_tmp->points[i].y
                                    +cloud_tmp->points[i].z*cloud_tmp->points[i].z);
            if( cloud_tmp->points[i].ring==0)
                cloud_0->push_back(thispoint);
            if( cloud_tmp->points[i].ring==1)
                cloud_1->push_back(thispoint);
            if( cloud_tmp->points[i].ring==2)
                cloud_2->push_back(thispoint);
            if( cloud_tmp->points[i].ring==3)
                cloud_3->push_back(thispoint);
            if( cloud_tmp->points[i].ring==4)
                cloud_4->push_back(thispoint);
            if( cloud_tmp->points[i].ring==5)
                cloud_5->push_back(thispoint);
            if( cloud_tmp->points[i].ring==6)
                cloud_6->push_back(thispoint);
            if( cloud_tmp->points[i].ring==7)
                cloud_7->push_back(thispoint);
            if( cloud_tmp->points[i].ring==8)
                cloud_8->push_back(thispoint);
            if( cloud_tmp->points[i].ring==9)
                cloud_9->push_back(thispoint);
            if( cloud_tmp->points[i].ring==10)
                cloud_10->push_back(thispoint);
            if( cloud_tmp->points[i].ring==11)
                cloud_11->push_back(thispoint);
            if( cloud_tmp->points[i].ring==12)
                cloud_12->push_back(thispoint);
            if( cloud_tmp->points[i].ring==13)
                cloud_13->push_back(thispoint);
            if( cloud_tmp->points[i].ring==14)
                cloud_14->push_back(thispoint);
            if( cloud_tmp->points[i].ring==15)
                cloud_15->push_back(thispoint);
        }

    }
    extracted_get( cloud_0);
    extracted_get( cloud_1);
    extracted_get( cloud_2);
    extracted_get( cloud_3);
    extracted_get( cloud_4);
    extracted_get( cloud_5);
    extracted_get( cloud_6);
    extracted_get( cloud_7);
    extracted_get( cloud_8);
    extracted_get( cloud_9);
    extracted_get( cloud_10);
    extracted_get( cloud_11);
    extracted_get( cloud_12);
    extracted_get( cloud_13);
    extracted_get( cloud_14);
    extracted_get( cloud_15);

    Publishe_surf_msg(cloud_IRTCN_surf );
    Publishe_edge_msg(cloud_IRTCN_edge );


}

void cloud_extract::extracted_get(pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_ring)
{
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_class(new pcl::PointCloud<PointXYZIRDCN>);
    for (std::size_t i = 5; i < cloud_ring->size()-5; i++)
    {
        float dis=cloud_ring->points[i-5].distance+cloud_ring->points[i-4].distance
                +cloud_ring->points[i-3].distance+cloud_ring->points[i-2].distance
                +cloud_ring->points[i-1].distance-cloud_ring->points[i].distance*10
                +cloud_ring->points[i+1].distance+cloud_ring->points[i+2].distance
                +cloud_ring->points[i+3].distance+cloud_ring->points[i+4].distance
                +cloud_ring->points[i+5].distance;

        PointXYZIRDCN thispoint;
        thispoint.x=cloud_ring->points[i].x;
        thispoint.y=cloud_ring->points[i].y;
        thispoint.z=cloud_ring->points[i].z;
        thispoint.intensity=cloud_ring->points[i].intensity;
        thispoint.ring=cloud_ring->points[i].ring;
        thispoint.distance=cloud_ring->points[i].distance;
        thispoint.curvature=dis*dis;
        thispoint.number=0;
        cloud_class->push_back(thispoint);

    }
    classify_get(cloud_class);
}


void cloud_extract::classify_get(pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_class)
{
    pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_IRTCN(new pcl::PointCloud<PointXYZIRDCN>);
    *cloud_IRTCN=*cloud_class;
    for (std::size_t i = 5; i < cloud_class->size()-5; i++)
    {
        if((cloud_class->points[i].distance-cloud_class->points[i+1].distance)>0.3 && cloud_IRTCN->points[i].number==0)
        {
            cloud_IRTCN->points[i].number=1;
            cloud_IRTCN->points[i-1].number=1;
            cloud_IRTCN->points[i-2].number=1;
            cloud_IRTCN->points[i-3].number=1;
            cloud_IRTCN->points[i-4].number=1;
        }
        if((cloud_class->points[i].distance-cloud_class->points[i+1].distance)<-0.3 && cloud_IRTCN->points[i].number==0)
        {
            cloud_IRTCN->points[i].number=1;
            cloud_IRTCN->points[i+1].number=1;
            cloud_IRTCN->points[i+2].number=1;
            cloud_IRTCN->points[i+3].number=1;
            cloud_IRTCN->points[i+4].number=1;
        }
        if(cloud_class->points[i].curvature <1 && cloud_IRTCN->points[i].number==0)
            cloud_IRTCN->points[i].number=-1;
        if(cloud_class->points[i].curvature >0 && cloud_IRTCN->points[i].number==0)
            cloud_IRTCN->points[i].number=1;

    }
    for (std::size_t i = 5; i < cloud_IRTCN->size()-5; i++)
    {
        if(cloud_IRTCN->points[i].number==1)
            cloud_IRTCN_edge->push_back(cloud_IRTCN->points[i]);
        else
            cloud_IRTCN_surf->push_back(cloud_IRTCN->points[i]);

    }

}

void cloud_extract::Publishe_surf_msg( pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_surf_msg )
{
    // 发布平面点云消息
    sensor_msgs::PointCloud2 output;//发布点云话题消息
    pcl::toROSMsg(*cloud_surf_msg, output);
    output.header.stamp = time;
    output.header.frame_id = "map";
    cloud_pub_surf.publish(output);
}

void cloud_extract::Publishe_edge_msg( pcl::PointCloud<PointXYZIRDCN>::Ptr cloud_edge_msg )
{
    // 发布角点点云消息
    sensor_msgs::PointCloud2 output;//发布点云话题消息
    pcl::toROSMsg(*cloud_edge_msg, output);
    output.header.stamp = time;
    output.header.frame_id = "map";
    cloud_pub_edge.publish(output);
}

int main(int argc,char** argv)
{
  ros::init (argc, argv, "cloud_extract");
  cloud_extract ce;

  //ros::MultiThreadedSpinner spinner(2);
  //spinner.spin();
  ros::spin();
  return 0;
}