系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！
本文先对**Trapezoidal Decomposition梯形分解算法（TCD）**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
Trapezoidal Decomposition梯形分解算法（TCD）
原理
Trapezoidal Decomposition是最简单的精确细胞分解方法。该方法先使机器人沿着空间的边缘行走一圈，构建整个区域地图，然后用一条竖直的切割线从左至右扫描过整个区域，当切割线与多边形障碍物的顶点相遇时，就会分解出一个子区域，这样整个自由空间就被分解为诸多子区域，每个子区域都是梯形。梯形分解法的时间复杂度通常为O(n log n)。机器人在每个子区域中进行往复运动对子区域进行覆盖。
（1）第一种原理
（2）第二种原理
梯形分解方法假设一条垂直线段(称为slice)从左到右扫过由多边形障碍物填充的有界环境。单元格cell是通过一系列打开和关闭操作形成的
事件类型
当slice遇到一个事件时,有三种类型的事件:IN、OUT和MIDDLE。
1、在IN事件中,当前单元格关闭(从而完成其构造),两个新单元格打开(从而启动其构造)(图2)。
2、OUT事件正好相反:两个单元格关闭,一个新单元格打开(图3)。
IN事件可以看作是一个单元格分解成两个单元格,而OUT事件是两个单元格合并成一个单元格。
3、在MIDDLE事件中,当前单元格关闭,一个新单元格形成。这些操作的结果是自由空间被分解成梯形单元格。
（3）第三种原理
通过cell的面积最大化来确定cell的真正边界
代码实现
cpp
# Find a path avoiding obstacles using Vertical Cell Decomposition
# Author -- Shikhar Dev Gupta
import sys
from helpers.graph import *
from helpers.geometry import *;
import matplotlib.pyplot as plt
# Check for empty lines
file_handler = open("input_file","r");
raw_data = file_handler.read();
raw_data = raw_data.split("\n");
if(len(raw_data) <2):
print("Incorrect format of the input file");
exit;
def parse_input_line(line):
temp2 = [];
line = [i.strip() for i in line.split(",")];
vertex = [];
for index,i in enumerate(line):
if(i[0] == "("):
i = i[1:];
if(i[len(i)-1] == ")"):
i= i[:-1];
vertex.append(int(i));
if(index%2 != 0):
temp2.append(vertex);
vertex = [];
return temp2;
# Draw the obstacles and point the source and the destination----------------------------------------------
def draw_problem():
bnd_x = [i.x for i in boundary];
bnd_x.append(boundary[0].x);
bnd_y = [i.y for i in boundary];
bnd_y.append(boundary[0].y);
poly_x = [];
poly_y = []
# Draw the boundary
plt.plot(bnd_x, bnd_y);
for index, i in enumerate(obstacles):
poly_x.append([p[0] for p in i]);
poly_y.append([p[1] for p in i]);
plt.fill( poly_x[index], poly_y[index], color="#512DA8");
plt.plot(source.x, source.y, marker="o");
plt.plot(dest.x, dest.y, marker="o");
plt.annotate('Source', xy=(source.x, source.y), xytext=(source.x+5, source.y-6) );
plt.annotate('Destination', xy=(dest.x, dest.y), xytext=(dest.x-4, dest.y-10) );
# Extract vertices----------------------------------------------
temp = parse_input_line(raw_data[0]);
boundary = [point(i[0], i[1]) for i in temp];
# Extract source and dest
temp = parse_input_line(raw_data[len(raw_data)-1]);
source = point(temp[0][0], temp[0][1]);
dest = point(temp[1][0], temp[1][1]);
# Extract obstacles
obstacles = [];
for i in raw_data[1:len(raw_data)-1]:
obstacles.append(parse_input_line(i) );
#sort by x-values
sorted_vertices = [];
for index,i in enumerate(obstacles):
for j in i:
j.append(index);
sorted_vertices.append(j);
sorted_vertices.sort(key=lambda x: x[0]);
# Draw the problem
draw_problem();
new_sorted_vertices = [];
for i in sorted_vertices:
temp = point(i[0], i[1], i[2]);
new_sorted_vertices.append(temp);
new_obstacles = [];
for index, i in enumerate(obstacles):
temp_obs = [];
for j in i:
temp = point(j[0], j[1], index);
temp_obs.append(temp);
new_obstacles.append(temp_obs);
#-----------------------------------------------------------
# Find vertical lines
open_line_segments = [];
y_limit_lower = boundary[0].y;
y_limit_upper = boundary[2].y;
for pt in new_sorted_vertices:
curr_line_segment = [ point(pt.x, y_limit_lower), point(pt.x, y_limit_upper) ];
lower_obs_pt = curr_line_segment[0];
upper_obs_pt = curr_line_segment[1];
upper_gone = False;
lower_gone = False;
break_now = False;
# Find intersection points with the vertical proposed lines. the intersection function returns false if segments are same, so no need to worry about same segment checking
for index,obs in enumerate(new_obstacles):
# Add the first point again for the last line segment of a polygon.
obs.append( obs[0] );
for vertex_index in range(len(obs)-1 ):
res = segment_intersection( curr_line_segment[0], curr_line_segment[1], obs[vertex_index], obs[vertex_index+1]);
if (res!=-1):
if ( index == pt.obstacle ):
if pt.equals( res ) == False:
if ( res.y > pt.y ):
upper_gone = True;
elif ( res.y < pt.y ):
lower_gone = True;
else:
if pt.equals( res ) == False:
if ( upper_gone is False ):
if ( (res.y > pt.y) and res.y < (upper_obs_pt.y) ):
upper_obs_pt = res;
if ( lower_gone is False ):
if ( (res.y < pt.y) and (res.y > lower_obs_pt.y) ):
lower_obs_pt = res;
if( upper_gone is True and lower_gone is True ):
break_now = True;
#No need to check for current point anymore...completely blocked
if(break_now is True):
break;
# Draw the vertical cell lines
if(lower_gone is False):
plt.plot( [lower_obs_pt.x, pt.x], [lower_obs_pt.y, pt.y] );
if(upper_gone is False):
plt.plot( [pt.x, upper_obs_pt.x], [pt.y, upper_obs_pt.y] );
# Add to the global segment list
if (lower_gone and upper_gone):
open_line_segments.append([None, None]);
elif (lower_gone):
open_line_segments.append([None, upper_obs_pt]);
elif (upper_gone):
open_line_segments.append([lower_obs_pt, None]);
else:
open_line_segments.append([lower_obs_pt, upper_obs_pt]);
#------------------------------------------------------
# Find Polygon cells naiively. Will improve next.
cells = [];
for index1 in range(len(open_line_segments) ):
curr_segment = open_line_segments[index1];
curr_vertex = new_sorted_vertices[index1];
break_now = False;
done = [False, False, True];
if( curr_segment[0] is None ):
done[0] = True;
if( curr_segment[1] is None ):
done[1] = True;
if( curr_segment[1] is None and open_line_segments[index1][0] is None):
done[2] = False;
for index2 in range(index1+1, len(open_line_segments) ):
next_segment = open_line_segments[index2];
next_vertex = new_sorted_vertices[index2];
double_index1 = -2;
double_index2 = -2;
lines_to_check = [];
trapezoids = [];
double_check = False;
if ( next_segment[0] is not None and next_segment[1] is not None ):
double_check = True;
if( done[0] is False ):
if( double_check ):
double_index1 = len(lines_to_check);
lines_to_check.append( [centroid([curr_segment[0], curr_vertex]), centroid([next_segment[0], next_vertex]), 0]);
lines_to_check.append( [centroid([curr_segment[0], curr_vertex]), centroid([next_segment[1], next_vertex]), 0]);
trapezoids.append([ curr_segment[0], next_segment[0], next_vertex, curr_vertex ]);
trapezoids.append([ curr_segment[0], next_vertex, next_segment[1], curr_vertex ]);
elif ( next_segment[0] is not None ):
lines_to_check.append( [centroid([curr_segment[0], curr_vertex]), centroid([next_segment[0], next_vertex]), 0]);
trapezoids.append([ curr_segment[0], next_segment[0], next_vertex, curr_vertex ]);
elif( next_segment[1] is not None ):
lines_to_check.append( [centroid([curr_segment[0], curr_vertex]), centroid([next_segment[1], next_vertex]), 0]);
trapezoids.append([ curr_segment[0], next_vertex, next_segment[1], curr_vertex ]);
else:
lines_to_check.append( [centroid([curr_segment[0], curr_vertex]), next_vertex, 0]);
trapezoids.append([ curr_segment[0], next_vertex, curr_vertex ]);
if( done[1] is False ):
if( double_check ):
double_index2 = len(lines_to_check);
lines_to_check.append( [centroid([curr_segment[1], curr_vertex]), centroid([next_segment[0], next_vertex]), 1]);
lines_to_check.append( [centroid([curr_segment[1], curr_vertex]), centroid([next_segment[1], next_vertex]), 1]);
trapezoids.append([ curr_vertex, next_segment[0], next_vertex , point(curr_segment[1].x, curr_segment[1].y,curr_segment[1].obstacle, 34)]);
trapezoids.append([ curr_vertex, next_vertex, next_segment[1], curr_segment[1] ]);
elif ( next_segment[1] is not None ):
lines_to_check.append( [centroid([curr_segment[1], curr_vertex]), centroid([next_segment[1], next_vertex]), 1]);
trapezoids.append([ curr_vertex, next_vertex, next_segment[1], curr_segment[1] ]);
elif( next_segment[0] is not None ):
lines_to_check.append( [centroid([curr_segment[1], curr_vertex]), centroid([next_segment[0], next_vertex]), 1]);
trapezoids.append([ curr_vertex, next_segment[0], next_vertex , curr_segment[1] ]);
else:
lines_to_check.append( [centroid([curr_segment[1], curr_vertex]), next_vertex, 1]);
trapezoids.append([ curr_vertex, next_vertex, curr_segment[1] ]);
if( done[2] is False ):
if(double_check):
double_index = len(lines_to_check);
lines_to_check.append( [curr_vertex, centroid([next_segment[0], next_vertex]), 2]);
trapezoids.append([ curr_vertex,next_segment[0], next_vertex ]);
lines_to_check.append( [curr_vertex, centroid([next_segment[1], next_vertex]), 2]);
trapezoids.append([ curr_vertex, next_vertex, next_segment[1] ]);
elif ( next_segment[0] is not None ):
lines_to_check.append( [curr_vertex, centroid([next_segment[0], next_vertex]), 2]);
trapezoids.append([ curr_vertex,next_segment[0], next_vertex ]);
elif( next_segment[1] is not None ):
lines_to_check.append( [curr_vertex, centroid([next_segment[1], next_vertex]), 2]);
trapezoids.append([ curr_vertex, next_vertex, next_segment[1] ]);
# Will this ever occur though??
else:
lines_to_check.append( [curr_vertex, next_vertex, 2]);
trapezoids.append([curr_vertex, next_vertex]);
temp_to_remove = [];
for index5,q in enumerate(lines_to_check):
ok = [True, True, True];
for index3,obs in enumerate(new_obstacles):
# Add the last line to make closed polygon
obs.append( obs[0] );
for index4 in range(len(obs)-1):
if (segment_intersection( q[0], q[1], obs[index4], obs[index4+1]) != -1):
ok[q[2]] = False;
if(index5 not in temp_to_remove):
temp_to_remove.append(index5);
if ( ok[q[2]] is True ):
done[q[2]] = True;
for i in range(len(lines_to_check)):
if i not in temp_to_remove:
cells.append(trapezoids[i]);
if( done[0] == True and done[1] == True and done[2] == True ):
break;
to_draw =[];
for i in cells:
i.append(i[0]);
to_draw.append(i);
#-------------------------------------------------------
# Merge overlapping Polygons
quad_cells = [i for i in cells if len(i)>3];
tri_cells = [i for i in cells if len(i)==3];
others = [i for i in cells if len(i)<3];
quads_to_remove = [];
quads_to_add = [];
quads_to_remove = [];
quads_to_add = [];
for index_cell in range(len(quad_cells)):
for index_cell2,cell in enumerate(quad_cells):
if(index_cell != index_cell2):
if(quad_cells[index_cell][0].x == cell[0].x and quad_cells[index_cell][1].x == cell[1].x):
temp1 = list(quad_cells[index_cell]);
temp1.append(temp1[0]);
temp2 = list(cell);
temp2.append(temp2[0]);
area1 = polygon_area(temp1,4); area2 = polygon_area(temp2,4);
new_quad=[];
new_quad.append( point(temp1[0].x, min(temp1[0].y, temp2[0].y)) );
new_quad.append( point(temp1[1].x, min(temp1[1].y, temp2[1].y)) );
new_quad.append( point(temp1[1].x, max(temp1[2].y, temp2[2].y)) );
new_quad.append( point(temp1[0].x, max(temp1[3].y, temp2[3].y)) );
new_quad.append( point(temp1[0].x, min(temp1[0].y, temp2[0].y)) );
area3 = polygon_area(new_quad, 4);
if( area1 + area2 >= area3):
#merge
quads_to_remove.append(index_cell);
quads_to_remove.append(index_cell2);
quads_to_add.append(new_quad);
quads_to_remove = list(set(quads_to_remove));
for index in sorted(quads_to_remove, reverse=True):
del quad_cells[index];
for i in quads_to_add:
quad_cells.append(i);
# Remove duplicates
to_remove = [];
for index1 in range(len(quad_cells)):
for index2 in range(index1+1, len(quad_cells)):
duplicate = True;
for k,m in zip(quad_cells[index1], quad_cells[index2]):
if k.equals(m) is False:
duplicate = False;
break;
if(duplicate is True):
if index2 not in to_remove:
to_remove.append(index2);
for index in sorted(to_remove, reverse=True):
del quad_cells[index];
# One more pass to remove extra quads generated because of cross - segments
quads_to_remove = [];
for index1 in range(len(quad_cells)):
for index2 in range(len(quad_cells)):
if(index1 != index2 and quad_cells[index1][0].x == quad_cells[index2][0].x and quad_cells[index1][1].x == quad_cells[index2][1].x):
if( (quad_cells[index1][0].y<= quad_cells[index2][0].y) and (quad_cells[index1][1].y<= quad_cells[index2][1].y)
and (quad_cells[index1][2].y>= quad_cells[index2][2].y) and (quad_cells[index1][3].y >= quad_cells[index2][3].y)):
quads_to_remove.append(index2);
quads_to_remove = list(set(quads_to_remove) );
for index in sorted(quads_to_remove, reverse=True):
del quad_cells[index];
#------------------------------------------------------
# Add boundary lines
if( boundary[0].x != new_sorted_vertices[0].x):
quad_cells.append([boundary[0], point(new_sorted_vertices[0].x, y_limit_lower), point(new_sorted_vertices[0].x, y_limit_upper), boundary[3]]);
if( boundary[1].x != new_sorted_vertices[len(new_sorted_vertices)-1].x):
quad_cells.append([point(new_sorted_vertices[len(new_sorted_vertices)-1].x ,y_limit_lower), boundary[1], boundary[2], point(new_sorted_vertices[len(new_sorted_vertices)-1].x, y_limit_upper) ]);
#-------------------------------------------------------
# Plot final cells
to_draw = quad_cells+tri_cells+others;
for i in to_draw:
x = [j.x for j in i];
y = [j.y for j in i];
plt.plot(x, y);
#----------------------------------------------------------------------
# Get the graph
graph_vertices = [];
graph_edges = [];
for index1 in range(len(quad_cells)):
same_boundary = [];
for index2 in range(len(quad_cells)):
if(index1 != index2):
if( (quad_cells[index1][1].x == quad_cells[index2][0].x ) and ((quad_cells[index1][2].y in [quad_cells[index2][0].y, quad_cells[index2][3].y]) or (quad_cells[index1][1].y in [quad_cells[index2][0].y, quad_cells[index2][3].y]) ) ):
same_boundary.append(index2);
temp = quad_cells[index1][0:4];
centroid_vertex = centroid(temp);
place = centroid_vertex.find_point(graph_vertices)
if( place == -1):
graph_vertices.append(centroid_vertex);
if(len(same_boundary)==1):
temp_edge_middle = centroid([quad_cells[index1][1], quad_cells[index1][2]]);
graph_vertices.append(temp_edge_middle);
n = len(graph_vertices)-1;
if(place != -1):
graph_edges.append([place, n]);
else:
graph_edges.append([n-1, n]);
temp = quad_cells[same_boundary[0]][0:4];
curr_centroid_vertex = centroid(temp);
place2 = curr_centroid_vertex.find_point(graph_vertices);
if( place2 == -1 ):
graph_vertices.append(curr_centroid_vertex);
graph_edges.append([n, n+1]);
else:
graph_edges.append([n, place2]);
elif(len(same_boundary)>1):
n = len(graph_vertices)-1;
if(place != -1):
use = place;
else:
use = n;
for index, i in enumerate(same_boundary):
temp = quad_cells[i][0:4];
curr_centroid_vertex = centroid(temp);
temp_edge_middle = centroid([quad_cells[i][0], quad_cells[i][3]]);
graph_vertices.append(temp_edge_middle);
pl1 =len(graph_vertices)-1;
hmmm= curr_centroid_vertex.find_point(graph_vertices);
if (hmmm == -1):
graph_vertices.append(curr_centroid_vertex);
pl2 =len(graph_vertices)-1;
else:
pl2 = hmmm;
graph_edges.append([use, pl1]);
graph_edges.append([pl1, pl2]);
# Add source and dest to graph
# Find the smallest distance vertex on graph and see if its clear to traverse
# Source------------------------------
min_ind = -1; min = 9999999;
for index, i in enumerate(graph_vertices):
if( check_obstruction(new_obstacles, [source, i]) is True ):
dist = find_dist(i, source);
if( dist < min):
min = dist;
min_ind = index;
graph_vertices.append(source);
m = len(graph_vertices)-1;
graph_edges.append([min_ind, m]);
# Destination------------------------------------
min_ind = -1; min = 9999999;
for index, i in enumerate(graph_vertices):
if( check_obstruction(new_obstacles, [dest, i]) is True ):
dist = find_dist(i, dest);
if( dist < min):
min = dist;
min_ind = index;
graph_vertices.append(dest);
m = len(graph_vertices)-1;
graph_edges.append([min_ind, m]);
# Convert graph in adjacency list format
graph = [];
for j in range(len(graph_vertices)):
graph.append([]);
for i in graph_edges:
if(i[0]==j):
graph[j].append(i[1]);
elif(i[1]==j):
graph[j].append(i[0]);
path = bfs(graph, len(graph_vertices)-2, len(graph_vertices)-1);
if(path is None):
print "No path found. Sorry";
sys.exit();
else:
print "Path found." ;
# Draw everything--------------
for index,i in enumerate(graph_vertices):
plt.annotate(str(index), xy=(i.x, i.y), xytext=(i.x+2, i.y-2) );
# plt.plot(i.x,i.y, marker="x");
for i in graph_edges:
temp_x = [graph_vertices[i[0]].x, graph_vertices[i[1]].x];
temp_y = [graph_vertices[i[0]].y, graph_vertices[i[1]].y];
plt.plot(temp_x,temp_y);
# draw path
temp_x = [graph_vertices[i].x for i in path];
temp_y = [graph_vertices[i].y for i in path];
plt.plot(temp_x,temp_y, color="#0F0F0F", linewidth=2);
#----------------------------------------------------
# output into a file
file_output = open("vertical_cell_output", "w" );
str_to_write = "";
for index in range(len(graph_vertices)):
str_to_write = str_to_write + ", "+str(index)+":"+"("+ str(int(graph_vertices[index].x) )+ ", "+ str(int(graph_vertices[index].y) ) + ")";
str_to_write = str_to_write[1:];
total_write = str_to_write+"\n";
str_to_write="";
for i in graph:
if (i == []):
continue;
str_to_write = str_to_write + ",(";
for j in i:
str_to_write = str_to_write + str(j) + ",";
str_to_write = str_to_write[:-1];
str_to_write = str_to_write + ")";
str_to_write = str_to_write[1:];
total_write = total_write+ str_to_write + "\n";
str_to_write = "";
str_to_write =','.join(str(x) for x in path);
total_write = total_write + str_to_write;
file_output.write(total_write);
print "Output written to file.. Drawing the result";
plt.show();优缺点
产生大量的子区域,造成了大量的路径冗余