Boustrophedon Cellular Decomposition的Python实现 |
您所在的位置：网站首页 › 标签管理体系 › Boustrophedon Cellular Decomposition的Python实现 |
Boustrophedon Cellular Decomposition的Python实现
|
Boustrophedon Cellular Decomposition的Python实现
算法简介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) 执行结果
|
【本文地址】
公司简介
联系我们
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |