[数据结构与算法]Python布雷森汉姆直线算法RViz可视化ROS激光占位网格映射

使用对数赔率映射已知姿势算法（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)

以下是一些保持算法简单的假设。

1. 我们从左到右画线。
2. x1 < x2 和 y1< y2
3. 线的斜率在 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 轴上移动。

1. 我们总是将 x 加 1，然后选择下一个 y，是需要去 y+1 还是留在 y 上。换句话说，从任何位置 (Xk, Yk) 我们需要在 (Xk + 1, Yk) 和 (Xk + 1, Yk + 1) 之间进行选择。

2. 我们想选择与更接近原始线的点对应的 $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。

此外，通常首选与 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) 位置变化超过指定阈值时更新地图
• 地图发布率有限

源代码

详情参阅http://viadean.com/py_log_ros.html