A* search, grid, 8 directions, octile distance as heuristic, not finding the direct path

Question

Could anybody help me to understand what is wrong with my A* search implementation?

I implemented a basic A* search based on this incredible helpful site: http://www.redblobgames.com/pathfinding/a-star/implementation.html#cplusplus (big thanks here to the author, Amit!).

I am using a grid, eight directions and the Octile distance as heuristic. Unfortunately my path, going from start(0,h/2) to end(w-1,h/2), is not the expected straight line but looks like this:

My code (should be compilable as is but requires 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;
}

Any insights or tips on how to get to the root of the problem are highly appreciated!

UPDATE: With Amit's suggestions I get the following paths now:

FOLLOW-UP (highly related that is why I am adding it here and don't open a new post):

If I use only four directions with a Manhattan distance as heuristic and a movement cost of 1 for all four steps, I get a jittery diagonal. Of course the algorithm has to take 'stairs" like this, but I would still expect something more straight - am I missing something obvious?

Answer 1

Your movement cost for diagonals is the same as for orthogonal steps.

A path going southeast, southeast, northeast, northeast is just as short as a path going east, east, east, east. Both have cost 4.

When there are multiple shortest paths, A* is giving you one of them, but it's not the one you want.

If you set diagonals to have a higher movement cost (sqrt(2) is what your heuristic states), then A* would prefer east, east, east, east. Change

int new_cost = cost_so_far[current] + 1;

to either use 1 or sqrt(2) depending on whether it's an orthogonal or diagonal step. You'll also need to make the costs into floats/doubles instead of ints, and make the priority queue do the same. (Alternatively, if you want to keep using ints, some people will use 14 and 10 as the costs, and scale the heuristic up to use 14 and 10 for D2 and D.)