【目标跟踪】3D点云跟踪

news2025/2/5 0:41:20

文章目录

  • 一、前言
  • 二、代码目录
  • 三、代码解读
    • 3.1、文件描述
    • 3.2、代码框架
  • 四、关联矩阵计算
    • 4.1、ComputeLocationDistance
    • 4.2、ComputeDirectionDistance
    • 4.3、ComputeBboxSizeDistance
    • 4.4、ComputePointNumDistance
    • 4.5、ComputePointNumDistance
    • 4.6、result_distance
  • 五、结果

一、前言

  1. 前博客一直介绍的是视觉方向的跟踪。不过在如今智能驾驶领域,雷达感知仍然占据主要部分。今天来分享下点云3D跟踪。
  2. 视觉跟踪输入就是目标检测的结果。雷达跟踪输入可以是点云检测的结果,也可以是点云聚类的结果。除了一些数据结构、匹配计算,雷达跟踪算法与前面介绍视觉跟踪方法大体相同。
  3. 本篇主要探讨雷达如何进行匹配、关联计算,同时解析下代码结构。参考的是 apollo 代码,整体效果还不错。

二、代码目录

雷达跟踪所有的代码文件
在这里插入图片描述

三、代码解读

3.1、文件描述

文件跳转较多,新手读起代码可能有点吃力。最好记录下每个文件是干什么的,有个大致印象即可。

.h文件描述
object_track.hclass ObjectTrackSet class ObjectTrack
object.h(1) struct Object; (2) struct SensorObjects
tracked_object.hstruct TrackedObject 数据类型 框、点云
hm_tracker.hstruct TrckerParm 全是参数
kalman.h卡尔曼滤波 预测、状态、运动方程
hungarian_matcher.h目标匹配
track_object_distance.h计算匹配矩阵权重
geometry_util.h计算所有点质心、计算3D框、数据转化等
feature_descriptor.h计算目标的形状特征

一些补充:

barycenter 点云几何中心点(质心)

目标框 direction 朝向角 默认状态(1, 0, 0)

目标框 size 长宽高 (length, width, height)

目标状态 (x, y, z, vx, vy, vz)

状态方程 匀速 x = x + v * t

3.2、代码框架

阅读代码整体顺序如下

(1)跟踪

hm_tracker.cpp

bool HmObjectTracker::Track()

A 初始化 B 数据转化 输入 C 预测 D 匹配 E 更新 F 结果

hm_tracker.cpp 文件 Track 函数基本就是整个运算主函数了

(2)匹配

hungarian_matcher.cpp

void HungarianMatcher::Match()

A 计算关联矩阵 B 计算连接的组件 C 匹配每个子图

这里面有很多种匹配方式,这里主要运用的是A 计算关联矩阵

(3)关联矩阵计算

track_object_distance.cpp

float TrackObjectDistance::ComputeDistance()

这个函数就是我们这次的主角了。计算目标与目标的距离,然后进行匈牙利匹配。


四、关联矩阵计算

这部分是核心,我们来好好研究下。截取部分代码

double TrackObjectDistance::s_location_distance_weight_ = 0.6;
double TrackObjectDistance::s_direction_distance_weight_ = 0.2;
double TrackObjectDistance::s_bbox_size_distance_weight_ = 0.1;
double TrackObjectDistance::s_point_num_distance_weight_ = 0.1;
double TrackObjectDistance::s_histogram_distance_weight_ = 0.5;

// new_object测量 track_predict预测 
float TrackObjectDistance::ComputeDistance(
    ObjectTrackPtr track, const Eigen::VectorXf& track_predict,
    const std::shared_ptr<TrackedObject>& new_object) {
  // Compute distance for given track & object
  float location_distance = ComputeLocationDistance(track, track_predict, new_object);  // 分速度慢 速度快
  float direction_distance = ComputeDirectionDistance(track, track_predict, new_object);
  float bbox_size_distance = ComputeBboxSizeDistance(track, new_object);
  float point_num_distance = ComputePointNumDistance(track, new_object);
  float histogram_distance = ComputeHistogramDistance(track, new_object);

  float result_distance = s_location_distance_weight_ * location_distance +
                          s_direction_distance_weight_ * direction_distance +
                          s_bbox_size_distance_weight_ * bbox_size_distance +
                          s_point_num_distance_weight_ * point_num_distance +
                          s_histogram_distance_weight_ * histogram_distance;
  return result_distance;
}

4.1、ComputeLocationDistance

计算中心点距离差 取值范围[0, + ∞ +\infty +

当current_object 中 V <= 2m/s 欧式距离

在这里插入图片描述

当current_object 中 V > 2m/s 根据速度方向分解 以速度方向与垂直速度方向建立坐标系。投影速度方向的偏差为1/2倍距离偏差, 投影垂直速度方向的偏差为2倍距离偏差。平方开方求最终偏差。

在这里插入图片描述

可以简单理解为:当目标高速行驶时,在速度方向上的位移偏差会稍大,为了补偿这部分偏差,采取降低方向上的位移权重计算最终位移偏差(我是这么理解的)。

4.2、ComputeDirectionDistance

计算方向上的距离 取值范围[0,2]

计算位移在速度方向上的余弦值cos_theta 最终return 1- cos_theta

在这里插入图片描述

这个也比较好理解。当物体与检测物体位移差方向与预测速度方向相近时,此时更相信是同一个目标。cos值为1时,说明位移偏差与预测速度同方向,则认为这两物体更容易匹配。

当位移偏差为0时,这里有设定默认cos值为0.994。

4.3、ComputeBboxSizeDistance

取值范围[0,1]

这个稍微有点复杂,待我娓娓道来

old_dir 当前目标的方向,默认偏航为0时 默认值为(1, 0, 0)

new_idr 检测目标的方向

old_size 当前目标的尺寸 (bbox.length, bbox.width, bbox.height)

new_size 检测目标的尺寸

在这里插入图片描述
nter&pos_id=img-506WdR2l-1706774251770)

计算dot_00, dot_01。dot_00 可以理解为两目标方向夹角,dot_01理解为目标与另一目标垂直方向夹角。这里不考虑超过90度的夹角,因为目标方向可以是alpha 或 180 - alpha。

在这里插入图片描述

为什么两种情况?可以理解为把目标长与宽对齐(我们事先并不知道目标对应的长宽),先根据目标角度的状态判定。当角度小于45°时,目标长与另一目标的长对齐。否则目标长与另一个目标宽对齐,最终计算长或者宽差值的比例,取最小值当做最终值。

4.4、ComputePointNumDistance

取值范围[0,1]

这个公式比较简单

在这里插入图片描述

这个很容易理解,点云个数越相近,越容易匹配上。

4.5、ComputePointNumDistance

取值范围[0,3]

直方图距离 把目标所有点云以当个坐标轴分为10个区间 再以xyz三轴共分为30个区间。

如果点云都是均匀排布那么目标形状特征 shape_features = [0.1] * 30

shape_features具体计算过程。以x轴为例,y轴、z轴同理。

计算目标点云 x轴最值,把区间划分为10等分。记录所有点在10个区间点云个数。

如果完全均匀排布则结果为

shape_feature_x = [0.1]*10

shape_features = shape_feature_x + shape_feature_y + shape_feature_z

知道了目标形状特征的定义,可得

在这里插入图片描述

4.6、result_distance

最终距离为上述计算的5个距离量乘以对应系数和

在这里插入图片描述

五、结果

由于 rviz 无法显示点云跟踪结果,那我们把雷达跟踪结果 topic 录制下来,然后再可视化。当然也可以在过程中保存图片。

在这里插入图片描述

整体跟踪效果不错。赞!

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

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

相关文章

实现div拖拽demo

示例代码&#xff1a; <!DOCTYPE html> <html lang"en"><head><meta charset"UTF-8" /><meta name"viewport" content"widthdevice-width, initial-scale1.0" /><title>Document</title>&…

51单片机编程应用(C语言):数码管

目录 1.数码管原理 一位数码管引脚定义&#xff1a; 四位一体数码管&#xff1a; 多个数码管同时显示不同数字 51单片机的数码管的原理图 51单片机实现静态显示和动态显示 静态显示&#xff1a; 动态显示&#xff1a; 1.数码管原理 一位数码管引脚定义&#xff1a; 数码…

idea查看日志的辅助插件 --- Grep Console (高亮、取消高亮)

&#x1f680; 分享一款很有用的插件&#xff1a;Grep Console &#x1f680; 我们在查看日志的时候可能会有遗漏&#xff0c;使用这款插件可以让特定的关键词高亮&#xff0c;可以达到不遗漏的效果&#xff01; 如果你是一个开发者或者对日志文件分析感兴趣&#xff0c;不要…

linux搭建jupyter

查看虚拟环境 conda info --envs进入虚拟环境 conda activate my_env pip install jupyter pip install ipykernel1. jupyter notebook启动 1.1 创建临时jupyter notebook任务 jupyter notebook --ip0.0.0.0 --no-browser --allow-root --notebook-dir/home/xxx1.2 jupyter…

Xcode报fatal error: ‘XXX.h‘ file not found

在Xcode中遇到 "fatal error: XXX.h file not found" 的错误通常是由于缺少头文件或头文件路径配置不正确导致的。 以下仅为我的解决方案&#xff1a; 1.点击项目名——>显示此页面 2.选择Build Settings——>输入 Search Paths 3. 点击空白处添加路径 4…

使用浏览器开发工具分析性能

使用浏览器开发工具分析性能 一、网络分析二、性能分析 一、网络分析 1、面板概览&#xff1a; Controls (控件) : 控制面板的功能Filters (过滤器) : 控制在请求列表中显示哪些资源Overview (概览) : 展示检索资源的时间轴&#xff0c;多个垂直堆叠的栏意味着这些资源被同时…

2024杭州国际安防展览会:引领数字城市安全与智能未来

随着科技的不断进步&#xff0c;数字城市已经成为未来城市发展的重要趋势。作为数字城市建设的重要组成部分&#xff0c;安防技术的创新与应用对于保障城市安全、提高生活品质具有重要意义。为此&#xff0c;2024杭州国际安防展览会将于4月份在杭州国际博览中心隆重召开&#x…

储能新纪元:第十三届中国国际储能大会(CIES2023)深度洞察与未来趋势

随着全球能源结构的持续优化和可再生能源的迅猛发展&#xff0c;储能技术作为支撑能源转型的关键力量&#xff0c;日益受到世界各国的关注。 在这一背景下&#xff0c;2023年第十三届中国国际储能大会&#xff08;CIES2023&#xff09;的召开&#xff0c;无疑为行业内外人士提…

PDF中公式转word

效果&#xff1a;实现pdf中公式免编辑 step1: 截图CtrlAltA&#xff0c;复制 step2: SimpleTex - Snip & Get 网页或客户端均可&#xff0c;无次数限制&#xff0c;效果还不错。还支持手写、文字识别 单张图片&#xff1a;选 手写板 step3: 导出结果选择 注&#xff1a;…

网络协议与攻击模拟_12DNS协议及Windows部署DNS服务器

1、了解域名的结构 2、DNS查询过程 3、Windwos server部署DNS服务器 4、分析流量 实施DNS欺骗 再分析 一、DNS域名系统 1、DNS简介 DNS&#xff08;Domain Name system&#xff09;域名系统&#xff0c;作为将域名的IP地址的相互映射关系存放在一个分布式的数据库&#xff0c…

OBB头篇 | 原创自研 | YOLOv8 更换 SEResNeXtBottleneck 头 | 附详细结构图

左图:ResNet 的一个模块。右图:复杂度大致相同的 ResNeXt 模块,基数(cardinality)为32。图中的一层表示为(输入通道数,滤波器大小,输出通道数)。 1. 思路 ResNeXt是微软研究院在2017年发表的成果。它的设计灵感来自于经典的ResNet模型,但ResNeXt有个特别之处:它采用…

小米服务治理——客户端熔断器(Google SRE客户端熔断器)

目录 前言 一、什么是Google SRE熔断器 二、Google SRE 熔断器的工作流程&#xff1a; 三、客户端熔断器 (google SRE 熔断器) golang GRPC 实现 四、客户端熔断器 (google SRE 熔断器) golang GRPC单元测试 大家可以关注个人博客&#xff1a;xingxing – Web Developer …

【动态规划】【C++算法】1340. 跳跃游戏 V

作者推荐 【动态规划】【字符串】【表达式】2019. 解出数学表达式的学生分数 本文涉及知识点 动态规划汇总 LeetCode1340跳跃游戏 V 给你一个整数数组 arr 和一个整数 d 。每一步你可以从下标 i 跳到&#xff1a; i x &#xff0c;其中 i x < arr.length 且 0 < x…

提及闭包函数,不得不说【python装饰器】

什么是装饰器 1. python装饰器是用于拓展原来函数功能的一种函数&#xff0c;目的是在不改变原函数的情况下&#xff0c;给函数增加功能。 2. 装饰器是通过闭包实现&#xff0c;所以讲装饰器首先得知道什么是闭包。 前言 什么是闭包 一、什么是闭包 1. 一个定义在函数内部的函…

[职场] 个人简历教育背景怎么填 #其他#笔记#媒体

个人简历教育背景怎么填 一个人的受教育程度很大部分决定了此人的专业素质&#xff0c;简历中的教育背景是HR衡量求职者的标准&#xff0c;因此教育背景部分在简历中比较靠前。 怎么写教育背景&#xff1f; 1、时间 首先&#xff0c;最重要的是你要注明好时间&#xff0c;时间都…

idea搭建spring5.3.x源码环境

1.写在前面的话 碰到了不少想阅读或者学习spring源码的同学&#xff0c;但是第一步搭建这个源码阅读环境就能难倒了一大批人。下面我就以spring5.3.x这个源码分支&#xff0c;来具体演示一下搭建过程。 2. 下载源码 下载源码这一步&#xff0c;说实话&#xff0c;由于某些原…

STM32控制DS18B20温度传感器获取温度

时间记录&#xff1a;2024/1/28 一、DS18B20温度传感器介绍 &#xff08;1&#xff09;测温范围-55℃~125℃&#xff0c;在-10℃到85℃范围内误差为0.4 &#xff08;2&#xff09;返回的温度数据为16位二进制数据 &#xff08;3&#xff09;STM32和DS18B20通信使用单总线协议…

算法模板 3.二分查找

二分查找 789. 数的范围 - AcWing题库 check函数&#xff08;lr&#xff08;这里要不要1&#xff09;&#xff09;>> 1 &#xff0c;要根据具体情况&#xff0c;如果是r mid的话lr就不用1&#xff0c;l mid的话就要1 #include <bits/stdc.h> using namespace s…

Android Retrofit添加header参数

前言 有些接口需要单独设置header参数 作为标记 如何灵活 或者统一设置请求header呢 注解Headers 顾名思义 Headers是指多个header 放在对象里 统一管理 //获取xxx Headers({"Content-Type: application/json","User-Agent: test"}) POST("xxxx&q…

小程序:类型三级分类

一、效果图片 二、代码 <template><view class"customPosition"><!-- header --><navBar :border"false" :hasBack"true" :title"titleName"></navBar><!-- 查询 --><view class"search…