深蓝学院高翔《自动驾驶与机器人中的SLAM技术》学习记录3 点云处理，栅格匹配

Zcoasting

已于 2023-11-09 22:21:09 修改

阅读量422

点赞数

分类专栏：《自动驾驶与机器人中的slam技术》学习记录与代码理解文章标签：自动驾驶机器人学习

于 2023-11-09 22:09:53 首次发布

本文链接：https://blog.csdn.net/Zcoasting/article/details/134321655

版权

《自动驾驶与机器人中的slam技术》学习记录与代码理解专栏收录该内容

5 篇文章 5 订阅

订阅专栏

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》学习记录3 点云处理，栅格匹配

std 相关函数理解

std::for_each() 在给定范围内对每一个对象使用 _f() 函数，

  template<typename _InputIterator, typename _Function>
    _Function
    for_each(_InputIterator __first, _InputIterator __last, _Function __f)
    {
      // concept requirements
      __glibcxx_function_requires(_InputIteratorConcept<_InputIterator>)
      __glibcxx_requires_valid_range(__first, __last);
      for (; __first != __last; ++__first)
	__f(*__first);
      return __f; // N.B. [alg.foreach] says std::move(f) but it's redundant.
    }

std::min_element() 返回范围内使用comp函数定义的最小值，从广义上的第一个对象开始

  template<typename _ForwardIterator, typename _Compare>
    _GLIBCXX14_CONSTEXPR
    _ForwardIterator
    __min_element(_ForwardIterator __first, _ForwardIterator __last,
		  _Compare __comp)
    {
      if (__first == __last)
	return __first;
      _ForwardIterator __result = __first;
      while (++__first != __last)
	if (__comp(__first, __result))
	  __result = __first;
      return __result;
    }

std::vector::at() 返回第 n个对象的“值”

      at(size_type __n)
      {
	_M_range_check(__n);
	return (*this)[__n];
      }

hash_vec()

// 过度使用内联函数可能会导致代码膨胀，
// 因为每个函数调用都被替换为函数体的内容。
// 因此，通常只有函数体非常小的函数才会被声明为 inline。
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);
}

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);
}

哈希函数在对键 KeyType 是栅格的坐标，值 std::vector<size_t> 是该栅格中点的索引的grids_生效。

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

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

    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_;  //  栅格数据
    // // 键 KeyType 是栅格的坐标，值 std::vector<size_t> 是该栅格中点的索引。
    CloudPtr cloud_;

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

栅格法匹配流程

// 主测试函数
TEST(CH5_TEST, GRID_NN) {
    sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
    pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
    pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);

    if (first->empty() || second->empty()) {
        LOG(ERROR) << "cannot load cloud";
        FAIL();
    }

    // voxel grid 至 0.05 点云滤波
    sad::VoxelGrid(first);
    sad::VoxelGrid(second);

    LOG(INFO) << "points: " << first->size() << ", " << second->size();

    std::vector<std::pair<size_t, size_t>> truth_matches;
    sad::bfnn_cloud(first, second, truth_matches);

    // 对比不同种类的grid
    sad::GridNN<2> grid0(0.1, sad::GridNN<2>::NearbyType::CENTER), grid4(0.1, sad::GridNN<2>::NearbyType::NEARBY4),
        grid8(0.1, sad::GridNN<2>::NearbyType::NEARBY8);
    sad::GridNN<3> grid3(0.1, sad::GridNN<3>::NearbyType::NEARBY6);

    grid0.SetPointCloud(first);
    grid4.SetPointCloud(first);
    grid8.SetPointCloud(first);
    grid3.SetPointCloud(first);

    // 评价各种版本的Grid NN
    // sorry没有C17的template lambda... 下面必须写的啰嗦一些
    LOG(INFO) << "===================";
    std::vector<std::pair<size_t, size_t>> matches;
    sad::evaluate_and_call(
        [&first, &second, &grid0, &matches]() { grid0.GetClosestPointForCloud(first, second, matches); },
        "Grid0 单线程", 10);
    EvaluateMatches(truth_matches, matches);

    LOG(INFO) << "===================";
    sad::evaluate_and_call(
        [&first, &second, &grid0, &matches]() { grid0.GetClosestPointForCloudMT(first, second, matches); },
        "Grid0 多线程", 10);
    EvaluateMatches(truth_matches, matches);

注意！
在构造不同种类grid，因为类声明中explic的存在，必须显式地调用构造函数
SetPointCloud(first)函数将first赋值给类变量cloud_!!!，基本流程为，遍历点云中点的过程中，遇到一个点计算其栅格值KeyType，并通过哈希函数在grids_栅格无序图中寻找有没有已经被加入进来，如果已经加入进来就将该点的索引加入到该小栅格区域的最后，如果无序图中没有，在grids_中插入一个新的栅格，并把这个点也插入进去，其实就是插入一个键值对{key, {idx}}。

// 实现
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));
        //  find()利用哈希函数去寻找对应的索引
        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;
}

单线程，顺序调用

// 调用
grid0.GetClosestPointForCloud(first, second, matches)

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);
        }
    });

    return true;
}

多线程，多核调用，与串行版本基本一样，但matches需要预先生成，匹配失败时填入非法匹配

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};
        }
    });

    return true;
}

重点函数GetClosestPoint（），首先明确 this 中的 cloud_为first点云，此时两坨点云位移差距不大，所以将second点云中的点的栅格，就认定为first中的栅格。
具体流程见代码注释

template <int dim>
bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {
    // 在pt栅格周边寻找最近邻
    // 找到 pt 所在的网格以及其附近的网格中的所有点，
    // 这些点的索引被添加到 idx_to_check 中，
    // 以便后续检查哪个点是 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) {
        auto dkey = key + delta;
        // grids_ 为 cloud_ 中所有点建立的栅格，键为栅格坐标，值为点的索引。
        // 从 grids_ 中查找 dkey 对应的栅格，
        // 如果找到了，就将该栅格中的所有点的索引添加到 idx_to_check 中。
        auto iter = grids_.find(dkey);
        if (iter != grids_.end()) {
            // first是键，second是值，值为点的索引
            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]
    // nearby_idx中的idx为first点云中的索引，不一定“连续”
    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);
    }
    // bfnn_point返回pt在nearby_cloud中最近点的索引
    size_t closest_point_idx = bfnn_point(nearby_cloud, ToVec3f(pt));
    // 转换为在first点云中的索引
    // at(__n) -> ((*this)[__n])是一个整体，返回nearby_idx的第__n个元素，是cloud_中的索引;
    idx = nearby_idx.at(closest_point_idx);
    closest_pt = cloud_->points[idx];

    return true;
}