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

news2024/11/23 14:58:42

0. 简介

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

1. 主要方法

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

2. 动态障碍物过滤方法

本文介绍的方法，适用于所有的2D地面场景，由于地面的信息可以简化为 x , y , y a w x,y,yaw 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_); //设置栅格地图的清除栅格
}

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

…详情请参照古月居

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/46539.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

安全标准汇总

安全标准汇总

文章目录资源导航法律法规0x01常见标准代号0x02 2022年新发布0x03 按体系分类一般性法律规定规范和惩罚信息网络犯罪的法律直接针对信息安全的特别规定具体规范信息安全技术、信息安全管理0x04 安全等级保护0x05 数据安全声明资源导航 国家标准全文公开系统&#xff1a;国家标…
阅读更多...
代码随想录训练营第35天|LeetCode 860.柠檬水找零、406.根据身高重建队列、452. 用最少数量的箭引爆气球

代码随想录训练营第35天|LeetCode 860.柠檬水找零、406.根据身高重建队列、452. 用最少数量的箭引爆气球

参考 代码随想录 题目一&#xff1a;LeetCode 860.柠檬水找零 这个题在做的时候有误解&#xff0c;第一不能对数组bills排序&#xff0c;只能按照给定的顺序处理&#xff1b;第二&#xff0c;只能从头开始处理&#xff0c;不能中间的某个点开始。 其实这个题很简单&#xff…
阅读更多...
synchronized锁升级过程

synchronized锁升级过程

【一些面试真题】&#xff1a; 阿里P9——0x80的执行过程。 【 重温CAS过程 】&#xff1a; 【硬件】&#xff1a; Lock指令在执行后面指令的时候锁定一个北桥信号&#xff08;不采用锁总线的方式&#xff09;。 【用户态 与 内核态】&#xff1a; 作为操作系统来说&#x…
阅读更多...
WebRTC学习笔记二 基础概念

WebRTC学习笔记二 基础概念

一、WebRTC与架构 简单来说&#xff0c;WebRTC 是一个可以在 Web 应用程序中实现音频&#xff0c;视频和数据的实时通信的开源项目。在实时通信中&#xff0c;音视频的采集和处理是一个很复杂的过程。比如音视频流的编解码、降噪和回声消除等&#xff0c;但是在 WebRTC 中&…
阅读更多...
C. Strange Test(位运算或)

C. Strange Test(位运算或)

Problem - 1632C - Codeforces 伊戈尔正在读11年级。明天他将不得不写一份信息学测试&#xff0c;由学校最严格的老师帕维尔-杰尼索维奇负责。 伊戈尔知道测试将如何进行&#xff1a;首先&#xff0c;老师会给每个学生两个正整数a和b&#xff08;a<b&#xff09;。之后&…
阅读更多...
[2022-11-28]神经网络与深度学习 hw10 - LSTM和GRU

[2022-11-28]神经网络与深度学习 hw10 - LSTM和GRU

contentshw10 - LSTM 和GRU相关习题task 1题目内容题目分析题目解答题目总结task 2题目内容题目分析题目解答题目总结task 3题目内容题目分析题目解答题目总结task 4题目内容题目分析题目解答问题总结hw10 - LSTM 和GRU相关习题 task 1 题目内容 当使用公式htht−1g(xt,ht−…
阅读更多...
Linux系统中利用C语言控制LED的方法

Linux系统中利用C语言控制LED的方法

大家好&#xff0c; 今天主要和大家聊一聊&#xff0c;如何利用C语言控制LED灯的实验。 目录 ​第一&#xff1a;C语言板控制LED灯简介 第二&#xff1a;实验程序实现 ​第三&#xff1a;C语言实验控制程序 ​第一&#xff1a;C语言板控制LED灯简介 实际工作中很少会使用到汇…
阅读更多...
分离变数法

分离变数法

今天是2022年11月28号 我的方程学的不太好&#xff0c;一些讲宇宙的&#xff0c;讲技术的&#xff0c;方程实在是看不懂很多方程的解与参数不可分割期末来了 有的人回去了有的人要看光学了我呢&#xff0c;已经废物了&#xff0c;节日快乐&#xff0c;大家伙节日快乐啊&#xf…
阅读更多...
Spirng 痛苦源码学习（二）——手写spring大致总框架（一）

Spirng 痛苦源码学习（二）——手写spring大致总框架（一）

文章目录前言一、总体步骤如下1、spring 文件夹2、myProject 文件夹二、主要coding1、配置文件2、容器3、一些spring中的重要的注解4、项目中的使用5.重要的bean定义信息6、postProcessor重要&#xff0c;前置通知和后置主要项目的截图前言 本文主要基于spring的注解的方式完成…
阅读更多...
Spring相关源码解读

Spring相关源码解读

框架1.ApplicationContext refresh的流程2.spring bean 的生命周期3.spring事务失效的几种场景以及原因4.springMVC执行流程5.一些注解&#xff08;1&#xff09;Configuration&#xff08;2&#xff09;Import&#xff08;3&#xff09;SpringBootApplication6.spring中有哪些…
阅读更多...
BP神经网络详解，Python实现求解异或问题

BP神经网络详解，Python实现求解异或问题

BP神经网络 符号及其含义 nln_lnl​表示第lll层神经元的个数&#xff1b;f(⋅)f()f(⋅)表示神经元的激活函数&#xff1b;W(l)∈Rni∗ni−1W^{(l)}\in\mathbb R^{n_i*n_{i-1}}W(l)∈Rni​∗ni−1​表示第l−1l-1l−1层到第lll层的权重矩阵&#xff1b;wij(l)w_{ij}^{(l)}wij(l…
阅读更多...
基于tensorflow的ResNet50V2网络识别动物

基于tensorflow的ResNet50V2网络识别动物

前言 之前很多人在&#xff0c;如何进行XXX的识别&#xff0c;对应的神经网络如何搭建。对应神经网络怎么搭建&#xff0c;我也是照本宣科&#xff0c;只能说看得懂而已&#xff0c;没有对这块进行深入的研究&#xff0c;但是现在tensorflow&#xff0c;paddle这些工具&#x…
阅读更多...
长期稳定的项目—steam搬砖

长期稳定的项目—steam搬砖

大家好&#xff0c;我是阿阳 steam搬砖项目一直稳稳定定的进行着&#xff0c;有些朋友基本都观察了近2年 所以很多人问我公众号的项目是不能做了吗&#xff1f;怎么最近做新的去了&#xff1f;很明显这是几乎不可能的事情&#xff0c;steam做2年了&#xff0c;本公众号都能翻到…
阅读更多...
这几个数据分析项目，让我看到了什么才叫专业！！

这几个数据分析项目，让我看到了什么才叫专业！！

大家好&#xff0c;我是小一 新的一周又来了&#xff0c;从今天开始&#xff0c;会出一个新的系列《数分实验室》 实验室会介绍一些有内核、有科技的数据分析实战项目。 项目数据集、源代码都是公开的&#xff0c;非常适合想练手但是又没数据、没参考案例的同学 今天先热热…
阅读更多...
ES的基础概念

ES的基础概念

1、ES是什么 Elasticsearch 是一个分布式可扩展的实时搜索和分析引擎,一个建立在全文搜索引擎 Apache Lucene(TM) 基础上的搜索引擎.当然 Elasticsearch 并不仅仅是 Lucene 那么简单&#xff0c;它不仅包括了全文搜索功能&#xff0c;还可以进行以下工作:分布式实时文件存储&am…
阅读更多...
6-1分支限界法

6-1分支限界法

6-1分支限界法 1.分支限界法与回溯法的不同 &#xff08;1&#xff09;求解目标: 回溯法的求解目标是找出解空间树中满足约束条件的所有解&#xff08;或一个最优解&#xff09;&#xff0c; 而分支限界法的求解目标则是找出满足约束条件的一个解&#xff08;或最优解&#x…
阅读更多...
组织机器学习代码

组织机器学习代码

组织机器学习代码 从note本转移到 Python 脚本时组织代码。 Intuition 有组织的代码就是有可读的、可重现的、健壮的代码。您的团队、经理&#xff0c;最重要的是&#xff0c;您未来的自己&#xff0c;将感谢您为组织工作付出的最初努力。在本课中&#xff0c;将讨论如何将代码…
阅读更多...
pytest测试框架入门1

pytest测试框架入门1

pytest单元测试框架 单元测试是指在软件开发当中&#xff0c;针对软件的最小单位&#xff08;函数&#xff0c;方法&#xff09;进行正确性的检查测试 单元测试框架主要做什么 测试发现&#xff1a;从多个文件里面找到我们的测试用例测试执行&#xff1a;按照一定的顺序和规则…
阅读更多...
初学者指南: 使用NumPy数组进行图像处理

初学者指南: 使用NumPy数组进行图像处理

这里写自定义目录标题初学者指南: 使用NumPy数组进行图像处理1、加载图像2、裁剪图像3、分离颜色4、转换5、灰度转换6、图像分割结语初学者指南: 使用NumPy数组进行图像处理 由于图像也可以被视为由数组组成&#xff0c;因此我们也可以使用NumPy执行不同的图像处理任务。在本文…
阅读更多...
【Lilishop商城】No2-6.确定软件架构搭建五（本篇包括定时任务xxl-job）

【Lilishop商城】No2-6.确定软件架构搭建五（本篇包括定时任务xxl-job）

仅涉及后端&#xff0c;全部目录看顶部专栏&#xff0c;代码、文档、接口路径在&#xff1a; 【Lilishop商城】记录一下B2B2C商城系统学习笔记~_清晨敲代码的博客-CSDN博客 全篇只介绍重点架构逻辑&#xff0c;具体编写看源代码就行&#xff0c;读起来也不复杂~ 谨慎&#xf…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023