问在Opencv中将图像与深度图对齐

EN

# All what we need to align our images:

depth_scale = 0.001

fx_d = K_depth[0,0]
fy_d = K_depth[1,1]
cx_d = K_depth[0,2]
cy_d = K_depth[1,2]

fx_rgb = K_col[0,0]
fy_rgb = K_col[1,1]
cx_rgb = K_col[0,2]
cy_rgb = K_col[1,2]

height = depth.shape[0]
width = depth.shape[1]

aligned = np.zeros((height,width,6))

for v in range (height):
    for u in range (width):
        # Apply Depth intrinsics
        z = depth[v,u].sum() * depth_scale
        x = ((u - cx_d) * z) / fx_d
        y = ((v - cy_d) * z) / fy_d
        
        # Apply extrinsic
        transformed = np.dot(ext ,np.array([x,y,z,1])).T
        aligned[v,u,0] = transformed[0]
        aligned[v,u,1] = transformed[1]
        aligned[v,u,2] = transformed[2]
        
for v in range (height):
    for u in range (width):
        # Apply RGB intrinsic
        x = (aligned[v,u,0] * fx_rgb / aligned[v,u,2]) + cx_rgb
        y = (aligned[v,u,1] * fy_rgb / aligned[v,u,2]) + cy_rgb
        
        # Endle out of bound pixels
        if x > width-1 or y > height-1 or x < 0 or y < 0:
            pass
        
        else:
            x = int(round(x))
            y = int(round(y))
        
            aligned[v,u,3] = color[y, x, 0]
            aligned[v,u,4] = color[y, x, 1]
            aligned[v,u,5] = color[y, x, 2]
            
# Retrive RGB value from our aligned version
test = np.zeros((height,width,3))
for i in range (height):
    for j in range (width):
        test[i,j] = aligned[i,j][3:6]
plt.imshow(test.astype(int))