datetimepicker 更新值无效_文献阅读之Voronoi图的生成与更新

通俗的说，在机器人导航方面，Voronoi图是一种将地图分区的方法，分区的界限即Voronoi图的边，常用作机器人远离障碍物避障的规划路径。本文主要参考了 Boris Lau 的论文和代码，对Voronoi图的生成和更新进行分析。相关的3篇论文内容重合度比较高，我主要以《Efficient Grid-Based Spatial Representations for Robot Navigation in Dynamic Environments》为主。对代码的理解和注释，我已在GitHub上开源，欢迎大家一起讨论。

Boris Lau 的论文和代码www2.informatik.uni-freiburg.deComments for Voronoigithub.com

1. DM的更新概述

在机器人路径规划和避障的过程中，我们常常需要知道某个时刻机器人与最近障碍物的距离，以远离障碍物，或者进行碰撞检测。论文提出使用Distance Map（DM）和 Generalized Voronoi Diagrams（GVD）来解决这个问题。DM的建立和更新是GVD建立和更新的前提，方法来源于改进的brushfire算法，过程如图1-2所示。DM的每个栅格都会保存与最近障碍物点的距离，以及障碍物点的坐标（因此，障碍物的内部点是被忽略的，只有轮廓点被考察）。

图1A是论文算法的输入——已知的二值占据栅格地图，其中外围的黑色是地图外部区域，中间的黑色是障碍物，白色是可行驶区域。因为有边界和障碍物的存在，使得内部白色栅格与最近障碍物点的距离会减小（初始化为正无穷），因此要从障碍物栅格开始，逐步向外扩散更新，计算新的最近障碍物坐标与距离，反映为图1B-D中灰色逐步扩展，距离越近颜色越深。当所有栅格都被更新后，DM建立完成。

图1 建立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 更新DM

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;//如果在gridmap_中的邻居元素有任意一个没被占用（就是障碍物边界点）if (!gridMap_[nx][ny]) {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;     //更新gridmapsetObstacle(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;
}

图3

5. 移除障碍物

//要同时更新gridmap和data_
void DynamicVoronoi::clearCell(int x, int y) {gridMap_[x][y] = 0;     //更新gridmapremoveObstacle(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;
}

图4

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。

图5

图6

图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有最近障碍物 且 不raiseif (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是被占据的// LOWERc.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,nyint 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,yint 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更近的点作为候选。

图8

//将符合条件的(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_为空。

图9

图10

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;//如果(x,y)是occupied，无需处理，不可能是Voronoiif (data_[x][y].voronoi==occupied) continue;//如果(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 trl c rbl 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个元素都!=occupiedif(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个元素都!=occupiedif(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个元素都!=occupiedif(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 patternsbool 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;//既不是occupied又不是voronoiPrune，即可能保留的栅格bool b = (v<=free && v!=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）如下。