A* 搜索，网格，8 个方向，八分位距离作为启发式，没有找到直接路径

Question

谁能帮我理解我的 A* 搜索实现有什么问题？

我基于这个非常有用的网站实现了一个基本的 A* 搜索：http://www.redblobgames.com/pathfinding/a-star/implementation.html#cplusplus （非常感谢作者 Amit！）。

我使用网格、八个方向和 Octile 距离作为启发式方法。 不幸的是，我从 start(0,h/2) 到 end(w-1,h/2) 的路径不是预期的直线，但看起来像这样：

我的代码（应该可以按原样编译，但需要 OpenCv）：

struct PriorityQueue
{

    typedef pair<int, cv::Point> PQElement;

    struct SortPairPoints
    {
        bool operator()(const PQElement & l, const PQElement & r)
        {
            return (l.first > r.first);
        }
    };


    priority_queue<PQElement, vector<PQElement>, SortPairPoints> elements;

    inline bool empty() { return elements.empty(); }


    inline void put(int priority,cv::Point item)
    {
        elements.emplace(priority, item);
    }

    inline cv::Point get()
    {
        cv::Point best_item = elements.top().second;
        elements.pop();
        return best_item;
    }
};



template <class T>
inline void hash_combine(std::size_t& seed, const T& v)
{
    std::hash<T> hasher;
    seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
}

namespace std
{

    template <>
    struct hash<cv::Point>
    {
        size_t operator()(const cv::Point & p) const
        {
            size_t seed = 0;

            hash_combine(seed,p.x);
            hash_combine(seed,p.y);

            return seed;
        }
    };

}


int heuristic(cv::Point next, cv::Point goal)
{
//    int D = 1;
//    int dx = abs(next.x - goal.x);
//    int dy = abs(next.y - goal.y);
//    return D * (dx + dy);
//    return sqrt(dx * dx + dy * dy);
//    int D = 1;
//    int D2 = 1;
    int D = 1;
    int D2 = sqrt(2);
    int dx = abs(next.x - goal.x);
    int dy = abs(next.y - goal.y);
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy);
}



int w = 250;
int h = 250;
std::vector<cv::Point> pathDirs({cv::Point(1, 0),cv::Point(0, -1),cv::Point(0, 1),cv::Point(-1, 0), cv::Point(1, 1), cv::Point(-1, 1),cv::Point(-1, -1),cv::Point(1, -1)});
//std::vector<cv::Point> pathDirs({cv::Point(1, 0),cv::Point(0, -1),cv::Point(-1, 0),cv::Point(0, 1)});
cv::Rect scenebox(0,0,w,h);


void search(
            cv::Mat map,
            cv::Point start,
            cv::Point goal,
            unordered_map<cv::Point, cv::Point>& came_from,
            unordered_map<cv::Point, int>& cost_so_far
            )
{


    PriorityQueue frontier;
    frontier.put(0,start);

    came_from[start] = start;
    cost_so_far[start] = 0;

    while (!frontier.empty()) {
        auto current = frontier.get();

        if (current == goal) {
            break;
        }

        for (auto dir : pathDirs)
        {
            cv::Point next(current.x + dir.x, current.y + dir.y);
            if (scenebox.contains(next) && (map.at<uchar>(next) == 255))
            {

                int new_cost = cost_so_far[current] + 1;
                if (!cost_so_far.count(next) || new_cost < cost_so_far[next])
                {
                    cost_so_far[next] = new_cost;
                    int priority = new_cost + heuristic(next, goal);
                    frontier.put(priority,next);
                    came_from[next] = current;
                }
            }
        }
    }
}





vector<cv::Point> reconstruct_path(
                                   cv::Point start,
                                   cv::Point goal,
                                   unordered_map<cv::Point, cv::Point>& came_from
                                   )
{
    vector<cv::Point> path;
    cv::Point current = goal;
    path.push_back(current);
    while (current != start) {
        current = came_from[current];
        path.push_back(current);
    }
    std::reverse(path.begin(), path.end());
    return path;
}


int main(int argc, const char * argv[])
{

    cv::Mat tracemap = cv::Mat(w,h, CV_8UC1, cvScalar(255) );
    cv::Point start(0,h/2);
    cv::Point end(w-1,h/2);

//    cv::Point start(0,0);
//    cv::Point end(w-1,h-1);

//    cv::line(tracemap,
//             cv::Point (75,125),
//             cv::Point (125,75),
//             cvScalar(150),50);

    unordered_map<cv::Point, cv::Point> came_from;
    unordered_map<cv::Point, int> cost_so_far;

    search(tracemap, start, end, came_from, cost_so_far);
    vector<cv::Point> path = reconstruct_path(start, end, came_from);
    for(int i = 0; i < path.size(); i++)
    {
        tracemap.at<uchar>(path[i]) = 0;
    }

    imshow("tracemap", tracemap);


    cv::waitKey();
    return 0;
}

非常感谢任何有关如何找到问题根源的见解或提示！

更新：根据 Amit 的建议，我现在得到以下路径：

FOLLOW-UP（高度相关，这就是我在这里添加它并且不打开新帖子的原因）：

如果我只使用曼哈顿距离作为启发式的四个方向，并且所有四个步骤的移动成本为 1，我会得到一个抖动的对角线。当然，算法必须像这样走“楼梯”，但我仍然希望得到更直接的东西——我是否遗漏了一些明显的东西？

Answer 1

对角线的移动成本与正交步骤相同。

向东南、东南、东北、东北的路径与向东、向东、向东、向东的路径一样短。两者的成本都是 4。

当有多个最短路径时，A* 会给出其中之一，但它不是您想要的。

如果您将对角线设置为具有更高的移动成本（sqrt(2) 是您的启发式状态），那么 A* 会更喜欢东、东、东、东。改变

int new_cost = cost_so_far[current] + 1;

使用 1 或 sqrt(2) 取决于它是正交步还是对角步。您还需要将成本转换为浮点数/双精度数而不是整数，并使优先级队列也这样做。 （或者，如果你想继续使用整数，有些人会使用 14 和 10 作为成本，并将启发式扩展到 D2 和 D 使用 14 和 10。）