import open3d as o3d
import numpy as np
import cv2
file_path= 'C:/jsh/kitti/data_object_velodyne/training/velodyne/000000.bin'
# (N, 4) 형식의numpyarray로변환, 행크기는알아서결정하고열크기는4개
points = np.fromfile(file_path, dtype=np.float32).reshape(-1, 4)
print("배열크기:", points.shape) # 넘파이어레이크기출력
vis = o3d.visualization.Visualizer()
vis.create_window(window_name='kitti',width=960, height=540)
vis.get_render_option().point_size= 1.0
vis.get_render_option().background_color= np.zeros(3) # (0,0,0)->검정
axis_pcd= o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])
vis.add_geometry(axis_pcd)
dist = (points[:,2] <= -1.45) & (points[:,2] >= - 2. )
print("z -1.45 ~ -2 m 이하 포인트 수:", np.sum(dist)) # 출력
road_points = points[dist] # ture값만 사용
else_points = points[~dist]
pcd= o3d.geometry.PointCloud()# 포인트클라우드객체생성
pcd.points= o3d.utility.Vector3dVector(else_points[:, :3])# reflectance는제외
pcd.colors= o3d.utility.Vector3dVector(np.ones((else_points.shape[0], 3)))# (1,1,1)->흰색
vis.add_geometry(pcd)
# 2번 문제는 이 부분 제거 후 실행함
pcd_red= o3d.geometry.PointCloud()# 포인트클라우드객체생성
pcd_red.points= o3d.utility.Vector3dVector(road_points[:, :3])# reflectance는제외
red = np.zeros((road_points.shape[0], 3))
red[:, 0] = 1
pcd_red.colors= o3d.utility.Vector3dVector(red)
vis.add_geometry(pcd_red)
# set zoom, front, up, and lookat
vis.get_view_control().set_zoom(0.1)
vis.get_view_control().set_front([0, -1, 1])
vis.get_view_control().set_lookat([0, 0, 0])
vis.get_view_control().set_up([0, 1, 1])
vis.run()
vis.destroy_window()