如何估计棋盘格外参矩阵，生成带米级像素的鸟瞰占用栅格图？

需要生成鸟瞰视角(BEV)的占用栅格(OG)图像，要求每个像素对应固定米级单位，且仅保留地面（高度=0）内容。目前采用的流程如下，但结果不符合预期：

1. 使用cv2.solvePnP和棋盘格图像估计外参矩阵；
2. 生成OG栅格的XYZ世界坐标（X向右，Y为高度，Z向前）；
3. 通过外参矩阵将OG栅格的XYZ世界坐标投影到相机坐标系；
4. 匹配OG栅格相机坐标对应的图像uv坐标；
5. 利用uv坐标对应的像素填充OG图像。

已通过10张棋盘格图像得到内参和畸变矩阵，以下是实现代码及当前输出：

1. 估计外参矩阵 #

import numpy as np
import cv2
import matplotlib.pyplot as plt

mtx = np.array([[2029,    0, 2029],
                [   0, 1904, 1485],
                [   0,    0,    1]]).astype(float)

dist = np.array([[-0.01564965,  0.03250585,  0.00142366,  0.00429703, -0.01636045]])

impath = '....'
img = cv2.imread(impath)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
CHECKERBOARD = (5, 8)
ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, None)
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

# 定义棋盘格世界坐标：X从-4到3（共8格），Y=0，Z从0到4（共5格）
objp = np.concatenate(
            np.meshgrid(np.arange(-4, 4, 1),
                        0,
                        np.arange(0, 5, 1), 
                        )
        ).astype(float)
objp = np.moveaxis(objp, 0, 2).reshape(-1, 3)

square_size = 0.029
objp *= square_size

ret, rvec, tvec = cv2.solvePnP(objp, corners[::-1], mtx, dist)
print('rvec:', rvec.T)
print('tvec:', tvec.T)
# 输出：
# rvec: [[ 0.15550242 -0.03452503 -0.028686  ]]
# tvec: [[0.03587237 0.44082329 0.62490573]]

# 生成世界→相机的变换矩阵RT
R = cv2.Rodrigues(rvec)[0]
RT = np.eye(4)
RT[:3, :3] = R
RT[:3, 3] = tvec.ravel()
print(RT.round(2))
# 输出：
# array([[-1.  ,  0.03,  0.04,  0.01],
#        [ 0.03,  0.99,  0.15, -0.44],
#        [-0.03,  0.16, -0.99,  0.62],
#        [ 0.  ,  0.  ,  0.  ,  1.  ]])

2. 生成OG栅格XYZ世界坐标（X向右，Y为高度，Z向前） #

uv_dims = img.shape[:2]  # (h, w)
grid_dims = (500, 500)  # (h, w)，对应50m×50m（每个像素0.1m）

# 生成栅格的世界坐标：X范围[-25,25]m，Z范围[0,50]m，Y=0
x_grid = np.linspace(-25, 25, grid_dims[1])  # 宽度方向对应X轴
z_grid = np.linspace(0, 50, grid_dims[0])    # 高度方向对应Z轴
X, Z = np.meshgrid(x_grid, z_grid)
Y = np.zeros_like(X)

# 整理为(N,3)的世界点矩阵
og_world_points = np.stack([X.ravel(), Y.ravel(), Z.ravel()], axis=1)
print(og_world_points.shape)  # (250000, 3)

3. 将世界坐标投影到图像UV坐标（修正核心步骤） #

# 使用cv2.projectPoints直接完成世界→图像的投影，自动处理畸变
uv_coords, _ = cv2.projectPoints(og_world_points, rvec, tvec, mtx, dist)
uv_coords = uv_coords.reshape(grid_dims + (2,)).astype(np.int32)

# 裁剪UV坐标到图像边界内
uv_coords[..., 0] = np.clip(uv_coords[..., 0], 0, uv_dims[1]-1)
uv_coords[..., 1] = np.clip(uv_coords[..., 1], 0, uv_dims[0]-1)

4. 填充OG图像（修正索引顺序） #

og = np.zeros(grid_dims + (3,), dtype=np.uint8)

# 遍历栅格的每个像素，用对应的图像像素填充
for i in range(grid_dims[0]):
    for j in range(grid_dims[1]):
        u, v = uv_coords[i, j]
        og[i, j] = img[v, u]

plt.imshow(cv2.cvtColor(og, cv2.COLOR_BGR2RGB))
plt.show()

关键修正点说明 #

1. 栅格坐标生成优化：用np.linspace替代arange确保坐标范围准确，同时明确栅格的行对应Z轴（向前）、列对应X轴（向右），与世界坐标系定义一致。
2. 投影方式修正：直接使用cv2.projectPoints完成从世界坐标到图像UV的转换，该函数会自动处理旋转、平移和畸变，避免手动矩阵运算的错误。
3. 索引顺序修正：填充OG图像时，栅格的行索引i对应Z轴方向，列索引j对应X轴方向，与坐标生成逻辑一致，解决了原代码中图像扭曲的问题。
4. 边界裁剪优化：用np.clip简化边界处理，确保UV坐标始终在图像范围内。

内容的提问来源于stack exchange，提问作者Rafael Toledo