三维重建——RGBD生成点云
1.安装open3d
pip install open3d
2.三维重建代码
公式:
(
u
,
v
)
是像素坐标系
,
(
X
c
,
Y
c
)
是相机坐标系
(u,v)是像素坐标系,(X_c,Y_c)是相机坐标系
(u,v)是像素坐标系,(Xc,Yc)是相机坐标系
Z
c
[
u
v
1
]
=
[
k
x
u
0
k
y
v
0
1
]
[
X
c
Y
c
Z
c
]
Z_c\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} k_x & & u_0 \\ & k_y & v_0 \\ & & 1 \end{bmatrix}\begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}
Zc⎣
⎡uv1⎦
⎤=⎣
⎡kxkyu0v01⎦
⎤⎣
⎡XcYcZc⎦
⎤
注:相机坐标系和世界坐标系差一个旋转和平移的变换,但变换由于是一个刚体变换,所以三维场景的相对位置是不会发生变化的,所以三维重建在相机坐标系下恢复即可。
import open3d as o3d
import cv2
import numpy as np
from run import DepthDetect
import matplotlib.pyplot as plt
def RGBD2Point(depth_img, rgb_img):
# 相机内参
k_x = 5.775910000000000082e+02
k_y = 5.787300000000000182e+02
u_0 = 3.189049999999999727e+02
v_0 = 2.426839999999999975e+02
factor = 1
K = [[k_x, 0, u_0], [0, k_y, v_0], [0, 0, 1]]
# 逐点处理,此过程可以使用numpy优化
m, n = depth_img.shape # 480 640
color_map, point_cloud = [], []
for v in range(m): # 行相当于y坐标
for u in range(n): # 列相当于x坐标
if depth_img[v, u] == 0:
continue
rgb = rgb_img[v, u]
rgb = [rgb[0], rgb[1], rgb[2]]
rgb_info = np.array(rgb) / 255.0 # 颜色归一化到0-1之间
rgb_info = rgb_info[::-1] # cv2读取数据格式为BGR
color_map.append(rgb_info)
depth = depth_img[v, u]
# 矩阵运算速度较慢
# x_c, y_c, z_c = np.transpose(np.dot(np.linalg.inv(K), np.transpose(depth * np.array([u, v, 1]))))
z_c = depth / factor
x_c = (u - u_0) * z_c / k_x
y_c = (v - v_0) * z_c / k_y
point_cloud.append([x_c, y_c, z_c])
point_cloud = np.array(point_cloud)
color_map = np.array(color_map)
return point_cloud, color_map # shape都是(212342, 3) point为(x,y,z) color为(r,g,b)
depth_path = "test_imgs/0000_depth.npy"
color_path = "test_imgs/0000_color.png"
s_depth = np.load(depth_path) # 480 640
s_color = cv2.imread(color_path) # (480, 640, 3)
detector = DepthDetect()
s_depth = detector.run(color_path,depth_path)
points, color = RGBD2Point(s_depth, s_color)
pc = o3d.geometry.PointCloud()
pc.points = o3d.utility.Vector3dVector(points)
pc.colors = o3d.utility.Vector3dVector(color)
o3d.visualization.draw_geometries([pc]) # 可视化
o3d.io.write_point_cloud(color_path.replace("color.png","pointcloud_completion.ply"), pc) # 保存文件