Open3D点云库 C++学习笔记

几何篇（四）

---

前言

本章节将主要介绍K-d树和八叉树。

---

一、K-d树

Open3D 使用FLANN构建 KDTrees 以快速检索最近的邻。其搜索方式主要包括两种，一种是根据数量搜索，另一种是根据半径来搜索。

代码如下：

    auto new_cloud_ptr = std::make_shared<geometry::PointCloud>();//读入点云数据，并判断是否读入成功
    if (io::ReadPointCloud("../data/fragment.ply", *new_cloud_ptr)) {
        utility::LogInfo("Successfully read {}", "../data/fragment.ply");
    } else {
        utility::LogWarning("Failed to read {}", "../data/fragment.ply");
        return 1;
    }

    Eigen::Vector3d color(0.5,0.5,0.5); //把点云设置为灰色

    new_cloud_ptr->open3d::geometry::PointCloud::PaintUniformColor(color);//把点云设置为灰色

    //最大个数的搜索方法
    geometry::KDTreeFlann kdtree;
    int num =200; //最近邻个数
    kdtree.SetGeometry(*new_cloud_ptr);
    std::vector<int> new_indices_vec(num);//最近邻对应的点云索引序号
    std::vector<double> new_dists_vec(num);//最近邻离中心点的距离
    kdtree.SearchKNN(new_cloud_ptr->points_[1500], num, new_indices_vec, //将第1500个点设置为搜索的中心点
                     new_dists_vec);

    for (size_t i = 0; i < new_indices_vec.size(); i++) {
        utility::LogInfo("{:d}, {:f}", (int)new_indices_vec[i],
                         sqrt(new_dists_vec[i]));
        new_cloud_ptr->colors_[new_indices_vec[i]] =
                Eigen::Vector3d(1.0, 0.0, 0.0);  //最近邻涂成红色
    }

     new_cloud_ptr->colors_[1500] = Eigen::Vector3d(0.0, 1.0, 0.0); //搜索中心设置为绿色

     //最大半径的搜索方法
    float r = 0.1; //设置的搜索半径
    int k = kdtree.SearchRadius(new_cloud_ptr->points_[4000], r, new_indices_vec,
                                new_dists_vec);

    utility::LogInfo("======== {:d}, {:f} ========", k, r);
    for (int i = 0; i < k; i++) {
        utility::LogInfo("{:d}, {:f}", (int)new_indices_vec[i],
                         sqrt(new_dists_vec[i]));
        new_cloud_ptr->colors_[new_indices_vec[i]] =
                Eigen::Vector3d(0.0, 0.0, 1.0);//最近邻涂成蓝色
    }
    new_cloud_ptr->colors_[4000] = Eigen::Vector3d(1.0, 0.0, 0.0);//搜索中心设置为红色

    visualization::DrawGeometries({new_cloud_ptr}, "KDTreeFlann", 1600, 900);

结果如下：

二、八叉树

八叉树(Octree)是一种树型数据结构，其中每个内部节点都有八个子节点。八叉树通常用于 3D 点云的空间划分。八叉树的非空叶节点包含一个或多个落入同一空间的细分点。八叉树是 3D 空间的有用描述，可用于快速找到附近的点。Open3D 的Octree可用于创建、搜索和遍历，具有用户指定最大树深度的八叉树的几何类型。关于八叉树的原理部分可参考此篇文章：https://wenku.baidu.com/view/f9553133f28583d049649b6648d7c1c708a10bd4.html

代码如下：

bool f_traverse(const std::shared_ptr<geometry::OctreeNode>& node,
                const std::shared_ptr<geometry::OctreeNodeInfo>& node_info) {
    if (auto internal_node =
                std::dynamic_pointer_cast<geometry::OctreeInternalNode>(node)) {
        if (auto internal_point_node = std::dynamic_pointer_cast<
                    geometry::OctreeInternalPointNode>(internal_node)) {
            int num_children = 0;
            for (const auto& c : internal_point_node->children_) {
                if (c) num_children++;
            }
            utility::LogInfo(
                    "Internal node at depth {} with origin {} has "
                    "{} children and {} points",
                    node_info->depth_, node_info->origin_, num_children,
                    internal_point_node->indices_.size());
        }
    } else if (auto leaf_node = std::dynamic_pointer_cast<
                       geometry::OctreePointColorLeafNode>(node)) {
        utility::LogInfo(
                "Node at depth {} with origin {} has"
                "color {} and {} points",
                node_info->depth_, node_info->origin_, leaf_node->color_,
                leaf_node->indices_.size());
        // utility::LogInfo("Indices: {}", leaf_node->indices_);
    } else {
        utility::LogError("Unknown node type");
    }

    return false;
}

int main(int argc, char *argv[]) {
         auto pcd = io::CreatePointCloudFromFile("../data/fragment.ply");
        visualization::DrawGeometries({pcd},"input");

        constexpr int max_depth = 4; //设置最大深度为4
        auto octree = std::make_shared<geometry::Octree>(max_depth);
        octree->ConvertFromPointCloud(*pcd);

        octree->Traverse(f_traverse);

        std::cout << std::endl << std::endl;
        auto start = std::chrono::steady_clock::now();
        auto result = octree->LocateLeafNode(Eigen::Vector3d::Zero());
        auto end = std::chrono::steady_clock::now();
        utility::LogInfo(
                "Located in {} usec",
                std::chrono::duration_cast<std::chrono::microseconds>(end - start)
                        .count());
        if (auto point_node =
                    std::dynamic_pointer_cast<geometry::OctreePointColorLeafNode>(
                            result.first)) {
            utility::LogInfo(
                    "Found leaf node at depth {} with origin {} and {} indices",
                    result.second->depth_, result.second->origin_,
                    point_node->indices_.size());
        }
        std::cout << std::endl << std::endl;

        visualization::DrawGeometries({pcd, octree},"octree output");
        return 0;
}

输入点云图：

使用八叉数结构对点云图进行储存结果如下：

---

三、参考资料

http://www.open3d.org/docs/latest/tutorial/geometry/kdtree.html
http://www.open3d.org/docs/latest/tutorial/geometry/octree.html

总结

以上就是几何篇(四)的全部内容，完整的可执行代码可以在我的github仓库进行下载，文章会持续更新，如果文章中有写的不对的地方，希望大家可以在评论区进行批评和指正，大家一起交流，共同进步！