激光雷达定位与建图-最近邻问题_雷达建图算法使用-CSDN博客

本文链接：https://blog.csdn.net/u010196944/article/details/143211762

一、问题引出

最近邻问题：假设有两个点云集合， $\chi _{1} = \left \{ x_{1},\cdots x_{n} \right \}$ ， $\chi _{2} = \left \{ x_{1},\cdots x_{n} \right \}$ ，求点云集合 $\chi _{2}$ 中某个点，在点云集合 $\chi _{1}$ 中与它最近的点是？或者在点云集合 $\chi _{2}$ 中与它最近的K个点是？

常用解决方法有：暴力最近邻法、栅格与体素方法、二分树与k-d树、四叉树与八叉树和其它树类方法。

二、暴力最近邻搜索

暴力最近邻最朴素的思路：假设待查点point和点云，计算待查点对所有点云的距离，排序后给出最小距离；若求最近K个点云，则排序后，给出最近k个即可。

1.暴力 k 近邻（Brute Force k-Nearest Neighbors，简称 BFKNN）

std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k) {
    struct IndexAndDis {
        IndexAndDis() {}
        IndexAndDis(int index, double dis2) : index_(index), dis2_(dis2) {}
        int index_ = 0;
        double dis2_ = 0;
    };

    std::vector<IndexAndDis> index_and_dis(cloud->size());
    for (int i = 0; i < cloud->size(); ++i) {
        index_and_dis[i] = {i, (cloud->points[i].getVector3fMap() - point).squaredNorm()};
    }
    std::sort(index_and_dis.begin(), index_and_dis.end(),
              [](const auto& d1, const auto& d2) { return d1.dis2_ < d2.dis2_; });
    std::vector<int> ret;
    std::transform(index_and_dis.begin(), index_and_dis.begin() + k, std::back_inserter(ret),
                   [](const auto& d1) { return d1.index_; });
    return ret;
}

void bfnn_cloud_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k) {
    std::vector<size_t> index(cloud2->size());
    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });

    matches.resize(index.size() * k);
    std::for_each(std::execution::seq, index.begin(), index.end(), [&](auto idx) {
        auto v = bfnn_point_k(cloud1, ToVec3f(cloud2->points[idx]), k);
        for (int i = 0; i < v.size(); ++i) {
            matches[idx * k + i].first = v[i];
            matches[idx * k + i].second = idx;
        }
    });
}

这两个函数 bfnn_point_k 和 bfnn_cloud_k 实现了暴力 k 近邻（Brute Force k-Nearest Neighbors，简称 BFKNN）算法。下面是对这两个函数的详细解释：

`bfnn_point_k` 函数

这个函数用于在点云 cloud 中找到与给定点 point 最近的 k 个点的索引。

函数参数：

CloudPtr cloud：指向点云的智能指针。
const Vec3f& point：查询点的三维坐标。
int k：要找到的最近邻点的数量。

函数实现步骤：

定义结构体 IndexAndDis：用于存储点云中每个点的索引和它到查询点 point 的平方距离。
初始化 index_and_dis 向量：创建一个 IndexAndDis 类型的向量，大小与点云相同，并初始化每个元素的索引和平方距离。
计算距离并存储：遍历点云中的每个点，计算它与查询点 point 的平方距离，并将索引和距离存储在 index_and_dis 向量中。
排序：使用 std::sort 算法对 index_and_dis 向量进行排序，排序依据是每个元素的平方距离 dis2_。
提取最近邻索引：使用 std::transform 算法和 std::back_inserter 迭代器从排序后的 index_and_dis 向量中提取前 k 个元素的索引，并将它们添加到 ret 向量中。

返回值：

std::vector<int>：包含 k 个最近邻点的索引。

`bfnn_cloud_k` 函数

这个函数用于在两个点云 cloud1 和 cloud2 之间找到每个点的 k 个最近邻点。

函数参数：

CloudPtr cloud1：指向第一个点云的智能指针，将被搜索以找到最近邻点。
CloudPtr cloud2：指向第二个点云的智能指针，其点将作为查询点。
std::vector<std::pair<size_t, size_t>>& matches：一个引用，指向一个向量，该向量存储匹配对。每个匹配对包含 cloud1 中最近邻点的索引和 cloud2 中对应点的索引。
int k：要找到的最近邻点的数量。

函数实现步骤：

初始化索引向量：创建一个与 cloud2 大小相同的索引向量 index。
填充索引向量：使用 std::for_each 算法和一个 Lambda 表达式来填充索引向量。
调整匹配向量的大小：调整 matches 向量的大小以匹配 cloud2 的大小乘以 k，为存储每个点的 k 个最近邻匹配做准备。
寻找 k 个最近邻点：使用 std::for_each 算法和一个 Lambda 表达式遍历 index 向量。对于每个索引 idx，调用 bfnn_point_k 函数找到 cloud2 中点的 k 个最近邻点的索引，并将它们存储在 matches 向量中。

三、栅格与体素搜索

栅格与体素搜索是将点云在空间中划分为二维栅格或者三维体素，计算出每个点对应在栅格中的坐标位置，再对所有点进行索引。所以在查找最近邻时，可以先计算出被查找点所在的栅格，再在该栅格周围寻找最近邻。和暴力搜索相比，这种方法大大缩小了查找范围，从而提升了效率。

从上述描述看：
1.假设待划分的点云不是均匀分布的（实际也不是），所以需要根据点云的疏密程度，需要预先定义划分栅格的分辨率。假设栅格划得太大，一个格子里会有很多点，导致最近邻计算效率下降；如果划分太小，一方面格子数量多，另一方面，在查找相邻格子时，由于点云过于稀疏，最近邻可能并不在上下左右的格子中，可能找不到真正的最近邻，所以网格划分的分辨率需要根据经验来调整。也可以自适应分辨率，如根据点云的密度动态调整分辨率大小。

2.由于划分的格子是离散的，在查找最近邻时，除了在当前栅格内查找，还应该在它周边进行查找。对于二维栅格，一般可分为：上下4格，或加上四个角8格。三维体素，上下前后左右6个体素，或加上八个对角14个体素。

栅格搜索法示例代码，主要分为三步：
1.定义搜索近邻网格范围；
2.划分网格；
3.在待查找点pt栅格周边寻找最近邻。

1.定义搜索近邻网格范围
template <>
void GridNN<2>::GenerateNearbyGrids() {
    if (nearby_type_ == NearbyType::CENTER) {
        nearby_grids_.emplace_back(KeyType::Zero());
    } else if (nearby_type_ == NearbyType::NEARBY4) {
        nearby_grids_ = {Vec2i(0, 0), Vec2i(-1, 0), Vec2i(1, 0), Vec2i(0, 1), Vec2i(0, -1)};
    } else if (nearby_type_ == NearbyType::NEARBY8) {
        nearby_grids_ = {
            Vec2i(0, 0),   Vec2i(-1, 0), Vec2i(1, 0),  Vec2i(0, 1), Vec2i(0, -1),
            Vec2i(-1, -1), Vec2i(-1, 1), Vec2i(1, -1), Vec2i(1, 1),
        };
    }
}

2.划分网格
template <int dim>
bool GridNN<dim>::SetPointCloud(CloudPtr cloud) {
    std::vector<size_t> index(cloud->size());
    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });

    std::for_each(index.begin(), index.end(), [&cloud, this](const size_t& idx) {
        auto pt = cloud->points[idx];
        auto key = Pos2Grid(ToEigen<float, dim>(pt));
        if (grids_.find(key) == grids_.end()) {
            grids_.insert({key, {idx}});
        } else {
            grids_[key].emplace_back(idx);
        }
    });

    cloud_ = cloud;
    LOG(INFO) << "grids: " << grids_.size();
    return true;
}
template <int dim>
Eigen::Matrix<int, dim, 1> GridNN<dim>::Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {
    return pt.array().template round().template cast<int>(); //将每个点云坐标四舍五入、取整，划分为网格
    // Eigen::Matrix<int, dim, 1> ret;
    // for (int i = 0; i < dim; ++i) {
    //     ret(i, 0) = round(pt[i] * inv_resolution_); //根据分辨率，划分网格
    // }
    // return ret;
}

3.在待查找点pt栅格周边寻找最近邻
template <int dim>
bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {
    std::vector<size_t> idx_to_check;
    auto key = Pos2Grid(ToEigen<float, dim>(pt));

    std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &idx_to_check, this](const KeyType& delta) {
        auto dkey = key + delta;
        auto iter = grids_.find(dkey);
        if (iter != grids_.end()) {
            idx_to_check.insert(idx_to_check.end(), iter->second.begin(), iter->second.end());
        }
    });

    if (idx_to_check.empty()) {
        return false;
    }

    // brute force nn in cloud_[idx]
    CloudPtr nearby_cloud(new PointCloudType);
    std::vector<size_t> nearby_idx;
    for (auto& idx : idx_to_check) {
        nearby_cloud->points.template emplace_back(cloud_->points[idx]);
        nearby_idx.emplace_back(idx);
    }

    size_t closest_point_idx = bfnn_point(nearby_cloud, ToVec3f(pt));
    idx = nearby_idx.at(closest_point_idx);
    closest_pt = cloud_->points[idx];

    return true;
}

这段代码实现了一个基于网格的最近邻搜索框架，用于在点云数据中高效地查找最近邻点。以下是对代码的详细解析：

1. 定义搜索近邻网格范围

GenerateNearbyGrids 函数用于定义查询点周边的网格范围，这些网格将被用于后续的最近邻搜索。

nearby_type_ 是一个枚举类型，决定了搜索的范围。
NearbyType::CENTER 表示只搜索中心网格。
NearbyType::NEARBY4 表示搜索中心网格及其上下左右的4个相邻网格。
NearbyType::NEARBY8 表示搜索中心网格及其8个对角相邻网格。

2. 划分网格

SetPointCloud 函数用于将点云数据划分到不同的网格中。

使用 std::for_each 和 Lambda 表达式为点云中的每个点分配一个索引。
通过 Pos2Grid 函数将每个点的坐标转换为网格键。
使用 grids_ 容器（通常是一个哈希表）存储每个网格键及其对应的点索引。

Pos2Grid 函数用于将点的坐标转换为网格索引。

使用 round 函数将点的坐标四舍五入到最近的整数，然后转换为整数类型，作为网格索引。

3. 在待查找点pt栅格周边寻找最近邻

GetClosestPoint 函数用于在查询点 pt 的周边网格中寻找最近邻点。

使用 Pos2Grid 函数将查询点 pt 转换为网格键。
遍历 nearby_grids_ 中定义的网格范围，对于每个网格，检查是否存在对应的点索引。
如果找到对应的点索引，将它们添加到 idx_to_check 向量中。
如果 idx_to_check 向量不为空，使用 bfnn_point 函数在这些点中找到最近的点。
bfnn_point 函数是一个暴力搜索算法，它计算查询点与每个候选点之间的距离，并返回最近点的索引。
最后，将最近邻点的索引和坐标存储在 idx 和 closest_pt 中，并返回 true 表示找到最近邻点。

四、评价

从第三步栅格与体素划分可知，如果栅格划分太小，一方面格子数量多，另一方面，在查找相邻格子时，由于点云过于稀疏，最近邻可能并不在上下左右的格子中，可能找不到真正的最近邻。因此有必要对划分网格方法进行准确性评价。
栅格法最近邻可能存在两种错误的情况：
1.假阳性
栅格法检测出的最近邻，实际上并不是最近邻，这种情况称为假阳性，记作FP。
2.假阴性
实际中的某个最近邻，在栅格法中并没有检测到，这种情况称为假阴性，记作FN。
利用FP和FN的定义，可以定义算法的准确率和召回率。
假设近邻算法中总共计算了m次最近邻，而真值共给出了n个最近邻，那么准确率和召回率定位为：
准确率 = 1 - FP/m，召回率 = 1 - FN/n
暴力搜索匹配可以做到准确率和召回率都为100%，故使用暴力匹配结果作为真值。
示例代码：

//truth:为暴力搜索匹配结果
//esti：为栅格法搜索匹配结果
//以truth为真值，计算esti的准确率和召回率
void EvaluateMatches(const std::vector<std::pair<size_t, size_t>>& truth,
                     const std::vector<std::pair<size_t, size_t>>& esti) {
    int fp = 0;  // false-positive，esti存在但truth中不存在
    int fn = 0;  // false-negative, truth存在但esti不存在

    LOG(INFO) << "truth: " << truth.size() << ", esti: " << esti.size();

    /// 检查某个匹配在另一个容器中存不存在
    auto exist = [](const std::pair<size_t, size_t>& data, const std::vector<std::pair<size_t, size_t>>& vec) -> bool {
        return std::find(vec.begin(), vec.end(), data) != vec.end();
    };

    int effective_esti = 0;
    for (const auto& d : esti) {
        if (d.first != sad::math::kINVALID_ID && d.second != sad::math::kINVALID_ID) {
            effective_esti++;

            if (!exist(d, truth)) {
                fp++;
            }
        }
    }

    for (const auto& d : truth) {
        if (!exist(d, esti)) {
            fn++;
        }
    }

    float precision = 1.0 - float(fp) / effective_esti;
    float recall = 1.0 - float(fn) / truth.size();
    LOG(INFO) << "precision: " << precision << ", recall: " << recall << ", fp: " << fp << ", fn: " << fn;
}

上述定义了一个名为 EvaluateMatches 的函数，它用于评估两种匹配结果的准确率（precision）和召回率（recall）。这里的 “truth” 为暴力搜索匹配结果，而 “esti” 为栅格法搜索得到的匹配结果。该函数通过比较这两种结果来评估后者的性能。

函数参数：

const std::vector<std::pair<size_t, size_t>>& truth：为暴力搜索匹配结果。
const std::vector<std::pair<size_t, size_t>>& esti：为栅格法搜索得到的匹配结果。

函数实现步骤：

初始化计数器：初始化假阳性（false-positive, fp）和假阴性（false-negative, fn）计数器。
记录真实和估计的匹配数量：使用 LOG(INFO) 记录真实匹配和估计匹配的数量。
定义辅助函数 exist：定义一个 Lambda 表达式 exist，用于检查一个匹配对是否存在于给定的匹配对集合中。这个函数使用 std::find 算法来查找特定的匹配对。
计算假阳性（FP）：遍历估计的匹配结果 esti，对于每个有效的匹配对（即两个索引都不是无效值），检查它是否存在于真实匹配结果 truth 中。如果不存在，则增加 fp 计数器。
计算假阴性（FN）：遍历真实匹配结果 truth，对于每个匹配对，检查它是否存在于估计匹配结果 esti 中。如果不存在，则增加 fn 计数器。
计算准确率和召回率：
- 准确率（Precision）：正确匹配的数量除以估计匹配的总数（有效匹配加上假阳性）。
- 召回率（Recall）：正确匹配的数量除以真实匹配的总数（真实匹配减去假阴性）。
记录结果：使用 LOG(INFO) 记录准确率、召回率以及假阳性和假阴性的数量。