在给定距离变换的情况下寻找最佳路径
给定带有障碍物的地图的距离变换，如何获得从起始像素到目标像素的最低成本路径？距离变换图像具有到原始地图的最近障碍物的距离(欧几里得)，在每个像素中，即，如果在原始地图中，像素(i，j)在障碍物的左侧3个像素，在障碍物下方2个像素，那么在距离变换中，像素将具有sqrt(4+9) = sqrt(13)作为像素值。因此，较暗的像素表示接近障碍物，较亮的值表示它们远离障碍物。
我想使用这个距离变换提供的信息来规划从给定起点到目标像素的路径，并优化路径的成本，同时还有另一个限制，即路径永远不能到达距离障碍物小于'x‘像素的像素。
我该怎么做呢？
附注。一些关于算法的描述(或一些代码)将是有帮助的，因为我是规划算法的新手。
回答 3
Stack Overflow用户
发布于 2014-01-19 23:23:28
我在这一章的附录i中找到了一个算法
贾维斯，这是雷。基于距离变换的机器人导航路径规划。移动机器人的最新趋势，1993，11: 3-31。
我在Google books中完全可以看到这一章，这本书是
ZHENG，Yuang F. (主编)。移动机器人的最新趋势。“世界科学”，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;
}该算法使用简单的PPM图像格式，例如在休斯等人的《计算机图形学:原理和实践-第三版》一书的Chapter 15中解释。以便保存图像。
该算法从障碍物的图像(blocked)开始，计算距离变换(cell)；然后，从距离变换开始，用最陡峭的下降法计算最优路径:在变换距离势场中走下坡。所以你可以从你自己的距离变换图像开始。
请注意，在我看来，该算法没有满足您的附加约束，即：
路径永远不能到达离障碍物小于'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：
我看了一眼贾维斯的文章
CHIN, Yew Tuck, et al. Vision guided agv using distance transform. In: . 2001. p. 21.
更新：
下面的论文回顾了Jarvis的算法，其中作者指出：
由于路径是通过仅在相邻小区之间进行局部选择来找到的，因此所获得的路径可能是次优的
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. , 2013, 10.266.
Stack Overflow用户
发布于 2014-01-23 03:40:17
对于基于图形的解决方案，您可以查看本书的第15章
DE BERG，Mark等人。计算几何学。施普林格柏林海德堡，2008年。
它的标题是"Visibility Graphs - Finding the Shortest Route" and it is freely available at the publisher site。
本章解释了如何从所谓的可见性图开始计算欧几里得最短路径。可见性图是从障碍物集合开始计算的，每个障碍物被描述为一个多边形。
然后，将诸如Dijkstra算法的最短路径算法应用于可见性图，找到欧几里德最短路径。
在您的距离变换图像中，障碍物由零值像素表示，因此您可以尝试将它们近似为多边形，然后应用所引用的书中描述的方法。
Stack Overflow用户
发布于 2016-02-16 14:46:46
在像素的距离变换图中，您选择您的起始像素，然后选择其相邻像素的值小于您的起始像素-重复该过程，直到达到目标像素(值为零的像素)。
通常情况下，目标像素的值为零，这是所有可通过模式中的最低值。
通过生成距离变换图来解决障碍物不能靠近障碍物的问题，从而放大障碍物。例如，如果你想要一个dustance，如果任何障碍物有两个像素--只需添加两个障碍值的像素即可。通常情况下，无法通过的障碍被赋予负1的值。与边缘使用的值相同。另一种方法ua以非常高的起始值包围障碍物-不保证路径接近，但算法将尝试避开障碍物附近的路径。
https://stackoverflow.com/questions/21215244
复制相似问题
腾讯云开发者
