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

时间:2015-09-17 04:14:42

标签: c++ opencv path-finding

有人能帮我理解我的A *搜索实施有什么问题吗?

我基于这个令人难以置信的有用网站实施了基本的A *搜索:http://www.redblobgames.com/pathfinding/a-star/implementation.html#cplusplus (非常感谢作者,阿米特!)。

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

enter image description here

我的代码(应该按原样编译,但需要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的建议,我现在得到以下路径: enter image description here

FOLLOW-UP(高度相关,这就是为什么我在这里添加它并且不打开新帖子):

如果我只使用曼哈顿距离的四个方向作为启发式,并且所有四个步骤的移动成本为1,我会得到一个紧张的对角线。当然算法必须采取这样的“阶梯”,但我仍然期望更直接的东西 - 我错过了一些明显的东西吗?

enter image description here

1 个答案:

答案 0 :(得分:3)

对角线的移动费用与正交步骤相同。

东南,东南,东北,东北的路径与东,东,东,东的路径一样短。两者都花了4美元。

当有多条最短路径时,A *会给你一条,但它不是你想要的路径。

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

int new_cost = cost_so_far[current] + 1;

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