目的是对齐（配准）一张rgb和深度图，已知相机的内参和外参，现在已经能得到变换后的 坐标值xrgb和yrgb与其对应的深度值k，想把这些信息生成一张新的图片并显示出来。

import cv2
import matplotlib.pyplot as plt
import numpy as np
depth=cv2.imread("000000.png",flags=-1)
scale = 2.0
depth = cv2.resize(depth, (0,0), fx=scale, fy=scale)
rgb=cv2.imread('000000.jpg',flags=-1)
cx_d = 266.72 * scale
cy_d = 206.487 * scale
fx_d = 380.279 * scale
fy_d = 385.279 * scale
fx_rgb = 1081.37
fy_rgb = 1081.37
cx_rgb = 959.5
cy_rgb = 539.5
R = np.array([[0.9999, -0.0140, 0], [0.0140, 0.9999, 0], [0.0, 0.0, 1]])
T = np.array([-0.06015, 0.00221, 0.02714])

获取图像高度和宽度

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

#创建一幅图像
result = np.zeros((1080, 1920), np.uint16)

for u in range(height):
for v in range(width):
k = depth[u, v] # 列，行

    Wz = k
    if k == 0:
        continue
    Wx = (u - cx_d) * Wz / fx_d  # 行
    Wy = (v - cy_d) * Wz / fy_d  # 列

    # apply extrinsic calibration NEW 3D AXIS
    W3D = np.array([Wx, Wy, Wz])  # 3D坐标 in 米
    W3Dm = np.dot(R, W3D) - T  # 将3D真实坐标+RGB外参 or - 要注意是谁到谁
    # W3D' = R.W3D + T
    # 2D_rgb.x = (W3D'.x * fx_rgb / W3D'.z) + cx_rgb
    # 2D_rgb.y = (W3D'.y * fy_rgb / W3D'.z) + cy_rgb

    # 加入外参的3D 投影到 2D RGB
    xrgb = W3Dm[0] * fx_rgb / W3Dm[2] + cx_rgb  # 列
    yrgb = W3Dm[1] * fy_rgb / W3Dm[2] + cy_rgb  # 行
    yrgb = round(yrgb)

    xrgb = round(xrgb)

    if xrgb < 0 or yrgb < 0 or yrgb > 1919 or xrgb > 1079:
        continue

有点忘了，应该是有个save函数，保存就可以了额。