首页 > 其他分享 >栅格最近邻法与体素最近邻法

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

时间：2024-08-31 21:56:48浏览次数：9  
标签：std 邻法 idx size grids 栅格 哈希 体素 nearby

本博客为高翔《自动驾驶与机器人中的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] 最近的点，并将其信息存储在 cp 和 cp_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. 快速计算：

    • 哈希函数的计算应该尽可能快速，以最小化对性能的影响。复杂的哈希函数可能会增加计算时间，从而降低哈希表的整体效率。

标签：std,邻法,idx,size,grids,栅格,哈希,体素,nearby
From： https://blog.csdn.net/Joeybee/article/details/141753194

相关文章

  • 【智能算法应用】基于融合改进A星-麻雀搜索算法求解六边形栅格地图路径规划
    目录1.算法原理2.结果展示3.参考文献4.代码获取1.算法原理【智能算法】麻雀搜索算法（SSA）原理及实现六边形栅格地图分析一下地图：六边形栅格地图上移动可以看做6领域运动，偶数列与奇数列移动方式有所差异，将六边形栅格地图与二维栅格地图做映射可以发现：偶数列移动......
  • 用OpenCV画简单图形以及绘制栅格地图
    目录前言一、用函数绘制简单图形1.画直线2.画矩形3.画圆形二、绘制栅格地图前言    要完成opencv绘制栅格地图，需要具备的基础知识：opencv相关函数的简单使用（包括简单图形和网格的绘制）一、用函数绘制简单图形importcv2importnumpyasnpimportcv2：导入......
  • 栅格布局在 HarmonyOS 中的应用及扩展
    栅格布局作为一种经典的布局方式，广泛应用于不同类型的用户界面设计，尤其是在移动设备和响应式设计中，它表现出了强大的适应性。本文将深入探讨如何在HarmonyOS中使用栅格布局组件GridRow和GridCol，并通过多种示例来展示栅格布局的灵活性及扩展性。栅格布局的核心优势1.......
  • ArcGIS Pro 实现人口分布栅格TIFF数据的网格提取与可视化
    这里在分享一个人口1km精度栅格数据，LandScan是由美国能源部橡树岭国家实验室（ORNL）提供的全球人口分布数据集，具有最高分辨率的全球人口分布数据，是全球人口数据发布的社会标准，是全球最为准确、可靠，基于地理位置的，具有分布模型和最佳分辨率的全球人口动态统计分析数据库。这一数据......
  • Python 栅格数据处理教程（二）
    本文将介绍通过ArcGISPro的Python模块（arcpy）对栅格数据进行栅格计算及数据统计的方法。1数据来源及介绍本文使用的数据为国家青藏高原科学数据中心的中国1km分辨率逐月降水量数据集基础上通过《Python栅格数据处理教程（一）》中的方法提取出的吉林省范围降水量数据。该数据......
  • Python 栅格数据处理教程（一）
    本文将介绍通过ArcGISPro的Python模块（arcpy）对栅格数据定义投影及裁剪的方法。1数据来源及介绍降水量数据：国家青藏高原科学数据中心的中国1km分辨率逐月降水量数据集。行政区数据：天地图行政区划数据中的吉林省边界面数据，该数据为GeoJSON格式，可通过QGIS等软件将其转换......
  • 基于WEB的多媒体素材管理库/计算机毕设
    摘  要随着小视频的兴起，个人多媒体素材越来越多，使用多媒体素材管理库管理多媒体素材，不仅实现了智能化管理，还提高用户体感，给用户提供便利的查询多媒体素材功能。该管理库通过MVC的编程设计方式，使用了了Java语言和MySQL存储数据。该系统采用了一个基于SSM的框架结构，系统的......
  • NetCDF 文件批量转栅格并导出栅格各波段
    两年前，我曾发布过一篇名为《导出NetCDF栅格图层的各个波段》的公众号推文，讲述了通过网络中的ArcGIS工具将单个NetCDF文件的各个波段分别导出为tif文件的方法。该工具提供了arcpy源代码，我们以该代码为基础，将其转换为ArcGISPro环境下的Python3代码，并使程序可对多个文......
  • GeoServer+Postgis发布存储在Postgis中的栅格数据（二）--pgraster插件使用
    这一篇是前面一篇的续集，前一篇链接GeoServer+Postgis发布存储在Postgis中的栅格数据前期准备pgraster插件下载:还是提供一个maven地址，直接搜索pgraster即可，版本的话因为插件是gs-开头选择和GeoServer版本一致即可，前一篇使用的GeoServer版本为2.19.6，所以这里也选择2.19.6即......
  • 【智能算法应用】A*和改进A*求解大规模栅格地图路径规划问题
    目录1.算法原理2.二值图像构建大规模栅格地图3.结果展示4.代码获取1.算法原理精准导航：用A*算法优化栅格地图的路径规划【附Matlab代码】改进A*算法通过删除必要的拐点或简化路径来减少路径长度，使得路径更为直观和高效。2.二值图像构建大规模栅格地图给定一幅二......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99