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_POINT4DPCL_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]);elsecloud_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;
}

velodyne16点云特征分类相关推荐

PCL 点云特征描述与提取
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:开着拖拉机唱山歌链接:https://zhuanlan.zhihu.com/p/1032192 ...
点云特征图离散化_点云采样
原文链接点云采样分类点云采样的方法有很多种,常见的有均匀采样,几何采样,随机采样,格点采样等.下面介绍一些常见的采样方法. 格点采样格点采样,就是把三维空间用格点离散化,然后在每个格点里采样一个 ...
PCL中点云特征描述与提取精通级实例解析
PCL中点云特征描述与提取精通级实例解析 1 3D对象识别的假设验证 2 隐式形状模型方法 2.1 隐式形状模型识别方法概述 2.2 实例解析 1 3D对象识别的假设验证本教程将学习在高噪声和严 ...
点云智能分类研究进展与展望
点云智能分类研究进展与展望文章目录点云智能分类研究进展与展望摘要一.研究背景二.研究进展三.问题与挑战四.发展趋势五.总结摘要内容摘要:点云是目前摄影测量.遥感.计算机视觉等多个领 ...
3D点云形状分类简介
3D点云形状分类简介 3D形状分类主要有三种方法:基于多视图的(multi-view),基于体积的(volumetric-based),基于点的(point-based). 基于多视图的方法将非结构化 ...
PCL代码经典赏析七：PCL 点云特征描述与提取
文章目录更新:2019年8月说明目录索引 PCL 点云特征描述与提取 PCL 描述三维特征相关基础 PCL 法线估计实例 ------ 估计某一点的表面法线 PCL 法线估计实例 ------ ...
【点云压缩】点云概述：点云的分类与处理点云来源
点云概述一.什么是点云二.点云从哪来 1. 三维激光雷达扫描 2. 照相机扫描 3. 逆向工程三.点云的分类四.点云的相关处理 1. 点云分割 2. 点云补全 3. 点云上采样 4. 点云压缩 ...
PCL点云特征描述与提取（1）
3D点云特征描述与提取是点云信息处理中最基础也是最关键的一部分,点云的识别.分割,重采样,配准曲面重建等处理大部分算法,都严重依赖特征描述与提取的结果.从尺度上来分,一般分为局部特征的描述和全局特征的 ...
FCGF-基于稀疏全卷积网络的点云特征描述子提取(ICCV2019)
概要论文: Fully Convolutional Geometric Features 标签: ICCV 2019; feature, match, registration 作者: Christ ...

velodyne16点云特征分类

velodyne16点云特征分类相关推荐

最新文章

热门文章