【从kitti开始自动驾驶】--10.2 测距离jupyter-notebook实现

“命运对勇士低语:"你无法抵御风暴”

• 1. 程序分析
• 2.jupyter-notebook实现
• • 2.1 绘制2D框
  • 2.2 绘制3D框
  • 2.3 封装点到线算法
  • 2.4 调用点到线方法
  • 2.5 封装两矩形最短距离算法
  • 2.6 调用查看效果
• 3. jupytet源码

10.1得到了计算点到直线距离公式的数学表达式,本节在jupyter-notebook上实现:对任意两个矩形框,实现测量出来最短距离的函数,图形化显示出来

本节程序在3D侦测盒链接的程序后,增添代码.
程序路径为/test3_autodrive_ws/src/jupyter_prj/3D_tracking.ipynb工程后

1. 程序分析

• 两个侦测盒只需要实现在XOY平面的测距即可.因此抽象成两个矩形即可.

2.jupyter-notebook实现 2.1 绘制2D框

因为velodne坐标一直在00,根据图示给合适值即可.
再提取一个物体的侦测盒,只取二维坐标的

ego_car = np.array([[2.15, 0.9, -1.73], [2.15, -0.9, -1.73], [-1.95, -0.9, -1.73], [-1.95, 0.9, -1.73],
                   [2.15, 0.9, -0.23], [2.15, -0.9, -0.23], [-1.95, -0.9, -0.23], [-1.95, 0.9, -0.23]])


plt.axes().set_aspect('equal', 'datalim')
plt.plot(ego_car[:5, 0], ego_car[:5, 1], '-o')
plt.plot(corners_3d_velo.T[:5, 0], corners_3d_velo.T[:5, 1], '-o')

• 效果如下:

2.2 绘制3D框

代码如下:

fig, ax = plt.subplots(figsize=(20, 10))
draw_point_cloud(ax, points[::5], axes=[0,1])
draw_box(ax, corners_3d_velo, axes=[0, 1], color='r')
draw_box(ax, ego_car.T, axes=[0, 1], color='b')

效果如下:

2.3 封装点到线算法

• 输入量为点,线段两端点
• 锐角三角形:使用公式求解
• 钝角三角形比较PA和PB的距离即可
• 注意:np.dot是内积

# 该函数返回类型 距离, 坐标
def distance_point_to_segment(P, A, B):
    AP = P - A
    BP = P - B
    AB = B - A
    if np.dot(AB, AP)>=0 and np.dot(-AB, BP)>=0:     #向量点积,投影在线上
        return np.abs(np.cross(AP, AB))/np.linalg.norm(AB), np.dot(AP, AB)/np.dot(AB, AB) * AB + A
    
    d_PA = np.linalg.norm(AP)
    d_PB = np.linalg.norm(BP)
    # 投影在线外
    if d_PA < d_PB:
        return d_PA, A
    return d_PB, B
        

2.4 调用点到线方法

• 假定线外u一点为(1,2), 线段端点为(0, 1)和(2, 0)
• 调用后效果如下

2.5 封装两矩形最短距离算法

程序思路是:

1. 分两个for并列循环,分别对两个矩形进行遍历比较
2. 在每个for里,首先四个点四次循环
3. 其次在每个点对另外一个矩形的四条边进行比较
4. 然后不断对比最小值即可

def min_distance_cupoints(cub1, cub2):
    minD = 1e5   #先给一个非常大的值
    # 最短距离在本车的端点
    for i in range(4):    #四个顶点遍历
        for j in range(4): #另一个长方形的四条边遍历
            d, Q = distance_point_to_segment(cub1[i, :2], cub2[j, :2], cub2[j+1, :2])
            if d < minD:  #迭代最小的几个点的数值
                minD = d
                minP = ego_car[i, :2]
                minQ = Q
                
    # 最短距离在其他物体的端点
    for i in range(4):    #四个顶点遍历
        for j in range(4): #另一个长方形的四条边遍历
            d, Q = distance_point_to_segment(cub2[i, :2], cub1[j, :2], cub1[j+1, :2])
            if d < minD:  #迭代最小的几个点的数值
                minD = d
                minP = corners_3d_velo.T[i, :2]
                minQ = Q
   
    # 遍历完毕
    return minP, minQ, minD

2.6 调用查看效果

取点,代码如下:

plt.axes().set_aspect('equal', 'datalim')
plt.plot(ego_car[:5, 0], ego_car[:5, 1], '-o')
plt.plot(corners_3d_velo.T[:5, 0], corners_3d_velo.T[:5, 1], '-o')
minP, minQ, minD = min_distance_cupoints(ego_car, corners_3d_velo.T)
plt.plot((minP[0], minQ[0]), (minP[1], minQ[1]), 'y-')
print(minD)

minD = 9.75815835...
效果如下几种情况:

3. jupytet源码

源码放在资源3D_tracking (copy).ipynb中