栅格地图： 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;
}

回到顶部

通用代码

• 针对不同斜率、不同dx、dy情况

void bresenham_hector(nav_msgs::OccupancyGrid &map,int x0, int y0, int x1, int y1  ){
    ostringstream ostr;
    int dx = x1-x0;    int dy = y1-y0;
    int abs_dx = std::abs(dx),abs_dy = std::abs(dy);    
    int offset_dx = dx>0 ? 1: -1;         
    int offset_dy = (dy>0 ? 1: -1) ;
    // ostr<< "dx= " << dx << " , dy= " <<dy<< "\n";
    // ostr<< "abs_dx= " << abs_dx << " , abs_dy= " <<abs_dy<< "\n";
    // ostr<< "off_dx= " << offset_dx << " , off_dy= " <<offset_dy<< "\n";
    // cout << ostr.str()<<endl;
    // ostr.clear();
    int startOffset  = map.info.width*y0 + x0;
    int   endOffset  = map.info.width*y1 + x1;
    
    map.data[startOffset] = 120;        // 起点
    if(abs_dx> abs_dy){        
        int error =  (-abs_dx);
        int x,y;
        for(auto x=x0,y=y0;x!=x1;){
            x+=offset_dx;
            error += (2*abs_dy);
            if(error> 0){
                error -= (2*abs_dx);
                y+=offset_dy;
            }
            map.data[y * (map.info.width) + x] = 0;
        }        
    }
    else{
         int error =  (-abs_dy);
        int x,y;
        for(auto x=x0,y=y0;y!=y1;){
            y+=offset_dy;
            error += (2*abs_dx);
            if(error> 0){
                error -= (2*abs_dy);                
                x+=offset_dx;
            }
            map.data[y * (map.info.width) + x] = 0;
        }
    }
    map.data[endOffset] = 100;       // 终点
}

• 画圆

std::vector<int> endpoint{50,50};	
    int cell_l = 15;  // 半径
    for(float i = -3.1415;i<3.14159;i+=0.1){
        int off_x = int( cell_l*cos(i));
        int off_y = int( cell_l*sin(i));
        bresenham_hector(tmpmap, endpoint[0],endpoint[1],endpoint[1]+off_x , endpoint[0]+off_y);
    }