Algorithm 给定距离变换寻找最佳路径

给定带有障碍物的地图的距离变换，如何获得从起始像素到目标像素的最小成本路径？距离变换图像在每个像素中具有到原始地图最近障碍物的距离（欧几里德），即，如果在原始地图中，像素（i，j）距离左侧3像素，距离障碍物下方2像素，则在距离变换中，像素将具有sqrt（4+9）=sqrt（13）作为像素值。因此，较暗的像素表示接近障碍物，较亮的值表示远离障碍物

我想使用这个距离变换提供的信息规划一条从给定起点到目标像素的路径，并优化路径的成本，还有另一个约束，即路径不应到达距离障碍物小于“x”像素的像素

我该怎么办

---

另外，对算法进行一点描述（或一点代码）会有所帮助，因为我对规划算法还不熟悉。

我在标题为

贾维斯，雷。基于距离变换的机器人导航路径规划。移动机器人的最新趋势，1993年，11:3-31

那一章对我来说是完全可见的，这本书是

郑元福（编）。移动机器人的最新趋势。《世界科学》，1993年

算法的C++实现如下：

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

#包括
#包括
#包括
#包括休斯等人出版的《计算机图形学：原理与实践——第三版》，以保存图像

该算法从障碍物的图像（
阻塞
）开始计算距离变换（
单元
）；然后，从距离变换开始，用最速下降法计算最佳路径：它在变换距离势场中下坡行走。因此，您可以从自己的距离变换图像开始

请注意，在我看来，该算法似乎没有满足您的附加约束，即：

路径不应到达距离“x”像素以下的像素
从一个障碍物

以下png图像是障碍物的图像，程序生成的
blocked.ppm
图像通过Gimp导出为png：


以下png图像是起点的图像，程序生成的
start.ppm
图像通过Gimp导出为png：


以下png图像是端点的图像，程序生成的
goal.ppm
图像通过Gimp导出为png：


以下png图像是计算路径的图像，程序生成的
path.ppm
图像通过Gimp导出为png：


以下png图像是距离变换的图像，程序生成的
cell.ppm
图像通过Gimp导出为png：


我看过Jarvis的文章后找到了


更新：

以下文章回顾了Jarvis算法，作者指出：

因为路径是通过在邻居之间进行局部选择来找到的
对于单元，得到的路径可以是次优的

我在标题为

贾维斯，雷。基于距离变换的机器人导航路径规划。移动机器人的最新趋势，1993年，11:3-31

那一章对我来说是完全可见的，这本书是

郑元福（编）。移动机器人的最新趋势。《世界科学》，1993年

算法的C++实现如下：
#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;
}

#包括
#包括
#包括
#包括休斯等人出版的《计算机图形学：原理与实践——第三版》，以保存图像

该算法从障碍物的图像（
阻塞
）开始计算距离变换（
单元
）；然后，从距离变换开始，用最速下降法计算最佳路径：它在变换距离势场中下坡行走。因此，您可以从自己的距离变换图像开始

请注意，在我看来，该算法似乎没有满足您的附加约束，即：

路径不应到达距离“x”像素以下的像素
从一个障碍物

以下png图像是障碍物的图像，程序生成的
blocked.ppm
图像通过Gimp导出为png：


以下png图像是起点的图像，程序生成的
start.ppm
图像通过Gimp导出为png：


以下png图像是端点的图像，程序生成的
goal.ppm
图像通过Gimp导出为png：


以下png图像是计算路径的图像，程序生成的
path.ppm
图像通过Gimp导出为png：


以下png图像是距离变换的图像，程序生成的
cell.ppm
图像通过Gimp导出为png：


我看过Jarvis的文章后找到了


更新：

以下文章回顾了Jarvis算法，作者指出：

因为路径是通过在邻居之间进行局部选择来找到的
对于单元，得到的路径可以是次优的

对于基于图形的解决方案，您可以查看本书的第15章

德伯格，马克，等。计算几何。施普林格柏林海德堡，2008年

它有标题

本章介绍如何从所谓的可见性图开始计算欧几里德最短路径。可见性图从一组障碍物开始计算，每个障碍物被描述为一个多边形

然后找到欧几里德最短路径