三维目标检测：（二）用ROS获取传感器(Kinect DK)点云并滤波

前期工作

• [ 1] 需要配置Clion

https://blog.csdn.net/weixin_38593194/article/details/85122716

• [ 2] 创建ros工作空间和功能包

https://blog.csdn.net/weixin_44858747/article/details/109479414

• [ 3] 安装kinectDK官方SDK ，和ros功能包

https://blog.csdn.net/qq_27399933/article/details/107356117

• [ 4] 需要在cmakelists文件和package.xml文件中添加opencv、pcl等库
  cmakelists.txt

find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
pcl_conversions
pcl_ros
roscpp
rospy
std_msgs
message_generation
)
find_package( PCL REQUIRED COMPONENTS common io visualization )

catkin_package(INCLUDE_DIRS include
LIBRARIES object_detection
CATKIN_DEPENDS
roscpp std_msgs message_generation message_runtime
)
include_directories(
$ {catkin_INCLUDE_DIRS} $ {PCL_INCLUDE_DIRS}
)
link_directories($ {PCL_LIBRARY_DIRS})
add_definitions($ {PCL_DEFINITIONS})
include_directories("./include/")
add_executable(main src/main.cpp include/pointcloud_seg/pointcloud_seg.h src/pointcloud_seg.cpp) 这部分自己改一下
target_link_libraries(main ${catkin_LIBRARIES} ${PCL_LIBRARIES})

package.xml（有些可能用不到，但是能跑通）

<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>libpcl-all-dev</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>image_transport</build_export_depend>
<build_export_depend>pcl_conversions</build_export_depend>
<build_export_depend>pcl_ros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>libpcl-all</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>pcl_conversions</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>libpcl-all</exec_depend>
<exec_depend>message_runtime</exec_depend>

首先连接摄像头，打开节点

$ roslaunch azure_kinect_ros_driver driver.launch

在ros中读取传感器点云

1.创建类

#include <ros/ros.h>
#include <string>
#include <iostream>
#include <image_transport/image_transport.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>

using namespace std;
using namespace ros;
using namespace pcl;
typedef pcl::PointXYZRGB PointType;
class pointcloud_seg
{
public:
    ros::Subscriber sub;
public:
    pointcloud_seg(ros::NodeHandle &n);
    void get_pointcloud_cb(const sensor_msgs::PointCloud2& cloud_msg);
    ~pointcloud_seg();
};
pointcloud_seg::pointcloud_seg(ros::NodeHandle &n)
{
    //用于接受点云的节点和发布点云的节点 接受官方ros包发出的点云节点/points2
    sub=n.subscribe("/points2",1,&pointcloud_seg::get_pointcloud_cb,this);
    //一会用于发布平面点，和平面之外的点，如果不发布进行平面提取等结果，不需要定义
    pub_plant=n.advertise<sensor_msgs::PointCloud2>("Plant",1000); //发布平面点
    pub_obj=n.advertise<sensor_msgs::PointCloud2>("Object",1000);   //发布平面外的点
}
//接受点云的回调函数
void pointcloud_seg::get_pointcloud_cb(const sensor_msgs::PointCloud2 &cloud_msg) {
    pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
    //ros获取的点云是pointcloud2类型的点云，需要转换为pcl类型的点云
    pcl::fromROSMsg(cloud_msg,*cloud);
    //cloud就是获取到的转换为pcl::PointXYZRGB的点云
    //可以在这部分对点云进行处理，包括滤波等操作
}
pointcloud_seg::~pointcloud_seg()
{
}
int main(int argc, char **argv)
{
    // Initialize ROS
    ros::init(argc, argv, "pointcloud_seg");
    ros::NodeHandle n;
    //定义类
    pointcloud_seg seg(n);
    ros::spin();
    return 0;
}

对点云进行采样和滤波

1.进行XYZ方向的直通滤波，可以保证点云的X,Y,Z范围在我们想要的范围内
需要头文件

#include <pcl/filters/passthrough.h>

    ///进行X方向的范围限制，点云中的所有点的x都在-1m到+1米
    pcl::PointCloud<PointType>::Ptr cloudPtrx(new pcl::PointCloud<PointType>);
    pcl::PassThrough<PointType> px_filter;
    //input点云
    px_filter.setInputCloud(cloud_msg_xyz); // Pass filtered_cloud to the filter
    px_filter.setFilterFieldName("x"); // Set axis x
    px_filter.setFilterLimits(-1, 1); // Set limits min_value to max_value
    //output点云
    px_filter.filter(*cloudPtrx); // Restore output data in second_cloud
	
    // Perform the PassThrough Filter to the Y axis
    pcl::PointCloud<PointType>::Ptr cloudPtry(new pcl::PointCloud<PointType>);
    pcl::PassThrough<PointType> py_filter;
    py_filter.setInputCloud(cloudPtrx); // Pass filtered_cloud to the filter
    py_filter.setFilterFieldName("y"); // Set axis y
    py_filter.setFilterLimits(-1, 1); // Set limits min_value to max_value
    py_filter.filter(*cloudPtry); // Restore output data in second_cloud

    // Perform the PassThrough Filter to the Z axis
    pcl::PointCloud<PointType>::Ptr cloudPtrz(new pcl::PointCloud<PointType>);
    pcl::PassThrough<PointType> pz_filter;
    pz_filter.setInputCloud(cloudPtry); // Pass filtered_cloud to the filter
    pz_filter.setFilterFieldName("z"); // Set axis z
    pz_filter.setFilterLimits(0, 1.5); // Set limits min_value to max_value
    pz_filter.filter(*cloudPtrz); // Restore output data in second_cloud
}
//最后处理过的点云cloudPtrz      x属于【-1，1】，y属于【-1，1】，z属于【0，1.5】 单位为m

2.进行体素滤波，目的是进行下采样，控制点云的密度
需要头文件

#include <pcl/filters/voxel_grid.h>

 // Perform the VoxelGrid Filter
    pcl::PointCloud<PointType>::Ptr cloudPtrv(new pcl::PointCloud<PointType>);
    pcl::VoxelGrid<PointType> v_filter;
    v_filter.setInputCloud(cloudPtrz); // Pass raw_cloud to the filter
    //设置体素网格的大小，越大点越少
    v_filter.setLeafSize(0.01, 0.01,0.01); // Set leaf size
    v_filter.filter(*cloudPtrv); // Store output data in first_cloud

3.去除离群点
需要头文件

#include<pcl/filters/radius_outlier_removal.h>

    //离群点去除
    pcl::RadiusOutlierRemoval<PointType> pcFilter;  //创建滤波器对象
    pcl::PointCloud<PointType>::Ptr cloud_filtered(new pcl::PointCloud<PointType>);
    pcFilter.setInputCloud(cloudPtrv);             //设置待滤波的点云
    pcFilter.setRadiusSearch(0.8);               // 设置搜索半径
    pcFilter.setMinNeighborsInRadius(2);      // 设置一个内点最少的邻居数目
    pcFilter.filter(*cloud_filtered);        //滤波结果存储到cloud_filtered
//上面就是如果某个点周围0.8的圆弧内有2个点就不算离群点，可能聚类以后再用效果比较好

根据半径内的点数确定某个点是否为噪声点，但是我感觉效果不明显，半径太短会很慢，半径太大效果不好，准备换一种方法

可以输出滤波后的点云了

需要转换为pointcloud2类型的数据

        sensor_msgs::PointCloud2 output;//声明的输出的点云的格式
        pcl::toROSMsg(*cloud_filtered, output);//第一个参数是输入，后面的是输出
        pub_obj.publish(output);

ROS中pcl的的转换代码

https://blog.csdn.net/u010284636/article/details/79214841

查看点云

打开rviz查看 查看output

rviz

代码小白，欢迎大家批评指正。