Python布雷森汉姆直线算法RViz可视化ROS激光占位网格映射

使用对数赔率映射已知姿势算法（ROS 包）。

布雷森汉姆直线算法

布雷森汉姆直线算法是一种线绘制算法，它确定应选择的 n 维栅格的点，以便形成两点之间的直线的近似值。它通常用于在位图图像中（例如在计算机屏幕上）绘制线条图元，因为它仅使用整数加法、减法和位移，所有这些在常用的计算机指令集（如 x86_64）中都是非常便宜的操作。它是一种增量误差算法，是计算机图形学领域最早开发的算法之一。

https://embed.notionlytics.com/wt/ZXlKd1lXZGxTV1FpT2lJek1qUmxNREkxWWpFMk9UTTBOakkzWVRsak1HTTBOelJpTW1NMlpHUTJaQ0lzSW5kdmNtdHpjR0ZqWlZSeVlXTnJaWEpKWkNJNklsZHNTR2hsVEZSUFdXeHpaVmRhUW1ZNU1YQmxJbjA9

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$ 值（在 $\mathrm{Y}{\mathrm{k}}+1$ 和 $\mathrm{Y}{\mathrm{k}}$ 中）。

我们需要一个决策参数来决定是选择 $\mathrm{Y}{\mathrm{k}}+1$ 还是 $\mathrm{Y}{\mathrm{k}}$ 作为下一个点。这个想法是跟踪从先前增量到 $y$ 的斜率误差。如果斜率误差大于 0.5，我们知道这条线已经向上移动了一个像素，我们必须增加 $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 = (y2 – y1)/(x2 – x1)

我们将两边乘以 (x2 – x1)。

我们还将 slope_error 更改为 slope_error * (x2 – x1)。为了避免与 0.5 比较，我们进一步将其更改为 slope_error * (x2 – x1) * 2。