大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
通俗的说，在机器人导航方面，Voronoi图是一种将地图分区的方法，分区的界限即Voronoi图的边，常用作机器人远离障碍物避障的规划路径。本文主要参考了 Boris Lau 的论文和代码，对Voronoi图的生成和更新进行分析。相关的3篇论文内容重合度比较高，我主要以《Efficient Grid-Based Spatial Representations for Robot Navigation in Dynamic Environments》为主。对代码的理解和注释，我已在GitHub上开源，欢迎大家一起讨论。
目录
1. DM的更新概述
在机器人路径规划和避障的过程中，我们常常需要知道某个时刻机器人与最近障碍物的距离，以远离障碍物，或者进行碰撞检测。论文提出使用Distance Map（DM）和 Generalized Voronoi Diagrams（GVD）来解决这个问题。DM的建立和更新是GVD建立和更新的前提，方法来源于改进的brushfire算法，过程如图1-2所示。DM的每个栅格都会保存与最近障碍物点的距离，以及障碍物点的坐标（因此，障碍物的内部点是被忽略的，只有轮廓点被考察）。
图1A是论文算法的输入——已知的二值占据栅格地图，其中外围的黑色是地图外部区域，中间的黑色是障碍物，白色是可行驶区域。因为有边界和障碍物的存在，使得内部白色栅格与最近障碍物点的距离会减小（初始化为正无穷），因此要从障碍物栅格开始，逐步向外扩散更新，计算新的最近障碍物坐标与距离，反映为图1B-D中灰色逐步扩展，距离越近颜色越深。当所有栅格都被更新后，DM建立完成。
当图1中的障碍物消失、新的障碍物出现时（图2B），相应的二值占据栅格地图会被更新，进而触发DM和GVD的更新。因为旧的障碍物（记为P）消失，那么周围以P为最近障碍物的栅格，暂时没有最近障碍物，其保存的最近障碍物距离也会被置为无效值（或正无穷，或初始值），所以这些栅格的状态更新是一个距离增大（raise）的过程。
类似的，因为新的障碍物（记为Q）出现，那么Q周围的栅格，其保存的最近障碍物距离被重新计算（可能是到Q的距离），所以这些栅格的状态更新是一个距离减小（lower）的过程。
当raise和lower的过程相遇，lower处理过的栅格不会受影响，但是raise处理过的栅格，这时就要考虑新出现的Q对其的影响，就要重新计算最近障碍物（可能是Q）的距离，所以raise过程结束，转变为lower过程（图2C）。
当raise和lower都不再进展，DM更新结束。在DM更新的过程中，GVD会同步更新，我会在接下来的代码中展示GVD的更新过程。障碍物的移动，也可以分解为原位置的障碍物消失、新位置的障碍物出现的过程。因为更新不会遍历所有的栅格（比如最外层的栅格，其最近的障碍物一定是地图边界，无需更新也不会更新），所以这是一个增量更新的过程，访问栅格少，实时性好。
2. Voronoi数据结构
// queues
//保存待考察的栅格
BucketPrioQueue<INTPOINT> open_;
//保存待剪枝的栅格
std::queue<INTPOINT> pruneQueue_;
//保存预处理后的待剪枝的栅格
BucketPrioQueue<INTPOINT> sortedPruneQueue_;
//保存移除的障碍物曾占据的栅格
std::vector<INTPOINT> removeList_;
//保存增加的障碍物要占据的栅格
std::vector<INTPOINT> addList_;
//保存上次添加的障碍物覆盖的栅格
std::vector<INTPOINT> lastObstacles_;
// maps
int sizeY_;
int sizeX_;
dataCell** data_; //保存了每个栅格与最近障碍物的距离、最近障碍物的坐标、是否Voronoi点的标志
bool** gridMap_; //true是被占用，false是没有被占用
bool allocatedGridMap_; //是否为gridmap分配了内存的标志位DM和GVD的栅格用dataCell二维数组表示，gridMap_是输入的二值占据栅格地图。
struct dataCell {
float dist;
char voronoi; //State的枚举值
char queueing; //QueueingState的枚举值
int obstX;
int obstY;
bool needsRaise;
int sqdist;
};使用到的枚举型状态量如下，最终state是voronoiKeep 的点，便是Voronoi的边上的点，组成了Voronoi图。QueueingState 的含义我没有搞明白，但是不妨碍理解算法的思路和流程。
typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State;
//下面这几个枚举状态没搞懂
typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState;
typedef enum {invalidObstData = SHRT_MAX/2} ObstDataState;
//表示剪枝操作时栅格的临时状态
typedef enum {pruned, keep, retry} markerMatchResult;3. 地图数据初始化
//输入二值地图gridmap，根据元素是否被占用，更新data_
void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
gridMap_ = _gridMap;
initializeEmpty(_sizeX, _sizeY, false);
for (int x=0; x<sizeX_; x++) {
for (int y=0; y<sizeY_; y++) {
if (gridMap_[x][y]) { //如果gridmap_中的(x,y)被占用了
dataCell c = data_[x][y];
if (!isOccupied(x,y,c)) { //如果c没有被占用，即data_中的(x,y)没被占用，需要更新
bool isSurrounded = true; //如果在gridmap_中的邻居元素全被占用
for (int dx=-1; dx<=1; dx++) {
int nx = x+dx;
if (nx<=0 || nx>=sizeX_-1) continue;
for (int dy=-1; dy<=1; dy++) {
if (dx==0 && dy==0) continue;
int ny = y+dy;
if (ny<=0 || ny>=sizeY_-1) continue;
if (!gridMap_[nx][ny]) { //如果在gridmap_中的邻居元素有任意一个没被占用（就是障碍物边界点）
isSurrounded = false;
break;
}
}
}
if (isSurrounded) { //如果九宫格全部被占用
c.obstX = x;
c.obstY = y;
c.sqdist = 0;
c.dist=0;
c.voronoi=occupied;
c.queueing = fwProcessed;
data_[x][y] = c;
} else {
setObstacle(x,y); //不同之处在于：将(x,y)加入addList_
}
}
}
}
}
}initializeEmpty()主要清空历史数据，为数组开辟内存空间，并赋初始值，将所有栅格设置为不被占用。然后，当gridMap_中的某栅格被占用，而data_中的该栅格却没被占用时，表示环境发生了变化，才需要更新栅格的信息。因为这是初始化操作，不会出现gridMap_中的某栅格不被占用、而data_中的该栅格却被占用的情况。如果一个栅格的8个邻居栅格全被占用，说明该栅格在障碍物内部，只需简单赋值，不会触发lower过程。如果8个邻居栅格没有全被占用，说明该栅格在障碍物边界上，调用setObstacle()，暂存会触发DM更新的点。
4. 添加障碍物
//要同时更新gridmap和data_
void DynamicVoronoi::occupyCell(int x, int y) {
gridMap_[x][y] = 1; //更新gridmap
setObstacle(x,y);
}//只更新data_
void DynamicVoronoi::setObstacle(int x, int y) {
dataCell c = data_[x][y];
if(isOccupied(x,y,c)) { //如果data_中的(x,y)被占用
return;
}
addList_.push_back(INTPOINT(x,y)); //加入addList_
c.obstX = x;
c.obstY = y;
data_[x][y] = c;
}对应文章中下图部分。
5. 移除障碍物
//要同时更新gridmap和data_
void DynamicVoronoi::clearCell(int x, int y) {
gridMap_[x][y] = 0; //更新gridmap
removeObstacle(x,y);
}//只更新data_
void DynamicVoronoi::removeObstacle(int x, int y) {
dataCell c = data_[x][y];
if(isOccupied(x,y,c) == false) { //如果data_中的(x,y)没有被占用，无需处理
return;
}
removeList_.push_back(INTPOINT(x,y)); //将(x,y)加入removeList_
c.obstX = invalidObstData;
c.obstY = invalidObstData;
c.queueing = bwQueued;
data_[x][y] = c;
}对应文章中下图部分。
commitAndColorize()分别处理addList_ 和 removeList_ 中的栅格，将它们加入open_优先队列。commitAndColorize()运行后，才算完成了图3-4所列的添加、移除障碍物。接下来才会更新DM。
//将发生状态变化（占用<-->不占用）的元素加入open_优先队列
void DynamicVoronoi::commitAndColorize(bool updateRealDist) {
//addList_和removeList_中是触发Voronoi更新的元素，因此都要加入open_
// ADD NEW OBSTACLES
//addList_中都是障碍物边界点
for (unsigned int i=0; i<addList_.size(); i++) {
INTPOINT p = addList_[i];
int x = p.x;
int y = p.y;
dataCell c = data_[x][y];
if(c.queueing != fwQueued){
if (updateRealDist) {
c.dist = 0;
}
c.sqdist = 0;
c.obstX = x;
c.obstY = y;
c.queueing = fwQueued; //已加入open_优先队列
c.voronoi = occupied;
data_[x][y] = c;
open_.push(0, INTPOINT(x,y)); //加入open_优先队列，加入open_的都是要更新的
}
}
// REMOVE OLD OBSTACLES
//removeList_中是要清除的障碍物栅格
for (unsigned int i=0; i<removeList_.size(); i++) {
INTPOINT p = removeList_[i];
int x = p.x;
int y = p.y;
dataCell c = data_[x][y];
//removeList_中对应的元素在data_中已经更新过，解除了占用
//如果这里又出现了该元素被占用，说明是后来加入的，这里不处理
if (isOccupied(x,y,c) == true) {
continue; // obstacle was removed and reinserted
}
open_.push(0, INTPOINT(x,y)); //加入open_优先队列
if (updateRealDist) {
c.dist = INFINITY;
}
c.sqdist = INT_MAX;
c.needsRaise = true; //因为清除了障碍物，最近障碍物距离要更新-增加
data_[x][y] = c;
}
removeList_.clear();
addList_.clear();
}
6. 更新障碍物
//用新的障碍物信息替换旧的障碍物信息
//如果points为空，就是清除障碍物；
//初始时lastObstacles_为空，第一次调用exchangeObstacles()就是纯粹的添加障碍物
void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT>& points) {
for (unsigned int i=0; i<lastObstacles_.size(); i++) {
int x = lastObstacles_[i].x;
int y = lastObstacles_[i].y;
bool v = gridMap_[x][y];
if (v) { //如果(x,y)被占用了，不处理，怀疑这里逻辑反了。
continue; //要移除旧的障碍物，这里应该是(!v)表示没被占用就不处理，占用了就移除
}
removeObstacle(x,y);
}
lastObstacles_.clear();
for (unsigned int i=0; i<points.size(); i++) {
int x = points[i].x;
int y = points[i].y;
bool v = gridMap_[x][y];
if (v) { //如果(x,y)被占用了，不处理。否则，添加占用
continue;
}
setObstacle(x,y);
lastObstacles_.push_back(points[i]);
}
}exchangeObstacles()用来使用新的障碍物替换旧的障碍物。当地图刚刚初始化，exchangeObstacles()第一次调用时，因为没有旧的障碍物需要移除，这就是纯粹的添加障碍物。当exchangeObstacles()的输入参数为空时，因为没有新的障碍物要添加，这就是纯粹的移除障碍物。所以，外界环境的变化通过exchangeObstacles()传入，这是更新DM的触发点。
7. 更新DM
这是论文和代码的核心环节，主要分为lower()和raise() 2部分，对应图5-7。
void DynamicVoronoi::update(bool updateRealDist) {
//将发生状态变化（占用<-->不占用）的元素加入open_优先队列
commitAndColorize(updateRealDist);
while (!open_.empty()) {
INTPOINT p = open_.pop();
int x = p.x;
int y = p.y;
dataCell c = data_[x][y];
if(c.queueing==fwProcessed) {
continue;
}
if (c.needsRaise) {
// RAISE
//2层for循环，考察8个邻居栅格
for (int dx=-1; dx<=1; dx++) {
int nx = x+dx;
if (nx<=0 || nx>=sizeX_-1) continue;
for (int dy=-1; dy<=1; dy++) {
if (dx==0 && dy==0) continue;
int ny = y+dy;
if (ny<=0 || ny>=sizeY_-1) continue;
dataCell nc = data_[nx][ny];
//nc有最近障碍物 且 不raise
if (nc.obstX!=invalidObstData && !nc.needsRaise) {
//如果nc原来的最近障碍物消失了
if(!isOccupied(nc.obstX, nc.obstY, data_[nc.obstX][nc.obstY])) {
open_.push(nc.sqdist, INTPOINT(nx,ny));
nc.queueing = fwQueued; //fwQueued表示刚加入open_排队？
nc.needsRaise = true; //需要raise，并清理掉原来的最近障碍物信息
nc.obstX = invalidObstData;
nc.obstY = invalidObstData;
if (updateRealDist) {
nc.dist = INFINITY;
}
nc.sqdist = INT_MAX;
data_[nx][ny] = nc;
} else { //如果nc原来的最近障碍物还存在
if(nc.queueing != fwQueued){
open_.push(nc.sqdist, INTPOINT(nx,ny));
nc.queueing = fwQueued;
data_[nx][ny] = nc;
}
}
}
}
}
c.needsRaise = false;
c.queueing = bwProcessed; //bwProcessed表示8个邻居元素raise处理完毕？
data_[x][y] = c;
}
else if (c.obstX != invalidObstData &&
isOccupied(c.obstX, c.obstY, data_[c.obstX][c.obstY])) {
//c是被占据的
// LOWER
c.queueing = fwProcessed; //fwProcessed表示8个邻居元素lower处理完毕？
c.voronoi = occupied;
for (int dx=-1; dx<=1; dx++) {
int nx = x+dx;
if (nx<=0 || nx>=sizeX_-1) continue;
for (int dy=-1; dy<=1; dy++) {
if (dx==0 && dy==0) continue;
int ny = y+dy;
if (ny<=0 || ny>=sizeY_-1) continue;
dataCell nc = data_[nx][ny];
if(!nc.needsRaise) {
int distx = nx-c.obstX;
int disty = ny-c.obstY;
int newSqDistance = distx*distx + disty*disty;
//nc到c的最近障碍物 比 nc到其最近障碍物 更近
bool overwrite = (newSqDistance < nc.sqdist);
if(!overwrite && newSqDistance==nc.sqdist) {
//如果nc没有最近障碍物，或者 nc的最近障碍物消失了
if (nc.obstX == invalidObstData ||
isOccupied(nc.obstX, nc.obstY, data_[nc.obstX][nc.obstY]) == false) {
overwrite = true;
}
}
if (overwrite) {
open_.push(newSqDistance, INTPOINT(nx,ny));
nc.queueing = fwQueued; //fwQueued表示加入到open_等待lower()？
if (updateRealDist) {
nc.dist = sqrt((double) newSqDistance);
}
nc.sqdist = newSqDistance;
nc.obstX = c.obstX; //nc的最近障碍物 赋值为c的最近障碍物
nc.obstY = c.obstY;
} else {
checkVoro(x,y,nx,ny,c,nc);
}
data_[nx][ny] = nc;
}
}
}
}
data_[x][y] = c;
}
}updata()先调用了commitAndColorize()，将状态发生了翻转变化（占用变成不占用、不占用变成占用）的栅格加入open_优先队列，然后遍历open_中的元素，并调用checkVoro()判断其是否属于Voronoi图边上的点。
8. 属于Voronoi的条件
void DynamicVoronoi::checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc) {
if ((c.sqdist>1 || nc.sqdist>1) && nc.obstX!=invalidObstData) {
if (abs(c.obstX-nc.obstX) > 1 || abs(c.obstY-nc.obstY) > 1) {
//compute dist from x,y to obstacle of nx,ny
int dxy_x = x-nc.obstX;
int dxy_y = y-nc.obstY;
int sqdxy = dxy_x*dxy_x + dxy_y*dxy_y;
int stability_xy = sqdxy - c.sqdist;
if (sqdxy - c.sqdist<0) return;
//compute dist from nx,ny to obstacle of x,y
int dnxy_x = nx - c.obstX;
int dnxy_y = ny - c.obstY;
int sqdnxy = dnxy_x*dnxy_x + dnxy_y*dnxy_y;
int stability_nxy = sqdnxy - nc.sqdist;
if (sqdnxy - nc.sqdist <0) return;
//which cell is added to the Voronoi diagram?
if(stability_xy <= stability_nxy && c.sqdist>2) {
if (c.voronoi != free) {
c.voronoi = free;
reviveVoroNeighbors(x,y);
pruneQueue_.push(INTPOINT(x,y));
}
}
if(stability_nxy <= stability_xy && nc.sqdist>2) {
if (nc.voronoi != free) {
nc.voronoi = free;
reviveVoroNeighbors(nx,ny);
pruneQueue_.push(INTPOINT(nx,ny));
}
}
}
}
}这段代码基本是复现图8的算法，不同之处在于，在检测(x,y)、(nx,ny)是否Voronoi备选点的同时，也把这2个点的各自8个邻居栅格也进行了检测，通过reviveVoroNeighbors()实现。通过检测的备选点加入到pruneQueue_ 中。因为还会对pruneQueue_ 中的元素进行剪枝操作，以得到精细准确的、单像素宽度的Voronoi边，pruneQueue_ 只是中间过程的存储容器，所以无需使用优先队列，只是普通的std::queue就可以。
图8中使用了6个判断条件，分别是：
- 第1条：s和n至少有一个不紧邻其obs，若都紧邻，无法判断s和n是不是GVD
- 第2条：n的最近obs是存在的，若不存在，无法判断s和n是不是GVD
- 第3条：s和n的最近obs是不同的，若相同，无法判断s和n是不是GVD
- 第4条：s的最近obs和n的最近obs不紧邻，若紧邻，则同属一个obs，无法判断s和n是不是GVD
- 第5-6条：属于GVD的点，一定是距周边obs最近的，所以倾向于选择距obs更近的点作为候选。
//将符合条件的(x,y)的邻居栅格也添加到需剪枝的Voronoi备选中
void DynamicVoronoi::reviveVoroNeighbors(int &x, int &y) {
for (int dx=-1; dx<=1; dx++) {
int nx = x+dx;
if (nx<=0 || nx>=sizeX_-1) continue;
for (int dy=-1; dy<=1; dy++) {
if (dx==0 && dy==0) continue;
int ny = y+dy;
if (ny<=0 || ny>=sizeY_-1) continue;
dataCell nc = data_[nx][ny];
if (nc.sqdist != INT_MAX && !nc.needsRaise &&
(nc.voronoi == voronoiKeep || nc.voronoi == voronoiPrune)) {
nc.voronoi = free;
data_[nx][ny] = nc;
pruneQueue_.push(INTPOINT(nx,ny));
}
}
}
}9. 剪枝
prune()的主要目的是将2个栅格宽度的Voronoi边精简为1个栅格宽度，分为2步，对应代码中的2个while()循环。
第1，遍历pruneQueue_，用图9中的模式去匹配每个元素，及该元素上下左右紧邻的4个栅格。若匹配成功，就加入sortedPruneQueue_，等待剪枝。这一步的目的是将被2条相距很近的Voronoi边包裹的单个栅格加入到备选中。
第2，遍历sortedPruneQueue_，用图10中的左侧2个模式或者右侧2个模式去匹配每个元素，匹配的过程由markerMatch()完成。若匹配的结果是pruned，该栅格被剪枝；keep，该栅格就是Voronoi图上的点；retry，将该栅格重新加入到pruneQueue_。注意，第1步完成后，pruneQueue_已经空了。如果sortedPruneQueue_第一次遍历完毕，会将pruneQueue_中的需要retry的元素转移到sortedPruneQueue_中，继续执行第2步的遍历，直到sortedPruneQueue_为空。
void DynamicVoronoi::prune() {
// filler
//先遍历pruneQueue_中的元素，判断是否要加入到sortedPruneQueue_，
//这一步的目的是合并紧邻的Voronoi边，将2条边夹着的栅格也设置为备选
//再遍历sortedPruneQueue_中的元素，判断其是剪枝、保留、重试。
while(!pruneQueue_.empty()) {
INTPOINT p = pruneQueue_.front();
pruneQueue_.pop();
int x = p.x;
int y = p.y;
if (data_[x][y].voronoi==occupied) continue; //如果(x,y)是occupied，无需处理，不可能是Voronoi
//如果(x,y)是freeQueued，已经加入到sortedPruneQueue_，略过
if (data_[x][y].voronoi==freeQueued) continue;
data_[x][y].voronoi = freeQueued;
sortedPruneQueue_.push(data_[x][y].sqdist, p);
/* tl t tr
l c r
bl b br */
dataCell tr,tl,br,bl;
tr = data_[x+1][y+1];
tl = data_[x-1][y+1];
br = data_[x+1][y-1];
bl = data_[x-1][y-1];
dataCell r,b,t,l;
r = data_[x+1][y];
l = data_[x-1][y];
t = data_[x][y+1];
b = data_[x][y-1];
//文章只提了对待考察栅格判断是否符合模式，这里为什么要对待考察栅格的上下左右4个邻居栅格都判断呢？
//我认为判断模式的目的就是将Voronoi边夹着的、包裹的栅格置为备选，因为待考察栅格是备选了，
//才使得周围栅格可能会被Voronoi边包裹，所以才要逐一检查。
if (x+2<sizeX_ && r.voronoi==occupied) {
// fill to the right
//如果r的上下左右4个元素都!=occupied，对应文章的P38模式
// | ? | 1 | ? |
// | 1 | | 1 |
// | ? | 1 | ? |
if (tr.voronoi!=occupied && br.voronoi!=occupied && data_[x+2][y].voronoi!=occupied) {
r.voronoi = freeQueued;
sortedPruneQueue_.push(r.sqdist, INTPOINT(x+1,y));
data_[x+1][y] = r;
}
}
if (x-2>=0 && l.voronoi==occupied) {
// fill to the left
//如果l的上下左右4个元素都!=occupied
if (tl.voronoi!=occupied && bl.voronoi!=occupied && data_[x-2][y].voronoi!=occupied) {
l.voronoi = freeQueued;
sortedPruneQueue_.push(l.sqdist, INTPOINT(x-1,y));
data_[x-1][y] = l;
}
}
if (y+2<sizeY_ && t.voronoi==occupied) {
// fill to the top
//如果t的上下左右4个元素都!=occupied
if (tr.voronoi!=occupied && tl.voronoi!=occupied && data_[x][y+2].voronoi!=occupied) {
t.voronoi = freeQueued;
sortedPruneQueue_.push(t.sqdist, INTPOINT(x,y+1));
data_[x][y+1] = t;
}
}
if (y-2>=0 && b.voronoi==occupied) {
// fill to the bottom
//如果b的上下左右4个元素都!=occupied
if (br.voronoi!=occupied && bl.voronoi!=occupied && data_[x][y-2].voronoi!=occupied) {
b.voronoi = freeQueued;
sortedPruneQueue_.push(b.sqdist, INTPOINT(x,y-1));
data_[x][y-1] = b;
}
}
}
while(!sortedPruneQueue_.empty()) {
INTPOINT p = sortedPruneQueue_.pop();
dataCell c = data_[p.x][p.y];
int v = c.voronoi;
if (v!=freeQueued && v!=voronoiRetry) {
continue;
}
markerMatchResult r = markerMatch(p.x,p.y);
if (r==pruned) {
c.voronoi = voronoiPrune; //对(x,y)即c剪枝
}
else if (r==keep) {
c.voronoi = voronoiKeep; //对(x,y)即c保留，成为Voronoi的边
}
else {
c.voronoi = voronoiRetry;
pruneQueue_.push(p);
}
data_[p.x][p.y] = c;
//把需要retry的元素由pruneQueue_转移到sortedPruneQueue_
//这样可以继续本while()循环，直到pruneQueue_和sortedPruneQueue_都为空
if (sortedPruneQueue_.empty()) {
while (!pruneQueue_.empty()) {
INTPOINT p = pruneQueue_.front();
pruneQueue_.pop();
sortedPruneQueue_.push(data_[p.x][p.y].sqdist, p);
}
}
}
}10. 栅格模式匹配
观察图10的4个模式 ，很容易理解为什么这样设计，因为在这4个模式中，栅格s有非常重要的联结作用，不可或缺，否则Voronoi边就会断掉。因此，符合模式的栅格会被保留，不符合的被剪枝。
//根据(x,y)邻居栅格的连接模式，判断是否要对(x,y)剪枝
DynamicVoronoi::markerMatchResult DynamicVoronoi::markerMatch(int x, int y) {
// implementation of connectivity patterns
bool f[8];
int nx, ny;
int dx, dy;
int i=0;
//voroCount是对所有邻居栅格的统计，voroCountFour是对上下左右4个邻居栅格的统计
int voroCount=0;
int voroCountFour=0;
for (dy=1; dy>=-1; dy--) {
ny = y+dy;
for (dx=-1; dx<=1; dx++) {
if (dx || dy) { //不考虑(x,y)点
nx = x+dx;
dataCell nc = data_[nx][ny];
int v = nc.voronoi;
bool b = (v<=free && v!=voronoiPrune); //既不是occupied又不是voronoiPrune，即可能保留的栅格
f[i] = b;
if (b) {
voroCount++;
if (!(dx && dy)) { //对上下左右4个点
voroCountFour++;
}
}
i++;
}
}
}
// i和位置的对应关系如下：
// | 0 | 1 | 2 |
// | 3 | | 4 |
// | 5 | 6 | 7 |
//8个邻居栅格中最多有2个，上下左右只有1个可能保留的栅格
if (voroCount<3 && voroCountFour==1 && (f[1] || f[3] || f[4] || f[6])) {
return keep;
}
// 4-connected
// | 0 | 1 | ? | | ? | 1 | 0 | | ? | ? | ? | | ? | ? | ? |
// | 1 | | ? | | ? | | 1 | | 1 | | ? | | ? | | 1 |
// | ? | ? | ? | | ? | ? | ? | | 0 | 1 | ? | | ? | 1 | 0 |
//对应《Efficient Grid-Based Spatial Representations for Robot Navigation in Dynamic Environments》
//中的4-connected P14模式，旋转3次90度
if ((!f[0] && f[1] && f[3]) || (!f[2] && f[1] && f[4]) || (!f[5] && f[3] && f[6]) ||
(!f[7] && f[6] && f[4])) {
return keep;
}
// | ? | 0 | ? | | ? | 1 | ? |
// | 1 | | 1 | | 0 | | 0 |
// | ? | 0 | ? | | ? | 1 | ? |
//对应文章中的4-connected P24模式，旋转1次90度
if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4])) return keep;
// keep voro cells inside of blocks and retry later
//(x,y)周围可能保留的栅格很多，此时无法判断是否要对(x,y)剪枝
if (voroCount>=5 && voroCountFour>=3 && data_[x][y].voronoi!=voronoiRetry) {
return retry;
}
return pruned;
}11. Voronoi图可视化
void DynamicVoronoi::visualize(const char *filename) {
FILE* F = fopen(filename, "w");
...
//fputc()执行3次，其实是依次对一个像素的RGB颜色赋值
for(int y = sizeY_-1; y >=0; y--){
for(int x = 0; x<sizeX_; x++){
unsigned char c = 0;
if (...) {
...
} else if(isVoronoi(x,y)){ //画Voronoi边
fputc( 0, F );
fputc( 0, F );
fputc( 255, F );
} else if (data_[x][y].sqdist==0) { //填充障碍物
fputc( 0, F );
fputc( 0, F );
fputc( 0, F );
} else { //填充Voronoi区块内部
float f = 80+(sqrt(data_[x][y].sqdist)*10);
if (f>255) f=255;
if (f<0) f=0;
c = (unsigned char)f;
fputc( c, F );
fputc( c, F );
fputc( c, F );
}
}
}
fclose(F);
}依次对每个像素的RGB通道赋值，一个典型的二值图输入（图11）和Voronoi图输出（图12-13）如下。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
