ros中使用pcl进行点云的滤波稀疏化(livox-mid40)
体素滤波
使用体素化网格方法实现下采样,即减少点的数量 减少点云数据,并同时保存点云的形状特征,在提高配准,曲面重建,形状识别等算法速度中非常实用,PCL是实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格,容纳后每个体素内用体素中所有点的重心来近似显示体素中其他点,这样该体素内所有点都用一个重心点最终表示。
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.1, 0.1, 0.1);//设置体元网格的叶子大小,单位:米
sor.filter (cloud_filtered);
半径滤波
用户指定邻居的个数,要每个点必须在指定半径内具有指定个邻居才能保留在PointCloud中。例如,如果指定了1个邻居,则只会从PointCloud中删除半径范围内一个邻近都没有的点。如果指定了2个邻居,则从PointCloud中删除半径范围内邻居个数小于2个的点。
outlierRemoval.setRadiusSearch(0.4);// 邻居个数
outlierRemoval.setMinNeighborsInRadius(2);
统计滤波
sor.setMeanK(50); //K近邻搜索点个数,计算每个点到所有K邻域点的平均距离。
sor.setStddevMulThresh(1.0); //计算整个点集距离容器的平均值和样本标准差,依次将距离阈值与每个点的distances[iii]比较 ,超出阈值的点被标记为离群点,并将其移除。距离阈值等于平均距离加上标准差倍数。
sor.setNegative(false); //保留未滤波点(内点)
sor.filter(*cloud_filter); //保存滤波结果到cloud_filter
==================================================================
ROS中实现pcl滤波的操作过程
一、创建、编译工作空间
首先运行roscore
。
mkdir -p ~/ws_pointcloudfilter/src
catkin_init_workspace
cd ~/ws_pointcloudfilter/
catkin_make
source devel/setup.bash
echo $ROS_PACKAGE_PATH
在echo返回的路径中找到当前工作空间的目录即为设置成功。
二、创建、编译功能包
cd ~/ws_pointcloudfilter/src
catkin_create_pkg pkg_voxelfilter roscpp pcl_ros pcl_conversions sensor_msgs
cd ~/ws_pointcloudfilter
catkin_make
. ~/ws_pointcloudfilter/devel/setup.bash
功能包的名称和后面指定依赖的库文件根据实际修改,注意其中pcl_ros、和pcl_conversions两项,这是ROS与pcl库的接口。
三、修改package.xml
文件
在package.xml文件中添加:
<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>
四、在功能包的src
文件夹下创建voxelfilter.cpp
文件
创建名为voxelfilter.cpp的文件。添加如下代码:
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//filters specific includes
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
//publisher
ros::Publisher pub;
//callback function
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; //存储滤波后的数据格式
//livox采集的点云数据转化为PCL中的点云的数据格式
pcl_conversions::toPCL(*cloud_msg, *cloud);
// Perform the actual filtering进行一个滤波处理-----------------1.体素滤波
pcl::VoxelGrid<pcl::PCLPointCloud2> filter; //创建滤波对象
filter.setInputCloud (cloudPtr); //设置输入的滤波,将需要过滤的点云给滤波对象
filter.setLeafSize (0.05, 0.05, 0.05); //设置滤波时创建的体素大小为xxxm的立方体
filter.filter (cloud_filtered); //执行滤波处理,存储输出cloud_filtered
// Convert to ROS data type //再将滤波后的点云的数据格式转换为ROS下的数据格式发布出去
sensor_msgs::PointCloud2 output; //声明的输出的点云的格式
pcl_conversions::moveFromPCL(cloud_filtered, output);//第一个参数是输入,后面的是输出
//output = *cloud_msg;//不滤波时使用
output.header.frame_id="livox";
// Publish the data
pub.publish (output);
}
int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "voxelfilter"); //声明节点的名称voxelfilter
ros::NodeHandle sj;
// 为接受点云数据创建一个订阅节点,订阅livox发布的话题/livox/lidar里面的消息
ros::Subscriber sub = sj.subscribe<sensor_msgs::PointCloud2> ("/livox/lidar", 1, cloud_cb);
//创建ROS的发布节点,发布到话题/output里面
pub = sj.advertise<sensor_msgs::PointCloud2> ("/output", 1);
// Spin
// 回调
ros::spin ();
}
五、修改功能包文件中的CMakeLists.txt
文件
cmake_minimum_required(VERSION 3.0.2)
project(pkg_voxelfilter)
find_package(catkin REQUIRED COMPONENTS
pcl_conversions
pcl_ros
roscpp
sensor_msgs
std_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES pkg_voxelfilter
CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs std_msgs
# DEPENDS system_lib
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(voxelfilter src/voxelfilter.cpp)
target_link_libraries(voxelfilter ${catkin_LIBRARIES})
六、编译功能包
cd ~/ws_pointcloudfilter/
catkin_make
source devel/setup.bash
七、打开livox-mid40,并publish话题/livox/lidar
将livox-mid40与英伟达NX连接并配置好之后(具体配置参考另一篇文章:livox-mid40的配置),输入以下命令:
cd ~/ws_livox
source devel/setup.bash
roslaunch livox_ros_driver livox_lidar_rviz.launch // livox_lidar_rviz.launch直接打开rviz显示原始点云
或者
roslaunch livox_ros_driver livox_lidar.launch // livox_lidar.launch不显示rviz,只publish原始点云
livox_lidar_rviz.launch或者livox_lidar.launch文件中的参数具体含义可参考如下:
| launch 文件名 | 功能 |
| ------------------------- | ------------------------------------------------------------ |
| livox_lidar_rviz.launch | 连接览沃雷达设备<br>向外发布 pointcloud2 格式的点云数据<br>自动加载rviz |
| livox_hub_rviz.launch | 连接览沃中心板设备<br>向外发布 pointcloud2 格式的点云数据<br>自动加载rviz |
| livox_lidar.launch | 连接览沃雷达设备<br>向外发布 pointcloud2 格式的点云数据 |
| livox_hub.launch | 连接览沃中心板设备<br>向外发布 pointcloud2 格式的点云数据 |
| livox_lidar_msg.launch | 连接览沃雷达设备<br>向外发布览沃自定义点云数据 |
| livox_hub_msg.launch | 连接览沃中心板设备<br/>向外发布览沃自定义点云数据 |
| lvx_to_rosbag.launch | 转换 lvx 文件为 rosbag 文件<br>直接将 lvx 文件转换为 rosbag 文件 |
| lvx_to_rosbag_rviz.launch | 转换 lvx 文件为 rosbag 文件<br>从 lvx 文件中读取点云数据,并转换为 pointcloud2 格式向外发布 |
览沃 ROS 驱动程序中的所有内部参数都位于 launch 文件中,下面将对经常用到的三个参数进行详细说明:
| 参数名 | 详细说明 | 默认值 |
| ------------ | ------------------------------------------------------------ | ------ |
| publish_freq | 设置点云发布频率 <br>浮点数据类型,推荐值 5.0,10.0,20.0,50.0 等。 | 10.0 |
| multi_topic | LiDAR 设备是否拥有独立的 topic 发布点云数据<br>0 -- 所有 LiDAR 设备共同使用同一个 topic 发送点云数据<br>1 -- 每个 LiDAR 设备各自拥有独立的 topic 发布点云数据 | 0 |
| xfer_format | 设置点云格式<br>0 -- 览沃 pointcloud2(PointXYZRTL) 点云格式<br>1 -- 览沃自定义点云数据格式<br>2 -- PCL库中标准 pointcloud2(pcl::PointXYZI) 点云格式 | 0 |
在rviz中配置topic
为/livox/lidar
配置fixed frame
为livox_frame
运行结果如下图:
八、运行滤波程序
为了接收激光雷达发布的话题/livox/lidar,并将滤波稀疏化后的点云发布到话题/output中
终端中输入:
rosrun pkg_voxelfilter voxelfilter //rosrun+功能包名+节点名,主要用来接收激光雷达发布的话题/livox/lidar,并将滤波稀疏化后的点云发布到话题/output中
打开终端,输入:
rviz
在rviz中Addpointcloud2
,
配置topic
为/output
配置fixed frame
为livox
(话题名与frame可在voxelfilter.cpp
中修改)
运行结果如下图:
==================================================================
遇见的问题
一、3.17晚九点记录:
注释掉滤波的代码,居然还能滤波???
3.18早上去了改一改,实在不行,重新创建功能包试一下。
3.18-15:25已经解决。
重新创建了功能包voxelfilter,出现了问题二描述的情况。具体解决见问题二。
二、3.18-11:00记录:
ros编译package后,devel文件夹内没有对应package的可执行文件:
CMakeLists.txt
缺少catkin_package()
命令导致的。