kdTree相關原理及c++實現

kdTree概念

        kd-tree或者k維樹是計算機科學中使用的一種數據結構，用來組織表示k維空間中點的集合。它是一種帶有其他約束條件的二分查找樹。Kd-tree對於區間和近鄰搜索十分有用。一般位於三維空間中的鄰域搜索常用kd-tree，因此本文中所有的kd-tree都是三維的kd-tree。

圖一

        Kd-tree也是二叉樹的一種，首先我們先選定一個維度用於第一次分類，如圖一所示，我們先選擇x維度方向作為分類方向，隨機選取一個值使得小於該值的點位於左邊，大於該值的點位於右邊。在左右區域分別再對第二個維度進行分類，這里以y軸方向作為第二維度，同理根據y分類設置z軸方向為第三維度進行分類。

Kd-tree數據結構定義

Node-data:數據矢量，數據集中某個數據點，是n維矢量（總維度，unsigned int）

Range:空間矢量，該節點所代表的的空間范圍（二維數組）

Split:整數，垂直於分割超平面的方向軸序號（int）

Left:k-d樹，由位於該節點分割超平面左側子空間內所有點構成的k-d樹（tuple<list,int>）

Right:k-d樹，由位於該節點分割超平面右側子空間內所有點構成的k-d樹（tuple<list,int>）

Parent:k-d樹，父節點（auto）

 

Kd-tree優化

        方案一：Kd-tree通過不同維度划分數據，節點的選擇顯得尤為重要。我們可以想象一組點雲，並不是完全隨機離散的，只在某一維度上點雲分布較為離散，其余維度相對集中。以三維空間為例，一組類似球狀的點雲在求每個方向的子節點能保證效率是最高的，但是數據接近一個平面時，在其中一個維度的划分就顯得十分困難。

        解決方法：首先，對於點雲分布不集中的那一維度來說，方差較大，我們可以通過最大方差法選擇每次需要分類的維度，即在每次進行新的划分之前，我們通過判斷方差選擇在哪個維度上進行划分。

        方案二：為了保證每次選擇的節點盡量位於中間位置，也就是讓二叉樹盡量為二叉平衡樹，從而保證節點兩側的點雲數目大致相等。

        解決方法：在選取節點前，我們對數據進行排序，選取中位數作為節點，這樣就能保證兩側數據大致相等。

 

 PCL庫c++源碼

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/io.h>
#include <pcl/kdtree/flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/impl/io.hpp>
#include <pcl/search/flann_search.h>
#include <pcl/surface/gp3.h>
//#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr inCloud(new pcl::PointCloud<pcl::PointXYZ>);
    //construct a plane, the equation is x + y + z = 1
    for (float x = -1.0; x <= 1.0; x += 0.005)
    {
        for (float y = -1.0; y <= 1.0; y += 0.005)
        {
            pcl::PointXYZ cloud;

            cloud.x = x;
            cloud.y = y;
            cloud.z = 1 - x - y;
        
            inCloud->push_back(cloud);
        }
    }

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr pcNormal(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(inCloud);
    ne.setInputCloud(inCloud);
    ne.setSearchMethod(tree);
    ne.setKSearch(50);
    //ne->setRadiusSearch (0.03); 
    ne.compute(*pcNormal);

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZINormal>);
    pcl::concatenateFields(*inCloud, *pcNormal, *cloud_with_normals);

    pcl::io::savePCDFile("plane_cloud_out.pcd", *cloud_with_normals);

    return 0;
}

 

 

【 結束 】