無人駕駛汽車系統入門（二十六）——基於深度學習的實時激光雷達點雲目標檢測及ROS實現

在前兩篇文章中，我們使用PCL實現了在點雲中對地面的過濾和點雲的分割聚類，通常來說，在這兩步以後我們將對分割出來的對象進行特徵提取，緊接着我們訓練一個分類器實現對這些對象的分類，這是一種基於激光雷達的目標檢測方法。近年來，隨着深度學習在圖像視覺領域的發展，一類基於單純的深度學習模型的點雲目標檢測方法被提出和應用，本文將詳細介紹其中一種模型——SqueezeSeg，並且使用ROS實現該模型的實時目標檢測。

傳統方法VS深度學習方法

實際上，在深度學習方法出現之前，基於點雲的目標檢測已經有一套比較成熟的處理流程：分割地面->點雲聚類->特徵提取->分類，典型的方法可以參考Velodyne的這篇論文：LIDAR-based 3D Object Perception

那麼傳統方法存在哪些問題呢？

第一步的地面分割通常依賴於人爲設計的特徵和規則，如設置一些閾值、表面法線等，泛化能力差；
多階段的處理流程意味着可能產生複合型錯誤——聚類和分類並沒有建立在一定的上下文基礎上，目標周圍的環境信息缺失；
這類方法對於單幀激光雷達掃描的計算時間和精度是不穩定的，這和自動駕駛場景下的安全性要求（穩定，小方差）相悖。

因此，近年來不少基於深度學習的點雲目標檢測方法被提出，本文介紹的SqueezeSeg就是其中一種，這類方法使用深度神經網絡提取點雲特徵，以接近於端到端的處理流程實現點雲中的目標檢測。

論文：SqueezeSeg: Convolutional Neural Nets with Recurrent CRF for Real-Time Road-Object Segmentation from 3D LiDAR Point Cloud, https://arxiv.org/pdf/1710.07368.pdf

SqueezeSeg理論部分

概括

SqueezeSeg使用的是CNN(卷積神經網絡)+CRF（Conditional Random Field，條件隨機場）這樣的結構。其中，CNN採用的是Forrest提出的SqueezeNet網絡（詳情見論文：“SqueezeNet: Alexnet-level accuracy with 50x fewer
parameters and < 0.5mb model size”, https://arxiv.org/pdf/1602.07360.pdf ）, 該網絡使用遠少於AlexNet的參數數量便達到了等同於AlexNet的精度，極少的參數意味着更快的運算速度和小的內存消耗，這是符合車載場景需求的。被預處理過的點雲數據（二維化）將被以張量的形式輸入到這個CNN中，CNN輸出一個同等寬高的標籤映射（label map），實際上就是對每一個像素進行了分類，然而單純的CNN逐像素分類結果會出現邊界模糊的問題，爲解決該問題，CNN輸出的標籤映射被輸入到一個CRF中，這個CRF的形式爲一個RNN，其作用是進一步的矯正CNN輸出的標籤映射。最終的檢測結果論文中使用了DBSCAN算法進行了一次聚類，從而得到檢測的目標實體。

下面我們從預處理出發，首先理解這一點雲目標檢測方法。

點雲預處理

傳統的CNN設計多用於二維的圖像模式識別（寬 $\times$ 高 $\times$ 通道數），三維的點雲數據格式不符合該模式，而且點雲數據稀疏無規律，這對特徵提取都是不利的，因此，在將數據輸入到CNN之前，首先對數據進行球面投影，從而到一個稠密的、二維的數據，球面投影示意圖如下：

其中， $\phi$ 和 $\theta$ 分別表示點的方位角（azimuth）和頂角（altitude），這兩個角如下圖所示：

通常來說，方位角是相對於正北方向的夾角，但是，在我們Lidar的座標系下,方位角爲相對於x方向（車輛正前方）的夾角， $\phi$ 和 $\theta$ 的計算公式爲：

$\theta = arcsin \frac{z}{\sqrt{x^2+y^2+z^2}}$
$\phi = arcsin \frac{y}{\sqrt{x^2+y^2}}$

其中， $(x,y,z)$ 爲三維點雲中每一個點的座標。所以對於點雲中的每一個點都可以通過其 $(x,y,z)$ 計算其 $(\theta, \phi)$ ,也就是說我們將三維空間座標系中的點都投射到了一個球面座標系，這個球面座標系實則已經是一個二維座標系了，但是，爲了便於理解，我們對其角度進行微分化從而得到一個二維的直角座標系：

$i = \frac{\theta}{\delta\theta}$
$j = \frac{\phi}{\delta\phi}$

那麼，球面座標系下的每一個點都可以使用一個直角座標系中的點表示，如下：

通過這麼一層變換，我們就將三維空間中任意一點的位置 $(x, y, z)$ 投射到了2維座標系下的一個點的位置 $(i, j)$ , 我們提取點雲中每一個點的5個特徵: $(x, y,z, intensity, range)$ 放入對應的二維座標 $(i,j)$ 內。從而得到一個尺寸爲 $(H, W, C)$ 張量（其中 $C=5$ ）,由於論文使用的是Kitti的64線激光雷達，所以 $H=64$ ，水平方向上，受Kitti數據集標註範圍的限制，原論文僅使用了正前方90度的Lidar掃描，使用512個網格對它們進行了劃分（即水平上採樣512個點）。所以，點雲數據在輸入到CNN中之前，數據被預處理成了一個尺寸爲 $(64 \times 512 \times 5)$ 的張量。

CNN結構

SqueezeSeg的CNN部分幾乎完全採用的SqueezeNet網絡結構，SqueezeNet是一個參數量極少但是能夠達到AlexNet精度的CNN網絡，在對實時性有要求的點雲分割應用場景中採用頗有意義。其網絡結構如下：

該網絡最大的特色爲兩個結構，被稱爲 fireModules 和 fireDeconvs，這兩種網絡層的具體結構如下：

由於輸入的張量的高度（64）要小於其寬度（512），該網絡主要對寬度進行降維，通過添加最大池化層（Max Pooling）降低數據的寬度。到Fire9輸出的是降維後的特徵映射。爲了得到一個完整的映射標籤，還需要對特徵映射進行還原（即還原到原尺寸），conv14層的輸出即對每個點的分類概率映射。輸出最後被輸入到一個條件隨機場中進行進一步的矯正。

SqueezeSeg中採用的CRF

在深度學習技術不斷進步的同時，概率圖形模型已被開發爲用於提高像素級標記任務準確性的有效方法。馬爾可夫隨機場（Markov Random Fields, MRF）及其變體——條件隨機場（Conditional Random Fields, CRF）已經成爲計算機視覺中最成功的概率圖模型之一。

由於CNN網絡的下采樣層（如最大池化層）的存在，使得數據的一些底層細節在CNN被拋棄，近而造成CNN輸出的預測分類存在邊界模糊的問題。高精度的逐像素分類不僅僅依賴於高層特徵，也受到底層細節信息的影響，細節信息對於標籤分類的一致性至關重要。打個比方，如果點雲中兩個點相近，同時具有類似的強度值（intensity），那麼它們就有可能屬於同一個目標（即具有一樣的分類）。

CRF推理應用於語義標記的關鍵思想是將標籤分配（對於像素分割來說就是像素標籤分配）問題表達爲包含類似像素之間具有一定標籤協議的假設的概率推理問題。CRF推理能夠改進像素級標籤預測，以產生清晰的邊界和細粒度的分割。因此，CRF可用於克服利用CNN進行像素級標記任務的缺點。爲了彌補下采樣過程中細節信息的損失，SqueezeSeg在最後使用RNN實現一個CRF推理，以對label map進行進一步精煉，這裏作者參考了論文： Conditional Random Fields as Recurrent Neural Networks ，改論文提出了mean-field 近似推理，以帶有高斯pairwise的勢函數的密集CRF作爲RNN，在前向過程中對CNN粗糙的輸出精細化，同時在訓練時將誤差返回給CNN。結合了CNN與RNN的模型可以正常的利用反向傳播來端對端的訓練。SqueezeSeg的CRF部分結構如下圖所示：

我們將CNN的輸出結果作爲CRF的輸入，根據原始點雲計算高斯濾波器，其有兩個高斯核，如下所示：

其中 $x$ 爲點的三維座標 $(x,y,z)$ ， $p$ 爲點經過球面投影得到的方位角和頂角 $(\theta,\phi)$ ，其他參數爲經驗性閾值。該高斯核衡量了兩點之間特徵的差異，兩點之間差異越大( $x$ 和 $p$ 相差越多)，高斯核的值就越小，兩點之間的相關性也就越小。在輸入圖像使用該高斯濾波器的過程稱爲message passing，可以初步聚合鄰域點的概率。接着，通過1x1大小的卷積核去微調每一個點的概率分佈權重，這一個過程稱爲re-weighting and compatibilty transformation，卷積核的值是通過學習得到。最後，以殘差方式將最初的便籤映射加到re-weighting的輸出結果並用softmax歸一化。在實際操作中，整個CRF以RNN層重複循環三次，並得到最終精煉後的標籤映射。

使用SqueezeSeg實現一個ROS節點進行點雲目標識別與分割

SqueezeSeg的模型訓練代碼在本文中不在贅述，感興趣的同學可以直接去看作者的開源代碼：

SqueezeSeg作者開源的模型訓練代碼：https://github.com/BichenWuUCB/SqueezeSeg

上面的代碼爲TensorFlow實現，基於上述倉庫，我們實現一個ROS節點，調用一個已經訓練好的SqueezeSeg模型，對輸入的點雲進行目標識別和分割。所以在運行下述實例代碼之前，需要自行安裝好TensorFlow-GPU版本（CPU版本亦可，但是運行速度相對要慢一些），本文假定大家已經安裝好TensorFlow環境，我們來繼續關注基於SqueezeSeg的ROS應用開發，我們採用論文作者公開的數據（來源於Kitti，採集自HDL-64雷達，同時已經完成了前向90度的切割，並且被保存成了npy文件）。

數據下載地址：https://www.dropbox.com/s/pnzgcitvppmwfuf/lidar_2d.tgz?dl=0 ，國內讀者如無法訪問，可以使用此地址下載：https://pan.baidu.com/s/1kxZxrjGHDmTt-9QRMd_kOA

將數據下載好以後解壓到ROS package的 script/data/ 目錄下，解壓以後的目錄結構爲：

squeezeseg_ros/script/data/lidar_2d/

完整代碼見文末github倉庫。

採用作者開源的數據的一個很重要的原因在於手頭沒有64線的激光雷達，首先我們看看launch文件內容：

<launch>
  <!--
      online segmentation using .npy & SqueezeSeg model
      this script can
                    1. read all .npy file from lidar_2d folder
                    2. predict label from SqueezeSeg model using tensorflow
                    3. publish to 'sqeeuze_seg/points' topic
  -->
  
  <param name="npy_path" type="string" value="$(find squeezeseg_ros)/script/data/lidar_2d/" />
  <param name="npy_file_list" type="string" value="$(find squeezeseg_ros)/script/data/ImageSet/val.txt" />
  <param name="pub_topic" type="string" value="/squeeze_seg/points" />
  <!-- should be the path for checkpoint -->
  <param name="checkpoint" type="string" value="$(find squeezeseg_ros)/script/data/SqueezeSeg/model.ckpt-23000" />
  <param name="gpu" type="string" value="0" />

  <node pkg="squeezeseg_ros" type="squeezeseg_ros_node.py" name="squeezeseg_ros_node" output="screen" />

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find squeezeseg_ros)/rviz/squeezeseg_ros.rviz" />

</launch>

npy_path參數即爲我們的數據的目錄，我們將其放在package的script/data目錄下，npy_file_list是個文本文件的路徑，它記錄了驗證集的文件名，pub_topic指定我們最後發佈出去的結果的點雲topic名稱，checkpoint參數指定我們預先訓練好的SqueezeSeg模型的目錄，它是一個TensorFlow 的checkpoint文件，gpu參數指定使用主機的那一快GPU（即指定GPU的ID），通常我們只有一塊GPU，所以這裏設置爲0,如果主機沒有安裝GPU（當然TensorFlow-gpu也就無法工作），則會使用CPU。squeezeseg_ros_node.py即爲我們調用模型的接口，最後我們在啓動Rviz，加載設定好的Rviz配置文件，即可將模型的識別結果可視化出來。

具體到squeezeseg_ros_node.py中，首先加載參數並且配置checkpoint路徑：

    rospy.init_node('squeezeseg_ros_node')

    npy_path = rospy.get_param('npy_path')
    npy_file_list = rospy.get_param('npy_file_list')

    pub_topic = rospy.get_param('pub_topic')
    checkpoint = rospy.get_param('checkpoint')
    gpu = rospy.get_param('gpu')

    FLAGS = tf.app.flags.FLAGS
    tf.app.flags.DEFINE_string(
        'checkpoint', checkpoint,
        """Path to the model paramter file.""")
    tf.app.flags.DEFINE_string('gpu', gpu, """gpu id.""")

    npy_tensorflow_to_ros = NPY_TENSORFLOW_TO_ROS(pub_topic=pub_topic,
                                                  FLAGS=FLAGS,
                                                  npy_path=npy_path,
                                                  npy_file_list=npy_file_list)

循環讀取npy數據文件，讀取文件的代碼如下：

    # Read all .npy data from lidar_2d folder
    def get_npy_from_lidar_2d(self, npy_path, npy_file_list):
        self.npy_path = npy_path
        self.npy_file_list = open(npy_file_list, 'r').read().split('\n')
        self.npy_files = []

        for i in range(len(self.npy_file_list)):
            self.npy_files.append(
                self.npy_path + self.npy_file_list[i] + '.npy')

        self.len_files = len(self.npy_files)

調用深度學習模型對點雲進行分割和目標檢測識別,並將檢測出來的結果以PointCloud2的msg格式發到指定的topic上：


    # Read all .npy data from lidar_2d folder
    def get_npy_from_lidar_2d(self, npy_path, npy_file_list):
        self.npy_path = npy_path
        self.npy_file_list = open(npy_file_list, 'r').read().split('\n')
        self.npy_files = []

        for i in range(len(self.npy_file_list)):
            self.npy_files.append(
                self.npy_path + self.npy_file_list[i] + '.npy')

        self.len_files = len(self.npy_files)

    def prediction_publish(self, idx):
        clock = Clock()

        record = np.load(os.path.join(self.npy_path, self.npy_files[idx]))
        lidar = record[:, :, :5]

        # to perform prediction
        lidar_mask = np.reshape(
            (lidar[:, :, 4] > 0),
            [self._mc.ZENITH_LEVEL, self._mc.AZIMUTH_LEVEL, 1]
        )

        norm_lidar = (lidar - self._mc.INPUT_MEAN) / self._mc.INPUT_STD

        pred_cls = self._session.run(
            self._model.pred_cls,
            feed_dict={
                self._model.lidar_input: [norm_lidar],
                self._model.keep_prob: 1.0,
                self._model.lidar_mask: [lidar_mask]
            }
        )
        label = pred_cls[0]

        # point cloud for SqueezeSeg segments
        x = lidar[:, :, 0].reshape(-1)
        y = lidar[:, :, 1].reshape(-1)
        z = lidar[:, :, 2].reshape(-1)
        i = lidar[:, :, 3].reshape(-1)
        label = label.reshape(-1)
        cloud = np.stack((x, y, z, i, label))

        header = Header()
        header.stamp = rospy.Time().now()
        header.frame_id = "velodyne_link"

        # point cloud segments
        msg_segment = self.create_cloud_xyzil32(header, cloud.T)

        # publish
        self._pub.publish(msg_segment)
        rospy.loginfo("Point cloud processed. Took %.6f ms.",
                      clock.takeRealTime())

不同於一般的PointCloud2 msg，這裏的每一個點除了包含x，y，z，intensity字段以外，還包含一個label字段（即分類的結果），構建5字段的PointCloud2 msg的代碼如下：

    # create pc2_msg with 5 fields
    def create_cloud_xyzil32(self, header, points):
        fields = [PointField('x', 0, PointField.FLOAT32, 1),
                  PointField('y', 4, PointField.FLOAT32, 1),
                  PointField('z', 8, PointField.FLOAT32, 1),
                  PointField('intensity', 12, PointField.FLOAT32, 1),
                  PointField('label', 16, PointField.FLOAT32, 1)]
        return pc2.create_cloud(header, fields, points)

使用launch文件啓動節點：

roslaunch squeezeseg_ros squeeze_seg_ros.launch

彈出Rviz界面，識別分割如下：

在我的 CPU:i7-8700 + GPU:GTX1070的環境下，處理一幀數據的耗時大約在50ms以內，如下：

對於semantic segmentationz這類任務而言，其速度已經比較可觀了，通常雷達頻率約爲10HZ，該速度基本達到要求。

無人駕駛汽車系統入門（二十六）——基於深度學習的實時激光雷達點雲目標檢測及ROS實現

無人駕駛汽車系統入門（二十六）——基於深度學習的實時激光雷達點雲目標檢測及ROS實現

傳統方法VS深度學習方法

SqueezeSeg理論部分

概括

點雲預處理

CNN結構

SqueezeSeg中採用的CRF

使用SqueezeSeg實現一個ROS節點進行點雲目標識別與分割

完整代碼：https://github.com/AbangLZU/SqueezeSeg_Ros

數據：https://pan.baidu.com/s/1kxZxrjGHDmTt-9QRMd_kOA

[轉帖]使用NMT和pmap解決JVM資源泄漏問題原創

Python實現大麥網搶票的四大關鍵技術點解析

Python 安裝庫指令大全

salesforce零基礎學習（一百三十八）零碎知識點小總結（十）

一款開源的.NET程序集反編譯、編輯和調試神器

關於接口協議，你必須要知道這些！

基於 Milvus + LlamaIndex 實現高級 RAG

【2024-05-21】以茶會友

自動駕駛系統進階與項目實戰（五）使用SC-LEGO-LOAM進行較大規模點雲地圖構建和閉環優化

自動駕駛系統進階與項目實戰（四）自動駕駛高精度地圖構建中的三維場景識別和閉環檢測

Move-it無人駕駛黑客馬拉松記錄

自動駕駛系統進階與項目實戰（三）基於全卷積神經網絡的點雲三維目標檢測和ROS實戰

自動駕駛系統進階與項目實戰（二）多激光雷達外參自動化標定算法及代碼實例

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結