在Gmapping中的Bresenham算法解析

引入

在gmapping算法中，需要计算激光在每个粒子相应的位姿所扫描到的区域，这一步骤用到了Bresenham算法，他的实现函数是：

void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )

刚好想了解一下这个算法具体实现流程，利用这个算法来完成其他工作。下面将从算法推导和代码实现来理解这个Bresenham算法。如果写的有问题，欢迎指出，谢谢。

参考资料：gmapping算法介绍
Bresenham算法理解

1、算法简介

bresenham算法是计算机图形学中为了“显示器（屏幕或打印机）系由像素构成”的这个特性而设计出来的算法，使得在求直线各点的过程中全部以整数来运算，因而大幅度提升计算速度。

原理
过各行、各列像素中心构造一组虚拟网格线，按直线从起点到终点的顺序计算直线各垂直网格线的交点，然后确定该列像素中与此交点最近的像素。

优点
该算法的优点在于可以采用增量计算，使得对于每一列，只要检查一个误差项的符号，就可以确定该列所求的像素。

2、算法推导

设start、end所在直线方程为y = kx + b，首先通过直线的斜率确定了在x方向进行单位步进还是y方向进行单位步进：当斜率k的绝对值|k|<1时，在x方向进行单位步进；当斜率k的绝对值|k|>1时，在y方向进行单位步进。

如上图，当我们通过斜率确定了是在x方向上进行单位步进的时候，理想情况下满足一个条件：当$x=X_m$ 时，$y=Y_m$ 。

但是，现实情况并不是这样，如上图所示，一条直线并不能完整地穿过那些点或栅格，所以需要通过某些计算来确定哪些点或栅格才能满足直线方程条件。

所以，问题来了。那么当x 执行一个单位步进时（即$x=X_m+1$时）, 是$y=Y_m$ 还是$y=Y_m+1$更符合这个直线方程呢？单凭肉眼我们很难得出结论，最好的办法当然是比较$Y_m，Y_m+1$和真实的方程的y值（即$Y_{real}=k\ast (X_m+1)+b$）的差是多少，看看哪一个更靠近真实的方程的y值。

那么，我们先假设$Dupper$表示$Y_m+1$和方程真实值$Y_{real}$的差，$Ddown$表示$Y_m$和方程真实值$Y_{real}$的差 ，求得如下：
$Dupper=Y_m+1-Y_{real}=Y_m+1-[k\ast (X_m+1)+b]$
$Ddown=Y_{real}-Y_m=[k\ast (X_m+1)+b]-Y_m$

那接着假设Diff为Dupper和Ddown之间的差：
$Diff=Dupper-Ddown=Y_m+1-[k\ast (X_m+1)+b]-[[k\ast (X_m+1)+b]-Y_m]$

令$△X$ 为线段x方向的间距（即$△X=|end.x-start.x|$），$△Y$ 为线段y方向的间距（即$$△Y=|end.y-start.y|$），则有：
$P_m=△X\ast Diff=△X\ast [Y_m+1-[k\ast (X_m+1)+b]]-△X\ast [[k\ast (X_m+1)+b]-Y_m]$

而且斜率$k=\frac{△Y}{△X}$，则有$△Y=△X\ast k$
$P_m=2\ast △X\ast Y_m-2\ast △X\ast k\ast X_m-2\ast △X\ast k-△X\ast (2b-1)$
$P_m=2\ast △X\ast Y_m-2\ast △Y\ast X_m-2\ast △Y-△X\ast (2b-1)$

那么，由$P_m$可以得到$P_{m+1}$：
$P_{m+1}=2\ast △X\ast Y_{m+1}-2\ast △Y\ast X_{m+1}-2\ast △Y-△X\ast (2b-1)$
此时，可以知道$X_{m+1}=X_m+1$，则两式相减可以得到：
$P_{m+1}-P_m=2\ast △X\ast (Y_{m+1}-Y_m)-2\ast △Y$
也就是，$P_{m+1}=P_m+2\ast △X\ast (Y_{m+1}-Y_m)-2\ast △Y$，但是上面那个求差的等式的右边是非常重要的，需要记住！

接下来，就是最重要的时候了，要对$(Y_{m+1}-Y_m)$进行讨论：
首先，我们知道$P_m=△X\ast Diff=△X\ast (Dupper-Ddown)$，所以$P_m$与$(Dupper-Ddown)$式子存在正相关关系。另外$Dupper$值越小说明$Y_m+1$比$Y_m$更接近$Y_{real}$，此时应该取$Y_m+1$；反之，就应该取$Y_m$。
（1）当$P_m>0$时，则$Y_{m+1}=Y_m$，那么$P_{m+1}=P_m-2\ast △Y$
（2）当$P_m<0$时，则$Y_{m+1}=Y_m+1$，那么$P_{m+1}=P_m+2\ast △X-2\ast △Y$

那么算法循环的重点就是循环判断$P_m$ ，如果小于0，则下一个点的纵坐标y加1。

最后根据等式$Diff=Dupper-Ddown=Y_m+1-[k\ast (X_m+1)+b]-[[k\ast (X_m+1)+b]-Y_m]$ ，$k=\frac{△Y}{△X}$以及$y_0=k\ast x_0+b$，我们可以得出起始像素$(x_0，y_0)$的参数$P_{m0}$的值为：
$P_{m0}=2\ast △X\ast y_0-2\ast △Y\ast x_0-2\ast △Y-△X\ast (2b-1)$
$P_{m0}=2\ast △X\ast k\ast x_0+2\ast △X\ast b-2\ast △Y\ast x_0-2\ast △Y-2\ast △X\ast b+△X$
$P_{m0}=△X-2\ast △Y$

算法就推导到这里了，下面看Gmapping里面的实现代码。

3、代码实现

其中，算法推导中的$P_m$相当于程序中的变量d；
变量incr1相当于算法推导中的情况(1)，变量incr2相当于算法推导中的情况(2)。

void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )
{
	int dx, dy;			//横纵坐标间距
	int incr1, incr2;	//P_m增量
	int d;					//P_m
	int x, y,xend, yend;//直线增长的首末端点坐标
	int xdirflag, ydirflag;//横纵坐标增长方向
	int cnt = 0;			//直线过点的点的序号
	
	dx = abs(end.x-start.x); 
	dy = abs(end.y-start.y);
  
	if (dy <= dx) 
	{//斜率k的绝对值|k|<1时，在x方向进行单位步进
		d = dx - 2*dy; ·	//初始点P_m0值
		incr1 = - 2 * dy;		//情况（1）
		incr2 = 2 * (dx - dy);//情况（2）
		if (start.x > end.x)
		{//起点横坐标比终点横坐标大，xdirflag = -1（负号可以理解为增长方向与直线始终点方向相反）
			x = end.x; y = end.y;	//设置增长起点，注意这里要选择小横坐标作为起点，用于增量时好理解
			ydirflag = (-1);				//此时默认(希望)纵坐标也是start.y > end.y的情况
			xend = start.x;		//设置增长终点横坐标
		} else {//xdirflag = 1 ,ydirflag = 1
			x = start.x; y = start.y;
			ydirflag = 1;
			xend = end.x;
		}
		line->points[cnt].x=x;	//加入起点坐标
		line->points[cnt].y=y;
		cnt++;
		if (((end.y - start.y) * ydirflag) > 0)
		 {//是预料到的情况，start.y > end.y
			while (x < xend)//开始进行增量递增
			{//x > xend，y > yend，处理向右上角爬升的直线群，下图1号所示直线
				x++;
				if (d > 0)
	  				d+=incr1;
				else {
	 				 y++; d+=incr2;		//纵坐标向正方向增长
				}
				line->points[cnt].x=x;//添加新的点
				line->points[cnt].y=y;
				cnt++;
     		}
    	} else {//x > xend，y < yend，处理向右下角降落的直线群，下图2号所示直线
      		while (x < xend) 
      		{
				x++;
				if (d > 0) 
	 				 d+=incr1;
				else {
	  				y--; d+=incr2;	//纵坐标向负方向增长
				}
				line->points[cnt].x=x;
				line->points[cnt].y=y;
				cnt++;
     		 }
   		 }		
 	 } else {//dy > dx，当斜率k的绝对值|k|>1时，在y方向进行单位步进
		d = dy - 2*dx;	//P_m0初值
		incr1 = -2*dx; 	//形同算法推导情况1
		incr2 = 2 * (dy - dx);
   	 	if (start.y > end.y) 
   	 	{
		      y = end.y; x = end.x;	//取最小的纵坐标作为起点
		      yend = start.y;
		      xdirflag = (-1);		//期望start.x > end.x
   		 } else {//start.y < end.y
		      y = start.y; x = start.x;
		      yend = end.y;
		      xdirflag = 1;
    	}
	    line->points[cnt].x=x;	//添加起点
	    line->points[cnt].y=y;
	    cnt++;
	    if (((end.x - start.x) * xdirflag) > 0) 
    	{//x > xend ，y > yend，处理向右上角爬升的直线群，下图3号所示直线
      		while (y < yend)
       		{
				y++;
				if (d > 0) 
					d+=incr1;
				else {
					x++; d+=incr2;		//横坐标向正方向增长
				}
				line->points[cnt].x=x;	//添加新的点
				line->points[cnt].y=y;
				cnt++;
     		 }
   		 } else 
   		 {//x < xend，y > yend，处理向左上角爬升的直线群，下图4号所示直线
			while (y < yend)
			{
			y++;
			if (d > 0) 
				d+=incr1;
			else {
				x--; d+=incr2;	横坐标向负方向增长
			}
			line->points[cnt].x=x;
			line->points[cnt].y=y;
			cnt++;
			}
    	}
 	 }
 	 line->num_points = cnt;		//记录添加所有点的数目
  }