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_); //设置栅格地图的清除栅格
}
下面这部分主要是设置分割点云的信息，并获取分辨率
void DynamicCloudDetector::devide_cloud(const CloudXYZINPtr &cloud, CloudXYZINPtr &dynamic_cloud, CloudXYZINPtr &static_cloud)
{
dynamic_cloud->points.clear();
static_cloud->points.clear();
for (const auto &pt : cloud->points)
{
if (-width_2_ <= pt.x && pt.x <= width_2_ && -width_2_ <= pt.y && pt.y <= width_2_)
{
const int index = get_index_from_xy(pt.x, pt.y); //获取点所在的栅格索引
if (0 <= index && index < grid_num_)
{
const double occupancy = occupancy_grid_map_[index].get_occupancy(); //获取栅格地图的占有率
if (occupancy < occupancy_threshold_) //如果占有率小于阈值，则认为是静态点云
{
dynamic_cloud->points.push_back(pt); //将静态点云添加到动态点云中
}
else
{
static_cloud->points.push_back(pt); //将静态点云添加到静态点云中
}
}
}
}
}
double DynamicCloudDetector::get_x_from_index(const int index)
{
return (get_x_index_from_index(index) - grid_width_2_) * resolution_;
}
double DynamicCloudDetector::get_y_from_index(const int index)
{
return (get_y_index_from_index(index) - grid_width_2_) * resolution_;
}
下面的两个函数是用于发布话题以及分割话题使用
void DynamicCloudDetector::publish_occupancy_grid_map(const ros::Time &stamp, const std::string &frame_id)
{
std::cout << "--- publish occupancy grid map ---" << std::endl;
nav_msgs::OccupancyGrid og;
og.header.stamp = stamp;
og.header.frame_id = frame_id;
og.info.resolution = resolution_;
og.info.width = grid_width_;
og.info.height = grid_width_;
og.info.origin.position.x = -width_2_;
og.info.origin.position.y = -width_2_;
og.info.origin.orientation.w = 1.0;
og.data.resize(grid_num_);
for (int i = 0; i < grid_num_; i++)
{
og.data[i] = occupancy_grid_map_[i].get_occupancy() * 100;
}
grid_pub_.publish(og);
}
std::string DynamicCloudDetector::remove_first_slash(std::string frame_id)
{
const int slash_pos = frame_id.find('/'); //找到第一个'/'的位置
if (slash_pos == 0) //如果第一个'/'的位置是0，说明是以'/'开头的，则去掉'/'
{
frame_id.erase(0, 1);
}
return frame_id;
}
最后一部分是根据当前的diff转化栅格地图使用，并在主函数中调用
void DynamicCloudDetector::transform_occupancy_grid_map(const Eigen::Vector2d &translation, double diff_yaw, OccupancyGridMap &map)
{
const double dx = translation(0); // x方向的偏移量
const double dy = translation(1); // y方向的偏移量
const double c_yaw = cos(diff_yaw); //旋转角度的cos值
const double s_yaw = sin(diff_yaw); //旋转角度的sin值
std::cout << "scrolling\n\tdx: " << dx << ", dy: " << dy << ", cos(theta): " << c_yaw << ", sin(theta)" << s_yaw << std::endl;
const double dx_grid = dx / resolution_; // x方向的偏移量对应的格子数
const double dy_grid = dy / resolution_; // y方向的偏移量对应的格子数
std::cout << "dx_grid: " << dx_grid << std::endl;
std::cout << "dy_grid: " << dy_grid << std::endl;
Eigen::Matrix3d affine; //定义一个3*3的矩阵,用于转换
affine << c_yaw, -s_yaw, dx_grid,
s_yaw, c_yaw, dy_grid,
0, 0, 1;
std::cout << "forward affine:\n"
<< affine << std::endl;
const Eigen::Matrix3d affine_inverse = affine.inverse(); //计算矩阵的逆矩阵
std::cout << "reversed affine:\n"
<< affine_inverse << std::endl;
OccupancyGridMap ogm(grid_num_); //定义一个格子数量为grid_num_的空间
// const int show_i = grid_num_ * 0.5 + grid_width_ - 1;
for (int i = 0; i < grid_num_; i++)
{
// if(i == show_i)
// std::cout << "i: " << i << std::endl;
const double x_i = get_x_index_from_index(i) - grid_width_2_; //获取x方向的格子索引
const double y_i = get_y_index_from_index(i) - grid_width_2_; //获取y方向的格子索引
Eigen::Vector3d ogm_i(x_i, y_i, 1); //定义一个3*1的向量，用于存储格子的坐标
// if(i == show_i)
// std::cout << "ogm_i.transpose(): " << ogm_i.transpose() << std::endl;
// if(i == show_i)
// std::cout << x_i * resolution_ << ", " << y_i * resolution_ << std::endl;
Eigen::Vector3d map_i = affine_inverse * ogm_i; //将上一时刻的点依据affine_inverse矩阵进行转换到当前时刻的点
// if(i == show_i)
// std::cout << "map_i.transpose(): " << map_i.transpose() << std::endl;
// if(i == show_i)
// std::cout << map_i(0) * resolution_ << ", " << map_i(1) * resolution_ << std::endl;
// bilinear interpolation
const int x_0 = std::floor(map_i(0)); //获取x方向的格子索引
const int x_1 = x_0 + 1;
const int y_0 = std::floor(map_i(1)); //获取y方向的格子索引
const int y_1 = y_0 + 1;
// if(i == show_i)
// std::cout << x_0 << ", " << x_1 << ", " << y_0 << ", " << y_1 << std::endl;
if (x_0 < -grid_width_2_ || grid_width_2_ <= x_1)
{
continue;
}
if (y_0 < -grid_width_2_ || grid_width_2_ <= y_1)
{
continue;
}
const int index_0_0 = (y_0 + grid_width_2_) * grid_width_ + x_0 + grid_width_2_; //获取左上角格子的索引
const int index_0_1 = (y_1 + grid_width_2_) * grid_width_ + x_0 + grid_width_2_;
const int index_1_0 = (y_0 + grid_width_2_) * grid_width_ + x_1 + grid_width_2_;
const int index_1_1 = (y_1 + grid_width_2_) * grid_width_ + x_1 + grid_width_2_;
// if(i == show_i)
// std::cout << index_0_0 << ", " << index_0_1 << ", " << index_1_0 << ", " << index_1_1 << std::endl;
const Eigen::Vector2d y_vec(y_1 - map_i(1), map_i(1) - y_0); //获取y方向的向量
const Eigen::Vector2d x_vec(x_1 - map_i(0), map_i(0) - x_0); //获取x方向的向量
Eigen::Matrix2d value_mat;
// value_mat << map[index_0_0].get_log_odds(), map[index_1_0].get_log_odds(),
// map[index_0_1].get_log_odds(), map[index_1_1].get_log_odds();
value_mat << map[index_0_0].get_occupancy(), map[index_1_0].get_occupancy(),
map[index_0_1].get_occupancy(), map[index_1_1].get_occupancy(); //获取左上角和右上角的格子的occupancy值
const double ogm_value = y_vec.transpose() * value_mat * x_vec; //计算格子的occupancy值
ogm[i].log_odds = std::log(ogm_value / (1 - ogm_value)); //计算格子的log_odds值
// if(i == show_i)
// std::cout << "\033[31m" << ogm_value << "\033[0m" << std::endl;
}
map.clear();
map = ogm;
}
void DynamicCloudDetector::set_clear_grid_cells(const std::vector<double> &beam_list, const std::vector<bool> &obstacle_indices, OccupancyGridMap &map)
{
std::vector<bool> clear_indices(grid_num_, false);
const double beam_angle_resolution = 2.0 * M_PI / (double)beam_num_; //获取激光的角度分辨率
for (int i = 0; i < beam_num_; i++)
{
double direction = i * beam_angle_resolution - M_PI; //获取激光的方向
direction = atan2(sin(direction), cos(direction)); //获取激光的方向
// std::cout << i << ": " << direction << ", " << beam_list[i] << std::endl;
const double c = cos(direction);
const double s = sin(direction);
for (double range = 0.0; range < beam_list[i]; range += resolution_)
{
const double x = range * c;
const double y = range * s;
if (is_valid_point(x, y))
{
const int index = get_index_from_xy(x, y); //获取格子的索引
if (!obstacle_indices[index])
{
clear_indices[index] = true; //设置格子为可清除的
}
else
{
break;
}
}
else
{
break;
}
}
}
for (int i = 0; i < grid_num_; ++i)
{
if (clear_indices[i])
{
map[i].add_log_odds(-log_odds_decrease_);
}
}
}
void DynamicCloudDetector::process(void)
{
ros::spin();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "dynamic_cloud_detector");
DynamicCloudDetector dynamic_cloud_detector;
dynamic_cloud_detector.process();
return 0;
}
当然这套方法最值得借鉴的是diff的求解，如果需要追求精度，仍然推荐使用kd-tree的方法完成动态障碍物的剔除。
评论（2）
您还未登录，请登录后发表或查看评论