Boustrophedon Cellular Decomposition的Python实现
算法简介
BCD(Boustrophedon Cellular Decomposition)是一种栅格地图的分解方法。完成该分解后,每个cell都可以通过一个牛耕式路径遍历。不同cell之间通过旅行商问题获得最优路径即可做到全地图的覆盖式清扫。详情可以参考论文
Choset, H. (2000). Coverage of Known Spaces: The Boustrophedon Cellular Decomposition. Autonomous Robots (Vol. 9).
算法原理非常简单。地图中的每一列称为一个slice
。遍历地图中的slice
,并计算区块的连通性。每当连通性增加,则触发一个IN事件,封闭当前cell,并开启两个新的cell。若遇到连通性降低,则触发一个OUT事件,当前的两个cell封闭,并开启一个新的cell。
Python代码
# This is the code of Boustrophedon Cellular Decomposition algorithm.
# I write it in python3.6.
# For more details, please read the paper:
# Choset, H. (2000). Coverage of Known Spaces: The Boustrophedon Cellular Decomposition. Autonomous Robots (Vol. 9).
#
#
# - Dechao Meng
import numpy as np
import cv2
from PIL import Image
from matplotlib import pyplot as plt
import matplotlib
from typing import Tuple, List
import random
# matplotlib.rcParams['figure.figsize'] = [30, 20]
Slice = List[Tuple[int, int]]
def calc_connectivity(slice: np.ndarray) -> Tuple[int, Slice]:
"""
计算一个slice的连通性,并且返回该slice的连通区域。
Args:
slice: rows. A slice of map.
Returns:
The connectivity number and connectivity parts.
Examples:
>>> data = np.array([0,0,0,0,1,1,1,0,1,0,0,0,1,1,0,1,1,0])
>>> print(calc_connectivity(data))
(4, [(4, 7), (8, 9), (12, 14), (15, 17)])
"""
connectivity = 0
last_data = 0
open_part = False
connective_parts = []
for i, data in enumerate(slice):
if last_data == 0 and data == 1:
open_part = True
start_point = i
elif last_data == 1 and data == 0 and open_part:
open_part = False
connectivity += 1
end_point = i
connective_parts.append((start_point, end_point))
last_data = data
return connectivity, connective_parts
def get_adjacency_matrix(parts_left: Slice, parts_right: Slice) -> np.ndarray:
"""
Get adjacency matrix of 2 neiborhood slices.
Args:
slice_left: left slice
slice_right: right slice
Returns:
[L, R] Adjacency matrix.
"""
adjacency_matrix = np.zeros([len(parts_left), len(parts_right)])
for l, lparts in enumerate(parts_left):
for r, rparts in enumerate(parts_right):
if min(lparts[1], rparts[1]) - max(lparts[0], rparts[0]) > 0:
adjacency_matrix[l, r] = 1
return adjacency_matrix
def bcd(erode_img: np.ndarray) -> Tuple[np.ndarray, int]:
"""
Boustrophedon Cellular Decomposition
Args:
erode_img: [H, W], eroded map. The pixel value 0 represents obstacles and 1 for free space.
Returns:
[H, W], separated map. The pixel value 0 represents obstacles and others for its' cell number.
"""
assert len(erode_img.shape) == 2, 'Map should be single channel.'
last_connectivity = 0
last_connectivity_parts = []
current_cell = 1
current_cells = []
separate_img = np.copy(erode_img)
for col in range(erode_img.shape[1]):
current_slice = erode_img[:, col]
connectivity, connective_parts = calc_connectivity(current_slice)
if last_connectivity == 0:
current_cells = []
for i in range(connectivity):
current_cells.append(current_cell)
current_cell += 1
elif connectivity == 0:
current_cells = []
continue
else:
adj_matrix = get_adjacency_matrix(last_connectivity_parts, connective_parts)
new_cells = [0] * len(connective_parts)
for i in range(adj_matrix.shape[0]):
if np.sum(adj_matrix[i, :]) == 1:
new_cells[np.argwhere(adj_matrix[i, :])[0][0]] = current_cells[i]
# 如果上一次的某个part与这次的多个parts相联通,说明发生了IN。
elif np.sum(adj_matrix[i, :]) > 1:
for idx in np.argwhere(adj_matrix[i, :]):
new_cells[idx[0]] = current_cell
current_cell = current_cell + 1
for i in range(adj_matrix.shape[1]):
# 如果这一次的某个part与上次的多个parts相联通,说明发生了OUT。
if np.sum(adj_matrix[:, i]) > 1:
new_cells[i] = current_cell
current_cell = current_cell + 1
# 如果这次的某个part不与上次任何一个part联通,说明发生了in
elif np.sum(adj_matrix[:, i]) == 0:
new_cells[i] = current_cell
current_cell = current_cell + 1
current_cells = new_cells
# 将分区信息画在地图上。
for cell, slice in zip(current_cells, connective_parts):
# print(current_cells, connective_parts)
separate_img[slice[0]:slice[1], col] = cell
# print('Slice {}: connectivity from {} to {}'.format(col, last_connectivity, connectivity))
last_connectivity = connectivity
last_connectivity_parts = connective_parts
return separate_img, current_cell
def display_separate_map(separate_map, cells):
display_img = np.empty([*separate_map.shape, 3], dtype=np.uint8)
random_colors = np.random.randint(0, 255, [cells, 3])
for cell_id in range(1, cells):
display_img[separate_map == cell_id, :] = random_colors[cell_id, :]
plt.imshow(display_img)
plt.show()
if __name__ == '__main__':
# print(get_adjacency_matrix([(138, 140), (310, 313), (337, 338)], [(138, 140), (310, 312), (337, 338)]))
# kernel = np.ones(12)
# plt.imshow(img, cmap='gray')
# plt.show()
# erode_img = 1 - cv2.erode(img, kernel) / 255
img = np.array(Image.open('../../map/test_map.png'))
if len(img.shape) > 2:
img = img[:, :, 0]
erode_img = img / np.max(img)
separate_img, cells = bcd(erode_img)
print('Total cells: {}'.format(cells))
display_separate_map(separate_img, cells)