3D帧间匹配-----剔除动态障碍物

0. 简介

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

1. 主要方法

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

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

2. 动态障碍物过滤方法

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

2.1 详细代码介绍

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

#ifndef __DYNAMIC_CLOUD_DETECTOR_H
#define __DYNAMIC_CLOUD_DETECTOR_H

#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_eigen/tf2_eigen.h>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Geometry>

// PCL
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types_conversion.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>

// OMP
#include <omp.h>

class DynamicCloudDetector
{
public:
    typedef pcl::PointXYZINormal PointXYZIN;
    typedef pcl::PointCloud<PointXYZIN> CloudXYZIN;
    typedef pcl::PointCloud<PointXYZIN>::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<GridCell> 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<double> &, const std::vector<bool> &, 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<sensor_msgs::PointCloud2, nav_msgs::Odometry> sync_subs;
    message_filters::Subscriber<sensor_msgs::PointCloud2> obstacles_cloud_sub_;
    message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
    message_filters::Synchronizer<sync_subs> 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<sensor_msgs::PointCloud2>("/cloud/dynamic", 1);   //发布动态点云
    static_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/cloud/static", 1);     //发布静态点云
    grid_pub_ = local_nh_.advertise<nav_msgs::OccupancyGrid>("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，初始化时候为当前yaw

    sync_.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 frame
    const std::string sensor_frame_id = remove_first_slash(msg_obstacles_cloud->header.frame_id); //获取消息的frame_id
    const std::string base_frame_id = remove_first_slash(msg_odom->child_frame_id);               //获取odom的child_frame_id
    try
    {
        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<double>(); //将变换消息转换为矩阵
        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 map
    const 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<double> 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<bool> obstacle_indices(grid_num_, false); //创建栅格地图的障碍物索引列表，默认为false
    for (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_); //设置栅格地图的清除栅格
}

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

…详情请参照古月居