iKdTree基本使用

https://github.com/hku-mars/ikd-Tree  ikd_tree链接

    // 创建一个空的ikd树
    KD_TREE<PointType>::Ptr ikdtree_ptr(new KD_TREE<PointType>(0.3, 0.6, 0.2));
    KD_TREE<PointType>      &ikd_Tree        = *ikdtree_ptr;

    // 创建一个自定义点云对象
    pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);

    // 向点云添加初始数据点
    PointType point1;
    point1.x = 1.0;
    point1.y = 2.0;
    point1.z = 3.0;
    point1.intensity = 4.0;
    point1.ring = 5;
    point1.timestamp = 6.0;
    cloud->push_back(point1);

    PointType point2;
    point2.x = 7.0;
    point2.y = 8.0;
    point2.z = 9.0;
    point2.intensity = 10.0;
    point2.ring = 11;
    point2.timestamp = 12.0;
    cloud->push_back(point2);


    // 进行构建
    ikd_Tree.Build((*cloud).points);

    // 进行最近邻搜索
    PointType target_point;
    target_point.x = 19.0;
    target_point.y = 20.0;
    target_point.z = 21.0;
    target_point.intensity = 22.0;
    target_point.ring = 23;
    target_point.timestamp = 24.0;
    PointVector search_result;
    std::vector<float> point_dist;
    ikd_Tree.Nearest_Search(target_point, 1, search_result, point_dist);

如果使用的点云的数据类型为自定义数据类型，则需在ikd-tree源码中添加以下内容

在ikd_Tree.h文件中添加自定义的数据类型

//自定义的数据类型

struct PointXYZIRT
{
    PCL_ADD_POINT4D;  // quad-word XYZ
    //std::uint8_t intensity;
    float intensity;
    std::uint16_t ring = 0;  /// laser ring number
    double timestamp = 0;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // ensure proper alignment
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(double, timestamp, timestamp))

class CloudData
{
  public:
    // Include intensity,ring and time
    using POINT = PointXYZIRT;
    using CLOUD = pcl::PointCloud<POINT>;
    using CLOUD_PTR = CLOUD::Ptr;

  public:
    CloudData()
        : time(0.0),
          cloud_ptr(new CLOUD())
    {
    }

  public:
    // 时间戳
    double time;

    // 点云数据
    CLOUD_PTR cloud_ptr;
};

在ikd_Tree.cpp文件的最后添加这一行

template class KD_TREE<CloudData::POINT>;