如何通过点对应关系计算球面相机在俯视图中的位置

问题背景 #

在等矩形全景图中标记了4个与俯视图对应的点，需要计算相机在俯视图中的位置。建立了射线方程后用最小二乘法求解，但结果不符合预期；已修正经度偏移（π/4）解决旋转偏差，但缩放仍存在问题。

数学模型 #

从未知相机中心 ( C=(C_x,C_y,C_z) ) 出发，穿过全景图点到达俯视图像素点 ( P_1-P_4 ) 的射线方程：

[Cx, Cy, Cz] + [Rx, Ry, Rz]*t = [x, y, 0]

对应4组独立方程：

C + R1*t1 = P1 = [x1, y1, 0]
C + R2*t2 = P2 = [x2, y2, 0]
C + R3*t3 = P3 = [x3, y3, 0]
C + R4*t4 = P4 = [x4, y4, 0]

系统包含7个未知数（( C_x,C_y,C_z,t_1,t_2,t_3,t_4 )）和12个方程，属于超定方程组，需用最小二乘法求解。

现有代码 #

import numpy as np

def equi2sphere(x, y):
    width = 2000
    height = 1000
    theta = 2 * np.pi * x  / width - np.pi
    phi = np.pi * y / height
    return theta, phi

HEIGHT = 1000
MAP_HEIGHT = 788
#
# HEIGHT = 0
# MAP_HEIGHT = 0

# Point in equirectangular image, bottom left = (0, 0)
xs = [1190, 1325, 1178, 1333]
ys = [HEIGHT - 730,   HEIGHT - 730,  HEIGHT - 756,  HEIGHT - 760]

# import cv2
# img = cv2.imread('equirectangular.jpg')
# for x, y in zip(xs, ys):
#     img = cv2.circle(img, (x, y), 15, (255, 0, 0), -1)
# cv2.imwrite("debug_equirectangular.png", img)

# Corresponding points in overhead map, bottom left = (0, 0)
px = [269, 382, 269, 383]
py = [778, 778, 736, 737]

# import cv2
# img = cv2.imread('map.png')
# for x, y in zip(px, py):
#     img = cv2.circle(img, (x, y), 15, (255, 0, 0), -1)
# cv2.imwrite("debug_map.png", img)

As = []
bs = []
for i in range(4):

    x, y = xs[i], ys[i]

    theta, phi = equi2sphere(x, y)

    # convert to spherical
    p = 1
    sx = p * np.sin(phi) * np.cos(theta)
    sy = p * np.sin(phi) * np.sin(theta)
    sz = p * np.cos(phi)

    print(x, y, '->', np.degrees(theta), np.degrees(phi), '->', round(sx, 2), round(sy, 2), round(sz, 2))

    block = np.array([
        [1, 0, 0, sx],
        [0, 1, 0, sy],
        [1, 0, 1, sz],
    ])

    y = np.array([px[i], py[i], 0])

    As.append(block)
    bs.append(y)



A = np.vstack(As)
b = np.hstack(bs).T
solution = np.linalg.lstsq(A, b)
Cx, Cy, Cz, t = solution[0]

import cv2
img = cv2.imread('map_overhead.png')

for i in range(4):

    x, y = xs[i], ys[i]

    theta, phi = equi2sphere(x, y)

    # convert to spherical
    p = 1
    sx = p * np.sin(phi) * np.cos(theta)
    sy = p * np.sin(phi) * np.sin(theta)
    sz = p * np.cos(phi)

    pixel_x = Cx + sx * t
    pixel_y = Cy + sy * t
    pixel_z = Cz + sz * t
    print(pixel_x, pixel_y, pixel_z)
    img = cv2.circle(img, (int(pixel_x), img.shape[0] - int(pixel_y)), 15, (255,255, 0), -1)
    img = cv2.circle(img, (int(Cx), img.shape[0] - int(Cy)), 15, (0,255, 0), -1)


cv2.imwrite("solution.png", img)


# print(A.dot(solution[0]))
# print(b)

核心错误分析 #

现有代码的关键问题是错误地将所有射线的参数 ( t ) 视为同一个值，但实际上每条射线的 ( t_1-t_4 ) 是独立未知数，不能共用。这导致方程约束逻辑错误，直接引发缩放偏差。

修正方案 #

1. 重构方程矩阵 #

针对每个点，将 ( t_i ) 作为独立未知数，构建正确的超定方程组：
对于第 ( i ) 个点，从射线方程推导：

• ( C_x + R_{x,i} \cdot t_i = P_{x,i} )
• ( C_y + R_{y,i} \cdot t_i = P_{y,i} )
• ( C_z + R_{z,i} \cdot t_i = 0 )（俯视图点在 ( z=0 ) 平面）

未知数向量为 ( [C_x, C_y, C_z, t_1, t_2, t_3, t_4]^T )，共7个参数。

2. 修正后的代码 #

import numpy as np
import cv2

def equi2sphere(x, y):
    width = 2000
    height = 1000
    theta = 2 * np.pi * x / width - np.pi
    phi = np.pi * y / height
    return theta, phi

# 全景图点（左下为(0,0)）
HEIGHT = 1000
xs = [1190, 1325, 1178, 1333]
ys = [HEIGHT - 730, HEIGHT - 730, HEIGHT - 756, HEIGHT - 760]

# 俯视图对应点（左下为(0,0)）
px = [269, 382, 269, 383]
py = [778, 778, 736, 737]

# 构建A矩阵和b向量
A = []
b = []
for i in range(4):
    theta, phi = equi2sphere(xs[i], ys[i])
    # 计算射线方向单位向量
    rx = np.sin(phi) * np.cos(theta)
    ry = np.sin(phi) * np.sin(theta)
    rz = np.cos(phi)
    
    # 每个点对应3行方程
    row1 = [1, 0, 0] + [rx if j == i else 0 for j in range(4)]
    row2 = [0, 1, 0] + [ry if j == i else 0 for j in range(4)]
    row3 = [0, 0, 1] + [rz if j == i else 0 for j in range(4)]
    
    A.append(row1)
    A.append(row2)
    A.append(row3)
    b.append(px[i])
    b.append(py[i])
    b.append(0)

# 转换为numpy数组
A = np.array(A, dtype=np.float64)
b = np.array(b, dtype=np.float64)

# 最小二乘法求解
solution, residuals, rank, singular_values = np.linalg.lstsq(A, b, rcond=None)
Cx, Cy, Cz, t1, t2, t3, t4 = solution
ts = [t1, t2, t3, t4]

# 验证并可视化结果
img = cv2.imread('map_overhead.png')
# 绘制相机位置（绿色）
cv2.circle(img, (int(Cx), img.shape[0] - int(Cy)), 15, (0,255,0), -1)

# 绘制每个射线投影点（蓝绿色）
for i in range(4):
    theta, phi = equi2sphere(xs[i], ys[i])
    rx = np.sin(phi) * np.cos(theta)
    ry = np.sin(phi) * np.sin(theta)
    rz = np.cos(phi)
    
    proj_x = Cx + rx * ts[i]
    proj_y = Cy + ry * ts[i]
    proj_z = Cz + rz * ts[i]
    print(f"点{i+1}投影坐标: ({proj_x:.2f}, {proj_y:.2f}, {proj_z:.2f})")
    
    cv2.circle(img, (int(proj_x), img.shape[0] - int(proj_y)), 15, (255,255,0), -1)

cv2.imwrite("corrected_solution.png", img)
print(f"相机位置: Cx={Cx:.2f}, Cy={Cy:.2f}, Cz={Cz:.2f}")

3. 额外注意事项 #

• 确认全景图坐标系统：equi2sphere 中 ( \phi ) 的计算是否匹配你的全景图定义（部分实现中 ( \phi ) 从顶部开始，需调整为 ( \phi = \pi - \pi * y / height )）。
• 俯视图坐标转换：代码中用 img.shape[0] - int(proj_y) 将左下原点转换为图像左上原点，需确保与你的图像坐标系一致。

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