ds图—最小生成树_MST （minimum spanning tree）最小生成树算法在三维点云的分割的应用...

一、概念准备

MST最小生成树算法是一种图论的算法。

• 连通图：无向图中，任意两个顶点都有路径相通。
• 强连通图：有向图中，任意两个顶点都有路径相通。
• 连通网：在连通图中，若图的边有权值；权代表着连接连个顶点的代价，称这种连通图叫做连通网。
• 生成树：一个连通图的生成树是指一个连通子图，它含有图中全部n个顶点，但只有足以构成一棵树的n-1条边。一颗有n个顶点的生成树有且仅有n-1条边，如果生成树中再添加一条边，则必定成环。
• 最小生成树：在连通网的所有生成树中，所有边的代价和最小的生成树，称为最小生成树。
• 并查集：并查集是一种树型的数据结构，用于处理一些不相交集合（disjoint sets）的合并及查询问题。常常在使用中以森林来表示。在一些有N个元素的集合应用问题中，我们通常是在开始时让每个元素构成一个单元素的集合，然后按一定顺序将属于同一组的元素所在的集合合并，其间要反复查找一个元素在哪个集合中。
• 并查集的功能：

1. 查找：查找元素所在的集合，即根节点。
2. 合并：将两个元素所在的集合合并为一个集合。合并之前，应先判断两个元素是否属于同一集 合，这可用上面的“查找”操作实现。

二、最小生成树获取算法

1. 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中最小代价边信息；

2. Kruskal算法

此算法可以称为“加边法”，初始最小生成树边数为0，每迭代一次就选择一条满足条件的最小代价边，加入到最小生成树的边集合里。

• (1)把图中的所有边按代价从小到大排序；
• (2)把图中的n个顶点看成独立的n棵树组成的森林；
• (3)按权值从小到大选择边，所选的边连接的两个顶点ui,viui,vi,应属于两颗不同的树，则成为最小生成树的一条边，并将这两颗树合并作为一颗树。
• (4)重复(3),直到所有顶点都在一颗树内或者有n-1条边为止。

三、点云处理

1. 点云地面去除

地面分割在点云处理中方法较多，最常用的就是ransac平面方程拟合的方法， 以后再详细写。

2.障碍物点云获取

对于去除地面的点云，不论是静止的花草树木，还是运动的车辆行人，直观上都是可以分割出来的，我们均称之为障碍物。

假设1： 障碍物在高度方向上是不重叠的，即一个障碍物不会骑在另一个头上， 或者及时多个在高度上重叠，检测出最小面的一个，也是有效检测。

3. 网格及图的建立

基于以上假设，将3D点云图可以在Z轴方向压缩，也就是拍扁形成鸟瞰图（BEV, BirdEyeView）。为了适配图论算法的使用，需要将点云鸟瞰图建立成网格，也就是节点，并给出节点之间的权值。这里需要三个参数：

• width：形成网格的宽度
• height：形成网格的高度
• range：物理世界的世界范围，如(-120m， 120m)

这三个参数直接反映了点云鸟瞰图网格化的分辨率，后续的图论的分割算法便在此网格上展开。

网格在生成过程中，对于每一个网格，都有一个点云的点的索引作为网格的值， 没有点云覆盖映射的网格，其值为零。 这样网格值大于零的格子就作为一个图的要素之一——节点。

对于一个图，如何获取节点之间的边的信息？

这里定义查找半径radius：仅记录网格距离横纵轴方向均小于此半径的节点之间的欧式距离（单位：格， d = sqrt(dx*dx + dy*dy)），将每一个有效的查找作为一条边，边的权为欧式距离，并将每个节点的边都存储到图中， 图的要素之二——边也就准备完成了。

4. 连通网的构成与存储

图1 连通图遍历查找存储边的信息

如上图所示，外侧边框内的网格即为图的基本网格。在查找边的时候，遍历橙色区域的网格，向前后和下方的搜索半径区域内（黄色网格即为每个节点（黑色网格）查找时会搜索到的网格）查找，并把这些所有的结果存储，组成一个并查集。如此一来， 连通网就已经构成了。

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 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算法）