深蓝学院高翔《自动驾驶与机器人中的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];
}
// 过度使用内联函数可能会导致代码膨胀,
// 因为每个函数调用都被替换为函数体的内容。
// 因此,通常只有函数体非常小的函数才会被声明为 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;
}