从RealsenseD435 摄像头获取点云数据并滤波后在ROS中通过rviz显示

从RealsenseD435 摄像头获取点云数据并滤波后在ROS中通过rviz显示

以体素滤波为例,参考以下博客：
https://blog.csdn.net/jack20030552/article/details/80269486
https://blog.csdn.net/jack20030552/article/details/80269486
1.创建ROS package
在catkin_ws—src路径下执行此命令，生成一个名为my_pcl_tutorial的文件夹。
catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs

2.在package.xml中增加如下代码
<build_depend>libpcl-all-dev</build_depend>
<run_depend>libpcl-all</run_depend>
注意位置：大括号之内，最后两行。

3.创建源代码文件example.cpp
在my_pcl_tutorial文件夹的src文件夹下新建一个.cpp文件，粘贴以下代码。
#include <ros/ros.h>
// PCL specific includes 。PCL 的相关的头文件
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//滤波的头文件
#include <pcl/filters/voxel_grid.h>
//申明发布器
ros::Publisher pub;
//回调函数
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// 声明存储原始数据与滤波后的数据的点云的格式
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;//存储滤波后的数据格式

// Convert to PCL data type。转化为PCL中的点云的数据格式
pcl_conversions::toPCL(*cloud_msg, *cloud);

// Perform the actual filtering进行一个滤波处理
pcl::VoxelGridpcl::PCLPointCloud2 sor; //创建滤波对象
sor.setInputCloud (cloudPtr); //设置输入的滤波，将需要过滤的点云给滤波对象
sor.setLeafSize (0.1, 0.1, 0.1); //设置滤波时创建的体素大小为1cm立方体
sor.filter (cloud_filtered);//执行滤波处理，存储输出cloud_filtered

// Convert to ROS data type。// 再将滤波后的点云的数据格式转换为ROS下的数据格式发布出去
sensor_msgs::PointCloud2 output;//声明的输出的点云的格式
pcl_conversions::moveFromPCL(cloud_filtered, output);//第一个参数是输入，后面的是输出

// Publish the data
pub.publish (output);
}

int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, “my_pcl_tutorial”);//声明节点的名称
ros::NodeHandle nh;

// Create a ROS subscriber for the input point cloud
// 为接受点云数据创建一个订阅节点
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> (“input”, 1, cloud_cb);

// Create a ROS publisher for the output point cloud
//创建ROS的发布节点
pub = nh.advertise<sensor_msgs::PointCloud2> (“output”, 1);

// Spin
// 回调
ros::spin ();
}
这个程序中的“output”即滤波输出点云的名称。

4.在CMakeLists .txt中增加下面的内容
add_executable(example src/example.cpp)//将src中的文件添加成名字为example的可执行文件
target_link_libraries(example ${catkin_LIBRARIES})//将相关的库和可执行文件链接
注意位置：CMakeLists文件中的各项语句是有顺序的，关于这个顺序问题可以参考链接 https://blog.csdn.net/qq_27806947/article/details/82709620

5.编译工作空间
cd ～/catkin_ws （首先进入这个路径）
catkin_make

6.运行roscore
Roscore

6.启动相机节点（新打开一个终端）
roslaunch realsense2_camera rs_rgbd.launch

7.运行我们编写的摄像头点云数据降采样程序（再新打开一个终端）
rosrun my_pcl_tutorial example input:=/camera/depth_registered/points
这里的input后的路径是为滤波前的点云数据路径，可以观察rviz界面左边栏目中pointcloud2下的 topic 项得到。my_pcl_tutorial是.cpp所在的包（文件夹）的名字。example是由于在CMakeLists .txt中增加的内容add_executable(example src/example.cpp)中example是src/example.cpp的可执行文件名

8.打开显示界面rviz
再打开一个新的终端，输入命令 rviz
至此，出现rviz界面。

9.要想显示滤波后的点云数据，首先需要把滤波前的pointcloud2勾掉，然后，add—By topic----滑到最下面有“/output”项，这即是我们example.cpp文件中经过滤波处理后得到的输出，把它的pointcloud2项添加到左边显示栏，打钩，即可显示滤波后的点云图像。

效果：
滤波前：

滤波后：

这个只是个体素滤波的例子，滤波效果并不好。
直通滤波可以滤掉大面积的噪声，还可以通过体素滤波、半径滤波等进一步优化。

另外，
如果有多个滤波器，有多个.cpp文件，如下分别在几个新终端中启动每个节点。
下面第一个是直通滤波，在cpp文件中可以看到它的输出作为了下面体素滤波的输入，所以启动体素滤波的命令行中才不必再有input相关信息。具体.cpp的代码可参见
https://blog.csdn.net/zhang970187013/article/details/81222802

指令中每个词的含义参考第7步的解释。

此指令回车后不会产生任何信息，但是在rviz界面的add–By topic中已经有了此直通滤波过后的点云数据项，添加显示即可。
再打开一个新的终端，输入以下命令，启动体素滤波。同样的，可以在rviz中找到滤波后的点云数据（注意看此点云的时候，把其他滤波的点云显示项勾掉）