本发明涉及一种3d目标检测与跟踪方法，特别涉及一种基于相机与激光雷达的3d目标检测与跟踪方法。
背景技术：
目前，无人驾驶技术的发展对提高道路交通安全，缓解城市交通拥堵，提升出行效率具有重要意义。无人驾驶系统主要包括环境感知，决策规划与控制执行等模块，其中环境感知系统主要负责感知车辆周围环境，包括检测障碍物的类别、位置和对障碍物进行跟踪等，是实现无人驾驶的前提和基础。
基于视觉的目标检测方法主要分为两大类，即双阶段法和单阶段法。双阶段法的典型代表是fasterrcnn，这类方法检测精度高，但是检测速度慢；单阶段法的典型代表是yolo，这类方法在检测精度稍微下降的前提下，极大的提高了检测速度。而且随着深度学习的发展，目前，单阶段网络可以在保证精度的前提下，实现实时检测。但是，基于视觉的目标检测方法受到相机成像的限制，缺失了深度信息，无法感知障碍物的空间位置，因此无法为自动驾驶提供直接决策依据。
激光雷达作为环境感知系统最重要的传感器之一，可以提供精确的深度信息，而且具有测距范围远，不易受光照和天气影响等优点。但是激光雷达采集的点云数据具有稀疏性和无序性，使得激光雷达难以对目标物体进行分类。
因此，基于单个传感器的感知系统无法满足自动驾驶的需求。基于多传感器融合的感知系统，在对多传感器进行较好的时空同步后，可以弥补单一传感器的不足，发挥各自传感器的优势，实现物体在3d空间的检测和追踪，从而为无人驾驶提供决策依据。
技术实现要素：
本发明的目的是为了解决现有的基于单个传感器的感知系统无法满足自动驾驶需求的问题而提供的一种基于相机与激光雷达的3d目标检测与跟踪方法。
本发明提供的基于相机与激光雷达的3d目标检测与跟踪方法，其方法如下所述：
步骤1、标定相机内参及激光雷达和相机之间的外参，并通过相机和激光雷达分别采集图像数据和点云数据；
步骤2、利用目标检测算法对图像进行目标检测，提取2d检测边框并分配编号；
步骤3、对图像进行特征点检测，描述子提取，以及前后两帧图像间的特征点匹配；
步骤4、对点云数据做数据预处理后进行平面拟合，去除地面点云；
步骤5、根据激光雷达和相机的外参将非地面点云投影到对应检测图像上；
步骤6、根据2d检测边框，提取对应点云，并基于欧式几何进行聚类，去除外点，提取出物体3d检测边框；
步骤7、根据前后两帧图像的2d检测边框和特征点匹配结果，对两帧图像的检测边框进行匹配关联，并替换编号，实现物体跟踪。
步骤1中相机内参标定具体方式如下：
自制棋盘格,采用ros系统或matlab的自带的标定工具箱进行标定，得到相机内参矩阵k；
在标定好相机内参并安装好激光雷达与相机后，以棋盘格标定板为目标，分别提出点云数据和图像数据的对应特征点坐标，采用最小化重投影误差的优化方式得出激光雷达和相机坐标系之间的旋转矩阵r和平移向量t。
步骤2中目标检测算法为单阶段目标检测算法，并采用迁移学习进行训练,具体如下：
(1)、制作数据集；
(2)、对数据集中的边框进行聚类分析，提取先验框；
(3)、进行数据增强，并利用目标检测网络原通用物体检测权重作为预训练权重，迭代训练至收敛，保存最好权重；
在原有检测网络输出的边框坐标，物体类比和置信度的基础上为每个检测边框增加追踪编号、对应点云、关键点和匹配点的存储结构，便于进行匹配追踪与点云聚类。
步骤3中的特征点检测算法和描述子提取为常用的特征点检测算法和描述子提取，具体如下：
(1)、特征点检测：所选算法包括但不限于harris或fast或brisk或orb或akaze和sift；
(2)、为提高特征点匹配的准确程度，需要为检测出的特征点增加描述子，描述子提取的方式包括但不限于brief或orb或freak或akaze和sift；
(3)、特征点匹配采用暴力匹配算法或k近邻匹配算法。
步骤4中去除地面点云的具体步骤如下：
(1)、设置距离阈值和最大迭代次数；
(2)、在点云中随机选择三个点：p1＝(x1,y1,z1)，p2＝(x2,y2,z2)，p3＝(x3,y3,z3)；
(3)、构造平面参数，具体参数如下：
a＝(y2-y1)*(z3-z1)-(z2-z1)*(y3-y1)；
b＝(z2-z1)*(x3-x1)-(x2-x1)*(z3-z1)；
c＝(x2-x1)*(y3-y1)-(y2-y1)*(x3-x1)；
d＝-(a*x1 b*y1 c*z1)；
(4)、计算点p(x,y,z)到平面的距离：
d＝|ax by cz d|/sqrt(a*a b*b c*c)；
(5)、在最大迭代次数之内，根据各点到平面的距离，选取距离平面在距离阈值之内的点最多的拟合平面作为地面拟合平面；
(6)、根据地面点云索引，在原始点云中去除属于地面的点。
步骤5中，点云投影的具体步骤如下：
(1)、对非地面点云数据进行裁剪，去除x方向点坐标为负数的点云，使得相机与激光雷达具有同样的视场范围；
(2)、根据两传感器之间的外参，将裁剪后点云中的点p(x,y,z)投影到图像平面，具体公式如下：
[u,v,1]＝k(rp t)；
其中，k是相机内参矩阵，r和t是激光雷达和相机坐标系之间的旋转矩阵和平移向量，(u,v)则是点p(x,y,z)对应的图像坐标；
步骤6中聚类物体3d检测边框具体步骤如下：
(1)、根据点云投影坐标和检测边框，筛选框内点云；
(2)、根据框内点云构建kd树，并根据欧式距离进行聚类；
(3)、如果聚类数量为0，则直接处理下一个边框；如果聚类数量为1，则继续进行下一步；如果聚类结果超过1，则只保留点数最多的聚类点云作为当前物体点云，实现外点去除后,继续进行下一步。
(4)、循环求出物体在x、y和z方向的最大和最小坐标；
(5)、利用pac算法求得当前点云在x和y轴的主方向，得出物体的3d包围盒。
步骤7中物体跟踪的具体步骤如下：
(1)、以前一帧中的2d检测边框为基准，循环查找当前帧中的2d检测边框匹配点；
(2)、大于匹配阈值且匹配点数最多的两个检测边框则认为匹配成功，将前一帧中检测框的编号替换为当前中检测边框的编号；
(3)、对于前一帧中没有跟踪上的边框则舍弃；
(4)、对于当前帧中新出现的物体分配新的编号。
本发明的有益效果：
本发明提供的基于相机与激光雷达的3d目标检测与跟踪方法是利用成熟的2d目标检测算法提出的检测边框和激光雷达与相机的投影图像，对点云进行聚类和分类，实现了3d物体检测。克服了相机缺失深度信息与激光雷达分类物体困难以及单一传感器对环境依赖性强,鲁棒性差的缺点且可靠性高、准确性好，满足了无人驾驶对环境感知的要求。本发明结合图像特征点匹配与2d检测边框和点云信息，实现了物体的3d目标跟踪，可以为无人驾驶物体轨迹预测提供基础。
附图说明
图1为本发明所述方法的整体流程图。
图2为本发明所述特征点采集示意图。
图3为本发明所述特征点匹配示意图。
图4为本发明所述原始点云示意图。
图5为本发明所述滤除地面后的点云示意图。
图6为本发明所述点云映射到对应检测图像示意图。
图7为本发明所述聚类物体3d检测边框流程图。
图8为本发明所述3d检测示意图。
图9为本发明所述匹配跟踪示意图。
具体实施方式
请参阅图1至图9所示：
本发明提供的基于相机与激光雷达的3d目标检测与跟踪方法，其方法如下所述：
步骤1、标定相机内参及激光雷达和相机之间的外参，并通过相机和激光雷达分别采集图像数据和点云数据；
步骤2、利用目标检测算法对图像进行目标检测，提取2d检测边框并分配编号；
步骤3、对图像进行特征点检测，描述子提取，以及前后两帧图像间的特征点匹配；
步骤4、对点云数据做数据预处理后进行平面拟合，去除地面点云；
步骤5、根据激光雷达和相机的外参将非地面点云投影到对应检测图像上；
步骤6、根据2d检测边框，提取对应点云，并基于欧式几何进行聚类，去除外点，提取出物体3d检测边框；
步骤7、根据前后两帧图像的2d检测边框和特征点匹配结果，对两帧图像的检测边框进行匹配关联，并替换编号，实现物体跟踪。
步骤1中相机内参标定具体方式如下：
自制棋盘格,采用ros系统或matlab的自带的标定工具箱进行标定，得到相机内参矩阵k；
在标定好相机内参并安装好激光雷达与相机后，以棋盘格标定板为目标，分别提出点云数据和图像数据的对应特征点坐标，采用最小化重投影误差的优化方式得出激光雷达和相机坐标系之间的旋转矩阵r和平移向量t。
步骤2中目标检测算法为单阶段目标检测算法，并采用迁移学习进行训练,具体如下：
(1)、制作数据集；
(2)、对数据集中的边框进行聚类分析，提取先验框；
(3)、进行数据增强，并利用目标检测网络原通用物体检测权重作为预训练权重，迭代训练至收敛，保存最好权重；
在原有检测网络输出的边框坐标，物体类比和置信度的基础上为每个检测边框增加追踪编号、对应点云、关键点和匹配点的存储结构，便于进行匹配追踪与点云聚类。
步骤3中的特征点检测算法和描述子提取为常用的特征点检测算法和描述子提取，具体如下：
(1)、特征点检测：所选算法包括但不限于harris或fast或brisk或orb或akaze和sift；
(2)、为提高特征点匹配的准确程度，需要为检测出的特征点增加描述子，描述子提取的方式包括但不限于brief或orb或freak或akaze和sift；
(3)、特征点匹配采用暴力匹配算法或k近邻匹配算法。
步骤4中去除地面点云的具体步骤如下：
(1)、设置距离阈值和最大迭代次数；
(2)、在点云中随机选择三个点：p1＝(x1,y1,z1)，p2＝(x2,y2,z2)，p3＝(x3,y3,z3)；
(3)、构造平面参数，具体参数如下：
a＝(y2-y1)*(z3-z1)-(z2-z1)*(y3-y1)；
b＝(z2-z1)*(x3-x1)-(x2-x1)*(z3-z1)；
c＝(x2-x1)*(y3-y1)-(y2-y1)*(x3-x1)；
d＝-(a*x1 b*y1 c*z1)；
(4)、计算点p(x,y,z)到平面的距离：
d＝|ax by cz d|/sqrt(a*a b*b c*c)；
(5)、在最大迭代次数之内，根据各点到平面的距离，选取距离平面在距离阈值之内的点最多的拟合平面作为地面拟合平面；
(6)、根据地面点云索引，在原始点云中去除属于地面的点。
步骤5中，点云投影的具体步骤如下：
(1)、对非地面点云数据进行裁剪，去除x方向点坐标为负数的点云，使得相机与激光雷达具有同样的视场范围；
(2)、根据两传感器之间的外参，将裁剪后点云中的点p(x,y,z)投影到图像平面，具体公式如下：
[u,v,1]＝k(rp t)；
其中，k是相机内参矩阵，r和t是激光雷达和相机坐标系之间的旋转矩阵和平移向量，(u,v)则是点p(x,y,z)对应的图像坐标；
步骤6中聚类物体3d检测边框具体步骤如下：
(1)、根据点云投影坐标和检测边框，筛选框内点云；
(2)、根据框内点云构建kd树，并根据欧式距离进行聚类；
(3)、如果聚类数量为0，则直接处理下一个边框；如果聚类数量为1，则继续进行下一步；如果聚类结果超过1，则只保留点数最多的聚类点云作为当前物体点云，实现外点去除后,继续进行下一步。
(4)、循环求出物体在x、y和z方向的最大和最小坐标；
(5)、利用pac算法求得当前点云在x和y轴的主方向，得出物体的3d包围盒。
步骤7中物体跟踪的具体步骤如下：
(1)、以前一帧中的2d检测边框为基准，循环查找当前帧中的2d检测边框匹配点；
(2)、大于匹配阈值且匹配点数最多的两个检测边框则认为匹配成功，将前一帧中检测框的编号替换为当前中检测边框的编号；
(3)、对于前一帧中没有跟踪上的边框则舍弃；
(4)、对于当前帧中新出现的物体分配新的编号。
1.一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：其方法如下所述：
步骤1、标定相机内参及激光雷达和相机之间的外参，并通过相机和激光雷达分别采集图像数据和点云数据；
步骤2、利用目标检测算法对图像进行目标检测，提取2d检测边框并分配编号；
步骤3、对图像进行特征点检测，描述子提取，以及前后两帧图像间的特征点匹配；
步骤4、对点云数据做数据预处理后进行平面拟合，去除地面点云；
步骤5、根据激光雷达和相机的外参将非地面点云投影到对应检测图像上；
步骤6、根据2d检测边框，提取对应点云，并基于欧式几何进行聚类，去除外点，提取出物体3d检测边框；
步骤7、根据前后两帧图像的2d检测边框和特征点匹配结果，对两帧图像的检测边框进行匹配关联，并替换编号，实现物体跟踪。
2.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤1中相机内参标定具体方式如下：
自制棋盘格,采用ros系统或matlab的自带的标定工具箱进行标定，得到相机内参矩阵k；
在标定好相机内参并安装好激光雷达与相机后，以棋盘格标定板为目标，分别提出点云数据和图像数据的对应特征点坐标，采用最小化重投影误差的优化方式得出激光雷达和相机坐标系之间的旋转矩阵r和平移向量t。
3.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤2中目标检测算法为单阶段目标检测算法，并采用迁移学习进行训练，具体如下：
(1)、制作数据集；
(2)、对数据集中的边框进行聚类分析，提取先验框；
(3)、进行数据增强，并利用目标检测网络原通用物体检测权重作为预训练权重，迭代训练至收敛，保存最好权重；
在原有检测网络输出的边框坐标，物体类比和置信度的基础上为每个检测边框增加追踪编号、对应点云、关键点和匹配点的存储结构，便于进行匹配追踪与点云聚类。
4.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤3中的特征点检测算法和描述子提取为常用的特征点检测算法和描述子提取，具体如下：
(1)、特征点检测：所选算法包括但不限于harris或fast或brisk或orb或akaze和sift；
(2)、为提高特征点匹配的准确程度，需要为检测出的特征点增加描述子，描述子提取的方式包括但不限于brief或orb或freak或akaze和sift；
(3)、特征点匹配采用暴力匹配算法或k近邻匹配算法。
5.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤4中去除地面点云的具体步骤如下：
(1)、设置距离阈值和最大迭代次数；
(2)、在点云中随机选择三个点：p1＝(x1,y1,z1)，p2＝(x2,y2,z2)，p3＝(x3,y3,z3)；
(3)、构造平面参数，具体参数如下：
a＝(y2-y1)*(z3-z1)-(z2-z1)*(y3-y1)；
b＝(z2-z1)*(x3-x1)-(x2-x1)*(z3-z1)；
c＝(x2-x1)*(y3-y1)-(y2-y1)*(x3-x1)；
d＝-(a*x1 b*y1 c*z1)；
(4)、计算点p(x,y,z)到平面的距离：
d＝|ax by cz d|/sqrt(a*a b*b c*c)；
(5)、在最大迭代次数之内，根据各点到平面的距离，选取距离平面在距离阈值之内的点最多的拟合平面作为地面拟合平面；
(6)、根据地面点云索引，在原始点云中去除属于地面的点。
6.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤5中点云投影的具体步骤如下：
(1)、对非地面点云数据进行裁剪，去除x方向点坐标为负数的点云，使得相机与激光雷达具有同样的视场范围；
(2)、根据两传感器之间的外参，将裁剪后点云中的点p(x,y,z)投影到图像平面，具体公式如下：
[u,v,1]＝k(rp t)；
其中，k是相机内参矩阵，r和t是激光雷达和相机坐标系之间的旋转矩阵和平移向量，(u,v)则是点p(x,y,z)对应的图像坐标。
7.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤6中聚类物体3d检测边框具体步骤如下：
(1)、根据点云投影坐标和检测边框，筛选框内点云；
(2)、根据框内点云构建kd树，并根据欧式距离进行聚类；
(3)、如果聚类数量为0，则直接处理下一个边框；如果聚类数量为1，则继续进行下一步；如果聚类结果超过1，则只保留点数最多的聚类点云作为当前物体点云，实现外点去除后,继续进行下一步；
(4)、循环求出物体在x、y和z方向的最大和最小坐标；
(5)、利用pac算法求得当前点云在x和y轴的主方向，得出物体的3d包围盒。
8.根据权利要求1所述的一种基于相机与激光雷达的3d目标检测与跟踪方法，其特征在于：所述的步骤7中物体跟踪的具体步骤如下：
(1)、以前一帧中的2d检测边框为基准，循环查找当前帧中的2d检测边框匹配点；
(2)、大于匹配阈值且匹配点数最多的两个检测边框则认为匹配成功，将前一帧中检测框的编号替换为当前中检测边框的编号；
(3)、对于前一帧中没有跟踪上的边框则舍弃；
(4)、对于当前帧中新出现的物体分配新的编号。
技术总结