def get_intersection_point_with(self, point: LidarPoint,
angle: float) -> LidarPoint:
"""
Calculates the intersection between two this line and the line formed by a point and angle
:param point: LidarPoint
:param angle: float in radians
:return: LidarPoint
"""
point2 = LidarPoint(x=point.x + math.cos(angle),
y=point.y + math.sin(angle))
vector = LidarVector(point, point2)
a1, b1, c1 = vector.get_abc()
a2, b2, c2 = self.get_abc()
if a1 == 0 and a2 == 0:
return None
elif b1 == 0 and b2 == 0:
return None
else:
a = np.array([[a1, b1], [a2, b2]])
b = np.array([c1, c2])
intersection = np.linalg.solve(a, b)
return LidarPoint(x=intersection[0], y=intersection[1])
def get_mid_points(self, min_wall_distance=300):
x_s = (self.wall1.start.x + self.wall2.start.x) / 2
y_s = (self.wall1.start.y + self.wall2.start.y) / 2
x_t = (self.wall1.end.x + self.wall2.end.x) / 2
y_t = (self.wall1.end.y + self.wall2.end.y) / 2
if self.wall_type == WallType.HORIZONTAL:
x_s -= min_wall_distance
x_t += min_wall_distance
else:
y_s -= min_wall_distance
y_t += min_wall_distance
return LidarVector(start=LidarPoint(x_s, y_s),
end=LidarPoint(x_t, y_t))
def get_distance_from_nearest_wall(wall_candidates: pd.DataFrame,
pt: LidarPoint, theta: float) -> float:
min_distance = None
if math.cos(math.radians(theta)) > 0:
wall_candidates = wall_candidates[(wall_candidates.xstart > pt.x) |
(wall_candidates.xend > pt.x)]
else:
wall_candidates = wall_candidates[(wall_candidates.xstart <= pt.x) |
(wall_candidates.xend <= pt.x)]
if math.sin(math.radians(theta)) > 0:
wall_candidates = wall_candidates[(wall_candidates.ystart > pt.y) |
(wall_candidates.yend > pt.y)]
else:
wall_candidates = wall_candidates[(wall_candidates.ystart <= pt.y) |
(wall_candidates.yend <= pt.y)]
for _, wall in wall_candidates.iterrows():
wall_start = LidarPoint(wall.xstart, wall.ystart)
wall_end = LidarPoint(wall.xend, wall.yend)
theta1 = math.degrees(LidarVector(pt, wall_start).direction)
theta2 = math.degrees(LidarVector(pt, wall_end).direction)
if theta1 < 0:
theta1 += 360
if theta2 < 0:
theta2 += 360
if theta1 > theta2:
theta1, theta2 = theta2, theta1
wall_start, wall_end = wall_end, wall_start
if theta2 - theta1 > 180:
theta1, theta2 = theta2 - 360, theta1
wall_start, wall_end = wall_end, wall_start
if theta1 <= theta <= theta2 or theta1 <= theta - 360 <= theta2:
wall_v = LidarVector(wall_start, wall_end)
poi = wall_v.get_intersection_point_with(pt, math.radians(theta))
distance = LidarVector(pt, poi).distance
if min_distance is None or distance < min_distance:
min_distance = distance
return min_distance
def get_wall_openings(self, min_wall_length=500) -> List[WallOpening]:
if self.wall_openings is None:
# Get short walls, walls with length < min_wall_length
walls = self.get_walls().copy()
walls['length'] = ((walls.xstart - walls.xend)**2 +
(walls.ystart - walls.yend)**2)**0.5
walls = walls[walls.length < min_wall_length]
walls['angle'] = walls.apply(lambda row: math.degrees(
math.atan2(row.yend - row.ystart, row.xend - row.xstart)),
axis=1)
self.wall_openings = []
walls_seen = set()
for _, wall1 in walls.iterrows():
w1 = LidarVector(start=LidarPoint(wall1.xstart, wall1.ystart),
end=LidarPoint(wall1.xend, wall1.yend))
if w1.__str__() in walls_seen:
continue
else:
walls_seen.add(w1.__str__())
temp_walls = walls.copy()
temp_walls['distance'] = temp_walls.apply(lambda row: (
(row.xstart - wall1.xstart)**2 +
(row.ystart - wall1.ystart)**2)**0.5,
axis=1)
temp_walls = temp_walls[temp_walls.distance > 0].sort_values(
by='distance')
wall2 = temp_walls.iloc[0]
w2 = LidarVector(start=LidarPoint(wall2.xstart, wall2.ystart),
end=LidarPoint(wall2.xend, wall2.yend))
if w2.__str__() in walls_seen:
continue
else:
walls_seen.add(w2.__str__())
self.wall_openings.append(WallOpening(wall1=w1, wall2=w2))
return self.wall_openings
def generate_lidar_points(m_csv: str, lp_csv: str, fp_csv: str, num_points=500, verbose=False) -> pd.DataFrame:
if not (os.path.exists(m_csv) and os.path.exists(fp_csv)):
return pd.DataFrame(data=[])
angles, distances = [], []
walls = pd.read_csv(m_csv, names=['xstart', 'ystart', 'xend', 'yend'])
pts = DroneMap(
flight_path_csv=fp_csv,
lidar_points_csv=lp_csv
).get_drone_positions()
for i in range(len(pts)):
pt = pts.iloc[i]
if verbose:
print("generating lidar points around ({:.4f}, {:.4f})".format(pt.x, pt.y))
angles.append(i)
distances.append(num_points)
for theta in np.linspace(start=0, stop=360, num=num_points, endpoint=False):
min_distance = get_distance_from_nearest_wall(
wall_candidates=walls,
pt=LidarPoint(pt.x, pt.y),
theta=theta
)
angles.append(round(360 - theta, 4))
distances.append(min_distance)
lp = pd.DataFrame(
data=dict(
angle=np.array(angles),
distance=np.array(distances).astype(int)
)
)
if os.path.exists(os.path.dirname(lp_csv)):
lp.to_csv(lp_csv, index=False, header=False)
if verbose:
print("Written the csv file: {}".format(os.path.realpath(lp_csv)))
return lp
def get_optimum_flight_path(self,
start: LidarPoint,
end: LidarPoint = None,
is_visit_all_rooms=False,
plot=False,
fp_csv=None,
verbose=False,
min_wall_distance=300) -> pd.DataFrame:
connections = []
points = self.get_all_points()
if verbose:
print("generating nodes")
if is_visit_all_rooms:
nodes = self.get_nodes(start=start)
else:
nodes = self.get_nodes(start=start, end=end)
for node in nodes:
for neighbor in node.neighbors:
vector = LidarVector(node, neighbor)
if vector.start.x < vector.end.x:
start_x, end_x = vector.start.x, vector.end.x
else:
end_x, start_x = vector.start.x, vector.end.x
if vector.start.y < vector.end.y:
start_y, end_y = vector.start.y, vector.end.y
else:
end_y, start_y = vector.start.y, vector.end.y
p = points[(points.x >= start_x) & (points.x <= end_x) &
(points.y >= start_y) & (points.y <= end_y)]
if not len(p):
connections.append((node, neighbor, vector.distance))
elif len(p) < 5:
obs_x, obs_y = p.x.mean(), p.y.mean()
distance = LidarVector(node, neighbor).get_distance_from(
LidarPoint(obs_x, obs_y))
if distance > min_wall_distance:
connections.append((node, neighbor, vector.distance))
else:
obs1 = LidarPoint(obs_x, obs_y + min_wall_distance)
obs2 = LidarPoint(obs_x, obs_y - min_wall_distance)
lv11 = LidarVector(node, obs1)
lv12 = LidarVector(obs1, neighbor)
lv21 = LidarVector(node, obs2)
lv22 = LidarVector(obs2, neighbor)
connections.append((node, obs1, lv11.distance))
connections.append((obs1, neighbor, lv12.distance))
connections.append((node, obs2, lv21.distance))
connections.append((obs2, neighbor, lv22.distance))
x_s, y_s = [], []
if verbose:
print("analyzing shortest path")
if is_visit_all_rooms:
paths = []
Graph(connections).shortest_path_traverse_all(start, paths=paths)
best_path, best_distance = None, math.inf,
best_dikstra_path = []
for bp, bc in paths:
df_best_path = self.get_optimum_flight_path(start=bp[len(bp) -
1],
end=end)
prev_row = df_best_path.iloc[0]
distance = 0
for _, curr_row in df_best_path[1:].iterrows():
distance += ((curr_row.x - prev_row.x)**2 +
(curr_row.y - prev_row.y)**2)**0.5
prev_row = curr_row
if distance < best_distance:
best_path, best_distance = bp, distance
best_dikstra_path = df_best_path
for pos in best_path:
x_s.append(pos.x)
y_s.append(pos.y)
for _, pos in best_dikstra_path.iterrows():
x_s.append(pos.x)
y_s.append(pos.y)
else:
positions = Graph(connections).shortest_path(start, end)
for pos in positions:
x_s.append(pos.x)
y_s.append(pos.y)
df = pd.DataFrame(data=list(zip(x_s, y_s)), columns=['x', 'y'])
if fp_csv is not None:
fp_csv_dir = os.path.dirname(fp_csv)
if not os.path.exists(fp_csv_dir):
os.makedirs(fp_csv_dir)
with open(fp_csv, 'w', newline='') as csvfile:
spamwriter = csv.writer(csvfile,
delimiter=',',
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
for i in range(len(df)):
row = df.iloc[i]
spamwriter.writerow([i, 1])
spamwriter.writerow([row.x / 1e3, row.y / 1e3])
if verbose:
print("Written the csv file: {}".format(
os.path.realpath(fp_csv)))
if plot:
df.plot(kind='line', x='x', y='y', color='r', ax=self.ax)
if verbose:
print("plotting results")
self.visualize_lidar_points(drone_pos=False)
return df
def get_walls(self, div=100):
if self.walls is None:
half_div = div / 2
df_points = self.get_all_points().copy()
walls = set()
for _, corner in self.get_all_corners().iterrows():
corner = LidarPoint(corner.x, corner.y)
df = self.get_all_corners().copy()
df['r'] = df.apply(lambda row: ((row.x - corner.x)**2 +
(row.y - corner.y)**2)**0.5,
axis=1)
top_corner = df[(df.y > corner.y)
& (abs(df.x - corner.x) < div)].sort_values(
by='r')[:1]
down_corner = df[(df.y < corner.y)
& (abs(df.x - corner.x) < div)].sort_values(
by='r')[:1]
left_corner = df[(df.x < corner.x)
& (abs(df.y - corner.y) < div)].sort_values(
by='r')[:1]
right_corner = df[(df.x > corner.x)
& (abs(df.y - corner.y) < div)].sort_values(
by='r')[:1]
if len(top_corner) and len(down_corner):
top_corner = LidarPoint(top_corner.iloc[0].x,
top_corner.iloc[0].y)
down_corner = LidarPoint(down_corner.iloc[0].x,
down_corner.iloc[0].y)
top_pts = df_points[(df_points.y < top_corner.y - half_div)
& (df_points.y > corner.y + half_div) &
(abs(df_points.x - corner.x) < div)]
down_pts = df_points[
(df_points.y < corner.y - half_div)
& (df_points.y > down_corner.y + half_div) &
(abs(df_points.x - corner.x) < div)]
if len(top_pts) > len(down_pts):
v_corner = LidarPoint(top_corner.x, top_corner.y)
else:
v_corner = LidarPoint(down_corner.x, down_corner.y)
elif len(top_corner):
v_corner = LidarPoint(top_corner.iloc[0].x,
top_corner.iloc[0].y)
else:
v_corner = LidarPoint(down_corner.iloc[0].x,
down_corner.iloc[0].y)
if len(right_corner) and len(left_corner):
right_corner = LidarPoint(right_corner.iloc[0].x,
right_corner.iloc[0].y)
left_corner = LidarPoint(left_corner.iloc[0].x,
left_corner.iloc[0].y)
right_pts = df_points[
(df_points.x < right_corner.x - half_div)
& (df_points.x > corner.x + half_div) &
(abs(df_points.y - corner.y) < div)]
left_pts = df_points[
(df_points.x < corner.x - half_div)
& (df_points.x > left_corner.x + half_div) &
(abs(df_points.y - corner.y) < div)]
if len(right_pts) > len(left_pts):
h_corner = LidarPoint(right_corner.x, right_corner.y)
else:
h_corner = LidarPoint(left_corner.x, left_corner.y)
elif not len(right_corner):
h_corner = LidarPoint(left_corner.iloc[0].x,
left_corner.iloc[0].y)
else:
h_corner = LidarPoint(right_corner.iloc[0].x,
right_corner.iloc[0].y)
wall_corner = WallCorner(cc=corner, cv=v_corner, ch=h_corner)
w = wall_corner.get_walls()
if w[0] not in walls:
walls.add(w[0])
if w[1] not in walls:
walls.add(w[1])
x_start, y_start = [], []
x_end, y_end = [], []
for wl in walls:
x_start.append(wl.start.x)
y_start.append(wl.start.y)
x_end.append(wl.end.x)
y_end.append(wl.end.y)
self.walls = pd.DataFrame(
data=list(zip(x_start, y_start, x_end, y_end)),
columns=['xstart', 'ystart', 'xend', 'yend']).sort_values(
by=['xstart', 'ystart', 'xend', 'yend'
]).drop_duplicates().reset_index(drop=True).astype(int)
return self.walls
print("plotting results")
self.visualize_lidar_points(drone_pos=False)
return df
if __name__ == '__main__':
dm = DroneMap(lidar_points_csv='./.cache/LIDARPoints.csv',
flight_path_csv='./.cache/FlightPath.csv')
"""Visualize Lidar points"""
# dm.visualize_lidar_points(by_scan_id=False)
# dm.generate_mapping_csv(csv_file="./.cache/Mapping.csv")
"""Test for connectivity between two points"""
# lp1 = LidarPoint(5e3, 4e3)
# lp2 = LidarPoint(15e3, 14e3)
# assert not dm.is_connected(lp1, lp2), "{} {} should not be connected".format(lp1, lp2)
# lp1 = LidarPoint(5e3, 4e3)
# lp2 = LidarPoint(7.5e3, 8e3)
# assert dm.is_connected(lp1, lp2), "{} {} should be connected".format(lp1, lp2)
"""Get Wall Openings"""
# print(dm.get_wall_openings())
"""Get nodes of each room"""
# nodes = dm.get_nodes(start=LidarPoint(7.5e3, 8e3), end=LidarPoint(17.5e3, 8e3))
# print(nodes)
"""Get Optimum Flight Path"""
dm.get_optimum_flight_path(
start=LidarPoint(7.5e3, 8e3),
end=LidarPoint(17.5e3, 12e3),
fp_csv='./.cache/fp.csv',
)
print("missing flight path csv")
elif args.lp_csv is None:
print("missing lidar points csv")
elif args.start_x is None:
print("missing starting point x coordinate")
elif args.start_y is None:
print("missing starting point y coordinate")
elif args.end_x is None:
print("missing end point x coordinate")
elif args.end_y is None:
print("missing end point y coordinate")
else:
dm = DroneMap(lidar_points_csv=args.lp_csv,
flight_path_csv='./.cache/FlightPath.csv')
dm.get_optimum_flight_path(
end=LidarPoint(args.end_x, args.end_y),
start=LidarPoint(args.start_x, args.start_y),
fp_csv=args.fp_csv,
plot=True,
is_visit_all_rooms=args.challenge == 3,
verbose=args.verbose,
)
elif args.challenge == 5:
"""
Challenge 5: Mapping
Input: LidarPoints.csv, FlightPath.csv
Output: Mapping.csv
"""
dm = DroneMap(lidar_points_csv=args.lp_csv,
flight_path_csv=args.fp_csv)