一、概念准备
MST最小生成树算法是一种图论的算法。
- 连通图:无向图中,任意两个顶点都有路径相通。
- 强连通图:有向图中,任意两个顶点都有路径相通。
- 连通网:在连通图中,若图的边有权值;权代表着连接连个顶点的代价,称这种连通图叫做连通网。
- 生成树:一个连通图的生成树是指一个连通子图,它含有图中全部n个顶点,但只有足以构成一棵树的n-1条边。一颗有n个顶点的生成树有且仅有n-1条边,如果生成树中再添加一条边,则必定成环。
- 最小生成树:在连通网的所有生成树中,所有边的代价和最小的生成树,称为最小生成树。
- 并查集:并查集是一种树型的数据结构,用于处理一些不相交集合(disjoint sets)的合并及查询问题。常常在使用中以森林来表示。在一些有N个元素的集合应用问题中,我们通常是在开始时让每个元素构成一个单元素的集合,然后按一定顺序将属于同一组的元素所在的集合合并,其间要反复查找一个元素在哪个集合中。
- 并查集的功能:
- 查找:查找元素所在的集合,即根节点。
- 合并:将两个元素所在的集合合并为一个集合。合并之前,应先判断两个元素是否属于同一集 合,这可用上面的“查找”操作实现。
二、最小生成树获取算法
- prim算法
此算法可以称为“加点法”,每次迭代选择代价最小的边对应的点,加入到最小生成树中。算法从某一个顶点s开始,逐渐长大覆盖整个连通网的所有顶点。
- 图的所有顶点集合为VV;初始令集合u={s},v=V−uu={s},v=V−u;
- 在两个集合u,vu,v能够组成的边中,选择一条代价最小的边(u0,v0)(u0,v0),加入到最小生成树中,并把v0v0并入到集合u中。
- 重复上述步骤,直到最小生成树有n-1条边或者n个顶点为止。
由于不断向集合u中加点,所以最小代价边必须同步更新;需要建立一个辅助数组closedge,用来维护集合v中每个顶点与集合u中最小代价边信息;
![516e9cefd88400f129f5608824a52df0.png](https://i-blog.csdnimg.cn/blog_migrate/e238b1beaecc6f54593d509128eaa1ef.jpeg)
2. Kruskal算法
此算法可以称为“加边法”,初始最小生成树边数为0,每迭代一次就选择一条满足条件的最小代价边,加入到最小生成树的边集合里。
- (1)把图中的所有边按代价从小到大排序;
- (2)把图中的n个顶点看成独立的n棵树组成的森林;
- (3)按权值从小到大选择边,所选的边连接的两个顶点ui,viui,vi,应属于两颗不同的树,则成为最小生成树的一条边,并将这两颗树合并作为一颗树。
- (4)重复(3),直到所有顶点都在一颗树内或者有n-1条边为止。
![2c78e0d9e3d61daf077edbb6731f10ab.png](https://i-blog.csdnimg.cn/blog_migrate/a50115ae341dd3d92b248eb37eec47b4.jpeg)
三、点云处理
- 点云地面去除
地面分割在点云处理中方法较多,最常用的就是ransac平面方程拟合的方法, 以后再详细写。
2.障碍物点云获取
对于去除地面的点云,不论是静止的花草树木,还是运动的车辆行人,直观上都是可以分割出来的,我们均称之为障碍物。
假设1: 障碍物在高度方向上是不重叠的,即一个障碍物不会骑在另一个头上, 或者及时多个在高度上重叠,检测出最小面的一个,也是有效检测。
3. 网格及图的建立
基于以上假设,将3D点云图可以在Z轴方向压缩,也就是拍扁形成鸟瞰图(BEV, BirdEyeView)。为了适配图论算法的使用,需要将点云鸟瞰图建立成网格,也就是节点,并给出节点之间的权值。这里需要三个参数:
- width:形成网格的宽度
- height:形成网格的高度
- range:物理世界的世界范围,如(-120m, 120m)
这三个参数直接反映了点云鸟瞰图网格化的分辨率,后续的图论的分割算法便在此网格上展开。
网格在生成过程中,对于每一个网格,都有一个点云的点的索引作为网格的值, 没有点云覆盖映射的网格,其值为零。 这样网格值大于零的格子就作为一个图的要素之一——节点。
对于一个图,如何获取节点之间的边的信息?
这里定义查找半径radius:仅记录网格距离横纵轴方向均小于此半径的节点之间的欧式距离(单位:格, d = sqrt(dx*dx + dy*dy)),将每一个有效的查找作为一条边,边的权为欧式距离,并将每个节点的边都存储到图中, 图的要素之二——边也就准备完成了。
4. 连通网的构成与存储
![ff99d82477efd8a07361690e2d291ec8.png](https://i-blog.csdnimg.cn/blog_migrate/b0cbd8ca6b29489452ace6e33abde9db.jpeg)
如上图所示,外侧边框内的网格即为图的基本网格。在查找边的时候,遍历橙色区域的网格,向前后和下方的搜索半径区域内(黄色网格即为每个节点(黑色网格)查找时会搜索到的网格)查找,并把这些所有的结果存储,组成一个并查集。如此一来, 连通网就已经构成了。
5. 最小生成树的获取
在已经准备好的连通网中,应用文章最开始提到的最小生成树获取算法,将整个连通网分割成多个最小生成树。
对于每一个最小生成树, 里面包含的所有的节点的点云索引,就属于同一个障碍物,一般称之为cluster, 这个过程也叫聚类。
并查集类源代码实现:
头文件:
class Universe {
struct Element {
int rank = 0;
int p = 0;
int size = 1;
};
public:
Universe() : elts_(), sets_num_(0) {}
~Universe() {
elts_.clear();
}
explicit Universe(const int elements_num);
// @brief: reset each element
void Reset(const int elements_num);
// @brief: find and return input element's parent
int Find(const int x);
// @brief: join the two elements into one set
void Join(const int x, const int y);
// @brief: get size of input element
int GetSize(const int x) const {
return elts_[x].size;
}
// @brief: get sets number
int GetSetsNum() const {
return sets_num_;
}
private:
std::vector<Element> elts_;
int sets_num_;
};
源文件:
#include "disjoint_set.h"
Universe::Universe(const int elements_num) : elts_(elements_num),
sets_num_(elements_num) {
for (int i = 0; i < elements_num; ++i) {
elts_[i].rank = 0;
elts_[i].size = 1;
elts_[i].p = i;
}
}
void Universe::Reset(const int elements_num) {
sets_num_ = elements_num;
elts_.resize(elements_num);
for (int i = 0; i < elements_num; ++i) {
elts_[i].rank = 0;
elts_[i].size = 1;
elts_[i].p = i;
}
}
int Universe::Find(const int x) {
int y = x;
while (y != elts_[y].p) {
y = elts_[y].p;
}
int w = x;
while (true) {
const int z = elts_[w].p;
if (z == w) {
break;
}
elts_[w].p = y;
w = z;
}
return y;
}
void Universe::Join(const int x, const int y) {
if (elts_[x].rank > elts_[y].rank) {
elts_[y].p = x;
elts_[x].size += elts_[y].size;
} else {
elts_[x].p = y;
elts_[y].size += elts_[x].size;
if (elts_[x].rank == elts_[y].rank) {
++elts_[y].rank;
}
}
--sets_num_;
}
部分引用: 最小生成树(Kruskal和Prim算法)