def polygons_from_txt(file_name):
file = open(file_name, "r")
line = file.readline()
array_polygons = []
amount_polygons = 0
while line != "#":
if line == "QUANTITY":
amount_polygons = int(file.readline())
line = amount_polygons
elif line == "VERTICES (X,Y)":
line = file.readline()
array_points_tuple = []
while line != "\n":
points = line.split()
array_points_tuple.append((float(points[0]), float(points[1])))
line = file.readline()
array_points_tuple = Polygon.set_points_to_positive(
array_points_tuple)
for i in range(amount_polygons):
array_polygons.append(
Polygon.create_polygon(np.array(array_points_tuple)))
else:
line = file.readline().strip()
file.close()
return array_polygons, return_limits_of_board_txt(file_name)
def decomposeWithOverlappingPoint(self,polygon):
"""
When there are points overlapping each other in a given polygon
First decompose this polygon into sub-polygons at the overlapping point
"""
# recursively break the polygon at any overlap point into two polygons until no overlap points are found
# here we are sure there is only one contour in the given polygon
ptDic = {}
overlapPtIndex = None
# look for overlap point and stop when one is found
for i,pt in enumerate(polygon[0]):
if pt not in ptDic:
ptDic[pt]=[i]
else:
ptDic[pt].append(i)
overlapPtIndex = ptDic[pt]
break
if overlapPtIndex:
polyWithoutOverlapNode = []
# break the polygon into sub-polygons
newPoly = Polygon.Polygon(polygon[0][overlapPtIndex[0]:overlapPtIndex[1]])
polyWithoutOverlapNode.extend(self.decomposeWithOverlappingPoint(newPoly))
reducedPoly = Polygon.Polygon(decomposition.removeDuplicatePoints((polygon-newPoly)[0]))
polyWithoutOverlapNode.extend(self.decomposeWithOverlappingPoint(reducedPoly))
else:
# no overlap point is found
return [polygon]
return polyWithoutOverlapNode
def landmark_contained(landmark, landmarks):
if (len(landmarks) == 0):
return False, None
if (len(landmark.points) > 0):
l_query = Polygon.Polygon(landmark.points)
else:
l_query = None
for i, l in enumerate(landmarks):
if (len(l.points) == 0 and len(landmark.points) == 0):
return True, i
elif (len(l.points) == 0 or len(landmark.points) == 0):
continue
l_curr = Polygon.Polygon(l.points)
l_int = l_curr & l_query
l_un = l_curr | l_query
if (l_un.area() == 0.0):
continue
if ((l_int.area() / l_un.area()) > 0.25):
return True, i
return False, None
def create_dock(L, eL, xL, dL, g, n):
dock = []
# Creating periodic pattern of docklets :
for i in range(n):
for j in range(n):
lablet = Polygon.Polygon(((i*dL, j*dL), (L+i*dL, j*dL), (L+i*dL, L+j*dL), (i*dL, L+j*dL)))
poly = [Polygon.Polygon(((xL+i*dL, xL+j*dL), (xL+eL+i*dL, xL+j*dL), (xL+eL+i*dL, xL+eL+j*dL), (xL+i*dL, xL+eL+j*dL))),
Polygon.Polygon(((L-(xL+eL)+i*dL, xL+j*dL), (L-xL+i*dL, xL+j*dL), (L-xL+i*dL, xL+eL+j*dL), (L-(xL+eL)+i*dL, xL+eL+j*dL))),
Polygon.Polygon(((L-(xL+eL)+i*dL, L-(xL+eL)+j*dL), (L-xL+i*dL, L-(xL+eL)+j*dL), (L-xL+i*dL, L-xL+j*dL), (L-(xL+eL)+i*dL, L-xL+j*dL))),
Polygon.Polygon(((xL+i*dL, L-(xL+eL)+j*dL), (xL+eL+i*dL, L-(xL+eL)+j*dL), (xL+eL+i*dL, L-xL+j*dL), (xL+i*dL, L-xL+j*dL)))]
dock.append(poly)
# print len(dock)
gAll = g * (n**2)
dock = list(itertools.chain(*dock))
# Plotting active surfaces of Dock
dockX = dock[1]
for i in range(len(dock)):
dockX = dockX | dock[i]
Polygon.IO.writeSVG('dock.svg', (dockX))
# print zip(*[dock, gAll])
return zip(*[dock, gAll])
def makeExteriorPolygons(tri, xRange, yRange):
"""Makes voronoi cell polygons on nodes that make up the convex hull.
Polygons are clipped to the bounding box.
"""
polyPoints = {}
nNodes = len(tri.x)
for iNode in tri.hull:
adjTriangles = getTrianglesForNode(iNode, tri)
cellVertices = [tri.circumcenters[adjTriangle] for adjTriangle in adjTriangles]
leadingNode = getLeadingNode(iNode, tri)
pcLead = getCircumcentreOfCommonTriangle(iNode, leadingNode, tri)
pdLead = makeExteriorPoint(iNode, leadingNode, pcLead, tri, xRange, yRange)
cellVertices.append(pdLead)
trailingNode = getTrailingNode(iNode, tri)
pcTrail = getCircumcentreOfCommonTriangle(iNode, trailingNode, tri)
pdTrail = makeExteriorPoint(iNode, trailingNode, pcTrail, tri, xRange, yRange)
cellVertices.append(pdTrail)
# cellVertices = convex_hull(cellVertices)
# extendedVoronoiPolygon = Polygon.Polygon((pcLead, pdLead, pdTrail, pcTrail))
# extendedVoronoiPolygon = Polygon.Polygon(cellVertices)
extendedVoronoiPolygon = Polygon.Utils.convexHull(Polygon.Polygon(cellVertices))
bbPolygon = Polygon.Polygon(((xRange[0],yRange[0]), (xRange[0],yRange[1]),
(xRange[1],yRange[1]), (xRange[1],yRange[0])))
clippedVoronoiPolygon = extendedVoronoiPolygon & bbPolygon
polyPoints[iNode] = Polygon.Utils.pointList(extendedVoronoiPolygon)
# polyPoints[iNode] = Polygon.Utils.pointList(clippedVoronoiPolygon)
return polyPoints
def __init__(self, name, cfg, x_offset=0, y_offset=100):
x0 = x_offset
y0 = y_offset
x1 = x0 + cfg.width
y1 = y0 + cfg.height
self.cfg = cfg
self.x0, self.y0 = x0, y0
self.x1, self.y1 = x1, y1
self.name = name
self.wall = Polygon.Polygon([
(x0, y0),
(x1, y0),
(x1, y1),
(x0, y1),
])
self.window = Polygon.Polygon([
(x0 + cfg.pad_x, y0 + cfg.pad_y),
(x1 - cfg.pad_x, y0 + cfg.pad_y),
(x1 - cfg.pad_x, y1 - cfg.pad_y),
(x0 + cfg.pad_x, y1 - cfg.pad_y),
])
self.result = Polygon.Polygon()
def decomposeToConvex(self):
print 'decomposing'
polygon = Polygon(self.points)
self.convexPolygons.extend(polygon.splitToConvex())
def detection_filtering(detections, groundtruths, threshold=0.5):
for gt_id, gt in enumerate(groundtruths):
if (gt[5] == '#') and (gt[1].shape[1] > 1):
gt_x = map(int, np.squeeze(gt[1]))
gt_y = map(int, np.squeeze(gt[3]))
gt_p = np.concatenate((np.array(gt_x), np.array(gt_y)))
gt_p = gt_p.reshape(2, -1).transpose()
gt_p = plg.Polygon(gt_p)
for det_id, detection in enumerate(detections):
detection = detection.split(',')
# detection = map(int, detection[0:-1])
detection = map(int, detection)
det_y = detection[0::2]
det_x = detection[1::2]
det_p = np.concatenate((np.array(det_x), np.array(det_y)))
det_p = det_p.reshape(2, -1).transpose()
det_p = plg.Polygon(det_p)
try:
# det_gt_iou = iod(det_x, det_y, gt_x, gt_y)
det_gt_iou = get_intersection(det_p, gt_p) / det_p.area()
except:
print(det_x, det_y, gt_x, gt_y)
if det_gt_iou > threshold:
detections[det_id] = []
detections[:] = [item for item in detections if item != []]
return detections
def remove_short_lines(self, minlen = 1): minlen*=minlen newp = Polygon() c=-1 for cont in self.poly: c+=1 #cont=self.poly[0] keeplist = [] lastp = None pindex = -1 for p in cont: pindex+=1 if lastp: #check here xd = p[0]-lastp[0] yd = p[0]-lastp[0] td = xd*xd+yd*yd if td < minlen: pass #lastp = None else: keeplist.append(p) lastp = p else: keeplist.append(p) lastp = p ishole = self.poly.isHole(c) newp.addContour(keeplist,ishole) self.poly = newp
def merc_poly(self):
"""like poly(), but transformed to mercator coordinates"""
p = self.poly()
mp = Polygon()
for i, c in enumerate(p):
mp.addContour(ll_to_xy(c), p.isHole(i))
return mp
def Intersect(rect1, rect2): #{{{
Width = rect1[2].x * 2
hw = Width/2 #half width
Height = rect1[2].y * 2
hh = Height/2 #half height
#A = math.radians(rect1[1])
A = rect1[1]
sina = math.sin(A)
cosa = math.cos(A)
ul1 = Vertex2(rect1[0].x - hw*cosa - hh*sina, rect1[0].y + hh*cosa - hw*sina)
ur1 = Vertex2(rect1[0].x + hw*cosa - hh*sina, rect1[0].y + hh*cosa + hw*sina)
dl1 = Vertex2(rect1[0].x - hw*cosa + hh*sina, rect1[0].y - hh*cosa - hw*sina)
dr1 = Vertex2(rect1[0].x + hw*cosa + hh*sina, rect1[0].y - hh*cosa + hw*sina)
Width = rect2[2].x * 2
hw = Width/2 #half width
Height = rect2[2].y * 2
hh = Height/2 #half height
#A = math.radians(rect2[1])
A = rect2[1]
sina = math.sin(A)
cosa = math.cos(A)
ul2 = Vertex2(rect2[0].x - hw * cosa - hh * sina, rect2[0].y + hh * cosa - hw*sina)
ur2 = Vertex2(rect2[0].x + hw * cosa - hh * sina, rect2[0].y + hh * cosa + hw*sina)
dl2 = Vertex2(rect2[0].x - hw * cosa + hh * sina, rect2[0].y - hh * cosa - hw*sina)
dr2 = Vertex2(rect2[0].x + hw * cosa + hh * sina, rect2[0].y - hh * cosa + hw*sina)
points1 = [ul1, ur1, dr1, dl1]
points2 = [ul2, ur2, dr2, dl2]
p1 = Polygon(points1)
p2 = Polygon(points2)
return p1.intersects(p2)
def __init__(self,
inpoints,
startpoints=None,
endpoints=None,
rotations=None):
if len(inpoints) > 2:
self.poly = Polygon.Polygon(inpoints)
else:
self.poly = Polygon.Polygon()
self.points = inpoints #for 3 axes, this is only storage of points. For N axes, here go the sampled points
if startpoints:
self.startpoints = startpoints #from where the sweep test begins, but also retract point for given path
else:
self.startpoints = []
if endpoints:
self.endpoints = endpoints
else:
self.endpoints = [] #where sweep test ends
if rotations:
self.rotations = rotations
else:
self.rotations = [] #rotation of the machine axes
self.closed = False
self.children = []
self.parents = []
#self.unsortedchildren=False
self.sorted = False #if the chunk has allready been milled in the simulation
self.length = 0
#this is total length of this chunk.
self.zstart = 0 # this is stored for ramps mainly, because they are added afterwards, but have to use layer info
self.zend = 0 #
def CutLineInside(Poly, Line):
P = Polygon.Polygon(Poly)
dx = 1e-4
PLine = np.array(Line.tolist() + Line.tolist()[::-1]).reshape((4, 2))
#PLine[2,0]+=dx
#PLine[3,0]+=2*dx
PLine[2:, :] += np.random.randn(2, 2) * 1e-6
P0 = Polygon.Polygon(PLine)
PP = np.array(P0 & P)[0].tolist()
PP.append(PP[0])
B0 = GiveB(Line[0], Line[1])
B = [GiveB(PP[i], PP[i + 1]) for i in range(len(PP) - 1)]
PLine = []
for iB in range(len(B)):
d = np.sum((B[iB] - B0)**2)
print d, PP[iB], PP[iB + 1]
if d == 0:
PLine.append([PP[i], PP[i + 1]])
LOut = np.array(PLine[0])
return LOut
def getPolyCSpace(self, polygon):
segments = []
obs = self.polygon.copy()
rob = polygon.copy()
for i in obs.segments:
segments.append((i.copy(), i.normalAngle("left"),"o"))
for j in rob.segments:
segments.append((j.copy(), j.normalAngle("right"),"r"))
sorted_segments = sorted(segments, key= itemgetter(1))
if sorted_segments[0][2] == "o":
sorted_segments[0][0].reverse()
for i in xrange(1, len(sorted_segments)):
if sorted_segments[i][2] == "o":
print sorted_segments[i][0]
sorted_segments[i][0].reverse()
sorted_segments[i][0].move(sorted_segments[i-1][0].p2)
vertices =[]
for s in sorted_segments:
vertices.append(s[0].p1.toTuple())
tempPoly = Polygon(self.polygon.window, vertices)
delta = self.findDelta(tempPoly)
tempPoly.moveDelta(delta[0],delta[1])
return tempPoly
def carve_shaped_hole(self, pos1=None, pos2=None, \
material_width=0.0, radius=0.0, \
circle_points=32):
if self.poly == None:
self.make_poly()
bounds = self.get_bounds()
# hollow entire interior (longitudinal axis) at cut radius +
# corner radius. This like the center 'cut' line if we were
# cutting with a 'radius' radius tool.
top = self.project_contour(surf="top", \
xstart=bounds[0][0], \
xend=bounds[1][0], \
ysize=material_width+radius)
bot = self.project_contour(surf="bottom", \
xstart=bounds[0][0], \
xend=bounds[1][0], \
ysize=material_width+radius)
top.reverse()
shape = top + bot
mask1 = Polygon.Polygon(shape)
# vertical column (narrowed by radius)
xstart = self.get_xpos(pos1) + radius
xend = self.get_xpos(pos2) - radius
shape = []
shape.append((xstart, bounds[0][1]))
shape.append((xend, bounds[0][1]))
shape.append((xend, bounds[1][1]))
shape.append((xstart, bounds[1][1]))
mask2 = Polygon.Polygon(shape)
# combined the shared area of the two hollowed shapes.
# Essentially if we sweep a circle centered on the enge of
# this polygon, the outer bounds of that cut is the final
# shape we want
mask_cut = mask1 & mask2
# pretend we are cutting by placing a 'radius' size circle at
# each point in the cut line and taking the union of all of
# those (incrementally)
mask = None
for p in mask_cut[0]:
circle = Polygon.Shapes.Circle(radius=radius,
center=p,
points=circle_points)
if mask == None:
mask = circle
else:
mask = Polygon.Utils.convexHull(mask | circle)
mask = self.reduce_degeneracy(mask)
mask = Polygon.Utils.convexHull(mask)
#for p in mask:
# print "contour..."
# print p
self.poly = self.poly - mask
def GetArea(M):
A = M.fragment_1
B = M.fragment_2
poly_a = Polygon.Polygon(A.points[0])
poly_b = Polygon.Polygon(B.points[0])
return poly_a.area() + poly_b.area()
def sizeTestExample():
import pickle
p = Polygon('testpoly.gpf')
p.write('sizetest.gpf')
tlen = len(open('sizetest.gpf', 'r').read())
blen = len(dumpBinary(p))
xlen = len(dumpXML(p))
plen = len(pickle.dumps(p))
print "Text(gpc): %d, Binary: %d, XML: %d, Pickle: %d" % (tlen, blen, xlen, plen)
def setUp(self):
star = Polygon.polyStar(radius=2.0,
center=(1.0, 3.0),
nodes=5,
iradius=1.4)
circle = Polygon.polyStar(radius=1.0, center=(1.0, 3.0), nodes=64)
self.cookie = star - circle
self.cont = ((0.0, 0.0), (2.0, 1.0), (1.0, 0.0), (-2.0, 1.0), (0.0,
0.0))
def evaluation(submit_file, eval_ann, threshold=0.1, confidence=0.3):
assert osp.isfile(submit_file)
assert osp.isfile(eval_ann)
with open(eval_ann, 'r', encoding='utf-8') as f:
gt_annotations = json.loads(f.read(), object_pairs_hook=OrderedDict)
with open(submit_file, 'r', encoding='utf-8') as f:
preds = json.loads(f.read(), object_pairs_hook=OrderedDict)
tp, fp, npos = 0, 0, 0
""" for validation the annotation is same as the trainset. """
for name, pred_ann in preds.items():
gt_annotation = gt_annotations[name] # is a list
# npos should filter the ignored box.
# npos += len(gt_annotation)
for gt_polygon_tran in gt_annotation:
if gt_polygon_tran["illegibility"]:
continue
npos += 1
cover = set()
# compare each pred_polygon with each gt_polygon
for pred_polygon_pro in pred_ann:
pred_polygon = np.array(
pred_polygon_pro["points"]) # shape: [n, 2]
pred_prob = np.float32(pred_polygon_pro["confidence"])
if pred_prob < confidence: # confidence threshold.
continue
pred_polygon = plg.Polygon(pred_polygon)
flag = False
is_ignore = False
for gt_id, gt_polygon_tran in enumerate(gt_annotation):
gt_illegibility = gt_polygon_tran["illegibility"]
gt_polygon = np.array(gt_polygon_tran["points"]).reshape(
-1, 2).astype(np.int64)
gt_polygon = plg.Polyogn(gt_polygon)
union = get_union(pred_polygon, gt_polygon)
intersection = get_intersection(pred_polygon, gt_polygon)
if intersection * 1.0 / union >= threshold and flag is False:
if gt_id not in cover:
flag = True
if gt_illegibility:
is_ignore = True
cover.add(gt_id)
if flag:
tp += 0.0 if is_ignore else 1.0
else:
fp += 1.0
precision = tp / (fp + tp)
recall = tp / npos # recall do not calculate the ignored ones.
hmean = 0 if (
precision +
recall) == 0 else 2 * precision * recall / (precision + recall)
print("p: {:f} r: {:f} h: {:f}".format(precision, recall, hmean))
def sizeTestExample():
import pickle
p = Polygon('testpoly.gpf')
p.write('sizetest.gpf')
tlen = len(open('sizetest.gpf', 'r').read())
blen = len(dumpBinary(p))
xlen = len(dumpXML(p))
plen = len(pickle.dumps(p))
print "Text(gpc): %d, Binary: %d, XML: %d, Pickle: %d" % (tlen, blen, xlen,
plen)
def clipVoronoiCells(cellDict, xRange, yRange):
"""docstring for clipVoronoiCells"""
bbPolygon = Polygon.Polygon(((xRange[0],yRange[0]), (xRange[0],yRange[1]),
(xRange[1],yRange[1]), (xRange[1],yRange[0])))
for i, cell in cellDict.iteritems():
print i, cell
extendedCell = Polygon.Polygon(cell)
clippedVoronoiPolygon = extendedCell & bbPolygon
cellDict[i] = Polygon.Utils.pointList(clippedVoronoiPolygon)
return cellDict
def sierpinskiTriangle(surface, centre, distFromCentre, isFilled): # create lists storing current and next set of triangles toBreak = [] brokenTriangles = [] # set width/fill of triangles width = 1 if isFilled: width = 0 # add first equilateral triangle defined by points A, B, and C pointA = centre - VectorInt(distFromCentre * Constants.SIN_120, distFromCentre * Constants.COS_120) pointB = centre - VectorInt(0, distFromCentre) pointC = centre - VectorInt(-1 * distFromCentre * Constants.SIN_120, distFromCentre * Constants.COS_120) toBreak.append(Polygon(surface, [pointA, pointB, pointC], Constants.COLOR_WHITE, width)) # continue to generate sierpinski triangles until break case is reached generate = True while generate: # for each sub-triangle in whole for triangle in toBreak: # get vector defined between two points vectorAB = triangle.coordToPointVector(triangle.coordinates[1]) - triangle.coordToPointVector(triangle.coordinates[0]) # if next set of triangles would be indistinguishable, stop generating if vectorAB.length() < 4: generate = False break # otherwise, get vertices of current triangle vertexA = triangle.coordToPointVector(triangle.coordinates[0]) vertexB = triangle.coordToPointVector(triangle.coordinates[1]) vertexC = triangle.coordToPointVector(triangle.coordinates[2]) # get vectors defined by other 2 sides of triangle vectorBC = vertexC - vertexB vectorAC = vertexC - vertexA # find bisecting points (x, y, z) of each side bisectorX = vertexA + vectorAB.scale(0.5) bisectorY = vertexB + vectorBC.scale(0.5) bisectorZ = vertexA + vectorAC.scale(0.5) # create / rescale triangles using vertices and bisectors brokenTriangles.append(Polygon(surface, [bisectorX, vertexB, bisectorY], Constants.COLOR_WHITE, width)) brokenTriangles.append(Polygon(surface, [bisectorZ, bisectorY, vertexC], Constants.COLOR_WHITE, width)) triangle.coordinates[1] = triangle.pointVectorToCoord(bisectorX) triangle.coordinates[2] = triangle.pointVectorToCoord(bisectorZ) brokenTriangles.append(triangle) # set next loop to generate sierpinski's triangle from new triangles toBreak = brokenTriangles # return list of 'broken' triangles to draw return brokenTriangles
def rect_nms(bbox_rects, bbox_scores, nms_thr=0.5):
"""
implemented for NMS of rect bboxes.
:param bbox_rects: rects which generated by get_text_cc.
:param bbox_scores:the corresponding confidence scores.
:param nms_thr: the filter threshold.
:return:
"""
assert len(bbox_rects) == len(bbox_scores)
bbox_rects = np.asarray(bbox_rects)
if len(bbox_rects) < 2:
return bbox_rects
bbox_scores = np.asarray(bbox_scores)
idx = np.argsort(bbox_scores)[::-1]
bbox_scores = bbox_scores[idx]
bbox_rects = bbox_rects[idx]
# use the bbox with max score to filter the small score bboxes.
# if the IoU > 0.7 then filter the small ones.
# first transform the rect to polygons.
rect_polygons = []
rect_areas = []
for index in range(len(bbox_rects)):
box = bbox_rects[index]
polygon = plg.Polygon(box)
rect_polygons.append(polygon)
rect_areas.append(polygon.area)
# to parallel deal with this task
areas = np.asarray(rect_areas)
idx = areas > 0
bbox_rects = bbox_rects[idx]
bbox_scores = bbox_scores[idx]
# sort by bbox_scores
_, order = torch.sort(bbox_scores, dim=0, descending=True)
order_indices = order
keep = []
while len(order_indices) > 0:
# filter for each bbox
keep.append(order_indices[0])
poly1 = plg.Polygon(bbox_rects[order_indices[0]])
tmp_rects = [bbox_rects[i] for i in order_indices[1:]]
iou = np.asarray(
list(
map(
lambda x: get_intersection(poly1, get_polygon(x)) /
get_union(poly1, get_polygon(x)), tmp_rects)))
order_indices = order_indices[1:][iou < nms_thr]
bbox_rects = bbox_rects[keep]
bbox_scores = bbox_scores[keep]
return bbox_rects, bbox_scores
def eval_detection(opts, net=None):
if net == None:
net = OctShuffleMLT(attention=True)
net_utils.load_net(opts.model, net)
if opts.cuda:
net.cuda()
images, gt_boxes = load_annotation(opts.eval_list)
true_positives = 0
false_positives = 0
false_negatives = 0
for i in range(images.shape[0]):
image = np.expand_dims(images[i], axis=0)
image_boxes_gt = np.array(gt_boxes[i])
im_data = net_utils.np_to_variable(image, is_cuda=opts.cuda).permute(0, 3, 1, 2)
seg_pred, rboxs, angle_pred, features = net(im_data)
rbox = rboxs[0].data.cpu()[0].numpy()
rbox = rbox.swapaxes(0, 1)
rbox = rbox.swapaxes(1, 2)
angle_pred = angle_pred[0].data.cpu()[0].numpy()
segm = seg_pred[0].data.cpu()[0].numpy()
segm = segm.squeeze(0)
boxes = get_boxes(segm, rbox, angle_pred, opts.segm_thresh)
if (opts.debug):
print(boxes.shape)
print(image_boxes_gt.shape)
print("============")
false_positives += boxes.shape[0]
false_negatives += image_boxes_gt.shape[0]
for box in boxes:
b = box[0:8].reshape(4,-1)
poly = Polygon.Polygon(b)
for box_gt in image_boxes_gt:
b_gt = box_gt[0:8].reshape(4,-1)
poly_gt = Polygon.Polygon(b_gt)
intersection = poly_gt | poly
union = poly_gt & poly
iou = (intersection.area()+1.0) / (union.area()+1.0)-1.0
if iou > 0.5:
true_positives+=1
false_negatives-=1
false_positives-=1
image_boxes_gt = np.array([bgt for bgt in image_boxes_gt if not np.array_equal(bgt, box_gt)])
break
print("tp: {} fp: {} fn: {}".format(true_positives, false_positives, false_negatives))
precision = true_positives / (true_positives+false_positives)
recall = true_positives / (true_positives+false_negatives)
f_score = 2*precision*recall/(precision+recall)
print("PRECISION: {} \t RECALL: {} \t F SCORE: {}".format(precision, recall, f_score))
def getOverLapArea(A,B,startA,endA,startB,endB,TB):
Bt=getTransformedFragment(B,TB)
Polygon.setDataStyle(Polygon.STYLE_NUMPY)
b_poly = Polygon.Polygon(Bt.points[0])
a_poly = Polygon.Polygon(A.points[0])
intersection_polygon=b_poly&a_poly
area = intersection_polygon.area()
return area
def getOverLapArea(A, B, startA, endA, startB, endB, TB):
Bt = getTransformedFragment(B, TB)
Polygon.setDataStyle(Polygon.STYLE_NUMPY)
b_poly = Polygon.Polygon(Bt.points[0])
a_poly = Polygon.Polygon(A.points[0])
intersection_polygon = b_poly & a_poly
area = intersection_polygon.area()
return area
def score_by_IC15(gts, tags, preds, th=0.5, tr=0.4, tp=0.4, tc=0.8, wr=1.0, wp=1.0):
num_gts = 0
num_preds = 0
# GTs
gt_list = []
for gt_id, gt in enumerate(gts):
gt = np.array(gt).reshape(-1)
gt = gt.reshape(int(gt.shape[0] / 2), 2)
gt_p = plg.Polygon(gt)
item = {'poly': gt_p, 'bbox': gt, 'valid': tags[gt_id], 'matches': []}
gt_list.append(item)
if tags[gt_id]:
num_gts += 1
# match predictions
pred_list = []
for pred_id, pred in enumerate(preds):
pred = np.array(pred).reshape(-1)
pred = pred.reshape(int(pred.shape[0] / 2), 2)
pred_p = plg.Polygon(pred)
item = {'poly': pred_p,'bbox': pred, 'valid': True, 'matches': []}
pred_list.append(item)
# match with ignored GTs
num_preds = len(pred_list)
num_igns = 0
for pred_id, pred_item in enumerate(pred_list):
pred_p = pred_item['poly']
for gt_id, gt_item in enumerate(gt_list):
gt_p = gt_item['poly']
if not gt_item['valid']:
inter = polygon_intersection(pred_p, gt_p)
pred_area = pred_p.area()
if inter * 1.0 / pred_area >= th:
pred_item['valid'] = False
num_igns += 1
break
num_preds -= num_igns
# match OO
precision = 0.0
recall = 0.0
# match OM One GT to Many dets
p, r = match_OM(gt_list, pred_list, tr, tc, wr)
precision += p
recall += r
# match MO Many GTs to One det
r, p = match_OM(pred_list, gt_list, tp, tc, wp)
precision += p
recall += r
# match OO
r, p = match_OO(pred_list, gt_list, th)
precision += p
recall += r
return (precision, num_preds, recall, num_gts, gt_list, pred_list)
def testCoverOverlap(self):
p1 = Polygon.polyStar(radius=1.0, nodes=6)
p2 = Polygon.Polygon(p1)
p2.scale(0.9, 0.9)
self.assertEqual(p1.covers(p2), 1)
self.assertEqual(p1.overlaps(p2), 1)
p2.shift(0.2, 0.2)
self.assertEqual(p1.covers(p2), 0)
self.assertEqual(p1.overlaps(p2), 1)
p2.shift(5.0, 0.0)
self.assertEqual(p1.covers(p2), 0)
self.assertEqual(p1.overlaps(p2), 0)
def active_area_process(embryo_list, mode='pca'):
active_area_list=[]
for egg in embryo_list:
poly = Polygon(egg)
bd = Boundary(egg)
bd.detect_head_tail(mode)
poly.detect_area(boundary=bd)
#poly.view_area()
active_area_list.append(poly)
return np.array(active_area_list)
def plg_overlaps(pred_boxes, gt_boxes):
pred_boxes, gt_boxes=preprocess_boxes(pred_boxes, gt_boxes)
boxes = [plg.Polygon(box.reshape((4,2))) for box in pred_boxes]
gts = [plg.Polygon(box.reshape((4,2))) for box in gt_boxes]
overlaps = np.zeros((len(boxes), len(gts)), dtype=np.float32)
for k in range(len(boxes)):
for n in range(len(gts)):
inter = boxes[k] & gts[n]
area_inter = inter.area() if len(inter)>0 else 0
area_union = boxes[k].area() + gts[n].area() - area_inter
overlaps[k,n] = area_inter / area_union
return overlaps
def plot_polygon(m, polygons, limit_x, limit_y, title):
for v in m.getVars():
if v.x == 1:
name = v.varName
name = name.split(" ")
i = int(name[0])
point = [int(name[1]), int(name[2])]
point[0] = point[0] - polygons[i][0][0]
point[1] = point[1] - polygons[i][0][1]
polygons[i] = Polygon.add_number_axis_x_y(polygons[i], point[0], point[1])
polygons = Polygon.create_polygons_to_plot(polygons)
visualiser = Visualizer(polygons, limit_x, limit_y, title)
visualiser.plot_polygons()
def testInit(self):
Polygon.setDataStyle(Polygon.STYLE_TUPLE)
# tuple
p = Polygon.Polygon(self.cont)
self.assertEqual(p[0], self.cont)
# list
p = Polygon.Polygon(list(self.cont))
self.assertEqual(p[0], self.cont)
if Polygon.withNumeric:
# array
import Numeric
p = Polygon.Polygon(Numeric.array(self.cont))
self.assertEqual(p[0], self.cont)
def testInit(self):
Polygon.setDataStyle(Polygon.STYLE_TUPLE)
# tuple
p = Polygon.Polygon(self.cont)
self.assertEqual(p[0], tuple(self.cont))
# list
p = Polygon.Polygon(self.cont)
self.assertEqual(p[0], tuple(self.cont))
if Polygon.withNumPy:
import numpy
a = numpy.array(self.cont)
p = Polygon.Polygon(a)
self.assertEqual(self.cont, list(p[0]))
def testInit(self):
Polygon.setDataStyle(Polygon.STYLE_TUPLE)
# tuple
p = Polygon.Polygon(self.cont)
self.assertEqual(p[0], self.cont)
# list
p = Polygon.Polygon(list(self.cont))
self.assertEqual(p[0], self.cont)
if Polygon.withNumeric:
# array
import Numeric
p = Polygon.Polygon(Numeric.array(self.cont))
self.assertEqual(p[0], self.cont)
def triangulate2(points):
if len(points) == 3:
return (points, )
if len(points) == 4:
return ((points[0], points[1], points[2]), (points[0], points[2],
points[3]))
import Polygon
q = Polygon.Polygon(points)
strips = Polygon.TriStrip(q)
tri = []
for t in strips:
for n in range(len(t) - 2):
tri.append((t[n], t[n + 1], t[n + 2]))
return tri
def intersection(g, p):
if USE_POLYGON:
g = Polygon.Polygon(g[:8].reshape((4, 2)))
p = Polygon.Polygon(p[:8].reshape((4, 2)))
inter = (g & p).area()
union = g.area() + p.area() - inter
if union == 0:
return 0
else:
return inter / union
else:
g = g[:8].reshape((4, 2))
p = p[:8].reshape((4, 2))
return poly_iou(g, p)
def reduceExample():
print('### Reduce points')
# read Polygon from file
p = Polygon('testpoly.gpf')
# use ireland only, I know it's contour 0
pnew = Polygon(p[0])
# number of points
l = len(pnew[0])
# get shift value to show many polygons in drawing
bb = pnew.boundingBox()
xs = 1.1 * (bb[1] - bb[0])
# list with polygons to plot
plist = [pnew]
labels = ['%d points' % l]
while l > 30:
# reduce points to the half
l = l // 2
print('Reducing contour to %d points' % l)
pnew = Polygon(reducePoints(pnew[0], l))
pnew.shift(xs, 0)
plist.append(pnew)
labels.append('%d points' % l)
# draw the results
writeSVG('ReducePoints.svg', plist, height=400, labels=labels)
if hasPDFExport:
writePDF('ReducePoints.pdf', plist)
def readShpPolygon(layer,fileName):# parameter fileName is the pathfile name without extension
indexName = fileName + '.shx'
shpFile = open(indexName,"rb")
fileLength = os.path.getsize(indexName)
polygonNum = (fileLength-100)/8
recordsOffset = []
print fileName
starttime = time.clock()
shpFile.seek(0)
s = shpFile.read(fileLength)
shpFile.close()
layer.minx, layer.miny, layer.maxx, layer.maxy = struct.unpack("<dddd",s[36:68])
pointer = 100
for i in range(0,polygonNum):
offset = struct.unpack('>i',s[pointer:pointer+4])
recordsOffset.append(offset[0]*2)
pointer += 8
shpFile.close()
shpFile = open(fileName+'.shp',"rb")
shpFile.seek(24)
s = shpFile.read(4)
header = struct.unpack(">i",s)
fileLength = header[0]*2
shpFile.seek(0)
s = shpFile.read(fileLength)
shpFile.close()
for offset in recordsOffset:
x, y = [], []
polygon = Polygon()
pointer = offset + 8 + 4
polygon.minx,polygon.miny,polygon.maxx,polygon.maxy = struct.unpack('dddd',s[pointer:pointer+32])
pointer = offset + 8 + 36
polygon.numParts, polygon.numPoints = struct.unpack('ii',s[pointer:pointer+8])
pointer += 8
str = ''
for i in range(polygon.numParts):
str = str+'i'
polygon.partsIndex = struct.unpack(str,s[pointer:pointer+polygon.numParts*4])
pointer += polygon.numParts*4
for i in range(polygon.numPoints):
pointx, pointy = struct.unpack('dd',s[pointer:pointer+16])
x.append(pointx)
y.append(pointy)
pointer+=16
polygon.x, polygon.y = x, y
layer.features.append(polygon)
ts = time.clock()-starttime
print ' %f seconds' % (ts)
def remove_some_pts(self, prop=.9): newp = Polygon() c=-1 for cont in self.poly: c+=1 cr = reducePoints(cont,int(len(cont)*prop)) ishole = self.poly.isHole(c) if len(cr)>2: newp.addContour(cr, ishole) parea = self.poly.area() nparea = newp.area() #areadiff = math.fabs(nparea-parea) arearatio = parea/nparea arearatio = math.fabs(arearatio-1) #print areadiff, arearatio if arearatio < 0.1: self.poly = newp
class Domen:
id = 0
polygon = Polygon()
def __init__(self, points, id=0):
self.polygon = Polygon(points)
def contains(self, point):
return self.polygon.isInside(point.x, point.y) == 1
def testCoverOverlap(self):
p1 = Polygon.polyStar(radius=1.0, nodes=6)
p2 = Polygon.Polygon(p1)
p2.scale(0.9, 0.9)
self.assertEqual(p1.covers(p2), 1)
self.assertEqual(p1.overlaps(p2), 1)
p2.shift(0.2, 0.2)
self.assertEqual(p1.covers(p2), 0)
self.assertEqual(p1.overlaps(p2), 1)
p2.shift(5.0, 0.0)
self.assertEqual(p1.covers(p2), 0)
self.assertEqual(p1.overlaps(p2), 0)
def reduceExample():
# read Polygon from file
p = Polygon('testpoly.gpf')
# use ireland only, I know it's contour 0
pnew = Polygon(p[0])
# number of points
l = len(pnew[0])
# get shift value to show many polygons in drawing
bb = pnew.boundingBox()
xs = 1.1 * (bb[1]-bb[0])
# list with polygons to plot
plist = [pnew]
while l > 30:
# reduce points to the half
l /= 2
print "Reducing contour to %d points" % l
pnew = Polygon(reducePoints(pnew[0], l))
pnew.shift(xs, 0)
plist.append(pnew)
# draw the results
print "Plotting ReduceExample.svg"
drawSVG(plist, height=400, ofile="ReduceTest.svg")
def reduceExample():
# read Polygon from file
p = Polygon('testpoly.gpf')
# use ireland only, I know it's contour 0
pnew = Polygon(p[0])
# number of points
l = len(pnew[0])
# get shift value to show many polygons in drawing
bb = pnew.boundingBox()
xs = 1.1 * (bb[1]-bb[0])
# list with polygons to plot
plist = [pnew]
while l > 30:
# reduce points to the half
l /= 2
print 'Reducing contour to %d points' % l
pnew = Polygon(reducePoints(pnew[0], l))
pnew.shift(xs, 0)
plist.append(pnew)
# draw the results
writeSVG('ReducePoints.svg', plist, height=400)
if hasPDFExport:
writePDF('ReducePoints.pdf', plist)
def testDataStyle(self):
p = Polygon.Polygon(self.cont)
# tuple
Polygon.setDataStyle(Polygon.STYLE_TUPLE)
self.assertEqual(p[0], tuple(self.cont))
# list
Polygon.setDataStyle(Polygon.STYLE_LIST)
self.assertEqual(p[0], self.cont)
if Polygon.withNumPy:
import numpy
Polygon.setDataStyle(Polygon.STYLE_NUMPY)
self.assertEqual(type(p[0]), numpy.ndarray)
def testDataStyle(self):
p = Polygon.Polygon(self.cont)
# tuple
Polygon.setDataStyle(Polygon.STYLE_TUPLE)
self.assertEqual(p[0], self.cont)
# list
Polygon.setDataStyle(Polygon.STYLE_LIST)
self.assertEqual(p[0], list(self.cont))
if Polygon.withNumeric:
# array
import Numeric
Polygon.setDataStyle(Polygon.STYLE_ARRAY)
self.assertEqual(p[0], Numeric.array(self.cont))
class Solid:
name = "solid"
id = 0
polygon = Polygon()
material = Material()
def __init__(self, points):
self.polygon = Polygon(points)
def fill(self, particles):
xmin, xmax, ymin, ymax = self.polygon.boundingBox()
d2 = 2 * self.material.d
x = xmin
while x < xmax:
x += d2
y = ymin
while y < ymax:
y += d2
if self.polygon.isInside(x, y):
id = len(particles)
particles.append(self.material.createParticle(x, y, id))
pass
def setInfo(self, name, id):
self.name = name
self.id = id
def setMaterial(self, material):
self.material = material
def scaleIn(self, scale):
self.polygon.scale(scale, scale, 0.0, 0.0)
def scaleOut(self, scale):
self.polygon.scale(1.0 / scale, 1.0 / scale, 0.0, 0.0)
def points(self):
return self.polygon[0]
def len(self):
return self.polygon.nPoints(0)
stat.write("\n")
stat.close()
lam_list=[]
#print "*** New Country! ***"
fline=f.readline().strip()
flist=fline.split(":")
if flist==[""]:break
country=flist[0].strip()
raw_points=flist[1].strip().split(";")
x_points=[float(raw_point.split(",")[0]) for raw_point in raw_points if raw_point!=""]
y_points=[float(raw_point.split(",")[1]) for raw_point in raw_points if raw_point!=""]
i_poly=Polygon([[x_points[i],y_points[i]] for i in range(len(x_points))])
i_area=i_poly.area()
print ("*** Point loaded for country %s." %(country))
x_points,y_points=pre(x_points,y_points)
if country in i_country_dict.keys():
i_country_dict[country]=i_country_dict[country]+len(x_points)
i_area_dict[country]=i_area_dict[country]+i_area
else:
i_country_dict[country]=len(x_points)
i_area_dict[country]=i_area
"""
# On vide la memoire
poly_start_1 = 0
poly_start_2 = 0
poly_f = 0
# On déplace le polygone n°5
i=5
x=0
while x<360 :
y=-90
print x
while y<90 :
path_poly_dir = path_BD_f + str(x) + '_' + str(y) + '_to_' + str(x + pas1) + '_' + str(y + pas1)
path_poly_1 = path_poly_dir +'/' + level + str(i) + '.dat'
poly_start_1 = Polygon(path_poly_1)
path_poly_f_dir = path_BD_f +'/'+ str(x) + '_' + str(y) + '_to_' + str(x + pas1) + '_' + str(y + pas1)
path_poly_f = path_poly_f_dir+ '/' + level + str(i) + '.dat'
#print path_poly_f
poly_start_1.write(path_poly_f)
y=y+pas1
x=x+pas1
# On vide la memoire
poly_start_1 = 0
from mrpolygons import mrpolygon, scalePolygon, polarPolygon2cartesian, transportPolygonGeometry, transportPolygon
import Polygon
Polygon.setTolerance(1e-3)
from Polygon import Polygon
from Polygon.Utils import fillHoles, prunePoints, reducePoints
import numpy
import os
import sys
import imp
from contiguity import weightsFromAreas
def line2pointIntersection(line,point,tol):
if line.distance(point) < tol:
return point
else:
return None
def line2mpointsIntersection(line,mpoints,tol):
result = []
for point in mpoints:
if line2pointIntersection(line,point,tol) != None:
result.append(point)
if len(result) >= 2:
intersection = result.sort(key=lambda x: Point(line.coords[0]).distance(Point(x)))
return result
class rimap():
def __init__(self,n=3600,N=30,alpha=[0.1,0.5],sigma=[1.1,1.4],dt=0.1,pg=0.01,pu=0.05,su=0.315,boundary=""):
"""Creates an irregular maps
:param n: number of areas
class motionControlHandler:
def __init__(self, proj, shared_data):
"""
Bug alogorithm motion planning controller
"""
# Information about the robot
## 1: Pioneer ; 2: 0DE
self.system = 1
#setting for plotting
##figure number
self.original_figure = 1
self.overlap_figure = 2
plt.figure(self.original_figure)
plt.figure(self.overlap_figure)
self.PLOT = True # plot with matplot
self.PLOT_M_LINE = True # plot m-line
self.PLOT_EXIT = True # plot exit point of a region
self.PLOT_OVERLAP = True # plot overlap area with the obstacle
# Get references to handlers we'll need to communicate with
self.drive_handler = proj.h_instance['drive']
self.pose_handler = proj.h_instance['pose']
if self.system == 1:
self.robocomm = shared_data['robocomm']
# Get information about regions
self.rfi = proj.rfi
self.coordmap_map2lab = proj.coordmap_map2lab
self.coordmap_lab2map = proj.coordmap_lab2map
self.last_warning = 0
###################################
########used by Bug algorithm######
###################################
# PARAMETERS
self.LeftRightFlag = 1 # originally set to right 1- right, 0 left
# Pioneer related parameters
self.PioneerWidthHalf = 0.20 # (m) width of Pioneer
self.PioneerLengthHalf = 0.25 # (m) lenght of Pioneer
self.ratioBLOW = 0.5 # blow up ratio of the pioneer box ######### 5 BOX
self.PioneerBackMargin= self.ratioBLOW*self.PioneerLengthHalf*2 # (in m)
self.range = 0.75 # (m) specify the range of the robot (when the normal circle range cannot detect obstacle)
self.obsRange = 0.50 # (m) range that says the robot detects obstacles
#self.shift = 0.20 # 0.15
## 2: 0DE
self.factorODE = 50
if self.system == 2:
self.PioneerWidthHalf = self.PioneerWidthHalf*self.factorODE
self.PioneerLengthHalf = self.PioneerLengthHalf*self.factorODE
self.range = self.range*self.factorODE
self.obsRange = self.obsRange*self.factorODE
self.PioneerBackMargin= self.PioneerBackMargin*self.factorODE
#build self.map with empty contour
self.map = Rectangle (1,1)
self.map -= self.map #Polygon built from the original map
#build self.ogr with empty contour
self.ogr = Rectangle (1,1) #Polygon built from occupancy grid data points
self.ogr -= self.ogr
self.freespace = None # contains only the boundary
self.map_work = None # working copy of the map. remove current region and next region
self.previous_current_reg = None # previous current region
self.currentRegionPoly = None # current region's polygon
self.nextRegionPoly = None # next region's polygon
self.q_g = [0,0] # goal point of the robot heading to
self.q_hit = [0,0] # location where the robot first detect an obstacle
self.boundary_following= False # tracking whether it is in boundary following mode
self.m_line = None # m-line polygon
self.trans_matrix = mat([[0,1],[-1,0]]) # transformation matrix for find the normal to the vector connecting a point on the obstacle to the robot
#self.corr_theta = pi/6
#self.corr_matrix = mat([[cos(self.corr_theta),-sin(self.corr_theta)],[sin(self.corr_theta),cos(self.corr_theta)]])
self.q_hit_count = 0
self.q_hit_Thres = 1000
self.prev_follow = [[],[]]
## Construct robot polygon (for checking overlap)
pose = self.pose_handler.getPose()
self.prev_pose = pose
self.robot = Rectangle(self.obsRange*3,self.PioneerBackMargin)
self.robot.shift(pose[0]-self.obsRange,pose[1]-2*self.PioneerLengthHalf)
self.robot = Circle(self.obsRange,(pose[0],pose[1])) - self.robot
self.robot.rotate(pose[2]-pi/2,pose[0],pose[1])
self.robot.shift(self.shift*cos(pose[2]),self.shift*sin(pose[2]))
#construct real robot polygon( see if there is overlaping with path to goal
self.realRobot = Rectangle(self.PioneerWidthHalf*2.5,self.PioneerLengthHalf*2 )
self.realRobot.shift(pose[0]-self.PioneerWidthHalf*1.25,pose[1]-self.PioneerLengthHalf*1)
self.realRobot.rotate(pose[2]-pi/2,pose[0],pose[1])
#constructing map with all the regions included
for region in self.rfi.regions:
if region.name.lower()=='freespace':
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
freespace = Polygon(regionPoints)
freespace_big= Polygon(regionPoints)
freespace_big.scale(1.2, 1.2)
self.freespace = freespace_big -freespace
self.map += freespace_big -freespace #without including freespace
else:
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.map += Polygon(regionPoints)
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.original_figure)
def gotoRegion(self, current_reg, next_reg, last=False):
"""
If ``last`` is True, we will move to the center of the destination region.
Returns ``True`` if we've reached the destination region.
"""
q_gBundle = [[],[]] # goal bundle
q_overlap = [[],[]] # overlapping points with robot range
pose = self.pose_handler.getPose() # Find our current configuration
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.original_figure)
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Check if Vicon has cut out
if math.isnan(pose[2]):
print "WARNING: No Vicon data! Pausing."
#self.drive_handler.setVelocity(0, 0) # So let's stop
time.sleep(1)
return False
###This part is run when the robot goes to a new region, otherwise, the original map will be used.
if not self.previous_current_reg == current_reg:
print 'getting into bug alogorithm'
#clean up the previous self.map
self.map_work = Polygon(self.map)
# create polygon list for regions other than the current_reg and the next_reg. This polygon will be counted as the obstacle if encountered.
if not self.rfi.regions[current_reg].name.lower() == 'freespace':
pointArray = [x for x in self.rfi.regions[current_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.map_work -= Polygon(regionPoints)
if not self.rfi.regions[next_reg].name.lower() == 'freespace':
pointArray = [x for x in self.rfi.regions[next_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.map_work -= Polygon(regionPoints)
# NOTE: Information about region geometry can be found in self.rfi.regions
# building current polygon and destination polygon
pointArray = [x for x in self.rfi.regions[next_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.nextRegionPoly = Polygon(regionPoints)
if self.rfi.regions[next_reg].name.lower()=='freespace':
for region in self.rfi.regions:
if region.name.lower() != 'freespace':
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.nextRegionPoly = self.nextRegionPoly - Polygon(regionPoints)
pointArray = [x for x in self.rfi.regions[current_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.currentRegionPoly = Polygon(regionPoints)
if self.rfi.regions[current_reg].name.lower()=='freespace':
for region in self.rfi.regions:
if region.name.lower() != 'freespace':
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.currentRegionPoly = self.currentRegionPoly - Polygon(regionPoints)
#set to zero velocity before finding the tranFace
self.drive_handler.setVelocity(0, 0)
if last:
transFace = None
else:
print "Current reg is " + str(self.rfi.regions[current_reg].name.lower())
print "Next reg is "+ str(self.rfi.regions[next_reg].name.lower())
for i in range(len(self.rfi.transitions[current_reg][next_reg])):
pointArray_transface = [x for x in self.rfi.transitions[current_reg][next_reg][i]]
transFace = asarray(map(self.coordmap_map2lab,pointArray_transface))
bundle_x = (transFace[0,0] +transFace[1,0])/2 #mid-point coordinate x
bundle_y = (transFace[0,1] +transFace[1,1])/2 #mid-point coordinate y
q_gBundle = hstack((q_gBundle,vstack((bundle_x,bundle_y))))
q_gBundle = q_gBundle.transpose()
# Find the closest face to the current position
max_magsq = 1000000
for tf in q_gBundle:
magsq = (tf[0] - pose[0])**2 + (tf[1] - pose[1])**2
if magsq < max_magsq:
pt1 = tf
max_magsq = magsq
transFace = 1
self.q_g[0] = pt1[0]
self.q_g[1] = pt1[1]
# Push the goal point to somewhere inside the next region to ensure the robot will go there.
if self.rfi.regions[current_reg].name.lower()=='freespace':
self.q_g = self.q_g+(self.q_g-asarray(self.nextRegionPoly.center()))/norm(self.q_g-asarray(self.nextRegionPoly.center()))*3*self.PioneerLengthHalf
if not self.nextRegionPoly.isInside(self.q_g[0],self.q_g[1]):
self.q_g = self.q_g-(self.q_g-asarray(self.nextRegionPoly.center()))/norm(self.q_g-asarray(self.nextRegionPoly.center()))*6*self.PioneerLengthHalf
else:
self.q_g = self.q_g+(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*3*self.PioneerLengthHalf
if self.currentRegionPoly.isInside(self.q_g[0],self.q_g[1]):
self.q_g = self.q_g-(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*6*self.PioneerLengthHalf
#plot exiting point
if self.PLOT_EXIT == True:
plt.figure(self.overlap_figure)
plt.clf()
plt.plot(q_gBundle[:,0],q_gBundle[:,1],'ko' )
plt.plot(self.q_g[0],self.q_g[1],'ro')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,1)
if transFace is None:
print "ERROR: Unable to find transition face between regions %s and %s. Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (self.rfi.regions[current_reg].name, self.rfi.regions[next_reg].name)
##################################################
#######check whether obstacle is detected#########
##################################################
#Update pose,update self.robot, self.realRobot orientation
self.robot.shift(pose[0]-self.prev_pose[0],pose[1]-self.prev_pose[1])
self.realRobot.shift(pose[0]-self.prev_pose[0],pose[1]-self.prev_pose[1])
self.robot.rotate(pose[2]-self.prev_pose[2],pose[0],pose[1])
self.realRobot.rotate(pose[2]-self.prev_pose[2],pose[0],pose[1])
self.prev_pose = pose
############################
########### STEP 1##########
############################
##Check whether obsRange overlaps with obstacle or the boundary
# for real Pioneer robot
if self.system == 1:
if self.boundary_following == False:
if self.robocomm.getReceiveObs() == False:
overlap = self.robot & ( self.map_work)
else:
overlap = self.robot & ( self.map_work | self.robocomm.getObsPoly())
else: #TRUE
# use a robot with full range all around it
Robot = Circle(self.obsRange,(pose[0],pose[1]))
Robot.shift(self.shift*cos(pose[2]),self.shift*sin(pose[2]))
if self.robocomm.getReceiveObs() == False:
overlap = Robot & ( self.map_work)
else:
overlap = Robot & ( self.map_work | self.robocomm.getObsPoly())
# for ODE
else:
if self.boundary_following == False:
overlap = self.robot & (self.map_work)
else:#TRUE
overlap = Robot & (self.map_work)
if self.boundary_following == False:
if bool(overlap): ## overlap of obstacles
#print "There MAYBE overlap~~ check connection to goal"
# check whether the real robot or and path to goal overlap with the obstacle
QGoalPoly= Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
path = convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path & ( self.map_work)
else:
pathOverlap = path & ( self.map_work | self.robocomm.getObsPoly())
else:
pathOverlap = path & ( self.map_work)
if bool(pathOverlap): # there is overlapping, go into bounding following mode
print "There IS overlap"
self.q_hit = mat([pose[0],pose[1]]).T
self.boundary_following = True
#Generate m-line polygon
QHitPoly = Circle(self.PioneerLengthHalf/4,(pose[0],pose[1]))
QGoalPoly= Circle(self.PioneerLengthHalf/4,(self.q_g[0],self.q_g[1]))
self.m_line = convexHull(QHitPoly + QGoalPoly)
#plot the first overlap
if self.PLOT_M_LINE == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(QHitPoly,'k')
self.plotPoly(QGoalPoly,'k')
self.plotPoly(overlap,'g')
self.plotPoly(self.m_line,'b')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,1)
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs() == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
print "no obstacles 2-ODE true"
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs() == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
print "no obstacles 1-ODE true"
if self.boundary_following == True:
self.q_hit_count += 1
# finding the point to go normal to (closest overlapping point)
j = 0
recheck = 0
while not bool(overlap):
# cannot see the obstacle. Based on the location of the previous point blow up the range of the robot on the left or on the right
j += 1
# finding whether the previous obstacle point is on the left side or the right side of the robot
# angle = angle of the previous point from the x-axis of the field
# omega = angle of the current Pioneer orientation from the x-axis of the field
# cc = differnece between angle and omega ( < pi = previous point on the left of robot, else on the right of robot)
x = self.prev_follow[0] -pose[0]
y = self.prev_follow[1] -pose[1]
angle = atan(y/x)
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x <0 and y < 0:
angle = pi + angle
else:
angle = 2*pi + angle
if pose[2] < 0:
omega = (2*pi + pose[2])
else:
omega = pose[2]
if omega > angle:
cc = 2*pi - (omega - angle)
else:
cc = angle - omega
# on the left
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print "on the left, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = Rectangle(self.range*2*j,self.range*2*j)
Robot.shift(pose[0]-self.range*j*2,pose[1]-self.range*j)
Robot.rotate(pose[2]-pi/2,pose[0],pose[1])
# on the right
else:
#print "on the right, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = Rectangle(self.range*2*j,self.range*2*j)
Robot.shift(pose[0],pose[1]-self.range*j)
Robot.rotate(pose[2]-pi/2,pose[0],pose[1])
if self.system == 1:
overlap = Robot & ( self.map_work | self.robocomm.getObsPoly())
else:
overlap = Robot & ( self.map_work)
self.plotPoly(Robot, 'm',2)
##extra box plotting in figure 1#
if self.PLOT_OVERLAP == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
self.plotPoly(overlap,'g',3)
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.original_figure,0)
# find the closest point on the obstacle to the robot
overlap_len = len(overlap)
for j in range(overlap_len):
BoundPolyPoints = asarray(overlap[j])
for i in range(len(BoundPolyPoints)-1):
bundle_x = (BoundPolyPoints[i,0] +BoundPolyPoints[1+i,0])/2 #mid-point coordinate x
bundle_y = (BoundPolyPoints[i,1] +BoundPolyPoints[1+i,1])/2 #mid-point coordinate y
q_overlap = hstack((q_overlap,vstack((bundle_x,bundle_y))))
bundle_x = (BoundPolyPoints[len(BoundPolyPoints)-1,0] +BoundPolyPoints[0,0])/2 #mid-point coordinate x
bundle_y = (BoundPolyPoints[len(BoundPolyPoints)-1,1] +BoundPolyPoints[0,1])/2 #mid-point coordinate y
q_overlap = hstack((q_overlap,vstack((bundle_x,bundle_y))))
q_overlap = q_overlap.transpose()
pt = self.closest_pt([pose[0],pose[1]], vstack((q_overlap,asarray(pointList(overlap)))))
self.prev_follow = pt
#calculate the vector to follow the obstacle
normal = mat([pose[0],pose[1]] - pt)
#find the distance from the closest point
distance = norm(normal)
velocity = normal * self.trans_matrix
vx = (velocity/norm(velocity)/3)[0,0]
vy = (velocity/norm(velocity)/3)[0,1]
# push or pull the robot towards the obstacle depending on whether the robot is close or far from the obstacle.
turn = pi/4*(distance-0.5*self.obsRange)/(self.obsRange) ### 5
corr_matrix = mat([[cos(turn),-sin(turn)],[sin(turn),cos(turn)]])
v = corr_matrix*mat([[vx],[vy]])
vx = v[0,0]
vy = v[1,0]
##plotting overlap on figure 2
if self.PLOT_OVERLAP == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(self.m_line,'b');
self.plotPoly(overlap,'r');
plt.plot(vx,vy,'ko')
plt.plot(pt[0],pt[1],'ro')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,1)
## conditions that the loop will end
#for 11111
RobotPoly = Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ####0.05
arrived = self.nextRegionPoly.covers(self.realRobot)
#for 33333
reachMLine= self.m_line.overlaps(RobotPoly)
# 1.reached the next region
if arrived:
self.boundary_following = False
self.q_hit_count = 0
print "arriving at the next region. Exit boundary following mode"
vx = 0
vy = 0
"""
# 2.q_hit is reencountered
elif norm(self.q_hit-mat([pose[0],pose[1]]).T) < 0.05 and self.q_hit_count > self.q_hit_Thres:
print "reencounter q_hit. cannot reach q_goal"
vx = 0
vy = 0
"""
# 3.m-line reencountered
elif reachMLine:
print >>sys.__stdout__, "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
if norm(self.q_g-mat([pose[0],pose[1]]).T) < norm(self.q_g-self.q_hit)-2*self.obsRange:
print "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
print "leaving boundary following mode"
self.boundary_following = False
self.q_hit_count = 0
leaving = False
# turn the robot till it is facing the goal
while not leaving:
x = self.q_g[0] -self.pose_handler.getPose()[0]
y = self.q_g[1] -self.pose_handler.getPose()[1]
angle = atan(y/x)
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x <0 and y < 0:
angle = pi + angle
else:
angle = 2*pi + angle
if self.pose_handler.getPose()[2] < 0:
omega = (2*pi + self.pose_handler.getPose()[2])
print >>sys.__stdout__,"omega<0: "+ str(omega)
else:
omega = self.pose_handler.getPose()[2]
print >>sys.__stdout__,"omega: "+ str(omega)
if omega > angle:
cc = 2*pi - (omega - angle)
else:
cc = angle - omega
# angle(goal point orientation) on the left of omega(robot orientation)
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
print>>sys.__stdout__, "turn left"
vx,vy = self.turnLeft(cc)
# on the right
else:
print>>sys.__stdout__, "turn right"
vx, vy = self.turnRight(2*pi-cc)
print>>sys.__stdout__, "omega: "+ str(omega) + " angle: "+ str(angle) + " (omega-angle): " + str(omega-angle)
self.drive_handler.setVelocity(vx,vy, self.pose_handler.getPose()[2])
if omega - angle < pi/6 and omega - angle > -pi/6:
leaving = True
#Check whether the robot can leave now (the robot has to be closer to the goal than when it is at q_hit to leave)
QGoalPoly= Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
path = convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path & ( self.map_work)
else:
pathOverlap = path & ( self.map_work | self.robocomm.getObsPoly())
else:
pathOverlap = path & ( self.map_work)
if not bool(pathOverlap):
print "There is NO MORE obstacles in front for now."
# check if the robot is closer to the goal compared with q_hit
if norm(self.q_hit-mat(self.q_g).T) > norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T) :
print "The robot is closer than the leaving point. The robot can leave"
self.boundary_following = False
self.q_hit_count = 0
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
else:
print "not leaving bug algorithm. difference(-farther) =" + str(norm(self.q_hit-mat(self.q_g).T) - norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T))
# Pass this desired velocity on to the drive handler
# Check if there are obstacles within 0.35m of the robot, if so, stop the robot
if self.system == 1:
if self.robocomm.getSTOP() == True:
vx = 0
vy = 0
self.drive_handler.setVelocity(vx,vy, pose[2])
# Set the current region as the previous current region(for checking whether the robot has arrived at the next region)
self.previous_current_reg = current_reg
# check whether robot has arrived at the next region
RobotPoly = Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ###0.05
departed = not self.currentRegionPoly.covers(RobotPoly)
arrived = self.nextRegionPoly.covers(self.realRobot)
if arrived:
self.q_hit_count = 0
self.boundary_following = False
if departed and (not arrived) and (time.time()-self.last_warning) > 0.5:
print "WARNING: Left current region but not in expected destination region"
# Figure out what region we think we stumbled into
for r in self.rfi.regions:
pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
vertices = mat(pointArray).T
if is_inside([pose[0], pose[1]], vertices):
#print "I think I'm in " + r.name
#print pose
break
self.last_warning = time.time()
return arrived
def plotPioneer(self,number,y = 1):
"""
Plotting regions and obstacles with matplotlib.pyplot
number: figure number (see on top)
y = 0 : plot self.map_work instead of self.map
"""
if not plt.isinteractive():
plt.ion()
plt.hold(True)
if y == 0:
BoundPolyPoints = asarray(pointList(self.map_work))
plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
else:
BoundPolyPoints = asarray(pointList(self.map))
plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
if self.system == 1:
if bool(self.robocomm.getObsPoly()):
BoundPolyPoints = asarray(pointList(self.robocomm.getObsPoly()))
plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
plt.xlabel('x')
plt.ylabel('y')
plt.figure(number).canvas.draw()
def turnLeft(self,a):
"""
Turn left with angle a
"""
vx = cos(self.pose_handler.getPose()[2]+a)* self.PioneerLengthHalf;
vy = sin(self.pose_handler.getPose()[2]+a)* self.PioneerLengthHalf;
return vx,vy
def turnRight(self,a):
"""
Turn right with angle a
"""
vx = cos(self.pose_handler.getPose()[2]-a)* self.PioneerLengthHalf;
vy = sin(self.pose_handler.getPose()[2]-a)* self.PioneerLengthHalf;
return vx,vy
def euclid(self,pt1, pt2):
"""
for calculating the minimum distance from a point on the obstacle
"""
pairs = zip(pt1, pt2) # Form pairs in corresponding dimensions
sum_sq_diffs = sum((a - b)**2 for a, b in pairs) # Find sum of squared diff
return (sum_sq_diffs)**(float(1)/2) # Take sqrt to get euclidean distance
def closest_pt(self,pt, vec):
"""
Returns the point in vec with minimum euclidean distance to pt
"""
return min(vec, key=lambda x: self.euclid(pt, x))
def plotPoly(self,c,string,w = 1):
"""
c = polygon to be plotted with matlabplot
string = string that specify color
w = width of the line plotting
"""
BoundPolyPoints = asarray(pointList(c))
plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],string,linewidth=w)
def dividePolygon(self,polygon,coveredArea,k,fill,rec=1):
if k == 1:
areas = [polygon]
areaUnion = polygon
self.lAreas += 1
elif k == 2:
areas = self.postDividePolygons([polygon],2)
areaUnion = polygon
self.lAreas += 2
else:
if k*self.pg >= 1:
area = polygon.area()*self.pg
else:
area = polygon.area()*(1/float(k))
ratio = (area/float(numpy.pi))**0.5
scale = ratio/self.mu
areas = []
uncoveredArea = polygon - coveredArea
fillOld = 0
count = 0
oldCovered = -1
areaUnion = Polygon()
newk = k
while uncoveredArea.area()/polygon.area() >= (1-fill): # Mientras se llena al p1%
count += 1
if oldCovered != coveredArea.area():
count = 0
if len(uncoveredArea) > 1:
uncovered2select = [Polygon(x) for x in uncoveredArea]
uncovered2select.sort(key=lambda x: x.area(),reverse=True)
uncovered2select = uncovered2select[0]
else:
uncovered2select = uncoveredArea
oldCovered = coveredArea.area()
if count >= 20:
count += 1
end = False
while end != True:
uncovered2select2 = self.postDividePolygons([uncovered2select],2)
for x in uncovered2select2:
if newk*x.area()/uncoveredArea.area() <= 1.5:
areas.append(x)
uncoveredArea = uncoveredArea - x
coveredArea = coveredArea | x
areaUnion = areaUnion | x
end = True
if uncoveredArea.area() > 0:
if newk*uncovered2select.area()/uncoveredArea.area() <= 1.5:
areas.append(uncovered2select)
uncoveredArea = uncoveredArea - uncovered2select
coveredArea = coveredArea | uncovered2select
areaUnion = areaUnion | uncovered2select
else:
center = uncoveredArea.sample(numpy.random.uniform)
angle = numpy.random.uniform(0,2)*numpy.pi
x = ratio*numpy.cos(angle) + center[0]
y = ratio*numpy.sin(angle) + center[1]
aPoint = (x,y)
alp = numpy.random.uniform(self.alpha[0],self.alpha[1])
sig = numpy.random.uniform(self.sigma[0],self.sigma[1])
a,r,sa,sr,X1,times = mrpolygon(alp,sig,self.mu,self.X_0,self.dt,self.N)
sa,sr = scalePolygon(sa,sr,scale)
polygoni = polarPolygon2cartesian(zip(sa,sr))
polygoni = transportPolygon(polygoni, center, aPoint)
polygoni = Polygon(polygoni)
polygoni = polygoni - coveredArea
polygoni = polygoni & polygon
if len(polygoni) > 1:
pl = [Polygon(x) for x in polygoni]
pl.sort(key=lambda x: x.area())
polygoni = pl[-1]
newN = numpy.round(newk*polygoni.area()/uncoveredArea.area())
rnd = numpy.random.uniform(0,1)
if rnd <= self.pu:
newN += numpy.round(numpy.random.uniform(0,self.su)*newk)
if newN >= 1:
areasi, coveredAreai = self.dividePolygon(polygoni,coveredArea,newN,fill,rec=rec+1)
newk -= newN
coveredArea = coveredArea | coveredAreai
areaUnion = areaUnion | coveredAreai
areas += areasi
uncoveredArea = polygon - coveredArea
areas = self.postCorrectionHoles(areaUnion,areas)
areas.sort(key=lambda x: x.area())
nAreas = []
for nx, x in enumerate(areas):
add = True
for nx2, x2 in enumerate(areas[nx+1:]):
if x2.overlaps(x):
if x2.covers(x):
add = False
break
else:
x = x - x2
if add:
nAreas.append(x)
areas = nAreas
if len(areas) < k:
areas = self.postDividePolygons(areas,k)
if len(areas) > k:
areas = self.postCorrectionDissolve(areas,k)
coveredArea = fillHoles(areaUnion)
return areas, coveredArea
def postDividePolygons(self,areas,nAreas):
def getPointInFace(face,bbox):
if face == 0:
x = bbox[0]
y = numpy.random.uniform(bbox[2],bbox[3])
elif face == 1:
y = bbox[3]
x = numpy.random.uniform(bbox[0],bbox[1])
elif face == 2:
x = bbox[1]
y = numpy.random.uniform(bbox[2],bbox[3])
elif face == 3:
y = bbox[2]
x = numpy.random.uniform(bbox[0],bbox[1])
return x,y
def getCornersOfFace(face,bbox):
if face == 0:
c1 = (bbox[0],bbox[2])
c2 = (bbox[0],bbox[3])
elif face == 1:
c1 = (bbox[0],bbox[3])
c2 = (bbox[1],bbox[3])
elif face == 2:
c1 = (bbox[1],bbox[3])
c2 = (bbox[1],bbox[2])
elif face == 3:
c1 = (bbox[1],bbox[2])
c2 = (bbox[0],bbox[2])
return c1,c2
while len(areas) < nAreas:
end = False
while not end:
areaOrder = range(0,len(areas))
areaOrder.sort(key=lambda x: areas[x].area(),reverse=True)
na = areaOrder[0]
#na = numpy.random.randint(0,len(areas))
if areas[na][0][0] != areas[na][0][-1]:
areas[na] = Polygon([x for x in areas[na][0]] + [areas[na][0][0]])
area = areas[na]
bbox = area.boundingBox()
bboxp = Polygon([(bbox[0]-1,bbox[2]-1),(bbox[0]-1,bbox[3]+1),
(bbox[1]+1,bbox[3]+1),(bbox[1]+1,bbox[2]-1),
(bbox[0]-1,bbox[2]-1)])
bbox = bboxp.boundingBox()
inside = False
errors = 0
warnings = 0
i = 0
while not inside:
if errors == 20 and na +1 < len(areas):
i += 1
na = areaOrder[i]
if areas[na][0][0] != areas[na][0][-1]:
areas[na] = Polygon([x for x in areas[na][0]] + [areas[na][0][0]])
area = areas[na]
bbox = area.boundingBox()
bboxp = Polygon([(bbox[0]-1,bbox[2]-1),(bbox[0]-1,bbox[3]+1),
(bbox[1]+1,bbox[3]+1),(bbox[1]+1,bbox[2]-1),
(bbox[0]-1,bbox[2]-1)])
bbox = bboxp.boundingBox()
errors = 0
f1 = numpy.random.randint(0,4)
f2 = (f1 + numpy.random.randint(1,4))%4
faces = [f1,f2]
faces.sort()
p1 = getPointInFace(faces[0],bbox)
if f1%2 == 0:
p2 = (p1[0],p1[1]+1)
else:
p2 = (p1[0]+1,p1[1])
p3 = getPointInFace(faces[1],bbox)
if f2%2 == 0:
p4 = (p3[0],p3[1]+1)
else:
p4 = (p3[0]+1,p3[1])
line = Polygon([p1,p2,p3,p4,p1])
inters = area & line
parts = line - area
if len(parts) == 2 and inters.area() >= line.area()*0.2 :
inside = True
else:
errors += 1
divPolygon = [p1]
for i in range(*faces):
c1,c2 = getCornersOfFace(i,bbox)
divPolygon.append(c2)
divPolygon.append(p3)
divPolygon.append(p1)
divPolygon = Polygon(divPolygon)
polygon1 = divPolygon & area
divPolygon = bboxp - divPolygon
polygon2 = area - polygon1
if len(polygon1) == 1 and \
len(polygon2) == 1 and \
polygon1.area() > 0 and \
polygon2.area() > 0 and \
abs(polygon1.area() + polygon2.area() - area.area()) <= 0.01:
warnings = 0
areas.pop(na)
areas.append(polygon1)
areas.append(polygon2)
end = True
else:
warnings += 1
areas_aux = areas[na] & areas[na]
if areas_aux > 1:
areas_aux = [Polygon(x) for x in areas_aux]
areas_aux.sort(key=lambda k: k.area(),reverse=True)
areas_aux = areas_aux[0]
while len(areas_aux) == 0:
areas_aux.sort(key=lambda k: k.area(),reverse=True)
areas_aux = Polygon(areas_aux[0])
areas_aux = areas_aux & areas_aux
areas[na] = areas_aux
return areas
def testPrune(self):
p1 = Polygon.Polygon(((0.0,0.0), (1.0,1.0), (2.0,2.0), \
(3.0,2.0), (3.0, 2.0), (3.0,1.0), \
(3.0,0.0), (2.0,0.0)))
p2 = Polygon.prunePoints(p1)
self.assertAlmostEqual(p1.area(), p2.area(), 10)
def tboundingBox(poly):
p = Polygon.pointList(poly, 0)
x = map(lambda a: a[0], p)
y = map(lambda a: a[1], p)
return min(x), max(x), min(y), max(y)
def setUp(self):
star = Polygon.polyStar(radius=2.0, center=(1.0, 3.0), nodes=5, iradius=1.4)
circle = Polygon.polyStar(radius=1.0, center=(1.0, 3.0), nodes=64)
self.cookie = star-circle
self.cont = ((0.0, 0.0), (2.0, 1.0), (1.0, 0.0), (-2.0, 1.0), (0.0, 0.0))
def __init__(self, proj, shared_data):
"""
Bug alogorithm motion planning controller
"""
# Information about the robot
## 1: Pioneer ; 2: 0DE
self.system = 1
#setting for plotting
##figure number
self.original_figure = 1
self.overlap_figure = 2
plt.figure(self.original_figure)
plt.figure(self.overlap_figure)
self.PLOT = True # plot with matplot
self.PLOT_M_LINE = True # plot m-line
self.PLOT_EXIT = True # plot exit point of a region
self.PLOT_OVERLAP = True # plot overlap area with the obstacle
# Get references to handlers we'll need to communicate with
self.drive_handler = proj.h_instance['drive']
self.pose_handler = proj.h_instance['pose']
if self.system == 1:
self.robocomm = shared_data['robocomm']
# Get information about regions
self.rfi = proj.rfi
self.coordmap_map2lab = proj.coordmap_map2lab
self.coordmap_lab2map = proj.coordmap_lab2map
self.last_warning = 0
###################################
########used by Bug algorithm######
###################################
# PARAMETERS
self.LeftRightFlag = 1 # originally set to right 1- right, 0 left
# Pioneer related parameters
self.PioneerWidthHalf = 0.20 # (m) width of Pioneer
self.PioneerLengthHalf = 0.25 # (m) lenght of Pioneer
self.ratioBLOW = 0.5 # blow up ratio of the pioneer box ######### 5 BOX
self.PioneerBackMargin= self.ratioBLOW*self.PioneerLengthHalf*2 # (in m)
self.range = 0.75 # (m) specify the range of the robot (when the normal circle range cannot detect obstacle)
self.obsRange = 0.50 # (m) range that says the robot detects obstacles
#self.shift = 0.20 # 0.15
## 2: 0DE
self.factorODE = 50
if self.system == 2:
self.PioneerWidthHalf = self.PioneerWidthHalf*self.factorODE
self.PioneerLengthHalf = self.PioneerLengthHalf*self.factorODE
self.range = self.range*self.factorODE
self.obsRange = self.obsRange*self.factorODE
self.PioneerBackMargin= self.PioneerBackMargin*self.factorODE
#build self.map with empty contour
self.map = Rectangle (1,1)
self.map -= self.map #Polygon built from the original map
#build self.ogr with empty contour
self.ogr = Rectangle (1,1) #Polygon built from occupancy grid data points
self.ogr -= self.ogr
self.freespace = None # contains only the boundary
self.map_work = None # working copy of the map. remove current region and next region
self.previous_current_reg = None # previous current region
self.currentRegionPoly = None # current region's polygon
self.nextRegionPoly = None # next region's polygon
self.q_g = [0,0] # goal point of the robot heading to
self.q_hit = [0,0] # location where the robot first detect an obstacle
self.boundary_following= False # tracking whether it is in boundary following mode
self.m_line = None # m-line polygon
self.trans_matrix = mat([[0,1],[-1,0]]) # transformation matrix for find the normal to the vector connecting a point on the obstacle to the robot
#self.corr_theta = pi/6
#self.corr_matrix = mat([[cos(self.corr_theta),-sin(self.corr_theta)],[sin(self.corr_theta),cos(self.corr_theta)]])
self.q_hit_count = 0
self.q_hit_Thres = 1000
self.prev_follow = [[],[]]
## Construct robot polygon (for checking overlap)
pose = self.pose_handler.getPose()
self.prev_pose = pose
self.robot = Rectangle(self.obsRange*3,self.PioneerBackMargin)
self.robot.shift(pose[0]-self.obsRange,pose[1]-2*self.PioneerLengthHalf)
self.robot = Circle(self.obsRange,(pose[0],pose[1])) - self.robot
self.robot.rotate(pose[2]-pi/2,pose[0],pose[1])
self.robot.shift(self.shift*cos(pose[2]),self.shift*sin(pose[2]))
#construct real robot polygon( see if there is overlaping with path to goal
self.realRobot = Rectangle(self.PioneerWidthHalf*2.5,self.PioneerLengthHalf*2 )
self.realRobot.shift(pose[0]-self.PioneerWidthHalf*1.25,pose[1]-self.PioneerLengthHalf*1)
self.realRobot.rotate(pose[2]-pi/2,pose[0],pose[1])
#constructing map with all the regions included
for region in self.rfi.regions:
if region.name.lower()=='freespace':
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
freespace = Polygon(regionPoints)
freespace_big= Polygon(regionPoints)
freespace_big.scale(1.2, 1.2)
self.freespace = freespace_big -freespace
self.map += freespace_big -freespace #without including freespace
else:
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.map += Polygon(regionPoints)
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.original_figure)
#!/usr/bin/env python
import math,re, os, random
import Polygon, Polygon.IO, Polygon.Utils
import project
from regions import *
import itertools
import decomposition
Polygon.setTolerance(0.1)
class parseLP:
"""
A parser to parse the locative prepositions in specification
"""
def __init__(self):
pass
def main(self,argv):
""" Main function; run automatically when called from command-line """
spec_file = argv
self.regionNear = []
self.regionBetween = []
defaultNearDistance = 50
# load data
self.proj = project.Project()
self.proj.setSilent(True)
self.proj.loadProject(spec_file)
lat=float(point.split(":")[0].strip())
lon=float(point.split(":")[1].strip())
cat=int(point.split(":")[2].strip())
cur_points.append([lat,lon])
if cat==4:
poly_list.append(cur_points)
cur_points=[]
best_point=(-1110,-1110)
peak_dense=-1
for poly in poly_list:
#print poly
i_poly=Polygon(poly)
fill_points=[]
maxlat=-1000000
minlat=1000000
maxlon=-1000000
minlon=1000000
for point in poly:
lat=point[0]
lon=point[1]
if lat>maxlat: maxlat=lat
if lat<minlat: minlat=lat
if lon>maxlon: maxlon=lon
if lon<minlon: minlon=lon
lat_values=[minlat+1.0/24*i for i in range(int((maxlat-minlat)*24))]
"""
if __name__ == '__main__':
f=open("formatted_adjust.txt","r")
x_points=[]
y_points=[]
new_x=[]
new_y=[]
country=""
lam=0.05
threshold=0.999
i_area=0
c_area=0
l_area=0
c_poly=Polygon([(0,0)])
l_poly=Polygon([(0,0)])
while True:
instr=raw_input("What now?")
if instr=="n":
fline=f.readline().strip()
flist=fline.split(":")
if flist==[""]:
print "*** New Country! ***"
fline=f.readline().strip()
flist=fline.split(":")
