如何通过点对对应计算球形相机位置？

Question

我在一个等距圆柱投影图像上标记了4个点。[红色点]

我还在一张俯视图上标记了4个相应的点[红点]。

我该如何计算相机在顶视图像上的位置？

目前我发现有4条射线（R1、R2、R3、R4）从未知相机中心C = (Cx, Cy, Cz)经过等距投影图像上的点并以顶视图像的像素坐标P1、P2、P3、P4结束。因此，需要四个形式为向量方程的公式：

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

对于每个对应关系。因此。

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个未知数和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)

最终相机位置（绿色）和投影点（青绿色）

编辑：修复了一个错误，即在 PI/4 中的等距投影图像中的经度偏移，这解决了旋转问题，但比例仍然有些不准确。

- nickponline

---

调查解决PnP问题。你是否足够了解等距圆柱投影模型，能够计算出这4个图像点在空间中的光线？ - Christoph Rackwitz

在可视化结果时，我认为将红色和青绿色连接起来以便看到点之间的对应关系是很好的。 - fana

@ChristophRackwitz 我熟悉solvePNP，但对应的针孔相机的内参是什么？ - nickponline

如果你想要一个小技巧，你可以制作一个虚拟摄像头，没有镜头畸变，并计算在等距投影中定义的那些点的屏幕空间点。然后，您可以将这些点和虚拟摄像机的内参数传递给solvePnP API。如果您的点不涵盖180度或更多的视野范围，这是一个可接受的小技巧。 - Christoph Rackwitz

我已经进行了一些纯数学思考，忽略了相机的高度，专注于其（x，y）位置：点之间的水平距离与等面图像上的宽度之比与俯视图矩形对应边的视角成正比；使用这些角度，您可以几何地构建视觉中心。然后，就可以从几何构造中计算出（x，y）。 - Swifty

Answer 1

为了进一步解释我的评论，这是我用来首先计算 Cx 和 Cy 的方法。之后将使用 Cx 和 Cy 来确定 Cz。

在这个俯视图中，圆圈是展开成全景图的圆柱体；A'、B'、C'和D'是代表该图像上的A、B、C、D点的点；A'和B'之间的水平距离等比于角度A-Camera-B，...。因此 A'B'/ circle-perimeter = A-Camera-B / 2pi，从而 A-Camera-B = A'B'/ circle-perimeter * 2pi（圆的周长为全景图的宽度）。我们将这个角度称为alpha。

这个图示说明了我们如何利用圆中角度的性质来确定相机可能的位置，从角度α开始：三个标记的角度等于α，因此tan（α）= AH / O1H，因此O1H = AH / tan（α）。现在我们有了O1的坐标（以A为原点的笛卡尔坐标系中的AB/2，AB/（2 tan（α））。

通过对[AD]段进行相同的操作，我们得到了相机可能位置的第二个圆。两个圆的交点是A和实际相机位置。

当然，确定位置的精度取决于等经纬度图像上A'、B'等坐标的精度；在这里，A'和D'（水平方向）只相隔6-7个像素，因此存在一定的波动。

现在计算Cz：在这个侧视图上，半圆展开成包含equirectangular图像中A'的像素列；类似于先前计算α的方式，A'I / 半圆长度（即图像高度）的比值等于倾斜角度/ pi，因此倾斜角度= A'I / 高度 * pi；在equirectangular图像上，A'I是A'的垂直像素坐标。基本三角函数得出：tan(倾斜角) = -AH/OH，所以Cz = OH = -AH/tan(倾斜角)。AH从之前计算的H的坐标中计算得出。

                 ---------------------------------------------------

这是计算的Python代码；对于圆的交点，我使用了this post中的代码；请注意，由于我们知道A是其中一个交点，因此代码可以简化（CamPos实际上是相对于（O1 O2）的A的对称反射）。

结果是（Cx，Cy）相对于A的像素值，然后是Cz，也是像素值。 请注意，仅当俯视图的尺寸与实际尺寸成比例时（因为只有在正交坐标系中计算距离才有意义），计算才有意义。

import math

# Equirectangular info
A_eq = (472,274)
B_eq = (542,274)
C_eq = (535,260)
D_eq = (479,260)

width = 805
height = 374

# Overhead info

A = (267,321)
B = (377,321)
C = (377,274)
D = (267,274)

Rect_width = C[0] - A[0]
Rect_height = A[1] - C[1]


# Angle of view of edge [AB]

alpha = (B_eq[0] - A_eq[0]) / width * 2 * math.pi

# Center and squared radius of the circle of camera positions related to edge [AB]

x0 = Rect_width / 2
y0 = Rect_width / (2* math.tan(alpha))
r02 = x0**2 + y0**2


# Angle of view of edge [AD]

beta = (D_eq[0] - A_eq[0]) / width * 2 * math.pi

# Center and squared radius of the circle of camera positions related to edge [AD]

x1 = Rect_height / (2* math.tan(beta))
y1 = -Rect_height / 2
r12 = x1**2 + y1**2


def get_intersections(x0, y0, r02, x1, y1, r12):
    # circle 1: (x0, y0), sq_radius r02
    # circle 2: (x1, y1), sq_radius r12

    d=math.sqrt((x1-x0)**2 + (y1-y0)**2)
    
    a=(r02-r12+d**2)/(2*d)
    h=math.sqrt(r02-a**2)
    x2=x0+a*(x1-x0)/d   
    y2=y0+a*(y1-y0)/d   
    x3=x2+h*(y1-y0)/d     
    y3=y2-h*(x1-x0)/d 

    x4=x2-h*(y1-y0)/d
    y4=y2+h*(x1-x0)/d
        
    return (round(x3,2), round(y3,2), round(x4,2), round(y4,2))


# The intersection of these 2 circles are A and Camera_Base_Position (noted H)
inters = get_intersections(x0, y0, r02, x1, y1, r12)
H = (Cx, Cy) = (inters[2], inters[3])

print(H)

def get_elevation(camera_base, overhead_point, equirect_point):
    tilt = (equirect_point[1])/height * math.pi
    x , y =  overhead_point[0] - A[0] , overhead_point[1] - A[1]
    base_distance = math.sqrt((camera_base[0] - x)**2 + (camera_base[1] - y)**2 )
    Cz = -base_distance / math.tan(tilt)
    return Cz

print(get_elevation(H, A, A_eq))
print(get_elevation(H, B, B_eq))
print(get_elevation(H, C, C_eq))
print(get_elevation(H, D, D_eq))

# (59.66, 196.19)   # These are (Cx, Cy) relative to point A

# 185.36640516274633  # These are the values of the elevation Cz
# 183.09278981601847  # when using A and A', B and B' ...
# 176.32257112738986
# 177.7819910650333

- Swifty

Answer 2

编辑：使用 MAP 图片的宽度/长度进行球面转换可以得到更好的相机中心结果。点的位置仍然有些混乱。 带有更好解决方案的地图相机中心：，点有些被压扁了

我稍微改写了一下代码，添加了变量和颜色来识别点（在您原来的代码中，一些点在不同的列表中顺序不同）。 如果想要处理更多的数据点，这是更可取的。是的，我选择了一个字典进行调试，但确实更好的选择是 N 个点的列表，只要它们在不同的投影之间正确地配对索引。

我还调整了坐标以匹配我拥有的图片。并且为了我的理解，使用了 x、y 变量的命名和用法。

它仍然不正确，但每个找到的位置之间存在某种一致性。

可能的原因

OpenCV 图像将 [0,0] 放在左上角。下面的代码与该约定一致，但我没有改变任何数学公式。

也许一些公式存在错误或矛盾之处，您可能需要再次检查符号、[0, 0]位置等约定。

我没有看到这些公式与摄像机位置和高度有关，这可能是错误的源头。

您可以查看此项目，该项目执行全景投影：https://github.com/NitishMutha/equirectangular-toolbox

from typing import Dict

import cv2
import numpy as np


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


WIDTH = 805
HEIGHT = 374  # using stackoverflow PNG
MAP_WIDTH = 662
MAP_HEIGHT = 1056  # using stackoverflow PNG

BLUE = (255, 0, 0)
GREEN = (0, 255, 0)
RED = (0, 0, 255)
CYAN = (255, 255, 0)
points_colors = [BLUE, GREEN, RED, CYAN]

TOP_LEFT = "TOP_LEFT"
TOP_RIGHT = "TOP_RIGHT"
BOTTOM_LEFT = "BOTTOM_LEFT"
BOTTOM_RIGHT = "BOTTOM_RIGHT"


class Point:
    def __init__(self, x, y, color):
        self.x = x
        self.y = y
        self.c = color

    @property
    def coords(self):
        return (self.x, self.y)


# coords using GIMP which uses upperleft [0,0]
POINTS_ON_SPHERICAL_MAP: Dict[str, Point] = {TOP_LEFT    : Point(480, 263, BLUE),
                                             TOP_RIGHT   : Point(532, 265, GREEN),
                                             BOTTOM_LEFT : Point(473, 274, RED),
                                             BOTTOM_RIGHT: Point(535, 275, CYAN),
                                             }
# xs = [480, 532, 473, 535, ]
# ys = [263, 265, 274, 275, ]

img = cv2.imread('equirectangular.png')
for p in POINTS_ON_SPHERICAL_MAP.values():
    img = cv2.circle(img, p.coords, 5, p.c, -1)
cv2.imwrite("debug_equirectangular.png", img)

# coords using GIMP which uses upperleft [0,0]
# px = [269, 382, 269, 383]
# py = [278, 278, 320, 319]
POINTS_ON_OVERHEAD_MAP: Dict[str, Point] = {TOP_LEFT    : Point(269, 278, BLUE),
                                            TOP_RIGHT   : Point(382, 278, GREEN),
                                            BOTTOM_LEFT : Point(269, 320, RED),
                                            BOTTOM_RIGHT: Point(383, 319, CYAN),
                                            }

img = cv2.imread('map.png')

for p in POINTS_ON_OVERHEAD_MAP.values():
    img = cv2.circle(img, p.coords, 5, p.c, -1)
cv2.imwrite("debug_map.png", img)

As = []
bs = []
for point_location in [TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT]:
    x_spherical, y_spherical = POINTS_ON_SPHERICAL_MAP[point_location].coords
    theta, phi = equi2sphere(x=x_spherical, y=y_spherical, width=MAP_WIDTH, height=MAP_HEIGHT) # using the overhead map data for conversions
    # 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(f"{x_spherical}, {y_spherical} -> {np.degrees(theta):+.3f}, {np.degrees(phi):+.3f} -> {sx:+.3f}, {sy:+.3f}, {sz:+.3f}")
    block = np.array([[1, 0, 0, sx],
                      [0, 1, 0, sy],
                      [1, 0, 1, sz], ])
    x_map, y_map = POINTS_ON_OVERHEAD_MAP[point_location].coords
    vector = np.array([x_map, y_map, 0])
    As.append(block)
    bs.append(vector)

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

img = cv2.imread("debug_map.png")

for point_location in [TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT]:
    x_spherical, y_spherical = POINTS_ON_SPHERICAL_MAP[point_location].coords
    theta, phi = equi2sphere(x=x_spherical, y=y_spherical, width=MAP_WIDTH, height=MAP_HEIGHT) # using the overhead map data for conversions
    # 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(f"{pixel_x:+0.0f}, {pixel_y:+0.0f}, {pixel_z:+0.0f}")
    img = cv2.circle(img, (int(pixel_x), int(pixel_y)), 5, POINTS_ON_SPHERICAL_MAP[point_location].c, -1)

img = cv2.circle(img, (int(Cx), int(Cy)), 4, (200, 200, 127), 3)

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

我的初始解决方案地图： 调试地图： 等距投影图像： 调试等距投影：

- LoneWanderer