从RGB-D生成点云

RGB图像+Depth图像生成点云数据

import os
os.environ['NUMEXPR_MAX_THREADS'] = '48'
import open3d as o3d
import cv2
import numpy as np

s_depth = cv2.imread("./depth.png", -1)
s_color = cv2.imread("./rgb.png")

def depth2pc(depth_img, rgb_img):

    # 相机内参
    cam_fx = 1067.29
    cam_fy = 1067.29
    cam_cx = 964.232
    cam_cy = 546.131
    factor = 1

    # 逐点处理，此过程可以使用numpy优化
    m, n = depth_img.shape
    color_map = []
    point_cloud = []
    for v in range(m):
        for u in range(n):
            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
            rgb_info = rgb_info[::-1]
            color_map.append(rgb_info)
            depth = depth_img[v, u]
            p_z = depth / factor
            p_x = (cam_cx - u) * p_z / cam_fx
            p_y = (cam_cy - v) * p_z / cam_fy
            point_cloud.append([p_z, p_x, p_y])
            
    point_cloud = np.array(point_cloud)
    # print(point_cloud)
    return point_cloud, color_map

points, color = depth2pc(s_depth,s_color)
print(points.shape)
point_cloud = o3d.geometry.PointCloud()
point_cloud.points = o3d.utility.Vector3dVector(points)
point_cloud.colors =o3d.utility.Vector3dVector(color)
o3d.visualization.draw_geometries([point_cloud])