栅格地图： Bresenham's line

参考：网易公开课，中国农业大学，Bresenham

解释：对于【图2】中，当d≥1的时候，减去1，实际测试不对，应该分界点是0.5，超过0.5就要减去1.0

【图3】中对于判断变量进行了改进，设置：e = d-0.5

按照上述理解，整理出如下浮点运算和整数运算的代码，

• 代码仅针对x0<x1,y0<y1，且dy/dx ∈[0,1]

1.浮点运算

void bresenham2d3(nav_msgs::OccupancyGrid & map, int x0,int y0, int x1,int y1){
	int dx = x1-x0; 
	int dy = y1-y0; 

	float k = std::abs(dy/float(dx)); 	// 计算斜率
	float d = 0;	
	int x = x0,y=y0;	
	for(;x<=x1;x++){
		map.data[y* (map.info.width) +x] =120;    // 在(x,y)处绘制cell绿色背景
		d = d+k;
		if (d>0.5){
			d = d-1.0;
			y++;
		}
	}    	
	map.data[y1* (map.info.width) +x1] =100;
}

如果是浮点数运算，会存在如下问题：
k = 0.388889
判断x=8时，计算y的位置时，由于d = 0.111111 ，所以y保持不变；同x=7在同一行；
判断x=9时，由于d =0.5 ,没有超过0.5，但是在执行【if (d>0.5)】比较时，程序认为条件满足了，所以进入了分支。所以y的栅格对应的上移了一格，就出现了图中绿色的栅格；

问题说明：浮点数的比较不稳定，还是需要转换为整数运算来计算；才能保证计算结果绝对准确；

2.整数运算

void bresenham2d4(nav_msgs::OccupancyGrid &map, int x0, int y0, int x1, int y1){
    int dx = x1 - x0;
    int dy = y1 - y0;
    int x, y;

    int d = -dx; // 初始值，对应上述右图中的step1；
    for (x = x0, y = y0; x <= x1; x++){
        map.data[y * (map.info.width) + x] = 0; // 在(x,y)处绘制cell背景，0表示白色背景
        d = d + 2 * dy;                         // 增量dy，对应上述右图中的step2
        if (d > 0){
            d = d - 2 * dx;             // 大于0了就减去dx，对应上述右图中的step3；
            y++;  
        }
    }
    map.data[y1 * (map.info.width) + x1] = 100;
}