Finding best path given a distance transform

Question

Given a distance transform of a map with obstacles in it, how do I get the least cost path from a start pixel to a goal pixel? The distance transform image has the distance(euclidean) to the nearest obstacle of the original map, in each pixel i.e. if in the original map pixel (i,j) is 3 pixels away to the left and 2 pixels away to the down of an obstacle, then in the distance transform the pixel will have sqrt(4+9) = sqrt(13) as the pixel value. Thus darker pixels signify proximity to obstacles and lighter values signify that they are far from obstacles.

I want to plan a path from a given start to goal pixel using the information provided by this distance transform and optimize the cost of the path and also there is another constraint that the path should never reach a pixel which is less than 'x' pixels away from an obstacle.

How do I go about this?

P.S. A bit of description on the algorithm (or a bit of code) would be helpful as I am new to planning algorithms.

Answer 1

I found an algorithm in the appendix I of the chapter titled

JARVIS, Ray. Distance transform based path planning for robot navigation. Recent trends in mobile robots, 1993, 11: 3-31.

That chapter is fully visible to me in Google books and the book is

ZHENG, Yuang F. (ed.). Recent trends in mobile robots. World Scientific, 1993.

A C++ implementation of the algorithm follows:

#include <vector>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <sstream>

/**
Algorithm in the appendix I of the chapter titled

JARVIS, Ray. Distance transform based path planning for robot navigation. *Recent trends in mobile robots*, 1993, 11: 3-31.

in the book

ZHENG, Yuang F. (ed.). *Recent trends in mobile robots*. World Scientific, 1993.

See also http://stackoverflow.com/questions/21215244/least-cost-path-using-a-given-distance-transform
*/

template < class T >
class Matrix
{
private:
    int    m_width;
    int    m_height;
    std::vector<T> m_data;

public:
    Matrix(int width, int height) :
        m_width(width), m_height(height), m_data(width *height) {}
    int width() const
    {
        return m_width;
    }
    int height() const
    {
        return m_height;
    }
    void set(int x, int y, const T &value)
    {
        m_data[x + y * m_width] = value;
    }
    const T &get(int x, int y) const
    {
        return m_data[x + y * m_width];
    }
};

float distance( const Matrix< float > &a, const Matrix< float > &b )
{
    assert(a.width() == b.width());
    assert(a.height() == b.height());
    float r = 0;
    for ( int y = 0; y < a.height(); y++ )
    {
        for ( int x = 0; x < a.width(); x++ )
        {
            r += fabs(a.get(x, y) - b.get(x, y));
        }
    }
    return r;
}


int PPMGammaEncode(float radiance, float d)
{
    //return int(std::pow(std::min(1.0f, std::max(0.0f, radiance * d)),1.0f / 2.2f) * 255.0f);
    return radiance;
}

void PPM_image_save(const Matrix<float> &img, const std::string &filename, float d = 15.0f)
{
    FILE *file = fopen(filename.c_str(), "wt");

    const int m_width  = img.width();
    const int m_height = img.height();
    fprintf(file, "P3 %d %d 255\n", m_width, m_height);

    for (int y = 0; y < m_height; ++y)
    {
        fprintf(file, "\n# y = %d\n", y);
        for (int x = 0; x < m_width; ++x)
        {
            const float &c(img.get(x, y));
            fprintf(file, "%d %d %d\n",
                    PPMGammaEncode(c, d),
                    PPMGammaEncode(c, d),
                    PPMGammaEncode(c, d));
        }
    }
    fclose(file);
}

void PPM_image_save(const Matrix<bool> &img, const std::string &filename, float d = 15.0f)
{
    FILE *file = fopen(filename.c_str(), "wt");

    const int m_width  = img.width();
    const int m_height = img.height();
    fprintf(file, "P3 %d %d 255\n", m_width, m_height);

    for (int y = 0; y < m_height; ++y)
    {
        fprintf(file, "\n# y = %d\n", y);
        for (int x = 0; x < m_width; ++x)
        {
            float v = img.get(x, y) ? 255 : 0;
            fprintf(file, "%d %d %d\n",
                    PPMGammaEncode(v, d),
                    PPMGammaEncode(v, d),
                    PPMGammaEncode(v, d));
        }
    }
    fclose(file);
}

void add_obstacles(Matrix<bool> &m, int n, int avg_lenght, int sd_lenght)
{
    int side = std::max(3, std::min(m.width(), m.height()) / 10);
    for ( int y = m.height() / 2 - side / 2; y < m.height() / 2 + side / 2; y++ )
    {
        for ( int x = m.width() / 2 - side / 2; x < m.width() / 2 + side / 2; x++ )
        {
            m.set(x, y, true);
        }
    }

    /*
        for ( int y = m.height()/2-side/2; y < m.height()/2+side/2; y++ ) {
          for ( int x = 0; x < m.width()/2+side; x++ ) {
            m.set(x,y,true);
          }
        }
        */

    for ( int y = 0; y < m.height(); y++ )
    {
        m.set(0, y, true);
        m.set(m.width() - 1, y, true);
    }

    for ( int x = 0; x < m.width(); x++ )
    {
        m.set(x, 0, true);
        m.set(x, m.height() - 1, true);
    }
}

class Info
{
public:
    Info() {}
    Info(float v, int x_o, int y_o): value(v), x_offset(x_o), y_offset(y_o) {}
    float value;
    int x_offset;
    int y_offset;

    bool operator<(const Info &rhs) const
    {
        return value < rhs.value;
    }
};

void next(const Matrix<float> &m, const int &x, const int &y, int &x_n, int &y_n)
{
    //todo: choose the diagonal adiacent in case of ties.
    x_n = x;
    y_n = y;

    Info neighbours[8];
    neighbours[0] = Info(m.get(x - 1, y - 1), -1, -1);
    neighbours[1] = Info(m.get(x  , y - 1), 0, -1);
    neighbours[2] = Info(m.get(x + 1, y - 1), +1, -1);

    neighbours[3] = Info(m.get(x - 1, y  ), -1, 0);
    neighbours[4] = Info(m.get(x + 1, y  ), +1, 0);

    neighbours[5] = Info(m.get(x - 1, y + 1), -1, +1);
    neighbours[6] = Info(m.get(x  , y + 1), 0, +1);
    neighbours[7] = Info(m.get(x + 1, y + 1), +1, +1);

    auto the_min = *std::min_element(neighbours, neighbours + 8);
    x_n += the_min.x_offset;
    y_n += the_min.y_offset;
}

int main(int, char **)
{
    std::size_t xMax = 200;
    std::size_t yMax = 150;

    Matrix<float> cell(xMax + 2, yMax + 2);


    Matrix<bool> start(xMax + 2, yMax + 2);
    start.set(0.1 * xMax, 0.1 * yMax, true);

    Matrix<bool> goal(xMax + 2, yMax + 2);
    goal.set(0.9 * xMax, 0.9 * yMax, true);

    Matrix<bool> blocked(xMax + 2, yMax + 2);
    add_obstacles(blocked, 1, 1, 1);

    PPM_image_save(blocked, "blocked.ppm");
    PPM_image_save(start, "start.ppm");
    PPM_image_save(goal, "goal.ppm");

    for ( int y = 0; y <= yMax + 1; y++ )
    {
        for ( int x = 0; x <= xMax + 1; x++ )
        {
            if ( goal.get(x, y) )
            {
                cell.set(x, y, 0.);
            }
            else
            {
                cell.set(x, y, xMax * yMax);
            }
        }
    }

    Matrix<float> previous_cell = cell;
    float values[5];
    int cnt = 0;
    do
    {
        std::ostringstream oss;
        oss << "cell_" << cnt++ << ".ppm";
        PPM_image_save(cell, oss.str());
        previous_cell = cell;
        for ( int y = 2; y <= yMax; y++ )
        {
            for ( int x = 2; x <= xMax; x++ )
            {
                if (!blocked.get(x, y))
                {
                    values[0] = cell.get(x - 1, y    ) + 1;
                    values[1] = cell.get(x - 1, y - 1) + 1;
                    values[2] = cell.get(x    , y - 1) + 1;
                    values[3] = cell.get(x + 1, y - 1) + 1;
                    values[4] = cell.get(x    , y    );
                    cell.set(x, y, *std::min_element(values, values + 5));
                }
            }
        }
        for ( int y = yMax - 1; y >= 1; y-- )
        {
            for ( int x = xMax - 1; x >= 1; x-- )
            {
                if (!blocked.get(x, y))
                {
                    values[0] = cell.get(x + 1, y    ) + 1;
                    values[1] = cell.get(x + 1, y + 1) + 1;
                    values[2] = cell.get(x    , y + 1) + 1;
                    values[3] = cell.get(x - 1, y + 1) + 1;
                    values[4] = cell.get(x    , y    );
                    cell.set(x, y, *std::min_element(values, values + 5));
                }
            }
        }
    }
    while (distance(previous_cell, cell) > 0.);

    PPM_image_save(cell, "cell.ppm");

    Matrix<bool> path(xMax + 2, yMax + 2);
    for ( int y_s = 1; y_s <= yMax; y_s++ )
    {
        for ( int x_s = 1; x_s <= xMax; x_s++ )
        {
            if ( start.get(x_s, y_s) )
            {
                int x = x_s;
                int y = y_s;
                while (!goal.get(x, y))
                {
                    path.set(x, y, true);
                    int x_n, y_n;
                    next(cell, x, y, x_n, y_n);
                    x = x_n;
                    y = y_n;
                }
            }
        }
    }

    PPM_image_save(path, "path.ppm");

    return 0;
}

The algorithm uses the simple PPM image format explained for example in the Chapter 15 from the book Computer Graphics: Principles and Practice - Third Edition by Hughes et al. in order to save the images.

The algorithm starts from the image of the obstacles (blocked) and computes from it the distance transform (cell); then, starting from the distance transform, it computes the optimal path with a steepest descent method: it walks downhill in the transform distance potential field. So you can start from your own distance transform image.

Please note that it seems to me that the algorithm does not fulfill your additional constraint that:

the path should never reach a pixel which is less than 'x' pixels away from an obstacle.

The following png image is the image of the obstacles, the program generated blocked.ppm image was exported as png via Gimp:

The following png image is the image of the start point, the program generated start.ppm image was exported as png via Gimp:

The following png image is the image of the end point, the program generated goal.ppm image was exported as png via Gimp:

The following png image is the image of the computed path, the program generated path.ppm image was exported as png via Gimp:

The following png image is the image of the distance transform, the program generated cell.ppm image was exported as png via Gimp:

I found the Jarvis' article after having a look at

CHIN, Yew Tuck, et al. Vision guided agv using distance transform. In: Proceedings of the 32nd ISR (International Symposium on Robotics). 2001. p. 21.

Update:

The Jarvis' algorithm is reviewed in the following paper where the authors state that:

Since the path is found by choosing locally only between neighbour cells, the obtained path can be sub optimal

ELIZONDO-LEAL, Juan Carlos; PARRA-GONZÁLEZ, Ezra Federico; RAMÍREZ-TORRES, José Gabriel. The Exact Euclidean Distance Transform: A New Algorithm for Universal Path Planning. Int J Adv Robotic Sy, 2013, 10.266.

Answer 2

For a graph-based solution you can check for example chapter 15 of the book

DE BERG, Mark, et al. Computational geometry. Springer Berlin Heidelberg, 2008.

which has title "Visibility Graphs - Finding the Shortest Route" and it is freely available at the publisher site.

The chapter explains how to compute the Euclidean shortest path starting from the so-called visibility graph. The visibility graph is computed starting from the set of obstacles, each obstacle is described as a polygon.

The Euclidean shortest path is then found applying a shortest path algorithm such as Dijkstra's algorithm to the visibility graph.

In your distance transform image the obstacles are represented by pixels with zero value and so you can try to approximate them as polygons and after that apply the method described in the cited book.

Answer 3

In the distance transform map of pixels you choose your starting puxel and then select its neighbor with a lower value than your starting puxel - repeat the process until goal puxel is reached (a pixel with value zero).

Normally the goal pixel has value zero, the lowest number of any passable mode.

The problem of nor passing close to barriers is silver by generate the distance transform map so that barriers are enlarged. For example if you want a dustance if two pixels to any barrier - just add two pixels of barrier value. Normally the barriers wich could mot be passed is given a value of minus one. Same value i used for the edges. An alternative approach ua to surround barriers with a very hifh starting value - the path is not guaranteed to get close, but the algorithm will try to avoid paths un the proximity of barriers.