手机建设银行网站,企业软件项目管理系统,直播网站开发源码下载,flash网站引导页文章目录 一、前言二、代码目录三、代码解读3.1、文件描述3.2、代码框架 四、关联矩阵计算4.1、ComputeLocationDistance4.2、ComputeDirectionDistance4.3、ComputeBboxSizeDistance4.4、ComputePointNumDistance4.5、ComputePointNumDistance4.6、result_distance 五、结果 一… 文章目录 一、前言二、代码目录三、代码解读3.1、文件描述3.2、代码框架 四、关联矩阵计算4.1、ComputeLocationDistance4.2、ComputeDirectionDistance4.3、ComputeBboxSizeDistance4.4、ComputePointNumDistance4.5、ComputePointNumDistance4.6、result_distance 五、结果 一、前言
之前博客一直介绍的是视觉方向的跟踪。不过在如今智能驾驶领域雷达感知仍然占据主要部分。今天来分享下点云3D跟踪。视觉跟踪输入就是目标检测的结果。雷达跟踪输入可以是点云检测的结果也可以是点云聚类的结果。除了一些数据结构、匹配计算雷达跟踪算法与前面介绍视觉跟踪方法大体相同。本篇主要探讨雷达如何进行匹配、关联计算同时解析下代码结构。参考的是 apollo 代码整体效果还不错。 二、代码目录
雷达跟踪所有的代码文件
三、代码解读
3.1、文件描述
文件跳转较多新手读起代码可能有点吃力。最好记录下每个文件是干什么的有个大致印象即可。
.h文件描述object_track.hclass ObjectTrackSet class ObjectTrackobject.h(1) struct Object; (2) struct SensorObjectstracked_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_ptrTrackedObject new_object) {// Compute distance for given track objectfloat 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 检测目标的尺寸 nterpos_idimg-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 录制下来然后再可视化。当然也可以在过程中保存图片。 整体跟踪效果不错。赞
