把三维的点投影到二维图像,找图像边缘,然后反向映射,找到对应的边缘点
# coding:utf-8
import open3d as o3d
import numpy as np
import cv2
def img_edge(point_cloud_np):
# 把三维的点投影到二维图像,找图像边缘,然后反向映射,找到对应的边缘点
vol_bnds = np.zeros((3, 2))
vol_bnds[:, 0] = np.minimum(vol_bnds[:, 0], np.amin(point_cloud_np, axis=0))
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1], np.amax(point_cloud_np, axis=0))
print(vol_bnds)
# leafsize = 0.1
leafsize = 0.2
vol_dim = np.ceil((vol_bnds[:, 1] - vol_bnds[:, 0]) / leafsize).copy(order='C').astype(int)
print(vol_dim)
# tsdf_vol_cpu = np.ones(vol_dim).astype(np.float32)
shift = 100
bev_map = np.zeros((vol_dim[1]+shift, vol_dim[0]+shift))
for p in point_cloud_np:
cx = np.floor((p[0] - vol_bnds[0, 0]) / leafsize)
cy = np.floor((p[1] - vol_bnds[1, 0]) / leafsize)