0. 简介

作为SLAMer在建图时最怕的就是大量的动态障碍物存在，这会导致建图的不精确，而本文主要围绕着如何剔除动态障碍物开始讲起，并提供一种快速的过滤障碍物的方法。

1. 主要方法

在调研的过程中主要存在有两种方法，第一种如文章《通过帧间确定动态障碍物，剔除动态3D点云数据后用于生成栅格地图》所说的方法。通过扫描局部地图，并使用kd-tree完成点云的过滤，通过两帧之间的变化消除动态障碍物点云。并通过SegBG函数选择一遍前景后，剩下的都是最高高度在4m以下的点云类。再通过FindACluster进一步确定是否为前景。只有当pLabel仍然等于0的才会被认为时前景，即动态障碍物。这个方法是一种比较通用且有效地方法。

而另一种则是PointCloud_DynamicObjectFinder这种方法，通过动态追踪，并将box内部的框选给删除，完成动态障碍物的剔除。

2. 动态障碍物过滤方法

本文介绍的方法，适用于所有的2D地面场景，由于地面的信息可以简化为x,y,yawx,y,yawx,y,yaw三个角度，并且地面的信息是格式化的。所以我们可以充分的使用栅格地图。下图为本文使用的方法的结果图。可以有效地在2D层面上过滤出障碍物。

2.1 详细代码介绍

首先我们来看一下头函数dynamic_cloud_detector.h，这部分代码申明了栅格地图的信息，以及定义有效范围等信息。这部分没有什么好说的。

#ifndef __DYNAMIC_CLOUD_DETECTOR_H
#define __DYNAMIC_CLOUD_DETECTOR_H#include #include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include // Eigen
#include 
#include // PCL
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include // OMP
#include class DynamicCloudDetector
{
public:typedef pcl::PointXYZINormal PointXYZIN;typedef pcl::PointCloud CloudXYZIN;typedef pcl::PointCloud::Ptr CloudXYZINPtr;class GridCell //定义的网格类{public:GridCell(void){log_odds = 0;}double get_occupancy(void){return 1.0 / (1 + exp(-log_odds));}double get_log_odds(void){return log_odds;}void add_log_odds(double lo){log_odds += lo;}double log_odds;private:};typedef std::vector OccupancyGridMap; //栅格地图DynamicCloudDetector(void);void callback(const sensor_msgs::PointCloud2ConstPtr &, const nav_msgs::OdometryConstPtr &);void input_cloud_to_occupancy_grid_map(const CloudXYZINPtr &);void devide_cloud(const CloudXYZINPtr &, CloudXYZINPtr &, CloudXYZINPtr &);int get_index_from_xy(const double x, const double y){const int _x = floor(x / resolution_ + 0.5) + grid_width_2_;const int _y = floor(y / resolution_ + 0.5) + grid_width_2_;return _y * grid_width_ + _x;}int get_x_index_from_index(const int index){return index % grid_width_;}int get_y_index_from_index(const int index){return index / grid_width_;}double get_x_from_index(const int);double get_y_from_index(const int);void publish_occupancy_grid_map(const ros::Time &, const std::string &);std::string remove_first_slash(std::string);bool is_valid_point(double x, double y) //判断点是否在地图范围内{const int index = get_index_from_xy(x, y);if (x < -width_2_ || x > width_2_ || y < -width_2_ || y > width_2_){return false;}else if (index < 0 || grid_num_ <= index){return false;}else{return true;}}void transform_occupancy_grid_map(const Eigen::Vector2d &, double, OccupancyGridMap &);void set_clear_grid_cells(const std::vector &, const std::vector &, OccupancyGridMap &);void process(void);private:double resolution_;double width_;double width_2_;int grid_width_;int grid_width_2_;int grid_num_;double occupancy_threshold_;int beam_num_;double log_odds_increase_;double log_odds_decrease_;ros::NodeHandle nh_;ros::NodeHandle local_nh_;tf2_ros::Buffer tf_buffer_;tf2_ros::TransformListener listener_;ros::Publisher dynamic_pub_;ros::Publisher static_pub_;ros::Publisher grid_pub_;typedef message_filters::sync_policies::ApproximateTime sync_subs;message_filters::Subscriber obstacles_cloud_sub_;message_filters::Subscriber odom_sub_;message_filters::Synchronizer sync_;Eigen::Vector2d last_odom_position;double last_yaw = tf2::getYaw;OccupancyGridMap occupancy_grid_map_;
};#endif // __DYNAMIC_CLOUD_DETECTOR_H

我们下面看一下主函数文件dynamic_cloud_detector_node.cpp，是一些配置文件的申明。

#include "dynamic_cloud_detector/dynamic_cloud_detector.h"DynamicCloudDetector::DynamicCloudDetector(void): local_nh_("~"), listener_(tf_buffer_), obstacles_cloud_sub_(nh_, "/velodyne_obstacles", 10), odom_sub_(nh_, "/odom/complement", 10), sync_(sync_subs(10), obstacles_cloud_sub_, odom_sub_)
{local_nh_.param("resolution", resolution_, {0.2});                   //设置分辨率local_nh_.param("width", width_, {40.0});                            //设置宽度local_nh_.param("occupancy_threshold", occupancy_threshold_, {0.2}); //设置占用率阈值local_nh_.param("beam_num", beam_num_, {720});                       //设置激光点数local_nh_.param("log_odds_increase", log_odds_increase_, {0.4});     //设置概率增加值local_nh_.param("log_odds_decrease", log_odds_decrease_, {0.2});     //设置概率减少值grid_width_ = width_ / resolution_;    //计算栅格宽度grid_num_ = grid_width_ * grid_width_; //计算栅格数量width_2_ = width_ / 2.0;               //计算宽度的一半grid_width_2_ = grid_width_ / 2.0;     //计算栅格宽度的一半dynamic_pub_ = nh_.advertise("/cloud/dynamic", 1);   //发布动态点云static_pub_ = nh_.advertise("/cloud/static", 1);     //发布静态点云grid_pub_ = local_nh_.advertise("occupancy_grid", 1); //发布栅格地图last_odom_position = Eigen::Vector2d(msg_odom->pose.pose.position.x, msg_odom->pose.pose.position.y); //记录上一次的位置，初始化时候为当前位置last_yaw = tf2::getYaw(msg_odom->pose.pose.orientation);                                              //记录上一次的yaw，初始化时候为当前yawsync_.registerCallback(boost::bind(&DynamicCloudDetector::callback, this, _1, _2)); //注册回调函数occupancy_grid_map_.resize(grid_num_); //设置栅格地图大小std::cout << "=== dynamic cloud detector ===" << std::endl;std::cout << "resolution: " << resolution_ << std::endl;std::cout << "width: " << width_ << std::endl;std::cout << "width_2: " << width_2_ << std::endl;std::cout << "grid_num: " << grid_num_ << std::endl;std::cout << "occupancy_threshold: " << occupancy_threshold_ << std::endl;std::cout << "log_odds_increase: " << log_odds_increase_ << std::endl;std::cout << "log_odds_decrease: " << log_odds_decrease_ << std::endl;
}

callback内部的函数完成了一帧数据的分割。当中值得注意的是diff的求解。详细的注解已经在代码中完全申明了

void DynamicCloudDetector::callback(const sensor_msgs::PointCloud2ConstPtr &msg_obstacles_cloud, const nav_msgs::OdometryConstPtr &msg_odom)
{const double start_time = ros::Time::now().toSec(); //记录开始时间std::cout << "--- callback ---" << std::endl;CloudXYZINPtr cloud_ptr(new CloudXYZIN);           //创建点云指针pcl::fromROSMsg(*msg_obstacles_cloud, *cloud_ptr); //将含有障碍物消息的原始点云转换为点云指针std::cout << "received cloud size: " << cloud_ptr->points.size() << std::endl;// transform pointcloud to base frameconst std::string sensor_frame_id = remove_first_slash(msg_obstacles_cloud->header.frame_id); //获取消息的frame_idconst std::string base_frame_id = remove_first_slash(msg_odom->child_frame_id);               //获取odom的child_frame_idtry{geometry_msgs::TransformStamped transform;                                                      //创建变换消息transform = tf_buffer_.lookupTransform(base_frame_id, sensor_frame_id, ros::Time(0));           //将sensor_frame_id转换为base_frame_id坐标系上const Eigen::Matrix4d mat = tf2::transformToEigen(transform.transform).matrix().cast(); //将变换消息转换为矩阵pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, mat);                                          //将点云转换为base_frame_id坐标系上cloud_ptr->header.frame_id = base_frame_id;                                                     //设置点云的frame_id为base_frame_id}catch (tf2::TransformException &ex){std::cout << ex.what() << std::endl; //打印错误信息return;}// transform occupancy grid mapconst Eigen::Vector2d odom_position(msg_odom->pose.pose.position.x, msg_odom->pose.pose.position.y); //获取odom的位置const double yaw = tf2::getYaw(msg_odom->pose.pose.orientation);const Eigen::Vector2d diff_odom = Eigen::Rotation2Dd(-last_yaw).toRotationMatrix() * (odom_position - last_odom_position); //计算odom的位移，以上一次的yaw角作为基准double diff_yaw = yaw - last_yaw;diff_yaw = atan2(sin(diff_yaw), cos(diff_yaw)); //计算odom的yaw差值std::cout << "diff odom: " << diff_odom.transpose() << std::endl;std::cout << "diff yaw: " << diff_yaw << std::endl;transform_occupancy_grid_map(-diff_odom, -diff_yaw, occupancy_grid_map_); //计算栅格地图的变换input_cloud_to_occupancy_grid_map(cloud_ptr); //将点云转换为栅格地图publish_occupancy_grid_map(msg_odom->header.stamp, base_frame_id); //发布栅格地图CloudXYZINPtr dynamic_cloud_ptr(new CloudXYZIN); //创建动态点云指针dynamic_cloud_ptr->header = cloud_ptr->header;CloudXYZINPtr static_cloud_ptr(new CloudXYZIN); //创建静态点云指针static_cloud_ptr->header = cloud_ptr->header;devide_cloud(cloud_ptr, dynamic_cloud_ptr, static_cloud_ptr); //将点云分为动态点云和静态点云dynamic_pub_.publish(dynamic_cloud_ptr); //发布动态点云static_pub_.publish(static_cloud_ptr);   //发布静态点云last_odom_position = odom_position; //记录odom的位置，并留给下一时刻last_yaw = yaw;                     //记录odom的yaw，并留给下一时刻std::cout << "time: " << ros::Time::now().toSec() - start_time << "[s]" << std::endl;
}

这部分的函数主要是获得当前帧的占用栅格地图的情况，用于拿到不同区域是否存在动态点云信息。

void DynamicCloudDetector::input_cloud_to_occupancy_grid_map(const CloudXYZINPtr &cloud_ptr)
{std::cout << "--- input cloud to occupancy grid map ---" << std::endl;std::vector beam_list(beam_num_, sqrt(2) * width_2_);        //创建激光雷达的距离列表，每个点所在的位置，其中最大为sqrt(2)*width_2_const double beam_angle_resolution = 2.0 * M_PI / (double)beam_num_; //激光雷达的角度分辨率// occupancy_grid_map_.clear();// occupancy_grid_map_.resize(grid_num_);const int cloud_size = cloud_ptr->points.size();      //获取点云的大小std::vector obstacle_indices(grid_num_, false); //创建栅格地图的障碍物索引列表，默认为falsefor (int i = 0; i < cloud_size; i++){const auto &p = cloud_ptr->points[i]; //获取点云的点if (!is_valid_point(p.x, p.y))        //如果点不在栅格地图上，则跳过{continue;}// occupancy_grid_map_[get_index_from_xy(p.x, p.y)].add_log_odds(0.01);const double distance = sqrt(p.x * p.x + p.y * p.y);               //计算点到栅格地图中心的距离const double direction = atan2(p.y, p.x);                          //计算点到栅格地图中心的方向const int beam_index = (direction + M_PI) / beam_angle_resolution; //计算点到栅格地图中心的方向所对应的激光雷达角度索引if (0 <= beam_index && beam_index < beam_num_){beam_list[beam_index] = std::min(beam_list[beam_index], distance); //更新激光雷达的距离列表}const int index = get_index_from_xy(p.x, p.y); //获取点所在的栅格索引if (index < 0 || grid_num_ <= index)           //如果点所在的栅格索引不在栅格地图上，则跳过{continue;}obstacle_indices[get_index_from_xy(p.x, p.y)] = true;}for (int i = 0; i < grid_num_; i++){if (obstacle_indices[i]){occupancy_grid_map_[i].add_log_odds(log_odds_increase_);}}set_clear_grid_cells(beam_list, obstacle_indices, occupancy_grid_map_); //设置栅格地图的清除栅格
}

下面这部分主要是设置分割点云的信息，并获取分辨率

…详情请参照古月居