一、问题引出
最近邻问题:假设有两个点云集合, χ 1 = { x 1 , ⋯ x n } \chi _{1} = \left \{ x_{1},\cdots x_{n} \right \} χ1={x1,⋯xn} , χ 2 = { x 1 , ⋯ x n } \chi _{2} = \left \{ x_{1},\cdots x_{n} \right \} χ2={x1,⋯xn} ,求点云集合 χ 2 \chi _{2} χ2中某个点,在点云集合 χ 1 \chi _{1} χ1中与它最近的点是?或者在点云集合中与它最近的K个点是?
常用解决方法有:暴力最近邻法、栅格与体素方法、二分树与k-d树、四叉树与八叉树和其它树类方法。
二、暴力最近邻搜索
暴力最近邻最朴素的思路:假设待查点point: 和点云,则计算对所有点云的距离,排序后给出最小距离;若求最近K个点云,则排序后,给出最近k个即可。
1.暴力 k 近邻(Brute Force k-Nearest Neighbors,简称 BFKNN)
std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k) {struct IndexAndDis {IndexAndDis() {}IndexAndDis(int index, double dis2) : index_(index), dis2_(dis2) {}int index_ = 0;double dis2_ = 0;};std::vector<IndexAndDis> index_and_dis(cloud->size());for (int i = 0; i < cloud->size(); ++i) {index_and_dis[i] = {i, (cloud->points[i].getVector3fMap() - point).squaredNorm()};}std::sort(index_and_dis.begin(), index_and_dis.end(),[](const auto& d1, const auto& d2) { return d1.dis2_ < d2.dis2_; });std::vector<int> ret;std::transform(index_and_dis.begin(), index_and_dis.begin() + k, std::back_inserter(ret),[](const auto& d1) { return d1.index_; });return ret;
}void bfnn_cloud_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k) {std::vector<size_t> index(cloud2->size());std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });matches.resize(index.size() * k);std::for_each(std::execution::seq, index.begin(), index.end(), [&](auto idx) {auto v = bfnn_point_k(cloud1, ToVec3f(cloud2->points[idx]), k);for (int i = 0; i < v.size(); ++i) {matches[idx * k + i].first = v[i];matches[idx * k + i].second = idx;}});
}
这两个函数 bfnn_point_k
和 bfnn_cloud_k
实现了暴力 k 近邻(Brute Force k-Nearest Neighbors,简称 BFKNN)算法。下面是对这两个函数的详细解释:
bfnn_point_k
函数
这个函数用于在点云 cloud
中找到与给定点 point
最近的 k 个点的索引。
函数参数:
CloudPtr cloud
:指向点云的智能指针。const Vec3f& point
:查询点的三维坐标。int k
:要找到的最近邻点的数量。
函数实现步骤:
-
定义结构体
IndexAndDis
:用于存储点云中每个点的索引和它到查询点point
的平方距离。 -
初始化
index_and_dis
向量:创建一个IndexAndDis
类型的向量,大小与点云相同,并初始化每个元素的索引和平方距离。 -
计算距离并存储:遍历点云中的每个点,计算它与查询点
point
的平方距离,并将索引和距离存储在index_and_dis
向量中。 -
排序:使用
std::sort
算法对index_and_dis
向量进行排序,排序依据是每个元素的平方距离dis2_
。 -
提取最近邻索引:使用
std::transform
算法和std::back_inserter
迭代器从排序后的index_and_dis
向量中提取前 k 个元素的索引,并将它们添加到ret
向量中。
返回值:
std::vector<int>
:包含 k 个最近邻点的索引。
bfnn_cloud_k
函数
这个函数用于在两个点云 cloud1
和 cloud2
之间找到每个点的 k 个最近邻点。
函数参数:
CloudPtr cloud1
:指向第一个点云的智能指针,将被搜索以找到最近邻点。CloudPtr cloud2
:指向第二个点云的智能指针,其点将作为查询点。std::vector<std::pair<size_t, size_t>>& matches
:一个引用,指向一个向量,该向量存储匹配对。每个匹配对包含cloud1
中最近邻点的索引和cloud2
中对应点的索引。int k
:要找到的最近邻点的数量。
函数实现步骤:
-
初始化索引向量:创建一个与
cloud2
大小相同的索引向量index
。 -
填充索引向量:使用
std::for_each
算法和一个 Lambda 表达式来填充索引向量。 -
调整匹配向量的大小:调整
matches
向量的大小以匹配cloud2
的大小乘以 k,为存储每个点的 k 个最近邻匹配做准备。 -
寻找 k 个最近邻点:使用
std::for_each
算法和一个 Lambda 表达式遍历index
向量。对于每个索引idx
,调用bfnn_point_k
函数找到cloud2
中点的 k 个最近邻点的索引,并将它们存储在matches
向量中。
三、栅格与体素搜索
栅格与体素搜索是将点云在空间中划分为二维栅格或者三维体素,计算出每个点对应在栅格中的坐标位置,再对所有点进行索引。所以在查找最近邻时,可以先计算出被查找点所在的栅格,再在该栅格周围寻找最近邻。和暴力搜索相比,这种方法大大缩小了查找范围,从而提升了效率。
从上述描述看:
1.假设待划分的点云不是均匀分布的(实际也不是),所以需要根据点云的疏密程度,需要预先定义划分栅格的分辨率。假设栅格划得太大,一个格子里会有很多点,导致最近邻计算效率下降;如果划分太小,一方面格子数量多,另一方面,在查找相邻格子时,由于点云过于稀疏,最近邻可能并不在上下左右的格子中,可能找不到真正的最近邻,所以网格划分的分辨率需要根据经验来调整。也可以自适应分辨率,如根据点云的密度动态调整分辨率大小。
2.由于划分的格子是离散的,在查找最近邻时,除了在当前栅格内查找,还应该在它周边进行查找。对于二维栅格,一般可分为:上下4格,或加上四个角8格。三维体素,上下前后左右6个体素,或加上八个对角14个体素。
栅格搜索法示例代码,主要分为三步:
1.定义搜索近邻网格范围;
2.划分网格;
3.在待查找点pt栅格周边寻找最近邻。
1.定义搜索近邻网格范围
template <>
void GridNN<2>::GenerateNearbyGrids() {if (nearby_type_ == NearbyType::CENTER) {nearby_grids_.emplace_back(KeyType::Zero());} 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),};}
}2.划分网格
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));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;
}
template <int dim>
Eigen::Matrix<int, dim, 1> GridNN<dim>::Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {return pt.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;
}3.在待查找点pt栅格周边寻找最近邻
template <int dim>
bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {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;auto iter = grids_.find(dkey);if (iter != grids_.end()) {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]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;
}
这段代码实现了一个基于网格的最近邻搜索框架,用于在点云数据中高效地查找最近邻点。以下是对代码的详细解析:
1. 定义搜索近邻网格范围
GenerateNearbyGrids
函数用于定义查询点周边的网格范围,这些网格将被用于后续的最近邻搜索。
nearby_type_
是一个枚举类型,决定了搜索的范围。NearbyType::CENTER
表示只搜索中心网格。NearbyType::NEARBY4
表示搜索中心网格及其上下左右的4个相邻网格。NearbyType::NEARBY8
表示搜索中心网格及其8个对角相邻网格。
2. 划分网格
SetPointCloud
函数用于将点云数据划分到不同的网格中。
- 使用
std::for_each
和 Lambda 表达式为点云中的每个点分配一个索引。 - 通过
Pos2Grid
函数将每个点的坐标转换为网格键。 - 使用
grids_
容器(通常是一个哈希表)存储每个网格键及其对应的点索引。
Pos2Grid
函数用于将点的坐标转换为网格索引。
- 使用
round
函数将点的坐标四舍五入到最近的整数,然后转换为整数类型,作为网格索引。
3. 在待查找点pt栅格周边寻找最近邻
GetClosestPoint
函数用于在查询点 pt
的周边网格中寻找最近邻点。
-
使用
Pos2Grid
函数将查询点pt
转换为网格键。 -
遍历
nearby_grids_
中定义的网格范围,对于每个网格,检查是否存在对应的点索引。 -
如果找到对应的点索引,将它们添加到
idx_to_check
向量中。 -
如果
idx_to_check
向量不为空,使用bfnn_point
函数在这些点中找到最近的点。 -
bfnn_point
函数是一个暴力搜索算法,它计算查询点与每个候选点之间的距离,并返回最近点的索引。 -
最后,将最近邻点的索引和坐标存储在
idx
和closest_pt
中,并返回true
表示找到最近邻点。
四、评价
从第三步栅格与体素划分可知,如果栅格划分太小,一方面格子数量多,另一方面,在查找相邻格子时,由于点云过于稀疏,最近邻可能并不在上下左右的格子中,可能找不到真正的最近邻。因此有必要对划分网格方法进行准确性评价。
栅格法最近邻可能存在两种错误的情况:
1.假阳性
栅格法检测出的最近邻,实际上并不是最近邻,这种情况称为假阳性,记作FP。
2.假阴性
实际中的某个最近邻,在栅格法中并没有检测到,这种情况称为假阴性,记作FN。
利用FP和FN的定义,可以定义算法的准确率和召回率。
假设近邻算法中总共计算了m次最近邻,而真值共给出了n个最近邻,那么准确率和召回率定位为:
准确率 = 1 - FP/m,召回率 = 1 - FN/n
暴力搜索匹配可以做到准确率和召回率都为100%,故使用暴力匹配结果作为真值。
示例代码:
//truth:为暴力搜索匹配结果
//esti:为栅格法搜索匹配结果
//以truth为真值,计算esti的准确率和召回率
void EvaluateMatches(const std::vector<std::pair<size_t, size_t>>& truth,const std::vector<std::pair<size_t, size_t>>& esti) {int fp = 0; // false-positive,esti存在但truth中不存在int fn = 0; // false-negative, truth存在但esti不存在LOG(INFO) << "truth: " << truth.size() << ", esti: " << esti.size();/// 检查某个匹配在另一个容器中存不存在auto exist = [](const std::pair<size_t, size_t>& data, const std::vector<std::pair<size_t, size_t>>& vec) -> bool {return std::find(vec.begin(), vec.end(), data) != vec.end();};int effective_esti = 0;for (const auto& d : esti) {if (d.first != sad::math::kINVALID_ID && d.second != sad::math::kINVALID_ID) {effective_esti++;if (!exist(d, truth)) {fp++;}}}for (const auto& d : truth) {if (!exist(d, esti)) {fn++;}}float precision = 1.0 - float(fp) / effective_esti;float recall = 1.0 - float(fn) / truth.size();LOG(INFO) << "precision: " << precision << ", recall: " << recall << ", fp: " << fp << ", fn: " << fn;
}
上述定义了一个名为 EvaluateMatches
的函数,它用于评估两种匹配结果的准确率(precision)和召回率(recall)。这里的 “truth” 为暴力搜索匹配结果,而 “esti” 为栅格法搜索得到的匹配结果。该函数通过比较这两种结果来评估后者的性能。
函数参数:
const std::vector<std::pair<size_t, size_t>>& truth
:为暴力搜索匹配结果。const std::vector<std::pair<size_t, size_t>>& esti
:为栅格法搜索得到的匹配结果。
函数实现步骤:
-
初始化计数器:初始化假阳性(false-positive, fp)和假阴性(false-negative, fn)计数器。
-
记录真实和估计的匹配数量:使用
LOG(INFO)
记录真实匹配和估计匹配的数量。 -
定义辅助函数
exist
:定义一个 Lambda 表达式exist
,用于检查一个匹配对是否存在于给定的匹配对集合中。这个函数使用std::find
算法来查找特定的匹配对。 -
计算假阳性(FP):遍历估计的匹配结果
esti
,对于每个有效的匹配对(即两个索引都不是无效值),检查它是否存在于真实匹配结果truth
中。如果不存在,则增加fp
计数器。 -
计算假阴性(FN):遍历真实匹配结果
truth
,对于每个匹配对,检查它是否存在于估计匹配结果esti
中。如果不存在,则增加fn
计数器。 -
计算准确率和召回率:
- 准确率(Precision):正确匹配的数量除以估计匹配的总数(有效匹配加上假阳性)。
- 召回率(Recall):正确匹配的数量除以真实匹配的总数(真实匹配减去假阴性)。
-
记录结果:使用
LOG(INFO)
记录准确率、召回率以及假阳性和假阴性的数量。
五、参考
<<自动驾驶与机器人中的SLAM技术从理论到实践>>