通俗的說，在機器人導航方面，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 Voronoi?github.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

圖7void 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

圖10void 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）如下。

圖11 二值地圖輸入

圖12 prune前的Voronoi圖

圖13 prune后的Voronoi圖

總結

以上是生活随笔為你收集整理的datetimepicker 更新值无效_文献阅读之Voronoi图的生成与更新的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。