def test():
# 相机参数, 59.8度640x480相机
CAM_FX, CAM_FY = 795.209, 793.957
CAM_CX, CAM_CY = 332.031, 231.308
CAM_DVEC = np.array([-0.33354, 0.00924849, -0.000457208, -0.00215353, 0.0])
img_dep = np.genfromtxt(
'/Users/seaside/Desktop/【第02部分 三维点云专题】三位建模感知与点云视频教程 5套/03.2020年3D感知技术培训视频教程（附代码、讲义、参考资料）49课/00.配套资料（代码、讲义、作业、参考文献）/00.配套资料（代码、讲义、作业、参考文献）/02.3D传感器技术原理/CH2-HW/img_dep_640x480.csv',
delimiter=',').astype(np.float32)
IMG_HGT, IMG_WID = img_dep.shape
if True:
plt.imshow(img_dep, cmap='jet')
plt.show()
img_z = dist_to_z(img_dep, CAM_CX, CAM_CY,
(CAM_FX + CAM_FY) / 2.0) # 计算“视线距离”到Z的修正
# 计算点云
pc = img_z_to_pc(img_z, CAM_CX, CAM_CY, CAM_FX, CAM_FY)
# 显示点云
if True:
from pc_view import pc_view
pc_view(pc,\
CAM_FX,CAM_FY,CAM_CX,CAM_CY,IMG_WID,IMG_HGT,\
cz=0.5, \
dmin=0.5,dmax=1.5)
# 旋转平移
# from pc_trans import *
pc_new = pc + [0, 0, -0.5]
R = calc_matrix_roty(math.radians(30))
pc_new = np.dot(pc_new, R)
pc_new = pc_new + [0, 0, 1]
if False:
np.savetxt(
'/Users/seaside/Desktop/【第02部分 三维点云专题】三位建模感知与点云视频教程 5套/03.2020年3D感知技术培训视频教程（附代码、讲义、参考资料）49课/00.配套资料（代码、讲义、作业、参考文献）/00.配套资料（代码、讲义、作业、参考文献）/03.3D数据表示和转换/CH3-HW/pc_rot.csv',
pc_new,
fmt='%.8f',
delimiter=',',
newline='\n',
header='',
footer='',
comments='# ')
# 显示平移旋转后的点云
if True:
from pc_view import pc_view
pc_view(pc_new,\
CAM_FX,CAM_FY,CAM_CX,CAM_CY,IMG_WID,IMG_HGT,\
cz=0.5, \
dmin=0.5,dmax=1.5)
# 点云重新投影为深度图
img_z_new = pc_to_img_z(pc_new, CAM_FX, CAM_FY, CAM_CX, CAM_CY, IMG_HGT,
IMG_WID)
plt.imshow(img_z_new, cmap='jet')
plt.show()
# “透射”消除
img_z_new2 = remove_hole(img_z_new)
plt.imshow(img_z_new2, cmap='jet')
plt.show()
import numpy as np
GRID_SIZE=0.01 # 格点尺寸
# 加载原始点云数据
pc=np.genfromtxt('pc.csv', delimiter=',').astype(np.float32)
###############################
# 需要你在下面填写代码实现点云降采样
# 下面的代码只是一个随机生成的点云例子，需要你修改
pc_new=np.random.rand(50000,3)*0.1-0.05+[0,0,1]
###############################
# 保存结果
np.savetxt('pc_new.csv', pc_new, fmt='%.8f', delimiter=',', newline='\n')
# 3D显示结果
pc_new=np.genfromtxt('pc_new.csv',delimiter=',').astype(np.float32) # 加载保存的数据
# 显示参数
CAM_WID,CAM_HGT = 640,480
CAM_FX,CAM_FY = 795.209,793.957
CAM_CX,CAM_CY = 332.031,231.308
from pc_view import pc_view
pc_view(pc, CAM_FX,CAM_FY,CAM_CX,CAM_CY,CAM_WID,CAM_HGT)
pc_view(pc_new, CAM_FX,CAM_FY,CAM_CX,CAM_CY,CAM_WID,CAM_HGT)
# 程序常量
TH_DIST = 1.5 # 距离门限，截取这个距离内的点云
TH1 = 0.012 # 背景分离门限
TH2 = 0.008 # 离群点滤除参数和物体点云聚合的距离门限
TH_OBJ_SZ = 500 # 尺寸门限，滤除点云太小的物体
print('loading CSV...')
pc = np.genfromtxt('pc.csv', delimiter=',').astype(np.float32)
pc_bg = np.genfromtxt('pc_bg.csv', delimiter=',').astype(np.float32)
pc_view(pc,
CAM_FX,
CAM_FY,
CAM_CX,
CAM_CY,
CAM_WID,
CAM_HGT,
cz=1,
dmin=0,
dmax=2)
pc_view(pc_bg,
CAM_FX,
CAM_FY,
CAM_CX,
CAM_CY,
CAM_WID,
CAM_HGT,
cz=1,
dmin=0,
dmax=2)
tab_dep_to_z = 1.0 / np.sqrt(tab_x**2 / (CAM_FX**2) + tab_y**2 /
(CAM_FY**2) + 1)
tab_dep_to_x = tab_x / CAM_FX * tab_dep_to_z
tab_dep_to_y = tab_y / CAM_FX * tab_dep_to_z
# 加载数据
img_dep = np.genfromtxt(fname,
delimiter=',').reshape(CAM_HGT,
CAM_WID).astype(np.float32)
img_dep = cv2.undistort(img_dep, CAM_MAT, CAM_DIST)
# 深度转点云坐标
pc_z = img_dep * tab_dep_to_z
pc_x = img_dep * tab_dep_to_x
pc_y = img_dep * tab_dep_to_y
pc = np.array([pc_x.ravel(), pc_y.ravel(), pc_z.ravel()]).T
if False:
plot_pc(pc)
else:
from pc_view import pc_view
pc_view(pc + [-0.5, -0.5, 0.5],
CAM_CX,
CAM_CY,
CAM_FX,
CAM_FY,
CAM_WID,
CAM_HGT,
dmin=0,
dmax=2)