1 前视图基本原理

????????雷达在工作时通常是围绕一个轴进行旋转扫描。对于单线激光雷达来说，旋转扫描数据可以得到一个圆形投影；而对于多线激光雷达来说，旋转扫描数据可以得到一个圆柱形投影。

????????雷达点云中点的距离有远有近，那么如何投影成一个圆柱面呢？答案是按照方向上来投影。每次采集一个点的方向都是不同的，也不会出现远近重叠。因此，圆柱面投影是按照方向进行投影，方向由雷达的照射角度来进行表征。

2 圆柱投影几何解释

????????以KITTI激光雷达点云为例，x表示车辆行驶方向，y表示车身方向，z表示高度方向。定义xy平面的方向角为θ，z方向上的方向角为φ。如下图所示，θ由反正切arctan（y, x）得到。在numpy中，arctan2将反正切函数的值域从（-pi/2, pi/2）变换到（-pi, pi）。

?????????从图中可以看到，车正前方（x轴方向）角度为0，正后方右侧角度为-pi，正后方左侧角度为pi。那么水平扫描方向认为是从车身正后方逆时针开始旋转一周，起始位置为投影后图像水平零点，终止位置为投影后图像的宽度。将圆柱投影面展开矩形图像平面的方法，相当于从正后方将圆柱面剪开得到。

????????同样道理，垂直方向上的角度φ由反正切arctan（z, sqrt(x^2+y^2)）得到。

?????????KITTI用到的激光雷达水平方向上的角度分辨率为0.09度，总视角为360度。垂直方向上的视角为26.8度，共64线，分辨率约等于0.42度。（分辨率取值的描述可能有误，欢迎指正。）那么，投影后FV图像的坐标img_x等于θ除以水平角度分辨率；img_y等于φ除以垂直角度分辨率。雷达参数如下：

1 × Velodyne HDL-64E rotating 3D laser scanner, 
10 Hz, 64 beams, 0.09 degree angular resolution, 
2 cm distance accuracy, collecting ～ 1.3 million points/second,
field of view: 360? horizontal, 26.8? vertical, range: 120 m

3 前视图获取步骤

? ? ? （1）读取雷达数据，Nx4，x、y、z、r。

? ? ? （2）计算每个点的方向角，θ、φ。

? ? ? （3）根据方向角和角度分辨率计算前视图图像坐标，img_x、img_y。

? ? ? （4）用雷达反射强度r作为前视图的灰度值。

? ? ? （5）结果可视化。

4 参考代码

? ? ? ? 数据采用的是Mini Kitti数据集，详细介绍和下载地址请参考：KITTI数据集简介 — Mini KITTI_Coding的叶子的博客-CSDN博客。

# -*- coding: utf-8 -*-
"""
乐乐感知学堂公众号
@author: https://blog.csdn.net/suiyingy
"""

import os
import cv2
import numpy as np

def test():
    lidar_path = os.path.join('./data/KITTI/training', "velodyne/")
    lidar_file = lidar_path + '/' + '000016' + '.bin'

    #加载雷达数据
    print("Processing: ", lidar_file)
    lidar = np.fromfile(lidar_file, dtype=np.float32)
    lidar = lidar.reshape((-1, 4))
    v_res = 26.8/64
    h_res = 0.09
    # 转换为弧度
    v_res_rad = v_res * (np.pi/180)
    h_res_rad = h_res * (np.pi/180)
    angels = np.zeros((lidar.shape[0], 2))
    angels[:, 0] = np.arctan2(lidar[:, 1], lidar[:, 0])
    angels[:, 1] = np.arctan2(lidar[:, 2], np.sqrt(lidar[:, 0]**2+lidar[:, 1]**2))
    img_x = angels[:, 0]/h_res_rad
    img_x = img_x.astype(np.int)
    img_x = img_x - min(img_x)
    img_y = angels[:, 1]/v_res_rad
    img_y = img_y.astype(np.int)
    print(min(img_y), max(img_y))
    img_y = img_y - min(img_y)
    img_y = max(img_y) - img_y
    print(min(img_x), max(img_x))
    print(min(img_y), max(img_y))
    fv_img = np.zeros((max(img_y)+1, max(img_x)+1))
    fv_img[img_y, img_x] = lidar[:, -1]
    cv2.namedWindow('FV', 0)
    cv2.imshow('FV', fv_img)
    cv2.waitKey(0)
    
if __name__ == '__main__':
    test()