BFNN暴力K近邻

此篇博客是高翔的《自动驾驶中的SLAM》书中的学习笔记，用于记录和注释书中的源码。 

bfnn.h 以下是头文件部分

//
// Created by xiang on 2021/8/18.
//

#ifndef SLAM_IN_AUTO_DRIVING_BFNN_H
#define SLAM_IN_AUTO_DRIVING_BFNN_H

#include "common/eigen_types.h"
#include "common/point_types.h"

#include <thread>  //这是 C++11 标准库中的头文件，它提供了线程相关的功能。通过包含这个头文件，程序可以使用 C++11 的线程库来创建和管理线程。

namespace sad {

/**
 * Brute-force Nearest Neighbour
 * @param cloud 点云
 * @param point 待查找点
 * @return 找到的最近点索引
 */
int bfnn_point(CloudPtr cloud, const Vec3f& point);  //Vec3f 是一个表示三维向量的类型，通常包含三个浮点数（x, y, z 坐标）。const 关键字表示这个参数在函数内部不会被修改。& 表示这是一个引用参数，这样可以避免在函数调用时复制整个向量，提高效率。

/**
 * Brute-force Nearest Neighbour, k近邻
 * @param cloud 点云
 * @param point 待查找点
 * @param k 近邻数
 * @return 找到的最近点索引
 */
std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k = 5);  //std::vector<int>这是函数的返回类型，表示 bfnn_point_k 函数将返回一个整数向量。

/**
 * 对点云进行BF最近邻
 * @param cloud1  目标点云
 * @param cloud2  被查找点云
 * @param matches 两个点云内的匹配关系
 * @return
 */
void bfnn_cloud(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches);  //在 C++ 标准库中，std::pair 是一个模板类，用于存储两个元素的配对，这两个元素可以是不同的类型。

/**
 * 对点云进行BF最近邻 多线程版本
 * @param cloud1
 * @param cloud2
 * @param matches
 */
void bfnn_cloud_mt(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches);

/**
 * 对点云进行BF最近邻 多线程版本，k近邻
 * @param cloud1
 * @param cloud2
 * @param matches
 */
void bfnn_cloud_mt_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k = 5);
}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_BFNN_H

bfnn.cc 以下是源文件部分

//
// Created by xiang on 2021/8/18.
//

#include "ch5/bfnn.h"
#include <execution>  //#include <execution> 是 C++ 标准库中的一个头文件，它提供了与并行算法执行相关的功能。这个头文件是 C++17 标准的一部分，它引入了一种新的方式来指定并行算法的执行策略，允许开发者更轻松地利用多核处理器的并行处理能力。

namespace sad {

int bfnn_point(CloudPtr cloud, const Vec3f& point) {
    return std::min_element(cloud->points.begin(), cloud->points.end(),
                            [&point](const PointType& pt1, const PointType& pt2) -> bool {
                                return (pt1.getVector3fMap() - point).squaredNorm() <
                                       (pt2.getVector3fMap() - point).squaredNorm();
                            }) - cloud->points.begin();
} //std::min_element：这是 C++ 标准库中的一个算法，用于在给定范围内找到最小元素。它接受三个参数：开始迭代器、结束迭代器和一个可选的比较函数。

//

• [&point](const PointType& pt1, const PointType& pt2) -> bool { ... }：这是一个 lambda 表达式，用作 std::min_element 函数的比较函数。

• -> bool：这是 lambda 表达式的返回类型，指定它返回一个布尔值。

• return (pt1.getVector3fMap() - point).squaredNorm() < (pt2.getVector3fMap() - point).squaredNorm()：这是 lambda 表达式的主体，它计算两个点 pt1 和 pt2 与给定点 point 之间的平方欧几里得距离，并比较这两个距离。返回 true 表示 pt1 更接近 point，这将使得 std::min_element 选择 pt1 作为更小的元素。

• return std::min_element(...) - cloud->points.begin();：这行代码返回找到的最近点的索引。std::min_element 返回一个迭代器，指向找到的最小元素。通过从这个迭代器中减去 cloud->points.begin()，我们可以得到从点云开始到这个点的索引。

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;
    };  //这是一个结构体，用于存储点云中每个点的索引和它与给定点 point 之间的平方距离。

    std::vector<IndexAndDis> index_and_dis(cloud->size());

//这行代码创建了一个 IndexAndDis 类型的向量 index_and_dis，其大小与点云 cloud 中的点数相同。每个元素将存储一个点的索引和它与给定点的平方距离。
    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::sort 函数对 index_and_dis 向量进行排序。排序依据是每个点与给定点之间的平方距离 dis2_。这里使用了 lambda 表达式作为比较函数。
    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;
}

//使用 std::transform 函数从排序后的 index_and_dis 向量中提取前 k 个元素的索引，并将它们存储在新的向量 ret 中。这里使用了 std::back_inserter 来提供一个迭代器，它将新元素插入到 ret 向量的末尾。同时，lambda 表达式用于从 IndexAndDis 对象中提取 index_ 成员。

void bfnn_cloud_mt(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches) {
    // 先生成索引
    std::vector<size_t> index(cloud2->size());
    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });

//std::for_each(index.begin(), index.end(), ...：这是 std::for_each 算法的调用，它接受三个参数：开始迭代器 index.begin()，结束迭代器 index.end()，以及一个操作（在这里是一个 lambda 表达式）。

//mutable 关键字允许 lambda 表达式修改捕获的变量，即使它通过值捕获。

    // 并行化for_each
    matches.resize(index.size());
    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](auto idx) {
        matches[idx].second = idx;

//这行代码将 matches 向量中对应索引 idx 的第二个元素（.second）设置为 idx 本身。这是为了记录 cloud2 中点的原始索引。
        matches[idx].first = bfnn_point(cloud1, ToVec3f(cloud2->points[idx]));

//这行代码将找到的最近点的索引赋值给 matches 向量中对应索引 idx 的第一个元素（.first）。
    });
}   //par_unseq 表示并行未序列化执行策略，它允许算法以任意顺序执行操作，这通常用于可以独立执行且不需要考虑执行顺序的任务。

void bfnn_cloud(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches) {
    // 单线程版本
    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());
    std::for_each(std::execution::seq, index.begin(), index.end(), [&](auto idx) {
        matches[idx].second = idx;
        matches[idx].first = bfnn_point(cloud1, ToVec3f(cloud2->points[idx]));
    });

//std::execution::seq：这是一个执行策略标签，指示 std::for_each 以顺序方式执行操作。这是默认的执行策略，意味着没有特别的并行处理。
}

void bfnn_cloud_mt_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++; });

    // 并行化for_each
    matches.resize(index.size() * k);
    std::for_each(std::execution::par_unseq, 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;
        }
    });
}

}  // namespace sad