【点云学习PCL 】一

点云学习

说明：仅做学习使用，侵删
参考网址1

一、点云基础

0 概述

PCL（Point Cloud Library）是用于 2D/3D 图像和点云处理的大型开源跨平台的 C++ 编程库，PCL 框架实现了大量点云相关的通用算法和高效的数据结构 ，PCL 是 BSD 授权方式，可以免费进行商业和学术应用 。相比图像数据，点云数据多了一个维度，因此能够更全面的刻画三维场景中的对象

支持多种操作系统，可以在 Windows、Linux、MacOS X、Android、部分嵌入式实时系统上运行。

应用领域：机器人、CAD/CAM、逆向工程、遥感测量、VR/AR、人机交互等

PCL 内容涉及了点云的获取、滤波、分割、配准、检索、特征提取、特征估计，表面重建、识别、模型拟合、追踪、曲面重建、可视化等。

PCL 中的所有模块和算法都是通过 Boost 共享指针来传送数据，因而避免了多次复制系统中已存在的数据

1.1 点云及其可视化

基本显示的代码逻辑：

• 创建一个智能指针，用作指向点云文件

  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);

• 加载点云文件

  pcl::io::loadPCDFile("./data/pcl_logo.pcd", *cloud);

• 创建显示的对象

  pcl::visualization::CloudViewer viewer("Cloud Viewer");

• 对象调用显示点云的方法

  viewer.showCloud(cloud);

拓展：

• 如果显示点云之后，判断点云是否退出，如果不退出可以处理点云

   while (!viewer.wasStopped()) {
          // 你可以在这里对点云做很多处理
      }

输出结果为：

1.2 点云数据

根据激光测量原理得到的点云，包含三维坐标信息 和激光反射强度信息 ，激光反射强度与仪器的激光发射能量、波长，目标的表面材质、粗糙程度、入射角相关。根据摄影测量原理得到的点云，包括三维坐标（xyz）和颜色信息。结合两个原理的多传感器融合技术（多见于手持式三维扫描仪），能够同时得到这三种信息。

也即：三维坐标信息 ( x , y , z ) (x,y,z) (x,y,z) 强度信息 I I I（intensity）,颜色信息（RGB）

基本数据类型PointCloud

PCL的基本数据类型是PointCloud，一个PointCloud是一个C++的模板类，它包含了以下字段：

• width(int)

  ：指定点云数据集的宽度

  • 对于无组织格式的数据集，width代表了所有点的总数
  • 对于有组织格式的数据集，width代表了一行中的总点数

• height(int)

  ：制定点云数据集的高度

  • 对于无组织格式的数据集，值为1
  • 对于有组织格式的数据集，表示总行数

• points(std::vector<PointT>)：包含所有PointT类型的点的数据列表

1.2.1 内置数据类型

类型主要有：

• PointXYZ - float x, y, z
• PointXYZI - float x, y, z, intensity
• PointXYZRGB - float x, y, z, rgb
• PointXYZRGBA - float x, y, z, uint32_t rgba
• Normal - float normal[3], curvature 法线方向，对应的曲率的测量值
• PointNormal - float x, y, z, normal[3], curvature 采样点，法线和曲率
• Histogram - float histogram[N] 用于存储一般用途的n维直方图

1.2.2 生成PCD文件分析

以fastlio2输出的点云为例

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity normal_x normal_y normal_z curvature
SIZE 4 4 4 4 4 4 4 4
TYPE F F F F F F F F
COUNT 1 1 1 1 1 1 1 1
WIDTH 27211527
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 27211527
DATA binary
̆I��E�@���     

# .PCD v.7 - Point Cloud Data file format
VERSION .7			# 版本号
FIELDS x y z rgb		#  指定一个点可以有的每一个维度和字段的名字
SIZE 4 4 4 4			# 用字节数指定每一个维度的大小。例如：
TYPE F FFF			# 用一个字符指定每一个维度的类型
COUNT 1 1 1 1			# 指定每一个维度包含的元素数目，默认情况下，如果没有COUNT，所有维度的数目被设置成1
WIDTH 640    			# 像图像一样的有序结构，有640行和480列，
HEIGHT 480    			# 这样该数据集中共有640*480=307200个点
VIEWPOINT 0 0 0 1 0 0 0		# 指定数据集中点云的获取视点 视点信息被指定为平移（tx ty tz）+ 四元数（qw qx qy qz）
POINTS 307200			# 指定点云中点的总数。从0.7版本开始，该字段就有点多余了
DATA ascii			# 指定存储点云数据的数据类型。支持两种数据类型：ascii和二进制
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06

注解：

TYPE：用一个字符指定每一个维度的类型

• I 表示有符号类型 int8（char）、int16（short）和 int32（int）
• U 表示无符号类型 uint8（unsigned char）、uint16（unsigned short）和 uint32（unsigned int）
• F 表示浮点类型

FIELDS：指定一个点可以有的每一个维度和字段的名字

• FIELDS x y z
• FIELDS x y z rgb
• FIELDS x y z normal_x normal_y normal_z

PCD 文件的文件头部分必须按上面的顺序精确指定

1.2.3 点云序列化常用

• 反序列化---将生成的点云存储到pcd文件中，并选择对应的存储类型

  将对象的状态信息转换为可以存储或传输的形式（文件、缓冲、或经由网络中发送）的过程。例如，点云在 C++ 中以类对象的形式存放 ，而点云存放为文件的时候是一个点 PCD、PLY 等文件，将代码中暂存的点云对象转成点 PCD 文件这一操作就叫做序列化。

  对应的代码：

  //
  // Created by pf on 2023/10/25.
  //
  // 随机生成了 5 个点，并将之以 ASCII 形式保存（序列化）在 test_pcd.pcd 文件中：save_pcd.cpp
  #include <iostream>
  #include <pcl/io/pcd_io.h>
  #include <pcl/point_types.h>
  
  int main(int argc, char **argv) {
      pcl::PointCloud<pcl::PointXYZ> cloud;
      cloud.width =5;
      cloud.height = 1;
      cloud.is_dense = false;
      cloud.points.resize(cloud.width*cloud.height);
      // Fill in the cloud data
      std::cout << rand() << std::endl;
      std::cout << rand() / (RAND_MAX + 1.0f) << std::endl;
      std::cout << 1024 * rand() / (RAND_MAX + 1.0f) << std::endl;
      // 随机生成5个点
      for (size_t i = 0; i < cloud.points.size(); ++i) {
          cloud.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
          cloud.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
          cloud.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
      }
      pcl::io::savePCDFileASCII("pcd_serialization.pcd", cloud);
      // pcl::io::savePCDFileBinary("test_pcd_binary.pcd", cloud);
      std::cerr << "Saved " << cloud.points.size() << " data points to test_pcd.pcd." << std::endl;
  
      for (size_t i = 0; i < cloud.points.size(); ++i)
          std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
  
      return (0);
  }

• 反序列化

  和序列化相反，反序列化则是将一些已经序列化过的数据进行反序列化得到原来的对象。例如，一个点 PCD 文件，将它读取成一个点云对象这一操作就是反序列化

  对应的代码

  //
  // Created by pf on 2023/10/25.
  //
  // 对一个点云进行反序列化操作，将之保存到 PointCloud 对象中：load_pcd.cpp
  #include <iostream>
  #include <pcl/io/pcd_io.h>
  #include <pcl/point_types.h>
  
  int main(int argc, char **argv) {
      // 准备pcl::PointXYZ类型的点云
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
      // 将pcd中的数据加载到cloud中
      if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/bunny.pcd", *cloud) == -1) {         //* load the file
  
          PCL_ERROR ("Couldn't read file bunny.pcd \n");
          return (-1);
      }
      std::cout << "Loaded "
                << cloud->width * cloud->height
                << " data points from test_pcd.pcd with the following fields: "
                << std::endl;
      for (size_t i = 0; i < cloud->points.size(); ++i)
          std::cout << "    " << cloud->points[i].x
                    << " " << cloud->points[i].y
                    << " " << cloud->points[i].z << std::endl;
      return (0);
  }

其他文件格式

• PLY 是一种多边形文件格式，由 Stanford 大学的 Turk 等人设计开发
  常用的点云数据文件，ply 文件不仅可以存储点数据，而且可以存储网格数据
• STL 是 3D Systems 公司创建的模型文件格式，主要应用于 CAD、CAM 领域
• OBJ 是从几何学上定义的文件格式，首先由 Wavefront Technologies 开发
• X3D 是符合 ISO 标准的基于 XML 的文件格式，表示 3D 计算机图形数据

1.3 点云的基本处理方法

1.3.1文件简介

common

• pcl_common 中主要是包含了 PCL 库常用的公共数据结构和方法 ，比如 PointCloud 的类和许多用于表示点、曲面、法向量和特征描述 等点的类型，用于计算距离、均值、协方差、角度转换以及几何变化的函数

• angles.h 定义了标准的 C 接口的角度计算函数

• centriod.h 定义了中心点的估算以及协方差矩阵的计算

• commo.h 标准的 C 以及 C++ 类，是其他 common 函数的父类

• distance.h 定义标准的 C 接口用于计算距离

• file_io.h 定义了一些文件帮助写或者读方面的功能

• random.h 定义一些随机点云生成的函数

• geometry.h 定义一些基本的几何功能的函数

• intersection.h 定义线与线相交的函数

• norm.h 定义了标准的 C 方法计算矩阵的正则化

• time.h 定义了时间计算的函数

• Point_types.h 定义了所有 PCL 实现的点云的数据结构的类型

1.3.2 点云pcl和ROS之间相互转换

/*
ConstPtr：const pointer（常量指针），可理解为传递常量数据的共享指针
    1. 为什么要用指针而不直接传数据，因为数据太大了，值传递需要进行对数据的复制而指针引用不需要，因为涉及到右值传参和类型推断，不加引用的话类型可能发生变化
    2. ::constPtr 可以理解为指向常量的共享指针，::Ptr 可以理解为指向一个（可修改的）共享指针
    3. ::constPtr& 可以理解为对一个常量消息的共享指针的引用，::Ptr& 可以理解为对一个（可修改的）消息的共享指针的引用
    4. 使用共享指针，它是一种智能指针，可以自动管理他指向的内存的存储时间，不必手动释放内存
*/
void elevatedCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &input_cloud) {
    pcl::PointCloud<PointT>::Ptr output_cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*input_cloud, *output_cloud);
    //pcl::toROSMsg(pcl::PointCloud<pcl::PointXYZ>, sensor_msgs::PointCloud2); // 逆过程
}

1.4 点云可视化工具

1.4.1 pcl_viewer工具

• 基本使用

  进入：pcl_viewer xxxxx.pcd

  帮助：在界面中输入h，可以在控制台看到帮助信息

  退出：界面中输入q

  放大缩小：鼠标滚轮 或 Alt + [+/-]

  平移：Shift+鼠标拖拽

  旋转：Ctrl+鼠标拖拽

• 其他技巧

  修改点颜色：数字1,2,3,4,5...9，重复按1可切换不同颜色方便观察

  放大缩小点：放大Ctrl+Shift+加号，缩小 Ctrl+减号

  保存截图：j

  显示颜色尺寸：u

  显示比例尺寸：g

  在控制列出所有几何和颜色信息：l

• 鼠标选点打印坐标

  选点模式进入：pcl_viewer -use_point_picking bunny.pcd

  选择指定点：shift+鼠标左键

1.4.2 CloudViewer 程序的内部对象方法

//
// Created by pf on 2023/10/25.
//
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>

int count = 0;

void viewerPsycho(pcl::visualization::PCLVisualizer &viewer);

void viewerOneOff(pcl::visualization::PCLVisualizer &viewer);

int main() {
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::io::loadPCDFile("./data/scans.pcd", *cloud);
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
//    viewer.showCloud(cloud);
    viewer.runOnVisualizationThreadOnce(viewerOneOff);
    viewer.runOnVisualizationThread(viewerPsycho);
    while (!viewer.wasStopped()) {
        count++;
    }
    return 0;
}

void viewerPsycho(pcl::visualization::PCLVisualizer &viewer) {
    static unsigned cnt = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << cnt++;
    // 每次刷新时，移除text，添加新的text
    viewer.removeShape("text", 0);
    viewer.addText(ss.str(), 200, 300, "text", 0);
    //FIXME: possible race condition here:
    count++;
}

void viewerOneOff(pcl::visualization::PCLVisualizer &viewer) {
    // 设置背景色为粉红色
    viewer.setBackgroundColor(1.0, 0.5, 1.0);
    pcl::PointXYZ o;
    o.x = 1.0;
    o.y = 0.0;
    o.z = 0.0;
    //    添加一个圆心为o，半径为0.25m的球体
    viewer.addSphere(o, 0.25, "sphere", 0);
    std::cout << "i only run once" << std::endl;
}
对应的结果

PCLVisualizer 雷达对象方法

方法类似，不作演示，没有点云文件。

//
// Created by pf on 2023/10/25.
//
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char **argv) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("./data/bunny.pcd", *cloud);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_milk(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile("./data/milk_color.pcd", *cloud_milk);

    // 创建PCLVisualizer
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

    // 设置背景色为灰色（非必须）
    viewer->setBackgroundColor(0.05, 0.05, 0.05, 0);

    // 添加一个普通点云 (可以设置指定颜色，也可以去掉single_color参数不设置)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");

    // 再添加一个彩色点云及配置
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_milk);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud_milk, rgb, "sample cloud milk");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud milk");

    // 添加一个0.5倍缩放的坐标系（非必须）
    viewer->addCoordinateSystem(0.5);

    // 直到窗口关闭才结束循环
    while (!viewer->wasStopped()) {
        // 每次循环调用内部的重绘函数
        viewer->spinOnce();
    }
    return 0;
}

1.4.3 矩阵变换

将一个点云经过 z z z轴旋转45°，然后经过平移变换。

//
// Created by pf on 2023/10/25.
//
#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char **argv) {
    const std::string file = "./data/bunny.pcd";
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile(file, *source_cloud) < 0) {
        std::cout << "Error loading point cloud " << argv[1] << std::endl << std::endl;
        return -1;
    }
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    // 定义旋转矩阵
    auto m_data = float(M_PI_4);
    transform_1 << cos(m_data), -sin(m_data), 0, 2.5,
            sin(m_data), cos(m_data), 0, 0,
            0, 0, 0, 0,
            0, 0, 0, 0;

    // 使用仿射变换
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    transform_2.translation() << 0.8, 0.0, 0.0;
    transform_2.rotate(Eigen::AngleAxisf(m_data, Eigen::Vector3f::UnitZ()));
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);

    pcl::visualization::PCLVisualizer viewer("Matrix transformation example");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            source_cloud_color_handler(source_cloud, 255, 255, 255);// white
    viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red\

    viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
    // 设置坐标系系统
    viewer.addCoordinateSystem(0.5, "cloud", 0);
    // 设置背景色
    viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
    // 设置渲染属性（点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
    // 设置渲染属性（点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
    //viewer.setPosition(800, 400); // Setting visualiser window position

    while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
        viewer.spinOnce();
    }
    return 0;
}

二、点云的拓扑结构

3.1 点云存储结构，K-D树

通过3D相机（雷达、激光扫描、立体相机）获取到的点云，一般数据量较大，分布不均匀，数据主要表征了目标物表面的大量点的集合，这些离散的点如果希望实现基于邻域关系的快速查找比对功能 ，就必须对这些离散的点之间建立拓扑关系。常见的空间索引一般是自上而下逐级划分空间的各种索引结构，包括BSP树，k-d tree、KDB tree、R tree、CELL tree、八叉树等 。有了这些关系，我们就可以实现点云的降采样，计算特征向量，点云匹配，点云拆分等功能。

原理概述

k-d tree（ k-dimensional tree）是计算机科学中用于在k维空间中一些点建立关系的数据结构。它是一个包含特定约束的二叉搜索树。k-d tree对于范围搜索和最近邻居搜索非常有用。我们通常只处理三维空间的点云，因此我们所有的k-d树都是三维空间的。

k-d树的每个级别都使用垂直于相应轴的超平面沿特定维度拆分所有子级。在树的根部，所有子项都将根据第一维进行拆分（即，如果第一维坐标小于根，则它将位于左子树中，如果大于根，则显然位于右边的子树）。树中向下的每个级别都在下一个维度上划分，其他所有元素都用尽后，将返回到第一个维度。他们构建k-d树的最有效方法是使用一种分区方法，例如快速排序所用的一种方法，将中值点放置在根上，所有具有较小一维值的东西都放置在根部，而右侧则更大。然后，在左右两个子树上都重复此过程，直到要分区的最后一棵树仅由一个元素组成。

KD树的原理查资料即可。

手写kd树

//
// Created by pf on 2023/10/26.
//
#pragma once
#include <algorithm>
#include <iostream>
#include <set>
#include <stack>
#include <vector>
#include <cmath>
#include <cfloat>
struct KDNode
{
    int index;
    int axis;
    KDNode *left;
    KDNode *right;
    KDNode(int index, int axis, KDNode *left = nullptr, KDNode *right = nullptr)
    {
        this->index = index;
        this->axis = axis;
        this->left = left;
        this->right = right;
    }
};

template <class T>
class KDTree
{
private:
    int dimension;
    KDNode *root;
    KDNode *build(std::vector<T> &);
    std::set<int> visited;
    std::stack<KDNode *> queueNode;
    std::vector<T> m_data;

    void release(KDNode *);
    void printNode(KDNode *);
    int chooseAxis(std::vector<T> &);
    void dfs(KDNode *, T);
    // 点与点之间的距离
    inline double distanceT(KDNode *, T);
    inline double distanceT(int, T);
    // 点与超平面的距离
    inline double distanceP(KDNode *, T);
    // 检查父节点超平面是否在超球体中
    inline bool checkParent(KDNode *, T, double);

public:
    KDTree(std::vector<T> &, int);
    ~KDTree();
    void Print();
    int findNearestPoint(T);
};

template <class T>
KDTree<T>::KDTree(std::vector<T> &data, int dim)
{
    dimension = dim;
    m_data = data;
    root = build(data);
}

template <class T>
KDTree<T>::~KDTree()
{
    release(root);
}

template <class T>
void KDTree<T>::Print()
{
    printNode(root);
}

template <class T>
void KDTree<T>::release(KDNode *node)
{
    if (node)
    {
        if (node->left)
            release(node->left);
        if (node->right)
            release(node->right);
        delete node;
        node = nullptr;
    }
}

template <class T>
KDNode *KDTree<T>::build(std::vector<T> &data)
{
    if (data.empty())
        return nullptr;
    std::vector<T> temp = data;
    int mid_index = static_cast<int>(data.size() / 2);
    int axis = data.size() > 1 ? chooseAxis(temp) : -1;
    std::sort(temp.begin(), temp.end(), [axis](T a, T b)
    { return a[axis] < b[axis]; });
    std::vector<T> leftData, rightData;
    leftData.assign(temp.begin(), temp.begin() + mid_index);
    rightData.assign(temp.begin() + mid_index + 1, temp.end());
    KDNode *leftNode = build(leftData);
    KDNode *rightNode = build(rightData);
    KDNode *rootNode;
    rootNode = new KDNode(temp[mid_index].index, axis, leftNode, rightNode);
    return rootNode;

}

template <class T>
void KDTree<T>::printNode(KDNode *node)
{
    if (node)
    {
        std::cout << "Index: " << node->index << "\tAxis: " << node->axis << std::endl;
        printNode(node->left);
        printNode(node->right);
    }
}

template <class T>
int KDTree<T>::chooseAxis(std::vector<T> &data)
{
    int axis = -1;
    double maxVar = -1.0;
    size_t size = data.size();
    for (int i = 0; i < dimension; i++)// x数据方向的方差最大进行设置axis，也就是这个方向是最离散的
    {
        double mean = 0;
        double Var = 0;
        for (int j = 0; j < size; j++)
        {
            mean += static_cast<double>(data[j][i]);
        }
        mean = mean / static_cast<double>(size);
        for (int j = 0; j < size; j++)
        {
            Var += (static_cast<double>(data[j][i]) - mean) * (static_cast<double>(data[j][i]) - mean);
        }
        Var = Var / static_cast<double>(size);
        // 找到最大的这个方差对应的轴
        if (Var > maxVar)
        {
            axis = i;// 这个就是这个轴对应最大方差的轴
            maxVar = Var;
        }
    }
    return axis;
}

template <class T>
int KDTree<T>::findNearestPoint(T pt)
{
    visited.clear();
    while (!queueNode.empty())
        queueNode.pop();
    double min_dist = DBL_MAX;
    int resNodeIdx = -1;
    dfs(root, pt);
    while (!queueNode.empty())
    {
        KDNode *curNode = queueNode.top();
        queueNode.pop();
        double dist = distanceT(curNode, pt);
        if (dist < min_dist)
        {
            min_dist = dist;
            resNodeIdx = curNode->index;
        }

        if (!queueNode.empty())
        {
            KDNode *parentNode = queueNode.top();
            int parentAxis = parentNode->axis;
            int parentIndex = parentNode->index;
            if (checkParent(parentNode, pt, min_dist))
            {
                if (m_data[curNode->index][parentNode->axis] < m_data[parentNode->index][parentNode->axis])
                    dfs(parentNode->right, pt);
                else
                    dfs(parentNode->left, pt);
            }
        }
    }
    return resNodeIdx;
}

template <class T>
void KDTree<T>::dfs(KDNode *node, T pt)
{
    if (node)
    {
        if (visited.find(node->index) != visited.end())
            return;
        queueNode.push(node);
        visited.insert(node->index);
        if (pt[node->axis] <= m_data[node->index][node->axis] && node->left)
            dfs(node->left, pt);
        else if (pt[node->axis] >= m_data[node->index][node->axis] && node->right)
            dfs(node->right, pt);
        else if ((node->left == nullptr) ^ (node->right == nullptr))
        {
            dfs(node->left, pt);
            dfs(node->right, pt);
        }
    }
}

template <class T>
double KDTree<T>::distanceT(KDNode *node, T pt)
{
    double dist = 0;
    for (int i = 0; i < dimension; i++)
    {
        dist += (m_data[node->index][i] - pt[i]) * (m_data[node->index][i] - pt[i]);
    }
    return sqrt(dist);
}

template <class T>
double KDTree<T>::distanceT(int index, T pt)
{
    double dist = 0;
    for (int i = 0; i < dimension; i++)
    {
        dist += (m_data[index][i] - pt[i]) * (m_data[index][i] - pt[i]);
    }
    return sqrt(dist);
}

template <class T>
double KDTree<T>::distanceP(KDNode *node, T pt)
{
    int axis = node->axis;
    double dist = static_cast<double>(pt[axis] - m_data[node->index][axis]);
    return dist >= 0 ? dist : -dist;
}

template <class T>
bool KDTree<T>::checkParent(KDNode *node, T pt, double distT)
{
    double distP = distanceP(node, pt);
    return distP <= distT;
}