关于分区排序的算法思考及实现

一、引言

   对于很多机器人产品，都需要去某个区域进行工作（不限于扫地机的清扫任务、无人机的覆盖式搜索、割草机的区域割草等等），对于一次完整的任务而言，首先会将工作空间切割成若干个小区域，然后一个个小区域的完成，从而实现一次完整的任务。
    那么问题来了，如何进行小区域的排序是评判这次完整任优略的准则之一，好的排序可以提高效率、提升用户体验等等。

---

二、算法设计思路

1、用数据结构表征分区

假如有以下分区图，你如何用一种结构去描述呢？

我刚开始想到的就是以某个重要的区域作为根节点构建一颗多叉树，假如以1号区域作为根节点，则这棵多叉树可以表征为：

但大家可能一眼就看出来了，多叉树有一个很明显的问题，它是有向的，即parent节点指向child节点，而且多叉树的特性是一个parent可以有多个child，但是一个child只能有一个parent，所以它不能完整的表征分区关系（比如2-3是相连的，但多叉树无法进行表征）；
进一步的思考，图结构是不是最合适呢？分区图本身就是一种图，图结构就再合适不过了，如下图所示：

2、构建分区之间的图

定义一个简单的图结构体GNode

typedef struct GNode {
    int element;   // 当前元素
    set<GNode *> neighbors;  // 所有的邻居
} GNode;

建立连接关系

// 建立连接关系
void setNeighbor(GNode *node, GNode *neighbor) {
    node->neighbors.insert(neighbor);
    neighbor->neighbors.insert(node);
}

思路是不是很清奇，构建所有分区的GNode指针，然后setNeighbor，分区之间的图结构就结束了。
大家可能也发现了，其中的关键是怎么知道哪两个GNode是相邻的呢？请看下文。

3、计算分区之间的相邻关系

我们以一副实际地图为例：一个大区域被四根分割线分成5个小区域



为了输出两两相邻关系，我刚开的思路是这样的，先给区域编号，然后计算每个分割线的小垂线所经过的区域，准备实现时，发现这种方法貌似不太优雅，于是咨询了朋友，有了个更好的方案，如下：

第一步：分割线解耦。如下图所示，原始分割线的条数为4条，根据其交点关系变成互不耦合的6条。
思路：先判断哪些点是共线的，再将共线的点按照x值（或者y值）的从大到小（从小到大）排序，最后将共线组按照顺序前后组合即可得到6条分割线


第二步：求小区域轮廓。求取每个小区域的内轮廓。
思路：cv::findContours(map, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);可以实现


第三步：轮廓匹配。根据处理后分割线的中心点到每个轮廓的距离判断轮廓关系。即根据距离阈值进行两两配对。如下图所示1、4两区域分割线的中心点到两个区域轮廓的距离一定是最近的。
思路：cv::pointPolygonTest(contour, center_point, true)暴力求解每个点到轮廓的距离，根据距离阈值进行两两配对。


输出相邻关系：
neighbor: 0-1 、0-2 、1-0
neighbor: 1-2 、1-4
neighbor: 2-0 、2-4 、2-1 、2-3
neighbor: 3-2
neighbor: 4-2 、4-1

4、根据图结构输出分区顺序

输出分区顺序是需要根据需求来的，有了分区之间的图结构后，可玩性就很高了。我举例以其中一种bfs的方式进行输出

// BFS
void tranversalBfs(GNode *root) {
    deque<GNode *> node_deque;
    set<GNode *> node_set;
    // 先找到根结点的所有子结点
    for (const auto &p : root->neighbors) {
        node_deque.emplace_back(p);
        node_set.insert(p);
    }
    // 输出的结果 vector结构
    vec_results_.emplace_back(root_.element);
    // 为了去重  set结构
    set_results_.insert(root_.element);

    while (!node_deque.empty()) {
        for (const auto &p : node_deque) {
            cout << p->element << " - ";
        }
        cout << endl;

        // 取出最前面一个元素
        GNode *node = node_deque.front();
        node_deque.pop_front();
        vec_results_.emplace_back(node->element);
        set_results_.insert(node->element);

        // 找这个元素的下一个层级所有元素
        vector<GNode *> temp_vec;
        for (const auto &p : node->neighbors) {
            if (set_results_.count(p->element) == 0) {
                temp_vec.emplace_back(p);
            }
        }
        for (const auto &p : temp_vec) {
            // 如果当前元素等于上次最后一个，将其子放到最前面 最后一个赋予新值
            if (update_last_element) {
                node_deque.emplace_front(p);
                node_set.insert(p);
            } else {
                node_deque.emplace_back(p);
                node_set.insert(p);
            }
        }
    }
}