栅格最近邻法与体素最近邻法

本博客为高翔《自动驾驶与机器人中的SLAM技术》学习笔记,用于记录书中的源码与注释。

栅格最近邻法和体素最近邻法都是在空间位置上对点云进行索引。

由于二维栅格和三维体素(Voxel)的实现非常相似,所以高翔先生使用模板类同时实现它们。

gridnn.hpp 以下为头文件的源码

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

#ifndef SLAM_IN_AUTO_DRIVING_GRID2D_HPP
#define SLAM_IN_AUTO_DRIVING_GRID2D_HPP

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

#include <glog/logging.h>  //这个头文件是 Google 的 glog 库的一部分,它提供了日志记录功能。
#include <execution>  //这是 C++17 标准库中的一个头文件,它提供了并行算法的执行策略。
#include <map>  //通过包含这个头文件,你可以在代码中使用 std::map 来创建和操作映射表,它提供了基于键的快速查找、插入和删除操作。

namespace sad {

/**
 * 栅格法最近邻
 * @tparam dim 模板参数,使用2D或3D栅格
 */
template <int dim> //这是一个模板声明,dim 是模板参数,表示栅格的维度。它可以是 2 或 3,分别对应二维和三维空间。
class GridNN {
   public:
    using KeyType = Eigen::Matrix<int, dim, 1>;

//创建一个dim维的列向量,用于表示栅格中的键或索引。
    using PtType = Eigen::Matrix<float, dim, 1>;

//定义了 PtType 为一个 dim 维的浮点数向量,用于表示点的坐标。

    enum class NearbyType {
        CENTER,  // 只考虑中心
        // for 2D
        NEARBY4,  // 上下左右
        NEARBY8,  // 上下左右+四角

        // for 3D
        NEARBY6,  // 上下左右前后
    };

    /**
     * 构造函数
     * @param resolution 分辨率
     * @param nearby_type 近邻判定方法
     */
    explicit GridNN(float resolution = 0.1, NearbyType nearby_type = NearbyType::NEARBY4)
        : resolution_(resolution), nearby_type_(nearby_type) {
        inv_resolution_ = 1.0 / resolution_;

//explicit 关键字用于构造函数,它防止了隐式类型转换,这意味着你不能将这个构造函数用作转换操作。例如

#include <iostream>

class MyClass {
public:
    explicit MyClass(int value) {  // 注意 explicit 关键字
        std::cout << "MyClass constructed with value: " << value << std::endl;
    }
};

int main() {
    MyClass obj1(10);  // 显式调用,没问题
    // MyClass obj2 = 20;  // 这将导致编译错误,因为构造函数是 explicit 的
    MyClass obj3 = MyClass(20);  // 显式调用,没问题,需要使用括号
    return 0;
}

        // check dim and nearby
        if (dim == 2 && nearby_type_ == NearbyType::NEARBY6) {
            LOG(INFO) << "2D grid does not support nearby6, using nearby4 instead.";
            nearby_type_ = NearbyType::NEARBY4;
        } else if (dim == 3 && (nearby_type_ != NearbyType::NEARBY6 && nearby_type_ != NearbyType::CENTER)) {
            LOG(INFO) << "3D grid does not support nearby4/8, using nearby6 instead.";
            nearby_type_ = NearbyType::NEARBY6;
        }

        GenerateNearbyGrids();  //用于生成或初始化与当前栅格邻近的栅格信息。
    }

    /// 设置点云,建立栅格
    bool SetPointCloud(CloudPtr cloud);

    /// 获取最近邻
    bool GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx);

    /// 对比两个点云
    bool GetClosestPointForCloud(CloudPtr ref, CloudPtr query, std::vector<std::pair<size_t, size_t>>& matches);
    bool GetClosestPointForCloudMT(CloudPtr ref, CloudPtr query, std::vector<std::pair<size_t, size_t>>& matches);  //用于并发操作

   private:
    /// 根据最近邻的类型,生成附近网格
    void GenerateNearbyGrids();

    /// 空间坐标转到grid
    KeyType Pos2Grid(const PtType& pt);

//用于将空间坐标转换为对应的网格索引。这个函数将接受一个点的坐标(PtType 类型),并返回该点在网格中的索引(KeyType 类型)。

    float resolution_ = 0.1;       // 分辨率
    float inv_resolution_ = 10.0;  // 分辨率倒数

    NearbyType nearby_type_ = NearbyType::NEARBY4;
    std::unordered_map<KeyType, std::vector<size_t>, hash_vec<dim>> grids_;  //  栅格数据

//std::unordered_map 存储键值对,它不保证元素的顺序。

//KeyType键类型,std::vector<size_t>值类型,哈希函数hash_vec<dim>。

//补充一下作者在 eigen_types.h 头文件中定义的哈希函数

/// 矢量哈希
template <int N>
struct hash_vec {
    inline size_t operator()(const Eigen::Matrix<int, N, 1>& v) const;
};
//这是结构体中的一个成员函数声明,它是一个哈希函数。
//这个函数接受一个 Eigen::Matrix<int, N, 1> 类型的向量 v 作为参数,
//并返回一个 size_t 类型的哈希值。
template <>
inline size_t hash_vec<2>::operator()(const Eigen::Matrix<int, 2, 1>& v) const 
{
    return size_t(((v[0] * 73856093) ^ (v[1] * 471943)) % 10000000);
//它使用了一个简单的哈希算法,通过将每个向量分量与一个质数相乘,然后进行异或(XOR)操作,
//最后取模以得到一个较小的整数。这种哈希函数试图减少哈希冲突的概率。
//^表示按位异或
}

template <>
inline size_t hash_vec<3>::operator()(const Eigen::Matrix<int, 3, 1>& v) const 
{
    return size_t(((v[0] * 73856093) ^ (v[1] * 471943) ^ (v[2] * 83492791)) 
% 10000000);
}

//
    CloudPtr cloud_;

    std::vector<KeyType> nearby_grids_;  // 附近的栅格
};

// 实现
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 向量的每个元素时,它为每个元素调用 lambda 表达式。在每次调用中,lambda 表达式都会将 idx 的当前值赋给向量的当前元素,并将 idx 递增。这样,index 向量的每个元素就会被依次赋值为 0, 1, 2, ...,直到向量的最后一个元素。

    std::for_each(index.begin(), index.end(), [&cloud, this](const size_t& idx) {

//&cloud 表示通过引用捕获 cloud 变量,this 表示通过引用捕获当前对象的指针(即 GridNN 类实例的 this 指针)。
        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);
        }

//检查 grids_ 哈希表中是否已经存在 key。如果不存在,则插入一个新的键值对;如果存在,则将索引 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) {

//这是 Pos2Grid 函数的定义,它接受一个 dim 维的浮点数向量 pt 作为参数,并返回一个 dim 维的整数向量,表示点 pt 在网格中的索引。
    return pt.array().template round().template cast<int>();

//pt.array():将 Eigen::Matrix 类型的 pt 转换为一个 Eigen::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;

//这个转换是通过将点的坐标乘以分辨率的倒数(inv_resolution_),然后四舍五入到最近的整数来完成的。
}

template <>
void GridNN<2>::GenerateNearbyGrids() {
    if (nearby_type_ == NearbyType::CENTER) {
        nearby_grids_.emplace_back(KeyType::Zero());

//C++ 标准库中的 emplace_back 函数,它是 std::vector 容器的一个成员函数,用于在向量的末尾就地构造并添加一个新元素。这里,它被用来向 nearby_grids_ 向量中添加一个元素。
    } 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),
        };
    }
}

template <>
void GridNN<3>::GenerateNearbyGrids() {
    if (nearby_type_ == NearbyType::CENTER) {
        nearby_grids_.emplace_back(KeyType::Zero());
    } else if (nearby_type_ == NearbyType::NEARBY6) {
        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
    }
}

template <int dim>
bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {
    // 在pt栅格周边寻找最近邻
    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) {

//对于每个 delta,计算相邻栅格的索引 dkey 并检查 grids_ 中是否存在对应的点。
        auto dkey = key + delta;

//计算相邻网格的实际网格坐标,delta是GenerateNearbyGrids() 中生成的邻域模板向量!
        auto iter = grids_.find(dkey);
        if (iter != grids_.end()) {

//grids_.end() 返回一个迭代器,表示容器的尾后位置(即容器末尾之后的位置),这是一个无效的位置。在grids_中查找这个网格,(iter !=grids_.end())就是可以找到。
            idx_to_check.insert(idx_to_check.end(), iter->second.begin(), iter->second.end());

//将从 iter 指向的网格中获取到的所有点的索引添加到 idx_to_check 容器的末尾。
        }
    });

    if (idx_to_check.empty()) {
        return false;//如果idx_to_check为空,说明没有找到需要检查的点,直接返回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;
}

template <int dim>
bool GridNN<dim>::GetClosestPointForCloud(CloudPtr ref, CloudPtr query,
                                          std::vector<std::pair<size_t, size_t>>& matches) {
    matches.clear();
    std::vector<size_t> index(query->size());// 创建一个大小与查询点云相同的索引向量。
    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
    std::for_each(index.begin(), index.end(), [this, &matches, &query](const size_t& idx) {
        PointType cp;
        size_t cp_idx;
        if (GetClosestPoint(query->points[idx], cp, cp_idx)) {
            matches.emplace_back(cp_idx, idx);

//GetClosestPoint 方法的作用是找到与给定查询点 query->points[idx] 最近的点,并将其信息存储在 cpcp_idx 中。如果找到最近点,则返回 true
        }
    });

    return true;
}

template <int dim>
bool GridNN<dim>::GetClosestPointForCloudMT(CloudPtr ref, CloudPtr query,
                                            std::vector<std::pair<size_t, size_t>>& matches) {
    matches.clear();
    // 与串行版本基本一样,但matches需要预先生成,匹配失败时填入非法匹配
    std::vector<size_t> index(query->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::par_unseq, index.begin(), index.end(), [this, &matches, &query](const size_t& idx) {
        PointType cp;
        size_t cp_idx;
        if (GetClosestPoint(query->points[idx], cp, cp_idx)) {
            matches[idx] = {cp_idx, idx};
        } else {
            matches[idx] = {math::kINVALID_ID, math::kINVALID_ID};

//填入非法匹配值 math::kINVALID_ID
        }
    });

    return true;
}

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_GRID2D_HPP

//补充:

在 C++ 标准库中,std::unordered_map 是一种基于哈希表的关联容器,它通过哈希函数来快速地定位元素。哈希函数的作用是将键(key)映射到哈希表的一个位置,这个位置通常称为“哈希桶”或“槽”(bucket)。以下是哈希函数的几个关键作用:

  1. 快速查找

    • 哈希函数使得 std::unordered_map 能够在平均时间复杂度为 O(1) 内完成元素的查找。这是因为哈希函数可以将键快速映射到哈希表中的一个特定位置。
  2. 减少冲突

    • 不同的键可能会映射到同一个哈希桶,这种现象称为“哈希冲突”。一个好的哈希函数会尽量减少这种冲突的发生,以保持操作的效率。
  3. 均匀分布

    • 哈希函数应该能够将键均匀地分布在哈希表的所有桶中,这样可以避免某些桶过载(即包含太多元素),从而影响性能。
  4. 确定性

    • 对于同一个键,哈希函数总是应该返回相同的哈希值。这是确保 std::unordered_map 能够正确存储和检索元素的关键。
  5. 快速计算

    • 哈希函数的计算应该尽可能快速,以最小化对性能的影响。复杂的哈希函数可能会增加计算时间,从而降低哈希表的整体效率。
  • 11
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值