Project
[PCD] Cuboid 필터링 하기
scone
2023. 2. 1. 18:45
주의, 사내에서 출력되는 데이터를 가지고 진행하였기 때문에
일반적인 경우와는 전혀 다를 수 있습니다.
다음은 현재 사내에서 나오는 pcd 출력물 형태 입니다.
보시다시피 현재 pcd 와 관계 없이 cuboid를 출력하고 있습니다.
시뮬레이션에서 정보를 뽑아 오다 보니 pcd에서 cuboid를 뽑아온게 아니라
각 사물들의 위치를 모두 알고 있는 상태에서, cuboid 별도, pcd 별도로 뽑아와서 이렇게 된 것 같은데요.
저는 pointpillars 를 학습 하기 위해 각각의 객체의 cuboid와 그 안의 points를 모델이 input할 계획이기 때문에
points 가 없는 cuboid는 제거해주어야 할 것 같습니다.
현재 정부 과제 사업을 위해 시뮬레이션에서 데이터를 출력하는 시간 또한 매우 중요한 상황이기 때문에
위 내용은 후처리로 진행해야할 듯 싶습니다.
전략
1. objTag가 객체의 클래스를 나타내는 값이라는 것을 알았습니다.
2. objTag와 Cuboid 좌표를 이용하여, 각 Cuboid 안에서 points들의 갯수를 셀 것입니다.
3. points 갯수가 적은 Cuboid에 대하여 출력을 하지 않을 것입니다.
단순히 pcd를 그리는 것이라면 아래와 같이 o3d를 사용할 수 있습니다.
import open3d as o3d
import numpy as np
def show_open3d_pcd(pcd, show_origin=True, origin_size=3, show_grid=True):
cloud = o3d.geometry.PointCloud()
v3d = o3d.utility.Vector3dVector
if isinstance(pcd, type(cloud)):
pass
elif isinstance(pcd, np.ndarray):
cloud.points = v3d(pcd)
coord = o3d.geometry.TriangleMesh().create_coordinate_frame(size=origin_size, origin=np.array([0.0, 0.0, 0.0]))
# set front, lookat, up, zoom to change initial view
o3d.visualization.draw_geometries([cloud, coord])pcd_path = './gtprogram_ver.0.0.11/PCD/1674782460103.pcd'
pcd = o3d.io.read_point_cloud(pcd_path,format="xyz")
pcd = np.asarray(pcd.points)
show_open3d_pcd(pcd)
o3d.io.read_point_cloud는 단순히 x, y, z값 만 뽑아내는 것 같더라구요.
문제 해결
PCD를 Visualize 하는 코드
import open3d as o3d
import numpy as np
import json
import cv2
import open3d as o3d
import os
def show_open3d_pcd(pcd, show_origin=True, origin_size=3, show_grid=True):
cloud = o3d.geometry.PointCloud()
v3d = o3d.utility.Vector3dVector
if isinstance(pcd, type(cloud)):
pass
elif isinstance(pcd, np.ndarray):
cloud.points = v3d(pcd)
coord = o3d.geometry.TriangleMesh().create_coordinate_frame(size=origin_size, origin=np.array([0.0, 0.0, 0.0]))
# set front, lookat, up, zoom to change initial view
o3d.visualization.draw_geometries([cloud, coord])
Cuboid 오버레이 하는 코드
COLORS = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0]]
COLORS_IMG = [[0, 0, 255], [0, 255, 0], [255, 0, 0], [0, 255, 255]]
LINES = [
[0, 1],
[1, 2],
[2, 3],
[3, 0],
[4, 5],
[5, 6],
[6, 7],
[7, 4],
[2, 6],
[7, 3],
[1, 5],
[4, 0]
]
def npy2ply(npy):
ply = o3d.geometry.PointCloud()
ply.points = o3d.utility.Vector3dVector(npy[:, :3])
density = npy[:, 3]
colors = [[item, item, item] for item in density]
ply.colors = o3d.utility.Vector3dVector(colors)
return ply
def ply2npy(ply):
return np.array(ply.points)
def bbox_obj(points, color=[1, 0, 0]):
colors = [color for i in range(len(LINES))]
line_set = o3d.geometry.LineSet(
points=o3d.utility.Vector3dVector(points),
lines=o3d.utility.Vector2iVector(LINES),
)
line_set.colors = o3d.utility.Vector3dVector(colors)
return line_set
def vis_core(plys):
vis = o3d.visualization.Visualizer()
vis.create_window()
ctr = vis.get_view_control()
param = o3d.io.read_pinhole_camera_parameters('./viewpoint.json')
for ply in plys:
vis.add_geometry(ply)
ctr.convert_from_pinhole_camera_parameters(param)
vis.run()
# param = vis.get_view_control().convert_to_pinhole_camera_parameters()
# o3d.io.write_pinhole_camera_parameters(os.path.join(PAR, 'viewpoint.json'), param)
vis.destroy_window()
def vis_pc(pc, bboxes=None, labels=None):
'''
pc: ply or np.ndarray (N, 4)
bboxes: np.ndarray, (n, 7) or (n, 8, 3)
labels: (n, )
'''
if isinstance(pc, np.ndarray):
pc = npy2ply(pc)
mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=10, origin=[0, 0, 0])
if bboxes is None:
vis_core([pc, mesh_frame])
return
vis_objs = [pc, mesh_frame]
for i in range(len(bboxes)):
bbox = bboxes[i]
if labels is None:
color = [1, 0, 0]
else:
if labels[i] >= 0 and labels[i] < 3:
color = COLORS[labels[i]]
else:
color = COLORS[-1]
vis_objs.append(bbox_obj(bbox, color=color))
vis_core(vis_objs)
Cuboid 좌표 값 출력
def cuboid_word_list(json_path):
## json 파일 경로
with open(json_path, "r") as json_file :
data_info = json.load(json_file)
for i in range(len(data_info['gt_data']['label_data']['cuboid_world'])):
if i == 0 :
lidar_bboxes_points = np.array([[[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['right']['bottom'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['right']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['left']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['left']['bottom'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['right']['bottom'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['right']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['left']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['left']['bottom'].values()],
]])
else :
lidar_bboxes_points = np.append(lidar_bboxes_points,
np.array([[
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['right']['bottom'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['right']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['left']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['back']['left']['bottom'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['right']['bottom'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['right']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['left']['top'].values()],
[x for x in data_info['gt_data']['label_data']['cuboid_world'][i]['front']['left']['bottom'].values()],
]])
, axis = 0)
return lidar_bboxes_points
필터링 한 Cuboid
- np.loadtxt 를 이용해 pcd를 출력하여 가져왔습니다.
- pcd 안의 속성 중 OBJTag 를 이용하여, 객체에 대한 PCD 값 만을 가져왔습니다.
- Cuboid x, y, z 값 내부의 points 들의 점을 셌습니다.
- points가 10보다 작을 경우 제외하였습니다.
def filter_cuboid_list(pcd_path, json_path):
# load txt, skiprows를 통해 윗 줄 10줄 빼고 가져왔습니다.
pcd_array = np.loadtxt(pcd_path, dtype=np.float32, skiprows=10, delimiter=' ')
# OBJTag : pcd_array[:,-1] 가 0보다 크면 instance (OBJTag는 마지막 열에 있음)
instance_pcd = pcd_array[np.where(pcd_array[:,-1]>0)]
# Cuboid list를 가져옴 (n, 8, 3) 형태
cuboid_list = cuboid_word_list(json_path)
# 점의 갯수가 적은거 필터링 할꺼
filter_cuboid_list = []
for idx, obj in enumerate(cuboid_list):
# points_idx는 cuboid 범위 안의 points들 index
points_count = sum ( (min(obj[:,0]) <= instance_pcd[:,0]) & (instance_pcd[:,0] <= max(obj[:,0]))
& (min(obj[:,1]) <= instance_pcd[:,1]) & (instance_pcd[:,1] <= max(obj[:,1]))
& (min(obj[:,2]) <= instance_pcd[:,2]) & (instance_pcd[:,2] <= max(obj[:,2]))
)
# 점의 갯수가 10개보다 작으면 필터링
if points_count < 10 :
# 디버그를 위한 프린트
# print(len(points_idx))
continue
else :
filter_cuboid_list.append(idx)
return cuboid_list[filter_cuboid_list]
출력
- 출력 결과는 위의 이미지와 같습니다.
- 필터링 전 Cuboid는 점이 없음에도 출력되나, 필터링 이후 Cuboid는 점이 있는 Cuboid만을 출력합니다.
json_path = "./gtprogram_ver.0.0.11/outputJson/1674782462239.json"
pcd_path = "./gtprogram_ver.0.0.11/PCD/1674782462239.pcd"
pcd = o3d.io.read_point_cloud(pcd_path,format="xyz")
cuboids = filter_cuboid_list(pcd_path, json_path)
# pcd_print = np.asarray(pcd.points)
vis_pc(pcd, cuboids)