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，初始化時候為當前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<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 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<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); //創建柵格地圖的障礙物索引列表，默認為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_); //設置柵格地圖的清除柵格 }

下面這部分主要是設置分割點云的信息，并獲取分辨率

…詳情請參照古月居

總結

以上是生活随笔為你收集整理的3D帧间匹配-----剔除动态障碍物的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。