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

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

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

std 相关函数理解

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

  template<typename _InputIterator, typename _Function>_Functionfor_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();/// 空间坐标转到gridKeyType 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);// 对比不同种类的gridsad::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的 下面必须写的啰嗦一些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(), d(), [idx = 0](size_t& i) mutable { i = idx++; });std::for_each(index.begin(), d(), [&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(), d(), [idx = 0](size_t& i) mutable { i = idx++; });std::for_each(index.begin(), d(), [this, &matches, &query](const size_t& idx) {PointType cp;size_t cp_idx;if (GetClosestPoint(query->points[idx], cp, cp_idx)) {place_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(), d(), [idx = 0](size_t& i) mutable { i = idx++; });size(index.size());std::for_each(std::execution::par_unseq, index.begin(), d(), [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_d(), iter->second.begin(), iter-&d());}});if (idx_pty()) {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-&plate emplace_back(cloud_->points[idx]);place_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;
}