BFS深度理解与代码实现（Breadth First Search）

我搞slam

已于 2023-08-30 16:04:00 修改

阅读量213

点赞数

分类专栏：路径规划 & slam 文章标签： c++ 算法 opencv 自动驾驶

于 2023-08-30 15:54:23 首次发布

本文链接：https://blog.csdn.net/qq_37705385/article/details/132570275

版权

路径规划 & slam 专栏收录该内容

10 篇文章 2 订阅

订阅专栏

1、前言

BFS，即广度优先搜索，从字面意思理解就是广度撒网，不会有针对性，BFS是使用队列（queue，先进先出）来实现，它有一个孪生兄弟——DFS(深度优先搜索)，会向着某个方向一干到底，使用栈(Stack，先进后出)实现的。通过树状结构可以更方便我们理解，如下图所示：

其实大多数场景，我们用 DFS 的时候远远多于 BFS。如果我们使用 DFS/BFS 只是为了遍历一棵树、一张图上的所有结点的话， DFS 和 BFS 的能力没什么差别，我们当然更倾向于更方便写、空间复杂度更低的 DFS 遍历，不过，有些场景是 DFS 做不到的，只能使用 BFS 遍历，比如「层序遍历」、「最短路径」。
本文的主要目是同时计算多个目标点的最短路径。
层次遍历大家可以自行搜索（简贴一张图，如下图所示）。
层次遍历示意图

2、基本思想

BFS广度优先搜索是使用队列（queue，先进先出）来实现，整个过程也可以看做一个倒立的树形：
1、把根节点放到队列的末尾。
2、每次从队列的头部取出一个元素，查看这个元素所有的下一级元素，把它们放到队列的末尾(不需要排序，因为默认每次增加权重相同)。并把这个元素记为它下一级元素的前驱。
3、找到所要找的元素时结束程序。
4、如果遍历整个树还没有找到，结束程序。

有人会问，这与Dijkstra 算法好像没有区别啊？都可以解决最短路径问题啊？ 的确是这样，BFS可以看成是无权重的最短路径问题，所有遍历的节点不需要进行人工排序，插入普通队列队尾即排序成功；而Dijkstra 每次遍历的下一个节点带有权重（斜向移动和平向移动代价不一样，离障碍物距离不一样代价不一样），所以Dijkstra 一般使用优先队列来进行自动排序，从而保证每次取出的是最优节点。

知识扩展
1、DFS 可以理解为随机方向的一搜到底。
2、贪心算法（Greedy）则是每次搜索只考虑扩展节点到目标点的代价值最小（其实是有目标朝向的DFS），搜索到的路径不一定是全局最优。
3、BFS 可以理解为均匀的广度搜索。
4、Dijkstra 则是每次搜索只考虑扩展节点到起点的最小代价（其实是有权重的BFS），搜索到的路径一定是全局最优。
5、Astar 则可以理解为 Dijkstra + Greedy ，扩展搜索时既具有方向性（Greedy，加速了搜索效率），又有次优性（Dijkstra保证了路径的优越性）。

3、核心代码

下面代码实现了，输入一张CV 二值地图，给定一个起点、若干个终点，输出若干组路径或者若干组代价值（可以用于代价排序）。具体流程是遍历整张地图（不是到达目标点就退出，如果是单目标点可以自行魔改），再反向回溯得到路径或者直接获取终点代价。
下面代码也可以通过魔改得到Dijkstra算法（open_list_ 使用优先队列）、Astar（增加启发式代价H），有兴趣可以尝试。

// bfs.hpp
#include <iostream>
#include <memory>
#include <opencv2/opencv.hpp>
#include <queue>
#include <unordered_map>

namespace bfs_search {

enum NodeType { OBS = 0, FREE, OPEN, CLOSE };

struct Node {
    cv::Point point;             
    int F;                        
    std::shared_ptr<Node> parent; 
    Node(cv::Point _point = cv::Point(0, 0)) : point(_point), F(0), parent(nullptr) {}
};

class BFS {
public:
    // 传入二值地图
    void setMap(const cv::Mat &map);
    // 获取起点至各目标点的代价
    void getPathCost(const cv::Point &start, const std::vector<cv::Point> &ends, std ::vector<int> paths_cost);
    // 获取起点至各目标点的路径
    void findPath(const cv::Point &start, const std::vector<cv::Point> &ends, std::vector<std::vector<cv::Point>> &paths);

private:
    inline int point2index(cv::Point point) { return point.y * map_.cols + point.x; }
    inline cv::Point index2point(int index) { return cv::Point(int(index / map_.cols), index % map_.cols); }
    void expand();
    void getPath(std::shared_ptr<Node> end_node, std::vector<cv::Point> &path);

private:
    cv::Point start_point_;                // 起点
    std::vector<cv::Point> target_points_; // 目标点s
    std::vector<cv::Point> neighbor_;      // 搜索邻域
    cv::Mat map_;                          // 存放地图
    cv::Mat labelmap_;                     // 用于存放obs（障碍物）和free（空白）
    int width_;
    int height_;

    std::queue<std::pair<int, cv::Point>> open_list_;
    std::unordered_map<int, std::shared_ptr<Node>> expand_list_; 
};
}; // namespace bfs_search

// bfs.cpp
#include "bfs.hpp"
using namespace std;

namespace bfs_search {

void BFS::setMap(const cv::Mat &map) {
    map_ = map.clone();
    width_ = map_.cols;
    height_ = map_.rows;
    neighbor_.push_back(cv::Point(-1, -1));
    neighbor_.push_back(cv::Point(-1, 0));
    neighbor_.push_back(cv::Point(-1, 1));
    neighbor_.push_back(cv::Point(0, -1));
    neighbor_.push_back(cv::Point(0, 1));
    neighbor_.push_back(cv::Point(1, -1));
    neighbor_.push_back(cv::Point(1, 0));
    neighbor_.push_back(cv::Point(1, 1));

    cv::Mat temp_map = map_.clone();
    if (temp_map.channels() == 3) {
        cvtColor(temp_map, temp_map, cv::COLOR_BGR2GRAY);
    }

    labelmap_ = cv::Mat::zeros(height_, width_, CV_8UC1);
    for (int y = 0; y < height_; y++) {
        for (int x = 0; x < width_; x++) {
            if (temp_map.at<uchar>(y, x) == 0) {
                labelmap_.at<uchar>(y, x) = OBS;
            } else {
                labelmap_.at<uchar>(y, x) = FREE;
            }
        }
    }
}

void BFS::getPathCost(const cv::Point &start, const std::vector<cv::Point> &ends, std ::vector<int> paths_cost) {
    start_point_ = start;
    target_points_ = ends;
    expand();
    // 计算每个目标点代价
    for (const auto &target : target_points_) {
        int index = point2index(target);
        // 遍历过的点
        int path_cost = INT_MAX;
        if (expand_list_.find(index) != expand_list_.end()) {
            auto CurNode = expand_list_[index];
            path_cost = CurNode->F;
            cout << "target:" << target << "  g:" << path_cost / 10.0 << endl;
        } else {
            cout << "target:" << target << "  g:" << path_cost << endl;
        }
        paths_cost.emplace_back(path_cost);
    }
    cout << "get path cost success !!!" << endl;
}

void BFS::findPath(const cv::Point &start, const std::vector<cv::Point> &ends, std::vector<std::vector<cv::Point>> &paths) {
    start_point_ = start;
    target_points_ = ends;
    // Path expand
    expand();

    // 保存所有搜索的图片
    {
        cv::Mat search_map(map_.size(), 0, cv::Scalar(0));
        for (auto od = expand_list_.begin(); od != expand_list_.end(); od++) {
            (od->second)->point;
            search_map.at<uchar>((od->second)->point) = ((od->second)->F) / 25;
        }
        cv::imwrite("../data/search_map.png", search_map);
    }

    for (const auto &target : target_points_) {
        int index = point2index(target);
        std::vector<cv::Point> solution;
        if (expand_list_.find(index) != expand_list_.end()) {
            auto CurNode = expand_list_[index];
            getPath(CurNode, solution);
        } else {
            solution.emplace_back(start_point_);
            solution.emplace_back(target);
        }
        paths.emplace_back(solution);
    }
    cout << "find Path success !!!" << endl;
}

void BFS::expand() {
    cv::Mat _LabelMap = labelmap_.clone();
    std::shared_ptr<Node> startPointNode = std::make_shared<Node>((start_point_));
    open_list_.push(std::pair<int, cv::Point>(startPointNode->F, startPointNode->point));
    int index = point2index(startPointNode->point);
    expand_list_[index] = startPointNode;
    _LabelMap.at<uchar>(start_point_.y, start_point_.x) = OPEN;
    int k = 0;
    int search_index = 0;

    while (!open_list_.empty()) {
        search_index++;
        cv::Point CurPoint = open_list_.front().second;
        open_list_.pop();
        int index = point2index(CurPoint);
        auto CurNode = expand_list_[index];

        int curX = CurPoint.x;
        int curY = CurPoint.y;
        _LabelMap.at<uchar>(curY, curX) = CLOSE;

        for (int k = 0; k < neighbor_.size(); k++) {
            int y = curY + neighbor_[k].y;
            int x = curX + neighbor_[k].x;
            if (x < 0 || x >= width_ || y < 0 || y >= height_) {
                continue;
            }
            // 如果当前点是 空白点 或者 在openlist中
            if (_LabelMap.at<uchar>(y, x) == FREE || _LabelMap.at<uchar>(y, x) == OPEN) {
                // Determine whether a diagonal line can pass 判断是直线还是斜线
                int dist1 = abs(neighbor_[k].y) + abs(neighbor_[k].x);
                //如果是斜线 且 斜线穿过障碍物 ，继续回到for循环
                if (dist1 == 2 && _LabelMap.at<uchar>(y, curX) == OBS && _LabelMap.at<uchar>(curY, x) == OBS) continue;

                int addF, F;
                if (dist1 == 2) {
                    addF = 14;
                } else {
                    addF = 10;
                }
                F = CurNode->F + addF;

                if (_LabelMap.at<uchar>(y, x) == FREE) {
                    std::shared_ptr<Node> node = std::make_shared<Node>();
                    node->point = cv::Point(x, y);
                    node->parent = CurNode;

                    node->F = F;
                    open_list_.push(std::pair<int, cv::Point>(node->F, node->point));
                    int index = point2index(node->point);
                    expand_list_[index] = node;
                    _LabelMap.at<uchar>(y, x) = OPEN;
                } else {
                    // Find the node
                    int index = point2index(cv::Point(x, y));
                    auto node = expand_list_[index];
                    if (F < node->F) {
                        node->F = F;
                        node->parent = CurNode;
                    }
                }
            }
        }
    }
    cout << "search_index:" << search_index << endl;
}

void BFS::getPath(std::shared_ptr<Node> end_node, std::vector<cv::Point> &path) {
    std::vector<std::shared_ptr<Node>> path_list;
    path.clear();

    auto CurNode = end_node;
    while (CurNode != NULL) {
        path_list.push_back(CurNode);
        CurNode = CurNode->parent;
    }

    int length = path_list.size();
    for (int i = 0; i < length; i++) {
        path.push_back(path_list.back()->point);
        path_list.pop_back();
    }
}
} // namespace bfs_search