Question

我需要一个足够用于机器人导航的图搜索算法，我选择了Dijkstra的算法。

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

由于从开始到目标执行dijkstra算法会给我们一个从目标开始的反向路径，我决定向后执行dijkstra算法，这样我就可以检索从开始到开始的路径目标

从目标单元格开始，我将有8个邻居，其水平和垂直成本为1，而对角线只有在单元格可达时才是sqrt（2）（即不是越界和自由单元格）。

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

相邻单元格不在界外
未访问相邻单元格。
相邻单元格是一个可以通过二维网格图检查的自由单元格。

这是我的实施：

#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;
}

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

自身在2D网格地图中的位置。
累计费用
通过什么单元格获得累积成本（路径恢复）

结果如下：

enter image description here

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

在这个实现中，我只会在当前工作集中搜索最小值，并且不需要在整个成本矩阵中进行扫描（最初，所有单元的初始成本都设置为无穷大，起点为0）。保持工作集的单独向量我认为可以提供更好的代码性能，因为所有具有无穷大成本的单元肯定不会包含在工作集中，而只包含那些已被触摸的单元。

我还利用了C ++提供的STL。我决定使用std :: multimap，因为它可以存储重复键（这是成本），并自动对列表进行排序。但是，我被迫使用std :: find_if（）来查找访问集中的id（它是集合中当前单元格的行，col），以检查当前单元格是否在其上，从而承诺线性复杂性。我真的认为这是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

您的性能比实际情况差几个数量级，因为您使用图搜索算法来查看几何图形。与图搜索算法可以解决的问题相比，这种几何更简单，更不通用。此外，对于每个像素的顶点，即使它基本上不包含任何信息，您的图形也是巨大的。

我听说你要问＆＃34;如果不改变我的思维方式，我怎样才能做得更好？＆＃34;但是，我会告诉你一个完全不同的，更好的方法。

看起来您的机器人只能水平，垂直或对角线。这是真的还是仅仅是您选择图搜索算法的副作用？我会假设后者，让它向任何方向前进。

算法如下：（0）通过列出角来将障碍物表示为多边形。以实数工作，这样你就可以根据自己的喜好制作它们。（1）尝试在终点之间划一条直线。（2）检查该线是否穿过障碍物。要对任何线条执行此操作，请显示任何特定障碍物的所有角落位于线条的同一侧。为此，将所有点转换为该行一端的（-X，-Y），使该点位于原点，然后旋转直到另一个点位于X轴上。如果没有阻碍，现在所有角落应该具有相同的Y符号。使用渐变可能有更快的方法。（3）如果有障碍物，建议N个双段路径通过障碍物的N个角落。（4）递归所有段，剔除任何超出范围的段的路径。除非你有超出界限的障碍，否则这不会成为问题。（5）当它停止递归时，你应该有一个本地优化路径列表，你可以从中选择最短路径。（6）如果你真的想要将轴承限制在45度的倍数，那么你可以先做这个算法，然后用任何只有45个摆动避免障碍的摆动版本替换每个段。我们知道这样的版本存在是因为你可以经常摆动而非常接近原始线。我们也知道所有这些摇摆不定的路径都有相同的长度。

优化Dijkstra的算法

2 个答案: