def convex_hull(scan, debug_poly=None):
"""
Test visualization of convex hull
"""
heading = 0.0 # TODO
pts = []
for i, i_dist in enumerate(scan):
if i_dist == 0 or i_dist >= 10000:
continue
angle = math.radians(270 * (i / len(scan)) - 135) + heading
dist = i_dist / 1000.0
x, y = dist * math.cos(angle), dist * math.sin(angle)
pts.append((x, y))
hull = cv2.convexHull(np.array(pts, dtype=np.float32))
hull_pts = [p[0] for p in hull] + [
hull[0][0],
]
dist_arr = [distance(a, b) for a, b in zip(hull_pts[:-1], hull_pts[1:])]
#print('%.2f %.2f %.2f %.2f' % tuple(sorted(dist_arr, reverse=True)[:4]))
first, second = sorted(dist_arr, reverse=True)[:2]
i, j = dist_arr.index(first), dist_arr.index(second) # TODO argmax
A = ((hull_pts[i][0] + hull_pts[i + 1][0]) / 2,
(hull_pts[i][1] + hull_pts[i + 1][1]) / 2)
B = ((hull_pts[j][0] + hull_pts[j + 1][0]) / 2,
(hull_pts[j][1] + hull_pts[j + 1][1]) / 2)
if debug_poly is not None:
#debug_poly.append(hull_pts)
debug_poly.append([A, B])
return (A, B)
def findNearestEx(self, pos):
"""
Find nearest position on the polyline and return tuple:
- nearest point(x,y) on route
- distance to that point
- index of segment (negative) or waypoint index (0..N-1)
"""
if not self.pts:
return None
ref = self.conv.geo2planar(pos)
index = 0
best = prev = self.pts[0]
bestDist = distance(ref, best)
bestIndex = 0
for p in self.pts[1:]:
pos, dist, type = Line(prev, p).nearest(ref)
if dist < bestDist:
bestDist = dist
best = pos
if type < 0:
bestIndex = -index - 1
else:
bestIndex = index + type
prev = p
index += 1
return self.conv.planar2geo(best), bestDist, bestIndex
def findNearestEx( self, pos ):
"""
Find nearest position on the polyline and return tuple:
- nearest point(x,y) on route
- distance to that point
- index of segment (negative) or waypoint index (0..N-1)
"""
if not self.pts:
return None
ref = self.conv.geo2planar( pos )
index = 0
best = prev = self.pts[0]
bestDist = distance( ref, best )
bestIndex = 0
for p in self.pts[1:]:
pos, dist, type = Line(prev, p).nearest( ref )
if dist < bestDist:
bestDist = dist
best = pos
if type < 0:
bestIndex = -index -1
else:
bestIndex = index + type
prev = p
index += 1
return self.conv.planar2geo( best ), bestDist, bestIndex
def convex_hull(scan, debug_poly=None):
"""
Test visualization of convex hull
"""
heading = 0.0 # TODO
pts = []
for i, i_dist in enumerate(scan):
if i_dist == 0 or i_dist >= 10000:
continue
angle = math.radians(270 * (i / len(scan)) - 135) + heading
dist = i_dist/1000.0
x, y = dist * math.cos(angle), dist * math.sin(angle)
pts.append((x,y))
hull = cv2.convexHull(np.array(pts, dtype=np.float32))
hull_pts = [p[0] for p in hull] + [hull[0][0],]
dist_arr = [distance(a, b) for a, b in zip(hull_pts[:-1], hull_pts[1:])]
#print('%.2f %.2f %.2f %.2f' % tuple(sorted(dist_arr, reverse=True)[:4]))
first, second = sorted(dist_arr, reverse=True)[:2]
i, j = dist_arr.index(first), dist_arr.index(second) # TODO argmax
A = ((hull_pts[i][0] + hull_pts[i+1][0])/2, (hull_pts[i][1] + hull_pts[i+1][1])/2)
B = ((hull_pts[j][0] + hull_pts[j+1][0])/2, (hull_pts[j][1] + hull_pts[j+1][1])/2)
if debug_poly is not None:
#debug_poly.append(hull_pts)
debug_poly.append([A, B])
return (A, B)
def __init__(self, pts = [], conv = None, isLoop = None):
if conv == None:
conv = Convertor()
self.conv = conv
self.pts = [ self.conv.geo2planar(p) for p in pts ]
if isLoop == None and len(self.pts) > 1:
isLoop = (distance(self.pts[0], self.pts[-1]) < 5.0)
self.isLoop = isLoop
def __init__(self, pts=[], conv=None, isLoop=None):
if conv == None:
conv = Convertor()
self.conv = conv
self.pts = [self.conv.geo2planar(p) for p in pts]
if isLoop == None and len(self.pts) > 1:
isLoop = (distance(self.pts[0], self.pts[-1]) < 5.0)
self.isLoop = isLoop
def length( self ):
if not self.pts:
return None
sum = 0
prev = self.pts[0]
for p in self.pts:
sum += distance( prev, p )
prev = p
return sum
def length(self):
if not self.pts:
return None
sum = 0
prev = self.pts[0]
for p in self.pts:
sum += distance(prev, p)
prev = p
return sum