使用对数赔率映射已知姿势算法（ROS 包）。
布雷森汉姆直线算法
布雷森汉姆直线算法是一种线绘制算法，它确定应选择的 n 维栅格的点，以便形成两点之间的直线的近似值。 它通常用于在位图图像中（例如在计算机屏幕上）绘制线条图元，因为它仅使用整数加法、减法和位移，所有这些在常用的计算机指令集（如 x86_64）中都是非常便宜的操作。 它是一种增量误差算法，是计算机图形学领域最早开发的算法之一。
Python 实现算法
给定两点 A(x1, y1) 和 B(x2, y2) 的坐标。在像素的计算机屏幕上找到绘制线 AB 所需的所有中间点的任务。请注意，每个像素都有整数坐标。 例子：
Input : A(0,0), B(4,4)
Output : (0,0), (1,1), (2,2), (3,3), (4,4)
Input : A(0,0), B(4,2)
Output : (0,0), (1,0), (2,1), (3,1), (4,2)
以下是一些保持算法简单的假设。
- 我们从左到右画线。
- x1 < x2 和 y1< y2
- 线的斜率在 0 和 1 之间。我们从左下角到右上角画一条线。
让我们首先考虑幼稚的方式来理解这个过程。
// A naive way of drawing line
void naiveDrawLine(x1, x2, y1, y2)
{
m = (y2 - y1)/(x2 - x1)
for (x = x1; x <= x2; x++)
{
// Assuming that the round function finds
// closest integer to a given float.
y = round(mx + c);
print(x, y);
}
}
上述算法有效，但速度很慢。 布雷森汉姆算法的思想是避免浮点乘法和加法计算mx+c，然后在每一步计算(mx+c)的取整值。 在布雷森汉姆的算法中，我们以单位间隔在 x 轴上移动。
-
我们总是将 x 加 1，然后选择下一个 y，是需要去 y+1 还是留在 y 上。换句话说，从任何位置 (Xk, Yk) 我们需要在 (Xk + 1, Yk) 和 (Xk + 1, Yk + 1) 之间进行选择。
-
我们想选择与更接近原始线的点对应的
y
y
y 值（在
Y
∗
k
+
1
\mathrm{Y}*{\mathrm{k}}+1
Y∗k+1 和
Y
∗
k
\mathrm{Y}*{\mathrm{k}}
Y∗k 中）。
我们需要一个决策参数来决定是选择
Y
∗
k
+
1
\mathrm{Y}*{\mathrm{k}}+1
Y∗k+1 还是
Y
∗
k
\mathrm{Y}*{\mathrm{k}}
Y∗k 作为下一个点。 这个想法是跟踪从先前增量到
y
y
y 的斜率误差。 如果斜率误差大于 0.5，我们知道这条线已经向上移动了一个像素，我们必须增加
y
y
y 坐标并重新调整误差，以表示到新像素顶部的距离——这是通过减去一个来完成的 从错误。
// Modifying the naive way to use a parameter
// to decide next y.
void withDecisionParameter(x1, x2, y1, y2)
{
m = (y2 - y1)/(x2 - x1)
slope_error = [Some Initial Value]
for (x = x1, y = y1; x = 0.5)
{
y++;
slope_error -= 1.0;
}
}
上述算法仍然包括浮点运算。为避免浮点运算，请考虑低于值
m
m
m 的值。
m = (y2 – y1)/(x2 – x1)
我们将两边乘以 (x2 – x1)。
我们还将 slope_error 更改为 slope_error * (x2 – x1)。为了避免与 0.5 比较，我们进一步将其更改为 slope_error * (x2 – x1) * 2。
此外，通常首选与 0 进行比较而不是 1。
// Modifying the above algorithm to avoid floating
// point arithmetic and use comparison with 0.
void bresenham(x1, x2, y1, y2)
{
m_new = 2 * (y2 - y1)
slope_error_new = [Some Initial Value]
for (x = x1, y = y1; x = 0)
{
y++;
slope_error_new -= 2 * (x2 - x1);
}
}
slope_error_new 的初始值为 2*(y2 – y1) – (x2 – x1)。请参阅此处，以获取此值的证明。下面是上述算法的实现。
# Python 3 program for Bresenham’s Line Generation
# Assumptions :
# 1) Line is drawn from left to right.
# 2) x1 < x2 and y1 < y2
# 3) Slope of the line is between 0 and 1.
# We draw a line from lower left to upper
# right.
# function for line generation
def bresenham(x1,y1,x2, y2):
m_new = 2 * (y2 - y1)
slope_error_new = m_new - (x2 - x1)
y=y1
for x in range(x1,x2+1):
print("(",x ,",",y ,")\\n")
# Add slope to increment angle formed
slope_error_new =slope_error_new + m_new
# Slope error reached limit, time to
# increment y and update slope error.
if (slope_error_new >= 0):
y=y+1
slope_error_new =slope_error_new - 2 * (x2 - x1)
# driver function
if __name__=='__main__':
x1 = 3
y1 = 2
x2 = 15
y2 = 5
bresenham(x1, y1, x2, y2)
#This code is contributed by ash264
输出
(3,2)
(4,3)
(5,3)
(6,3)
(7,3)
(8,4)
(9,4)
(10,4)
(11,4)
(12,5)
(13,5)
(14,5)
(15,5)
上面的解释是为了提供算法背后的粗略概念。详细的解释和证明，读者可以参考下面的参考资料。
上面的程序只有在直线的斜率小于 1 时才有效。这是任何斜率的程序实现。
#include <iostream>
//#include <graphics.h>
//Uncomment the graphics library functions if you are using it
using namespace std;
void plotPixel(int x1, int y1, int x2, int y2, int dx, int dy, int decide)
{
//pk is initial decesion making parameter
//Note:x1&y1,x2&y2, dx&dy values are intercganged
//and passed in plotPixel function so
//it can handle both cases when m>1 & m<1
int pk = 2 * dy - dx;
for (int i = 0; i <= dx; i++)
{
cout << x1 << "," << y1 << endl;
//checking either to decrement or increment the value
//if we have to plot from (0,100) to (100,0)
x1 < x2 ? x1++ : x1--;
if (pk < 0)
{
//decesion value will decide to plot
//either x1 or y1 in x's position
if (decide == 0)
{
// putpixel(x1, y1, RED);
pk = pk + 2 * dy;
}
else
{
//(y1,x1) is passed in xt
// putpixel(y1, x1, YELLOW);
pk = pk + 2 * dy;
}
}
else
{
y1 < y2 ? y1++ : y1--;
if (decide == 0)
{
//putpixel(x1, y1, RED);
}
else
{
// putpixel(y1, x1, YELLOW);
}
pk = pk + 2 * dy - 2 * dx;
}
}
}
int main()
{
// int gd = DETECT, gm;
// initgraph(&gd, &gm, "xxx");
int x1 = 100, y1 = 110, x2 = 125, y2 = 120, dx, dy, pk;
//cin cout
dx = abs(x2 - x1);
dy = abs(y2 - y1);
//If slope is less than one
if (dx > dy)
{
//passing argument as 0 to plot(x,y)
plotPixel(x1, y1, x2, y2, dx, dy, 0);
}
//if slope is greater than or equal to 1
else
{
//passing argument as 1 to plot (y,x)
plotPixel(y1, x1, y2, x2, dy, dx, 1);
}
// getch();
}
视频演示（1m)
特征
- ROS（输出 /map 和输入/scan）
- 监听 map->base_link 变换
- 演示包可用
- 当 (x,y) 位置变化超过指定阈值时更新地图
- 地图发布率有限
源代码
详情参阅 亚图跨际
