KD-Tree 的开源实现与OpenCV中的实现

最新推荐文章于 2025-01-16 22:45:00 发布

原创

最新推荐文章于 2025-01-16 22:45:00 发布 · 898 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#opencv #c++ #自动驾驶

本文提供了一个自定义的KD-Tree模板类实现，支持最近邻搜索、K近邻搜索和一定范围内的邻居搜索。通过示例代码展示了如何使用该KD-Tree进行点查找操作，并与OpenCV的KDTree进行了KNN搜索速度的比较。总结指出，对于NN搜索，开源KD-Tree更快，而OpenCV的KNN搜索更优。

KD-Tree 开源实现代码

添加头文件 #include <KDTree.hpp>
测试代码及源文件下载链接（OpenCV version: 4.5.1）
hpp 文件名为 “KDTree.hpp”

#ifndef __KDTREE_H__
#define __KDTREE_H__

#include <algorithm>
#include <cmath>
#include <exception>
#include <functional>
#include <numeric>
#include <vector>

namespace obstarcalib::caliblink {
   
   
/** @brief k-d tree class.
 */
template <typename PointT, int DIM = 2, typename Allocator = std::allocator<PointT>>
class KDTree {
   
   
public:
    /** @brief The constructors.
     */
    KDTree()
        : root_(nullptr) {
   
   };
    KDTree(const std::vector<PointT, Allocator>& points)
        : root_(nullptr)
    {
   
   
        build(points);
    }

    /** @brief The destructor.
     */
    ~KDTree() {
   
    clear(); }

    /** @brief Re-builds k-d tree.
     */
    void build(const std::vector<PointT, Allocator>& points)
    {
   
   
        clear();

        points_ = points;

        std::vector<int> indices(points.size());
        std::iota(std::begin(indices), std::end(indices), 0);

        root_ = buildRecursive(indices.data(), (int)points.size(), 0);
    }

    /** @brief Clears k-d tree.
     */
    void clear()
    {
   
   
        clearRecursive(root_);
        root_ = nullptr;
        points_.clear();
    }

    /** @brief Validates k-d tree.
     */
    bool validate() const
    {
   
   
        try {
   
   
            validateRecursive(root_, 0);
        } catch (const Exception&) {
   
   
            return false;
        }

        return true;
    }

    /** @brief Searches the nearest neighbor.
     */
    int nnSearch(const PointT& query, double* minDist = nullptr) const
    {
   
   
        int guess;
        double _minDist = std::numeric_limits<double>::max();

        nnSearchRecursive(query, root_, &guess, &_minDist);

        if (minDist)
            *minDist = _minDist;
            
        return guess;
    }

    /** @brief Searches k-nearest neighbors.
     */
    std::vector<int> knnSearch(const PointT& query, int k) const
    {
   
   
        KnnQueue queue(k);
        knnSearchRecursive(query, root_, queue, k);

        std::vector<int> indices(queue.size());
        for (size_t i = 0; i < queue.size(); i++)
            indices[i] = queue[i].second;

        return indices;