11、基于ROS的通过计算夹角实现点云中的地面部分提取

本文主要是介绍11、基于ROS的通过计算夹角实现点云中的地面部分提取，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

基于ROS的通过计算夹角实现点云中的地面部分提取

一、理论基础

首先将整个点云中的地面部分提取出来，这能有效减少点云数量，减少后面的聚类开销。采用激光扫描到障碍物和地面的夹角不同的原理进行提取，具体如下：
激光扫描到地面点障碍物点判断示意图情况下所示：
在这里插入图片描述

激光未扫描到障碍物地面点时候，θ角很小（地面有一点坡道），甚至为0（平坦地面），激光未扫描到地面障碍物点示意图如下：
在这里插入图片描述

两个相邻激光线扫射到的点两个点P1、P2的水平方向距离可以在世界坐标系下（以激光雷达为圆心的坐标系）,使用欧氏距离计算公式算出：
在这里插入图片描述

其中，分别表示两个点的横坐标，y分别表示两个点的纵坐标。
然后计算两个点的连线和路面的夹角值θ，θ大于设定阈值则两个点不为地面区域。
在这里插入图片描述

依据上述方法可以提取出地面。

二、实现：

使用Voxel Grid Filter对原始点云进行降采样，只需定义pcl::VocelGrid并且指定输入点云和leaf size，在本例中，我们使用leaf size为 0.2。Voxel Grid Filter将输入点云使用0.2m0.2m0.2m的立方体进行分割，使用小立方体的形心（centroid）来表示这个立方体的所有点，保留这些点作为降采样的输出。雷达的水平光束发散间隔是0.18，据此将360°分为2000条射线，将其排序后计算其水平距离，判断前后两点的坡度是否大于设定的阈值8°，从而判断点是否为地面点。
基本代码文件编写：
建立pcl_ws/src，并编译，创建pcl_test包，依赖于roscpp、sensor_msgs、pcl_ros。

cd ~
mkdir -p pcl_ws/src
cd pcl_ws
catkin_make
source devel/setup.bash
cd src
catkin_create_pkg pcl_test roscpp sensor_msgs pcl_ros

修改pcl_test包下的CMakeList文件以及package.xml配置文件，如下：
CMakeList文件：

cmake_minimum_required(VERSION 2.8.3)
project(pcl_test)add_compile_options(-std=c++11)find_package(catkin REQUIRED COMPONENTS
pcl_ros
roscpp
sensor_msgs
)catkin_package(INCLUDE_DIRS includeCATKIN_DEPENDS roscpp sensor_msgs pcl_ros
)include_directories(include${catkin_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})add_executable(${PROJECT_NAME}_node src/pcl_test_node.cpp src/pcl_test_core.cpp)target_link_libraries(${PROJECT_NAME}_node${catkin_LIBRARIES}${PCL_LIBRARIES}
)

package.xml配置文件：

<?xml version="1.0"?>
<package><name>pcl_test</name><version>0.0.1</version><description>The pcl_test package</description><maintainer email="shenzb12@lzu.edu.cn">adam</maintainer><license>MIT</license><buildtool_depend>catkin</buildtool_depend><build_depend>roscpp</build_depend><build_depend>sensor_msgs</build_depend><build_depend>pcl_ros</build_depend><run_depend>roscpp</run_depend><run_depend>sensor_msgs</run_depend><run_depend>pcl_ros</run_depend>
</package>

在pcl_test/src目录下新建pcl_test_node.cpp文件，仅包含main函数，是节点的入口，用于初始化ros、创建句柄节点等：

#include "pcl_test_core.h"int main(int argc, char **argv)
{ros::init(argc, argv, "pcl_test");ros::NodeHandle nh;PclTestCore core(nh);return 0;
}

编写pcl_test_core.cpp:文件：

#include "pcl_test_core.h"PclTestCore::PclTestCore(ros::NodeHandle &nh){sub_point_cloud_ = nh.subscribe("/velodyne_points",10, &PclTestCore::point_cb, this);pub_filtered_points_ = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);ros::spin();
}PclTestCore::~PclTestCore(){}void PclTestCore::Spin(){}void PclTestCore::point_cb(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr){pcl::PointCloud<pcl::PointXYZI>::Ptr current_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr);pcl::VoxelGrid<pcl::PointXYZI> vg;vg.setInputCloud(current_pc_ptr);vg.setLeafSize(0.2f, 0.2f, 0.2f);vg.filter(*filtered_pc_ptr);sensor_msgs::PointCloud2 pub_pc;pcl::toROSMsg(*filtered_pc_ptr, pub_pc);pub_pc.header = in_cloud_ptr->header;pub_filtered_points_.publish(pub_pc);
}

在pcl_test/include下编写头文件include/pcl_test_core.h:

#pragma once
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>class PclTestCore
{private:ros::Subscriber sub_point_cloud_;ros::Publisher pub_filtered_points_;void point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud);public:PclTestCore(ros::NodeHandle &nh);~PclTestCore();void Spin();
};

编写launch文件pcl_test.launch来启动这个节点：

<launch><node pkg="pcl_test" type="pcl_test_node" name="pcl_test_node" output="screen"/>
</launch>

三、运行bag以及地面分割的试验效果：

1、在pcl_test下进行编译和更新：

catkin_make
source devel/setup.sh

2、启动这个节点：

roslaunch pcl_test pcl_test.launch

3、启动rviz(要在launch之后运行):

rviz

4、运行包：

rosbag play 11-12-boliu2.bag
//需要↑这个包可私信
/*
或者：from AdamShan:
rosbag play --clock test.bag
测试bag下载链接：https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w
*/

在ROS中，以/velodyne_points为话题接受全部的点云数据，将降采样的结果发布到/filtered_points话题上，将地面提取的结果发布到/filtered_points_ground话题上，将非地面提取的结果发布到/filtered_points_no_ground上予以显示：
在这里插入图片描述

使用11-12-boliu2.bag作为数据集进行实验。
实现分割效果如下：
原始点云：

地面提取效果：1、2

filtered_points_no_ground点云

分割出非地面点云之后，我们就可以让Lidar Detection的代码工作在/filtered_points_no_ground上了，从而排除了地面对于Lidar聚类以及Detection的影响。