使用Kdtree加速的DBSCAN進(jìn)行點(diǎn)云聚類

作者：姜小明 @github

日期：2020-06-28

關(guān)鍵字：Kdtreee, DBSCAN, PCL, 點(diǎn)云

DBSCAN算法適用于點(diǎn)云聚類原环，但是3d點(diǎn)云數(shù)據(jù)一般較大雏门，樸素的DBSCAN算法處理起來效率很低件余。對(duì)此，可以通過使用Kdtree檢索臨近點(diǎn)卿吐，從而加速DBSCAN算法灵嫌。

1. DBSCAN

在點(diǎn)云數(shù)據(jù)分析中，我們經(jīng)常需要對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行分割氛驮，提取感興趣的部分腕柜。聚類是點(diǎn)云分割中的一類方法（其他方法有模型擬合、區(qū)域增長矫废、基于圖的方法盏缤、深度學(xué)習(xí)方法等）。DBSCAN 是一種基于密度的聚類算法磷脯，具有抗噪聲蛾找、無需指定類別種數(shù)、可以在空間數(shù)據(jù)中發(fā)現(xiàn)任意形狀的聚類等優(yōu)點(diǎn)赵誓，適用于點(diǎn)云聚類打毛。

1.1 概念

DBSCAN中為了增加抗噪聲的能力柿赊，引入了核心對(duì)象等概念。

ε：參數(shù)幻枉，鄰域距離碰声。

minPts： 參數(shù)，核心點(diǎn)領(lǐng)域內(nèi)最少點(diǎn)數(shù)熬甫。

核心點(diǎn)： 在 $\epsilon$ 鄰域內(nèi)有至少 $minPts$ 個(gè)鄰域點(diǎn)的點(diǎn)為核心點(diǎn)胰挑。

直接密度可達(dá)： 對(duì)于樣本集合 $D$ ，如果樣本點(diǎn) $q$ 在 $p$ 的 $\epsilon$ 鄰域內(nèi)椿肩，并且 $p$ 為核心對(duì)象瞻颂，那么對(duì)象 $q$ 從對(duì)象 $p$ 直接密度可達(dá)。

密度可達(dá)： 對(duì)于樣本集合 $D$ 郑象，給定一串樣本點(diǎn) $p_1$ , $p_2$ , ..., $p_n$ 贡这， $p = p_1$ , $q = p_n$ , 假如對(duì)象 $p_i$ 從 $p_{i-1}$ 直接密度可達(dá)，那么對(duì)象 $q$ 從對(duì)象 $p$ 密度可達(dá)厂榛。

密度相連： 存在樣本集合 $D$ 中的一點(diǎn) $o$ 盖矫，如果對(duì)象 $o$ 到對(duì)象 $p$ 和對(duì)象 $q$ 都是密度可達(dá)的，那么 $p$ 和 $q$ 密度相聯(lián)击奶。

DBSCAN 算法核心是找到密度相連對(duì)象的最大集合辈双。參考百度百科-DBSCAN

[圖片上傳失敗...(image-2a5ab9-1593586772755)]

如圖， $minPts=4$ 柜砾，紅點(diǎn)為高密度核心點(diǎn)湃望，黃點(diǎn)為邊界點(diǎn)，藍(lán)點(diǎn)為低密度噪聲點(diǎn)局义。紅黃點(diǎn)組成了一個(gè)簇（聚類）喜爷。

核心點(diǎn)、邊界點(diǎn)萄唇、噪聲點(diǎn)對(duì)應(yīng)于不同密度，這就是 DBSCAN 屬于基于密度聚類方法的原因术幔，也是其具有抗噪聲能力的原因另萤。

1.2 算法

如前所述，DBSCAN 算法核心是找到密度相連對(duì)象的最大集合诅挑。為了實(shí)現(xiàn)該算法四敞，有兩種方法：

先遍歷所有的點(diǎn)根據(jù)鄰域點(diǎn)數(shù)找出所有核心點(diǎn)，然后采用區(qū)域增長方法對(duì)其聚類拔妥，再遍歷聚類中的點(diǎn)忿危，將其直接密度可達(dá)的點(diǎn)加入聚類，從而形成最終的聚類没龙。
逐點(diǎn)遍歷铺厨，如果該點(diǎn)非核心點(diǎn)缎玫，則認(rèn)為是噪聲點(diǎn)并忽視（噪聲點(diǎn)可能在后續(xù)被核心點(diǎn)歸入聚類中），若為核心點(diǎn)則新建聚類解滓，并將所有鄰域點(diǎn)加入聚類赃磨。對(duì)于鄰域點(diǎn)中的核心點(diǎn)，還要遞歸地把其鄰域點(diǎn)加入聚類洼裤。依此類推直到無點(diǎn)可加入該聚類邻辉，并開始考慮新的點(diǎn)，建立新的聚類腮鞍。

這里我們采用第二種方法值骇，優(yōu)點(diǎn)是只用遍歷一趟。
偽代碼如下（參考維基百科-DBSCAN）：

DBSCAN(DB, distFunc, eps, minPts) {
    C = 0                                                  /* Cluster counter */
    for each point P in database DB {
        if label(P) ≠ undefined then continue              /* Previously processed in inner loop */
        Neighbors N = RangeQuery(DB, distFunc, P, eps)     /* Find neighbors */
        if |N| < minPts then {                             /* Density check */
            label(P) = Noise                               /* Label as Noise */
            continue
        }
        C = C + 1                                          /* next cluster label */
        label(P) = C                                       /* Label initial point */
        Seed set S = N \ {P}                               /* Neighbors to expand */
        for each point Q in S {                            /* Process every seed point */
            if label(Q) = Noise then label(Q) = C          /* Change Noise to border point */
            if label(Q) ≠ undefined then continue          /* Previously processed */
            label(Q) = C                                   /* Label neighbor */
            Neighbors N = RangeQuery(DB, distFunc, Q, eps) /* Find neighbors */
            if |N| ≥ minPts then {                         /* Density check */
                S = S ∪ N                                  /* Add new neighbors to seed set */
            }
        }
    }
}

2. DBSCAN 算法改進(jìn)

這里算法的復(fù)雜度取決于鄰域點(diǎn)查找（即RangeQuery）的復(fù)雜度移国。最直觀的方法是使用線性掃描查找吱瘩，但是這樣算法整體時(shí)間復(fù)雜度為 $O(n^2)$ 。這里給出兩種改進(jìn)方法：

一種改進(jìn)的方法是通過預(yù)先計(jì)算距離矩陣后進(jìn)行鄰域查找桥狡。其復(fù)雜度為 $O(n^2/2)+O(n*k)$ 搅裙，其中 $k$ 為平均鄰域點(diǎn)數(shù)。但是這種改進(jìn)的缺點(diǎn)是需要額外 $O(n^2)$ 或 $O(n*k)$ 的空間裹芝。
另一種更大的改進(jìn)是使用索引方法查詢鄰域點(diǎn)部逮，如使用Kdtree。其復(fù)雜度為 $O(n*log(n)+k*log(n)*n)$ 嫂易，其中加號(hào)前一項(xiàng)為建樹時(shí)間復(fù)雜度兄朋，后一項(xiàng)為鄰域查找復(fù)雜度(未考證)。

2.1 算法實(shí)現(xiàn)

我們實(shí)現(xiàn)了上述三種方法怜械，這里我們重點(diǎn)介紹第三種實(shí)現(xiàn)颅和，即使用 Kdtree 進(jìn)行鄰域查找的 DBSCAN 算法。算法框架參考 wikipedia 給出的偽代碼(如上已列出缕允，對(duì)照偽代碼看下面的代碼更輕松~)峡扩，接口等參考 pcl::EuclideanClusterExtraction，并參考加入了參數(shù) MinClusterSize, MaxClusterSize 來控制聚類大小障本。算法的核心是radiusSearch() 實(shí)現(xiàn)使用了 pcl::search::KdTree 進(jìn)行鄰域搜索教届，而其內(nèi)部實(shí)際使用了 pcl::KdTreeFLANN 結(jié)構(gòu)來索引點(diǎn)云數(shù)據(jù)。

#ifndef DBSCAN_H
#define DBSCAN_H

#include <pcl/point_types.h>

#define UN_PROCESSED 0
#define PROCESSING 1
#define PROCESSED 2

inline bool comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b) {
    return (a.indices.size () < b.indices.size ());
}

template <typename PointT>
class DBSCANSimpleCluster {
public:
    typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
    typedef typename pcl::search::KdTree<PointT>::Ptr KdTreePtr;
    
    virtual void setInputCloud(PointCloudPtr cloud) {
        input_cloud_ = cloud;
    }

    void setSearchMethod(KdTreePtr tree) {
        search_method_ = tree;
    }

    void extract(std::vector<pcl::PointIndices>& cluster_indices) {
        std::vector<int> nn_indices;
        std::vector<float> nn_distances;
        std::vector<bool> is_noise(input_cloud_->points.size(), false);
        std::vector<int> types(input_cloud_->points.size(), UN_PROCESSED);
        for (int i = 0; i < input_cloud_->points.size(); i++) {
            if (types[i] == PROCESSED) {
                continue;
            }
            int nn_size = radiusSearch(i, eps_, nn_indices, nn_distances);
            if (nn_size < minPts_) {
                is_noise[i] = true;
                continue;
            }
            std::vector<int> seed_queue;
            seed_queue.push_back(i);
            types[i] = PROCESSED;
            for (int j = 0; j < nn_size; j++) {
                if (nn_indices[j] != i) {
                    seed_queue.push_back(nn_indices[j]);
                    types[nn_indices[j]] = PROCESSING;
                }
            } // for every point near the chosen core point.
            int sq_idx = 1;
            while (sq_idx < seed_queue.size()) {
                int cloud_index = seed_queue[sq_idx];
                if (is_noise[cloud_index] || types[cloud_index] == PROCESSED) {
                    // seed_queue.push_back(cloud_index);
                    types[cloud_index] = PROCESSED;
                    sq_idx++;
                    continue; // no need to check neighbors.
                }
                nn_size = radiusSearch(cloud_index, eps_, nn_indices, nn_distances);
                if (nn_size >= minPts_) {
                    for (int j = 0; j < nn_size; j++) {
                        if (types[nn_indices[j]] == UN_PROCESSED) {
                            
                            seed_queue.push_back(nn_indices[j]);
                            types[nn_indices[j]] = PROCESSING;
                        }
                    }
                }
                types[cloud_index] = PROCESSED;
                sq_idx++;
            }
            if (seed_queue.size() >= min_pts_per_cluster_ && seed_queue.size () <= max_pts_per_cluster_) {
                pcl::PointIndices r;
                r.indices.resize(seed_queue.size());
                for (int j = 0; j < seed_queue.size(); ++j) {
                    r.indices[j] = seed_queue[j];
                }
                // These two lines should not be needed: (can anyone confirm?) -FF
                std::sort (r.indices.begin (), r.indices.end ());
                r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

                r.header = input_cloud_->header;
                cluster_indices.push_back (r);   // We could avoid a copy by working directly in the vector
            }
        } // for every point in input cloud
        std::sort (cluster_indices.rbegin (), cluster_indices.rend (), comparePointClusters);
    }

    void setClusterTolerance(double tolerance) {
        eps_ = tolerance; 
    }

    void setMinClusterSize (int min_cluster_size) { 
        min_pts_per_cluster_ = min_cluster_size; 
    }

    void setMaxClusterSize (int max_cluster_size) { 
        max_pts_per_cluster_ = max_cluster_size; 
    }
    
    void setCorePointMinPts(int core_point_min_pts) {
        minPts_ = core_point_min_pts;
    }

protected:
    PointCloudPtr input_cloud_;
    double eps_ {0.0};
    int minPts_ {1}; // not including the point itself.
    int min_pts_per_cluster_ {1};
    int max_pts_per_cluster_ {std::numeric_limits<int>::max()};
    KdTreePtr search_method_;

    virtual int radiusSearch(
        int index, double radius, std::vector<int> &k_indices,
        std::vector<float> &k_sqr_distances) const
    {
        return this->search_method_->radiusSearch(index, radius, k_indices, k_sqr_distances);
    }
}; // class DBSCANCluster

#endif // DBSCAN_H

使用示例如下：

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(point_cloud_input);
    std::vector<pcl::PointIndices> cluster_indices;
    DBSCANKdtreeCluster<pcl::PointXYZ> ec;
    ec.setCorePointMinPts(20);

    ec.setClusterTolerance(0.05);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(25000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(point_cloud_input);
    ec.extract(cluster_indices);

2.2 項(xiàng)目描述

全部工程代碼：github-dbscan_kdtree

項(xiàng)目中包括了一些常見場景的點(diǎn)云處理流程驾霜，如點(diǎn)云數(shù)據(jù)的讀取案训、寫入、降采樣預(yù)處理粪糙、平面檢測與過濾强霎、點(diǎn)云聚類等，同時(shí)包含了一個(gè)測試點(diǎn)云數(shù)據(jù)蓉冈，對(duì)于想快速入門 PCL 點(diǎn)云處理的同學(xué)會(huì)有所幫助城舞。

項(xiàng)目中測試的點(diǎn)云聚類算法除了前面提到的三種 DBSCAN 聚類方法外轩触，還包含 pcl 自帶的 pcl::EuclideanClusterExtraction 聚類，算法接口相同可替換后重新編譯測試椿争。有人說 pcl::EuclideanClusterExtraction 算法是簡化版 DBSCAN怕膛，看過源碼后我覺得更應(yīng)該算作區(qū)域增長方法。該算法和 DBSCAN 算法共同的優(yōu)點(diǎn)是支持任意數(shù)量秦踪、任意形狀的聚類褐捻，缺點(diǎn)是少了抗噪聲能力。