def pipeline(scan,
label,
obstacle_lst,
verbose=False,
OBBoxes=False,
exec_time=False,
**params):
""" ROI filtering """
##########################################################################
start_time = datetime.now()
pcloud = pd.DataFrame(
np.concatenate((scan, label.reshape(len(label), 1)), axis=1),
columns=["x", "y", "z", "seg_id"],
)
pcloud = common.roi_filter(
pcloud,
min_x=params["roi_x_min"],
max_x=params["roi_x_max"],
min_y=params["roi_y_min"],
max_y=params["roi_y_max"],
min_z=params["roi_z_min"],
max_z=params["roi_z_max"],
verbose=False,
)
roi_time = (datetime.now() - start_time).total_seconds()
##########################################################################
""" Obstacles filtering """
##########################################################################
start_time = datetime.now()
pcloud = common.obstacle_filter(pcloud,
obstacle_lst,
proc_labels=True,
verbose=False)
obstacle_time = (datetime.now() - start_time).total_seconds()
##########################################################################
if len(pcloud) > 200:
# Getting voxel grid
start_time = datetime.now()
voxel_time = (datetime.now() - start_time).total_seconds()
""" Сlustering obstacles """
#######################################################################
start_time = datetime.now()
clusterer = DBSCAN(
eps=params["eps"],
min_samples=params["min_samples"],
algorithm="auto",
leaf_size=params["leaf_size"],
n_jobs=-1,
)
clusterer.fit(pcloud[["x", "y", "z"]])
pcloud["cluster_id"] = clusterer.labels_
cluster_time = (datetime.now() - start_time).total_seconds()
#######################################################################
""" Getting bounding boxes coord """
#######################################################################
start_time = datetime.now()
pcloud["norm"] = np.sqrt(
np.square(pcloud[["x", "y", "z"]]).sum(axis=1))
cluster_data = pd.DataFrame.from_dict({
"x": [],
"y": [],
"z": [],
"cluster_id": []
})
clusters = []
for _id in sorted(pcloud["cluster_id"].unique()):
if _id == -1 or not 50 < len(
pcloud[pcloud["cluster_id"] == _id]) < 5000:
continue
tcluster = pcloud[pcloud["cluster_id"] == _id]
tcluster = common.outlier_filter(tcluster, verbose=False)
cluster_data = cluster_data.append(tcluster)
if OBBoxes:
obb = common.get_OBB(tcluster[["x", "y", "z"]])
clusters.append(obb)
if not OBBoxes:
clusters = (cluster_data.groupby(["cluster_id"]).agg({
"x": ["min", "max"],
"y": ["min", "max"],
"z": ["min", "max"]
}).values)
bb_time = (datetime.now() - start_time).total_seconds()
#######################################################################
else:
clusters, cluster_data = np.empty((0, 0)), np.empty((0, 0))
voxel_time, cluster_time, bb_time = 0, 0, 0
if verbose:
print("Execution time:")
print("\n - ROI filtering: {:.5f}s".format(roi_time))
print("\n - Filtering obstacles: {:.5f}s".format(obstacle_time))
print("\n - Voxel grid: {:.5f}s".format(voxel_time))
print("\n - Clustering: {:.5f}s".format(cluster_time))
print("\n - Min-max cluster points: {:.5f}s \n".format(bb_time))
if exec_time:
return (
clusters,
cluster_data,
{
"roi_time": roi_time,
"filter_obstacle_time": obstacle_time,
"voxel_grid_time": voxel_time,
"clustering_time": cluster_time,
"outlier_filter_bbox_time": bb_time,
},
)
else:
return clusters, cluster_data
def pipeline_optimized_pcl(scan,
label,
obstacle_lst,
verbose=False,
exec_time=False,
**params):
# get segment id
start_time = datetime.now()
pcloud = pd.DataFrame(np.concatenate((scan, label.reshape(len(label), 1)),
axis=1),
columns=['x', 'y', 'z', 'seg_id'])
pcloud = common.roi_filter(pcloud,
min_x=params['roi_x_min'],
max_x=params['roi_x_max'],
min_y=params['roi_y_min'],
max_y=params['roi_y_max'],
min_z=params['roi_z_min'],
max_z=params['roi_z_max'],
verbose=False)
pcloud = common.obstacle_filter(pcloud,
obstacle_lst,
proc_labels=True,
verbose=False)
pcloud = pcloud.drop(['seg_id'], axis=1)
pcloud = pcloud.drop(['camera'], axis=1)
obstacle_time = datetime.now() - start_time
if (len(pcloud.index) > 0):
start_time = datetime.now()
pcloud_pcl = pcl.PointCloud()
pcloud_pcl.from_array(pcloud.to_numpy(dtype=np.float32))
convert_time = datetime.now() - start_time
# get voxel grid
start_time = datetime.now()
voxelgrid_id = pcl_utils.voxel_filter(
pcloud_pcl,
[params['x_voxels'], params['y_voxels'], params['z_voxels']])
#voxelgrid_id = pcloud_pcl
voxel_time = datetime.now() - start_time
# ROI filter
start_time = datetime.now()
pcloud_roi = pcl_utils.roi_filter(
voxelgrid_id,
[params['roi_x_min'], params['roi_x_max']],
[params['roi_y_min'], params['roi_y_max']],
[params['roi_z_min'], params['roi_z_max']],
)
roi_time = datetime.now() - start_time
# get cluster
start_time = datetime.now()
cluster_data = pcloud_roi.extract([], negative=True)
cluster_indices = pcl_utils.clustering(cluster_data,
params['tol_distance'],
params['min_cluster_size'],
150000)
clustering_time = datetime.now() - start_time
# get bboxes
start_time = datetime.now()
box_min_max_list, _ = pcl_utils.get_cluster_box_list(
cluster_indices,
cluster_data,
radius_search=params['radius_search'],
min_neighbors_in_radius=params['min_neighbors_in_radius'])
bbox_time = datetime.now() - start_time
else:
box_min_max_list, cluster_data = np.empty((0, 0)), np.empty((0, 0))
roi_time, obstacle_time, voxel_time, clustering_time, bbox_time = 0, 0, 0, 0, 0
if verbose:
print('Execution time:')
print('\n - ROI filtering: {:.5f} s'.format(roi_time.total_seconds()))
print('\n - Filtering obstacles: {:.5f} s'.format(
obstacle_time.total_seconds()))
print('\n - Voxel grid: {:.5f} s'.format(voxel_time.total_seconds()))
print('\n - Clustering: {:.5f} s'.format(
clustering_time.total_seconds()))
print('\n - Min-max cluster points: {:.5f} s \n'.format(
bbox_time.total_seconds()))
if exec_time:
return box_min_max_list, cluster_data, {
'roi_time': roi_time.total_seconds(),
'filter_obstacle_time': obstacle_time.total_seconds(),
'voxel_grid_time': voxel_time.total_seconds(),
'clustering_time': clustering_time.total_seconds(),
'outlier_filter_bbox_time': bbox_time.total_seconds(),
'convert_time': convert_time.total_seconds()
}
else:
return box_min_max_list, cluster_data
def pipeline_optimized_pcl(scan,
label,
obstacle_lst,
verbose=False,
exec_time=False,
**params):
# get segment id
start_time = datetime.now()
pcloud = pd.DataFrame(
np.concatenate((scan, label.reshape(len(label), 1)), axis=1),
columns=["x", "y", "z", "seg_id"],
)
pcloud = common.roi_filter(
pcloud,
min_x=params["roi_x_min"],
max_x=params["roi_x_max"],
min_y=params["roi_y_min"],
max_y=params["roi_y_max"],
min_z=params["roi_z_min"],
max_z=params["roi_z_max"],
verbose=False,
)
pcloud = common.obstacle_filter(pcloud,
obstacle_lst,
proc_labels=True,
verbose=False)
pcloud = pcloud.drop(["seg_id"], axis=1)
pcloud = pcloud.drop(["camera"], axis=1)
obstacle_time = datetime.now() - start_time
if len(pcloud.index) > 0:
start_time = datetime.now()
pcloud_pcl = pcl.PointCloud()
pcloud_pcl.from_array(pcloud.to_numpy(dtype=np.float32))
convert_time = datetime.now() - start_time
# get voxel grid
start_time = datetime.now()
voxelgrid_id = pcl_utils.voxel_filter(
pcloud_pcl,
[params["x_voxels"], params["y_voxels"], params["z_voxels"]])
# voxelgrid_id = pcloud_pcl
voxel_time = datetime.now() - start_time
# ROI filter
start_time = datetime.now()
pcloud_roi = pcl_utils.roi_filter(
voxelgrid_id,
[params["roi_x_min"], params["roi_x_max"]],
[params["roi_y_min"], params["roi_y_max"]],
[params["roi_z_min"], params["roi_z_max"]],
)
roi_time = datetime.now() - start_time
# get cluster
start_time = datetime.now()
cluster_data = pcloud_roi.extract([], negative=True)
cluster_indices = pcl_utils.clustering(cluster_data,
params["tol_distance"],
params["min_cluster_size"],
150000)
clustering_time = datetime.now() - start_time
# get bboxes
start_time = datetime.now()
box_min_max_list, _ = pcl_utils.get_cluster_box_list(
cluster_indices,
cluster_data,
radius_search=params["radius_search"],
min_neighbors_in_radius=params["min_neighbors_in_radius"],
)
bbox_time = datetime.now() - start_time
else:
box_min_max_list, cluster_data = np.empty((0, 0)), np.empty((0, 0))
roi_time, obstacle_time, voxel_time = 0, 0, 0
clustering_time, bbox_time = 0, 0
if verbose:
print("Execution time:")
print("\n-ROI filtering: {:.5f}s".format(roi_time.total_seconds()))
print("\n-Filtering obstacles: {:.5f}s".format(
obstacle_time.total_seconds()))
print("\n-Voxel grid: {:.5f}s".format(voxel_time.total_seconds()))
print("\n-Clustering: {:.5f}s".format(clustering_time.total_seconds()))
print("\n-Min-max cluster points: {:.5f} s \n".format(
bbox_time.total_seconds()))
if exec_time:
return (
box_min_max_list,
cluster_data,
{
"roi_time": roi_time.total_seconds(),
"filter_obstacle_time": obstacle_time.total_seconds(),
"voxel_grid_time": voxel_time.total_seconds(),
"clustering_time": clustering_time.total_seconds(),
"outlier_filter_bbox_time": bbox_time.total_seconds(),
"convert_time": convert_time.total_seconds(),
},
)
else:
return box_min_max_list, cluster_data
def pipeline(scan, label, obstacle_lst, verbose=False, OBBoxes=False, exec_time=False, **params):
""" ROI filtering """
##################################################################################################
start_time = datetime.now()
pcloud = pd.DataFrame(np.concatenate((scan, label.reshape(len(label), 1)), axis=1),
columns=['x', 'y', 'z', 'seg_id'])
pcloud = common.roi_filter(pcloud, min_x=params['roi_x_min'], max_x=params['roi_x_max'],
min_y=params['roi_y_min'], max_y=params['roi_y_max'],
min_z=params['roi_z_min'], max_z=params['roi_z_max'],
verbose=False)
roi_time = (datetime.now() - start_time).total_seconds()
###################################################################################################
""" Obstacles filtering """
###################################################################################################
start_time = datetime.now()
pcloud = common.obstacle_filter(pcloud, obstacle_lst, proc_labels=params['proc_labels'], verbose=False)
obstacle_time = (datetime.now() - start_time).total_seconds()
###################################################################################################
if len(pcloud) > 200:
# Getting voxel grid
start_time = datetime.now()
voxel_time = (datetime.now() - start_time).total_seconds()
""" Сlustering obstacles """
###############################################################################################
start_time = datetime.now()
clusterer = DBSCAN(eps=params['eps'], min_samples=params['min_samples'],
algorithm='auto', leaf_size=params['leaf_size'], n_jobs=-1)
clusterer.fit(pcloud[['x', 'y', 'z']])
pcloud['cluster_id'] = clusterer.labels_
cluster_time = (datetime.now() - start_time).total_seconds()
###############################################################################################
""" Getting bounding boxes coord """
###############################################################################################
start_time = datetime.now()
pcloud['norm'] = np.sqrt(np.square(pcloud[['x', 'y', 'z']]).sum(axis=1))
cluster_data = pd.DataFrame.from_dict({'x': [], 'y': [], 'z': [],'cluster_id': []})
clusters = []
for _id in pcloud['cluster_id'].unique():
if _id == -1 or len(pcloud[pcloud['cluster_id'] == _id]) < 100 or len(pcloud[pcloud['cluster_id'] == _id]) > 2500:
continue
tcluster = common.outlier_filter(pcloud[pcloud['cluster_id'] == _id], verbose=False)
cluster_data = cluster_data.append(tcluster)
if OBBoxes:
obb = OBB.build_from_points(tcluster[['x', 'y', 'z']].values)
clusters.append([x.tolist() for x in obb.points])
if not OBBoxes:
clusters = cluster_data.groupby(['cluster_id']).agg({ 'x': ['min', 'max'],
'y': ['min', 'max'],
'z': ['min', 'max'] }).values
bb_time = (datetime.now() - start_time).total_seconds()
###############################################################################################
else:
clusters, cluster_data = np.empty((0, 0)), np.empty((0, 0))
voxel_time, cluster_time, bb_time = 0, 0, 0
if verbose:
print('Execution time:')
print('\n - ROI filtering: {:.5f}s'.format(roi_time))
print('\n - Filtering obstacles: {:.5f}s'.format(obstacle_time))
print('\n - Voxel grid: {:.5f}s'.format(voxel_time))
print('\n - Clustering: {:.5f}s'.format(cluster_time))
print('\n - Min-max cluster points: {:.5f}s \n'.format(bb_time))
if exec_time:
return clusters, cluster_data, {'roi_time': roi_time,
'filter_obstacle_time': obstacle_time,
'voxel_grid_time': voxel_time,
'clustering_time': cluster_time,
'outlier_filter_bbox_time': bb_time}
else:
return clusters, cluster_data
def test_obstacle_filter_3(self):
pcloud = get_pcloud(scan_lst[2], label_lst[2])
cloud = common.obstacle_filter(pcloud, obstacle_lst, proc_labels=False)
seg_lst = list(cloud['seg_id'].unique())
for seg in seg_lst:
assert seg in list(obstacle_lst.keys())