优化Dijkstra算法

Question

我需要一个在我们的机器人导航应用中足够的图搜索算法，因此我选择了Dijkstra的算法。

给出了包含自由，占用和未知单元格的网格图，其中仅允许机器人通过自由单元格。 用户将输入起始位置和目标位置。 作为回报，我将检索引导机器人从起始位置到达与路径相对应的目标位置的空闲单元序列。

由于从头到尾执行dijkstra的算法会给我们从目标到起点的反向路径，因此我决定向后执行dijkstra的算法，以便检索从头到目标的路径。

从目标单元格开始，我将有8个邻居，它们的水平和垂直方向的成本为1，而对角线只有在这些单元格可以到达的情况下才为sqrt（2）（即，无界和自由单元格）。

这是更新相邻小区时应遵守的规则，当前小区只能在以下条件下假设8个相邻小区可达（例如，距离1或sqrt(2) ）：

1. 相邻单元格不超出范围
2. 不访问相邻小区。
3. 相邻单元是一个空闲单元，可以通过2-D网格图进行检查。

这是我的实现：

#include <opencv2/opencv.hpp>
#include <algorithm>
#include "Timer.h"

/// CONSTANTS
static const int UNKNOWN_CELL  = 197;
static const int FREE_CELL     = 255;
static const int OCCUPIED_CELL = 0;

/// STRUCTURES for easier management.
struct vertex {
    cv::Point2i id_;
    cv::Point2i from_;

    vertex(cv::Point2i id, cv::Point2i from)
    {
        id_ = id;
        from_ = from;
    }
};

/// To be used for finding an element in std::multimap STL.
struct CompareID
{
    CompareID(cv::Point2i val) : val_(val) {}
    bool operator()(const std::pair<double, vertex> & elem) const {
        return val_ == elem.second.id_;
    }
private:
    cv::Point2i val_;
};

/// Some helper functions for dijkstra's algorithm.
uint8_t get_cell_at(const cv::Mat & image, int x, int y)
{
    assert(x < image.rows);
    assert(y < image.cols);
    return image.data[x * image.cols + y];
}

/// Some helper functions for dijkstra's algorithm.
bool checkIfNotOutOfBounds(cv::Point2i current, int rows, int cols)
{
    return (current.x >= 0 && current.y >= 0 &&
            current.x < cols && current.y < rows);
}

/// Brief: Finds the shortest possible path from starting position to the goal position
/// Param gridMap: The stage where the tracing of the shortest possible path will be performed.
/// Param start: The starting position in the gridMap. It is assumed that start cell is a free cell.
/// Param goal: The goal position in the gridMap. It is assumed that the goal cell is a free cell.
/// Param path: Returns the sequence of free cells leading to the goal starting from the starting cell.
bool findPathViaDijkstra(const cv::Mat& gridMap, cv::Point2i start, cv::Point2i goal, std::vector<cv::Point2i>& path)
{
    // Clear the path just in case
    path.clear();
    // Create working and visited set.
    std::multimap<double,vertex> working, visited;

    // Initialize working set. We are going to perform the djikstra's
    // backwards in order to get the actual path without reversing the path.
    working.insert(std::make_pair(0, vertex(goal, goal)));

    // Conditions in continuing
    // 1.) Working is empty implies all nodes are visited.
    // 2.) If the start is still not found in the working visited set.
    // The Dijkstra's algorithm
    while(!working.empty() && std::find_if(visited.begin(), visited.end(), CompareID(start)) == visited.end())
    {

        // Get the top of the STL.
        // It is already given that the top of the multimap has the lowest cost.
        std::pair<double, vertex> currentPair = *working.begin();
        cv::Point2i current = currentPair.second.id_;
        visited.insert(currentPair);
        working.erase(working.begin());

        // Check all arcs
        // Only insert the cells into working under these 3 conditions:
        // 1. The cell is not in visited cell
        // 2. The cell is not out of bounds
        // 3. The cell is free
        for (int x = current.x-1; x <= current.x+1; x++)
            for (int y = current.y-1; y <= current.y+1; y++)
            {

                if (checkIfNotOutOfBounds(cv::Point2i(x, y), gridMap.rows, gridMap.cols) &&
                        get_cell_at(gridMap, x, y) == FREE_CELL &&
                        std::find_if(visited.begin(), visited.end(), CompareID(cv::Point2i(x, y))) == visited.end())
                {
                    vertex newVertex = vertex(cv::Point2i(x,y), current);
                    double cost = currentPair.first + sqrt(2);
                    // Cost is 1
                    if (x == current.x || y == current.y)
                        cost = currentPair.first + 1;
                    std::multimap<double, vertex>::iterator it =
                            std::find_if(working.begin(), working.end(), CompareID(cv::Point2i(x, y)));
                    if (it == working.end())
                        working.insert(std::make_pair(cost, newVertex));
                    else if(cost < (*it).first)
                    {
                        working.erase(it);
                        working.insert(std::make_pair(cost, newVertex));
                    }
                }
            }
    }

    // Now, recover the path.
    // Path is valid!
    if (std::find_if(visited.begin(), visited.end(), CompareID(start)) != visited.end())
    {
        std::pair <double, vertex> currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(start));
        path.push_back(currentPair.second.id_);
        do
        {
            currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(currentPair.second.from_));
            path.push_back(currentPair.second.id_);
        } while(currentPair.second.id_.x != goal.x || currentPair.second.id_.y != goal.y);
        return true;
    }
    // Path is invalid!
    else
        return false;

}

int main()
{
    //    cv::Mat image = cv::imread("filteredmap1.jpg", CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat image = cv::Mat(100,100,CV_8UC1);
    std::vector<cv::Point2i> path;

    for (int i = 0; i < image.rows; i++)
        for(int j = 0; j < image.cols; j++)
        {
            image.data[i*image.cols+j] = FREE_CELL;

            if (j == image.cols/2 && (i > 3 && i < image.rows - 3))
                image.data[i*image.cols+j] = OCCUPIED_CELL;

            //            if (image.data[i*image.cols+j] > 215)
            //                image.data[i*image.cols+j] = FREE_CELL;
            //            else if(image.data[i*image.cols+j] < 100)
            //                image.data[i*image.cols+j] = OCCUPIED_CELL;
            //            else
            //                image.data[i*image.cols+j] = UNKNOWN_CELL;
        }

    // Start top right
    cv::Point2i goal(image.cols-1, 0);
    // Goal bottom left
    cv::Point2i start(0, image.rows-1);

    // Time the algorithm.
    Timer timer;
    timer.start();
    findPathViaDijkstra(image, start, goal, path);
    std::cerr << "Time elapsed: " << timer.getElapsedTimeInMilliSec() << " ms";


    // Add the path in the image for visualization purpose.
    cv::cvtColor(image, image, CV_GRAY2BGRA);
    int cn = image.channels();
    for (int i = 0; i < path.size(); i++)
    {
        image.data[path[i].x*cn*image.cols+path[i].y*cn+0] = 0;
               image.data[path[i].x*cn*image.cols+path[i].y*cn+1] = 255;
               image.data[path[i].x*cn*image.cols+path[i].y*cn+2] = 0;

    }
    cv::imshow("Map with path", image);
    cv::waitKey();
    return 0;
}

对于算法实现，我决定有两个集合，即访问集和工作集，每个元素包含：

1. 自身在2D网格图中的位置。
2. 累计费用
3. 它通过哪个单元获取了累积成本（用于路径恢复）

结果如下：

黑色像素代表障碍物，白色像素代表自由空间，绿色线代表计算的路径。

在这种实现方式下，我只需要在当前工作集中搜索最小值，而无需扫描整个成本矩阵（最初，所有像元的初始成本都设置为无穷大，起点为0）。 我认为维护工作集的单独向量可以保证更好的代码性能，因为所有具有无穷大代价的像元肯定不会包括在工作集中，而只会包括那些被触摸过的像元。

我还利用了C ++提供的STL。 我决定使用std :: multimap，因为它可以存储重复键（这是成本），并且可以自动对列表进行排序。 但是，我被迫使用std :: find_if（）在被访问的集合中找到ID（即集合中当前单元格的行，列），以检查当前单元格是否在其上，从而保证了线性复杂度。 我真的认为这是Dijkstra算法的瓶颈。

我很清楚A *算法比Dijkstra算法快得多，但是我想问的是，我对Dijkstra算法的实现是最优的吗？ 即使我使用Dijkstra的当前实现方式实现了A *算法（我认为它不是最优的），因此A *算法也将不是最优的。

我可以做些什么改善？ 哪种STL最适合此算法？ 特别是如何改善瓶颈？

Answer 1

您正在使用std::multimap进行“工作”和“访问”。 那不是很好

您应该做的第一件事是将visited更改为每个顶点的标记，这样您就可以在恒定时间内而不是线性时间执行find_if ，并且也可以使访问顶点列表上的运算采用恒定时间而不是对数时间。 您知道所有顶点都是什么，可以将它们平凡地映射为小整数，因此可以使用std::vector或std::bitset 。

您应该做的第二件事是将working变成优先级队列，而不是平衡的二叉树结构，这样操作就可以更快地成为（较小的）恒定因子。 std::priority_queue是准系统二进制堆。 高基数的堆（比如说具体的四进制）由于其深度减小而在现代计算机上可能会更快。 安德鲁·戈德堡（Andrew Goldberg）提出了一些基于存储桶的数据结构。 如果您到那个阶段，我可以为您提供参考。 （它们不太复杂。）

处理完这两件事后，您可能会考虑使用A *或中间相遇的技巧来加快处理速度。

Answer 2

您的性能比可能要好几个数量级，这是因为您正在使用图形搜索算法查找看起来像几何的图形。 与图形搜索算法可以解决的问题相比，这种几何结构要简单得多且通用性较低。 此外，即使每个像素基本不包含任何信息，它的每个像素都有一个顶点。

我听说您问过“如何在不改变我所想的情况下使它变得更好”，但是我会告诉您一种完全不同且更好的方法。

看来您的机器人只能水平，垂直或对角线运动。 这是选择图搜索算法的真实结果还是只是副作用？ 我将假设后者，并使其朝任何方向发展。

该算法如下所示：（0）通过列出拐角将障碍物表示为多边形。 以实数工作，因此您可以使它们尽可能薄。 （1）尝试在端点之间形成一条直线。 （2）检查那条线是否穿过障碍物。 要对任何一条线执行此操作，请显示任何特定障碍物的所有角都位于该线的同一侧。 为此，用线的一端的（-X，-Y）平移所有点，使该点位于原点，然后旋转直到另一点在X轴上。 现在，如果没有障碍物，则所有角都应具有相同的Y符号。 仅使用渐变可能会有更快的方法。 （3）如果有障碍物，则建议通过障碍物的N个拐角走N条两条路。 （4）对所有线段进行递归，以线段超出范围的方式选择任何路径。 除非您遇到障碍，否则这将不是问题。 （5）当它停止递归时，您应该有一个本地优化路径列表，您可以从中选择最短路径。 （6）如果您确实希望将方位角限制为45度的倍数，则可以先执行此算法，然后将每个线段替换为任何仅45个摆动式的避免障碍的版本。 我们知道存在这样的版本是因为您可以通过频繁地摆动来保持与原始行非常接近。 我们也知道所有这些摆动路径都具有相同的长度。