VTK相机正射投影中通过多个2D坐标计算3D坐标

VTK相机正射投影中通过多个2D坐标计算3D坐标

• 介绍
• • 思路1
  • • 1.背景
    • 2.数学推导
    • [3.Python 实现（假设已有 parallel_scale）](#3.Python 实现（假设已有 parallel_scale）)
    • [4. 关于尺度与不确定性的说明](#4. 关于尺度与不确定性的说明)
  • 思路2
  • • [1. 背景](#1. 背景)
    • [2. 投影矩阵的意义](#2. 投影矩阵的意义)
    • [3. 代入我们的约束](#3. 代入我们的约束)
    • [4. 直观理解](#4. 直观理解)
    • 5.完整代码

介绍

三维坐标系中，给定一个虚拟相机，使用正射投影，从不同角度拍摄的n张图像，给定它们的相机vtk参数，每个vtk参数包含相机位置、焦点位置、向上方向。已知n张图像的宽和高，现在从 n n n张图像中同时选择同一物体的同一个位置的2d图像点坐标，根据这 n n n个2D图像点坐标计算该点实际的3D坐标。

思路1

1.背景

在正交投影（orthographic）下，每台相机只给出两个线性约束 ：点在相机坐标系的 x 和 y 分量（即沿相机的右向和上向的投影）等于该像素对应的图像坐标对应的世界平面点（即把像素坐标换算到世界单位坐标）。因此从 n n n 台相机我们得到 2 n 2n 2n 个线性方程，未知量是世界坐标 X = ( X x , X y , X z ) T X=(X_x,X_y,X_z)^T X=(Xx,Xy,Xz)T。把这些方程堆成矩阵形式后用线性最小二乘解出 X X X。

关键要素（必须有或需要明确）：

• 每个相机的方向基（在世界坐标下）：右向 r i \mathbf{r}_i ri，上向 u i \mathbf{u}_i ui，视线方向（投影方向） w i \mathbf{w}_i wi（通常 w i \mathbf{w}_i wi = normalize(focal-point − position)。

• 像素坐标 ( p x , p y ) (p_{x},p_{y}) (px,py) 到相机"图像平面坐标"（世界单位）的映射尺度（即每像素对应多少世界长度，或者等价地相机的parallel scale / 视窗半高）。

  • 如果没有给出尺度，只用像素单位做归一化（例如把像素映射到 [ − 1 , 1 ] [-1,1] [−1,1]），则只能恢复点的位置 相对于每台相机视窗的尺度，整体绝对大小会有尺度模糊（需用户给出至少一个相机的平行缩放参数）。

• 把每个相机的图像平面中心（像主点）取为相机的 position，并基于它把像素映射到世界单位坐标（下面代码用 position 作为中心点）。

在 VTK 中，parallel_scale 表示：

图像高度的一半在世界坐标中的大小（沿着相机 up 向量的方向）。

具体来说：

• 如果相机图像的像素高度是 H，
• 并且 parallel_scale = s，
  那么 整个图像在世界坐标系中的高度就是 2 * s 。
  于是：
• 世界空间中 1 个单位长度 = 图像高度对应的 H / ( 2 s ) H / (2s) H/(2s) 个像素。
• 或者反过来：1 像素对应的世界长度 = 2 s / H 2s / H 2s/H。

---

2.数学推导

对第 (i) 台相机，定义单位向量：
z i = n o r m a l i z e ( f i − p i ) ( 视线方向 ) \mathbf{z}_i = \mathrm{normalize}(\mathbf{f}_i - \mathbf{p}_i) \quad(\text{视线方向}) zi=normalize(fi−pi)(视线方向)
x i = n o r m a l i z e ( z i × v i ) ( 相机右向 ) \mathbf{x}_i = \mathrm{normalize}(\mathbf{z}_i \times \mathbf{v}_i) \quad(\text{相机右向}) xi=normalize(zi×vi)(相机右向)
y i = x i × z i ( 相机上向 ) \mathbf{y}_i = \mathbf{x}_i \times \mathbf{z}_i \quad(\text{相机上向}) yi=xi×zi(相机上向)

把像素 ( p x , p y ) (p_x,p_y) (px,py) 规范化到 ( x ~ , y ~ ) (\tilde x,\tilde y) (x~,y~)（范围 [ − 1 , 1 ] [-1,1] [−1,1]）：
x ~ = p x − ( W / 2 ) W / 2 , y ~ = ( H / 2 ) − p y H / 2 \tilde x = \frac{p_x - (W/2)}{W/2},\quad \tilde y = \frac{(H/2) - p_y}{H/2} x~=W/2px−(W/2),y~=H/2(H/2)−py

（注意 y 通常要翻转）

给定该相机的 parallel_scale s i s_i si（表示图像平面从中心到顶边的世界单位距离），假设横向和纵向平行尺度相同，像素在世界平面上的坐标：
P i = p o s i + ( x ~ ⋅ s i ) x i + ( y ~ ⋅ s ) y i \mathbf{P}_i = \mathbf{pos}_i + (\tilde{x} \cdot s_i) \mathbf{x}_i + (\tilde{y} \cdot s)\mathbf{y}_i Pi=posi+(x~⋅si)xi+(y~⋅s)yi

或者在横向和纵向平行尺度不相同情况下，像素在世界平面上的坐标：
P i = p o s i + ( x ~ ⋅ s i ⋅ W H ) x i + ( y ~ ⋅ s ) y i \mathbf{P}_i = \mathbf{pos}_i + (\tilde{x} \cdot s_i \cdot \tfrac{W}{H}) \mathbf{x}_i + (\tilde{y} \cdot s)\mathbf{y}_i Pi=posi+(x~⋅si⋅HW)xi+(y~⋅s)yi

其中：

• p o s \mathbf{pos} pos：相机位置（作为平面坐标系的中心）。
• x , y \mathbf{x}, \mathbf{y} x,y：相机的右 / 上方向向量。
• x ~ , y ~ \tilde{x}, \tilde{y} x~,y~：归一化像素坐标 [ − 1 , 1 ] [-1,1] [−1,1]。
• s s s：parallel_scale。

计算 P i \mathbf{P}_i Pi在 x i \mathbf{x}_i xi和 y i \mathbf{y}i yi方向的投影值，即
b i , x = P i x = p o s i ⋅ x i + x ~ ⋅ s i b i , y = P i y = p o s i ⋅ y i + y ~ ⋅ s i b{i,x} = \mathbf{P}_i x = \mathbf{pos}_i \cdot \mathbf{x}i + \tilde x \cdot s_i \\ b{i,y} = \mathbf{P}_i y = \mathbf{pos}_i \cdot \mathbf{y}_i + \tilde y \cdot s_i bi,x=Pix=posi⋅xi+x~⋅sibi,y=Piy=posi⋅yi+y~⋅si

因为对于目标 3D 点 X = ( X x , X y , X z ) T \mathbf{X}=(\mathbf{X}_x,\mathbf{X}_y,\mathbf{X}_z)^T X=(Xx,Xy,Xz)T：
x i T X = b i , x , y i T X = b i , y . \mathbf{x}i^T \mathbf{X} = b{i,x},\quad \mathbf{y}i^T \mathbf{X} = b{i,y}. xiTX=bi,x,yiTX=bi,y.

把所有摄像机的这些方程堆叠成线性系统 (A X = b)（其中每台相机贡献两行）。用最小二乘解：
X = ( A T A ) − 1 A T b X = (A^T A)^{-1} A^T b X=(ATA)−1ATb

或用稳健数值方法 np.linalg.lstsq。

---

3.Python 实现（假设已有 parallel_scale）

下面代码实现了上述步骤，输入为每台相机的 position,focal_point,view_up,parallel_scale,width,height 与像素点 px,py（对应同一 3D 点在每张图像的像素坐标）。

import numpy as np

def normalize(v):
    v = np.asarray(v, dtype=float)
    norm = np.linalg.norm(v)
    if norm < 1e-12:
        return v
    return v / norm

def camera_axes(position, focal_point, view_up):
    z = normalize(np.array(focal_point) - np.array(position))  # viewing direction
    up = normalize(view_up)
    x = normalize(np.cross(z, up))
    y = np.cross(x, z)
    return x, y, z

def pixel_to_normalized(px, py, width, height):
    # 将像素映射到 [-1,1]，并将y翻转（图像坐标 -> 数学坐标）
    nx = (px - (width / 2.0)) / (width / 2.0)
    ny = ((height / 2.0) - py) / (height / 2.0)
    return nx, ny

def reconstruct_point_from_ortho(cameras, pixels):
    """
    cameras: list of dicts, each with keys:
      'position', 'focal_point', 'view_up', 'parallel_scale', 'width', 'height'
    pixels: list of (px, py) pairs (same length as cameras)
    returns: 3D point X (shape (3,))
    """
    assert len(cameras) == len(pixels)
    rows = []
    rhs = []
    for cam, (px, py) in zip(cameras, pixels):
        pos = np.array(cam['position'], dtype=float)
        fpt = np.array(cam['focal_point'], dtype=float)
        view_up = np.array(cam['view_up'], dtype=float)
        s = float(cam['parallel_scale'])  # half-height in world units
        W = cam['width']
        H = cam['height']

        x_axis, y_axis, z_axis = camera_axes(pos, fpt, view_up)
        nx, ny = pixel_to_normalized(px, py, W, H)

        # b values: dot(X, axis) = b
        b_x = np.dot(pos, x_axis) + nx * s
        b_y = np.dot(pos, y_axis) + ny * s

        rows.append(x_axis)  # equation: x_axis^T X = b_x
        rhs.append(b_x)
        rows.append(y_axis)
        rhs.append(b_y)

    A = np.vstack(rows)   # shape (2n, 3)
    b = np.array(rhs)     # shape (2n,)

    # least squares solve
    X, residuals, rank, svals = np.linalg.lstsq(A, b, rcond=None)
    return X

# === 示例（伪数据） ===
if __name__ == "__main__":
    cameras = [
        {
            'position': [0,0,10],
            'focal_point': [0,0,0],
            'view_up': [0,1,0],
            'parallel_scale': 5.0,  # 视窗半高 world units
            'width': 640, 'height': 480
        },
        {
            'position': [10,0,0],
            'focal_point': [0,0,0],
            'view_up': [0,1,0],
            'parallel_scale': 5.0,
            'width': 640, 'height': 480
        }
    ]
    pixels = [(500,500), (500,500)]  # 中心像素
    X = reconstruct_point_from_ortho(cameras, pixels)
    print("Reconstructed X:", X)

---

4. 关于尺度与不确定性的说明

1. 必须的尺度信息 ：正交投影缺少"透视收敛"信息，因此在没有 世界单位的像素尺度（parallel_scale / 每像素对应的世界长度） 的情况下，问题对整体尺度存在模糊：你只能恢复点在每个相机视窗坐标系下的位置比例，但不能得到唯一的绝对世界坐标（除非你知道至少一个尺度参数或场景中有尺度参照）。

2. 如果没有 parallel_scale，常见替代：

   • 若你只关心相对位置 或想在某个参考尺度下工作：可把 parallel_scale = 1.0（或任意常数），得到按比例的解；后续用已有的实际测量来缩放回真实世界。
   • 或者如果你有相机的"视域在世界坐标的边界"或"像素对应的真实世界间距"，可以用它来算 parallel_scale：例如 parallel_scale = (real_view_height/2)，而 real_view_height = pixels_in_height * meter_per_pixel。

3. 数值稳定性：当所有相机视线非常接近共面或平行（观测几何退化）时，矩阵 (A) 会接近欠定，解不可靠。最好保证相机视角分布良好（视线方向差异大）。

---

思路2

1. 背景

在正交投影下：

• 相机的视线方向是单位向量 z \mathbf{z} z。
• 从像素点换算出来的平面点是 P \mathbf{P} P。
• 我们要求的 3D 点 X \mathbf{X} X，必须满足：

( X − P ) ⊥ z (\mathbf{X} - \mathbf{P}) \perp \mathbf{z} (X−P)⊥z

也就是 X − P \mathbf{X} - \mathbf{P} X−P 与 z \mathbf{z} z 正交。

换句话说， X \mathbf{X} X 投影到 z \mathbf{z} z 的方向应该等于 P \mathbf{P} P 投影到 z \mathbf{z} z 的方向。

---

2. 投影矩阵的意义

数学上，np.outer(z, z) 表示向量 z z z 自身的外积（outer product），结果是一个矩阵。具体地，如果 z z z 是一个三维向量：
n p . o u t e r ( z , z ) = z z T = [ z 1 z 1 z 1 z 2 z 1 z 3 z 2 z 1 z 2 z 2 z 2 z 3 z 3 z 1 z 3 z 2 z 3 z 3 ] np.outer(z, z) = z z^T = \begin{bmatrix} z_1 z_1 & z_1 z_2 & z_1 z_3 \\ z_2 z_1 & z_2 z_2 & z_2 z_3 \\ z_3 z_1 & z_3 z_2 & z_3 z_3 \end{bmatrix} np.outer(z,z)=zzT= z1z1z2z1z3z1z1z2z2z2z3z2z1z3z2z3z3z3

由于 z z z 是单位向量（即 ∥ z ∥ = 1 \|z\| = 1 ∥z∥=1），这个矩阵有一些特殊的性质：

• 对称矩阵 ：显然， ( z z T ) T = z z T (z z^T)^T = z z^T (zzT)T=zzT。
• 秩为1的矩阵 ：因为所有列都是向量 z z z 的倍数。
• 投影矩阵 ：这个矩阵表示的是向量 z z z 所在方向的投影矩阵。换句话说，对于任意向量 x x x，有：

( z z T ) x = z ( z T x ) = ( z ⋅ x ) z (z z^T) x = z (z^T x) = (z \cdot x) z (zzT)x=z(zTx)=(z⋅x)z

也就是说，它将任意向量 x x x 投影到向量 z z z 所在的直线上（其结果仍然是一个向量）。

接下来看 I - np.outer(z, z)，其中 I I I 是单位矩阵：

I - z z\^T = \\begin{bmatrix} 1 - z_1 z_1 \& - z_1 z_2 \& - z_1 z_3 \\ * z_2 z_1 \& 1 - z_2 z_2 \& - z_2 z_3 \\ * z_3 z_1 \& - z_3 z_2 \& 1 - z_3 z_3 \\end{bmatrix}

这个矩阵也有明确的含义：

• 对称矩阵 ：显然， ( I − z z T ) T = I − z z T (I - z z^T)^T = I - z z^T (I−zzT)T=I−zzT。
• 投影矩阵 ：它表示的是垂直于向量 z z z 的子空间的投影矩阵。换句话说，对于任意向量 x x x，有：

( I − z z T ) x = x − ( z z T ) x = x − ( z ⋅ x ) z (I - z z^T) x = x - (z z^T) x = x - (z \cdot x) z (I−zzT)x=x−(zzT)x=x−(z⋅x)z

这正是将向量 x x x 投影到与 z z z 垂直的平面（或子空间）上的操作。

因此，投影到平面（垂直于 z 的方向） 的矩阵M实现如下：

I = np.eye(3)
M = I - np.outer(z, z)

---

3. 代入我们的约束

我们要求：
( X − P ) ⋅ z = 0 (\mathbf{X} - \mathbf{P}) \cdot \mathbf{z} = 0 (X−P)⋅z=0

等价于：
( I − z z T ) X = ( I − z z T ) P (I - \mathbf{z}\mathbf{z}^T)\mathbf{X} = (I - \mathbf{z}\mathbf{z}^T)\mathbf{P} (I−zzT)X=(I−zzT)P

也就是说，我们把 X \mathbf{X} X 和 P \mathbf{P} P 都投影到与 z \mathbf{z} z 垂直的平面上，它们必须重合。

在代码里实现：

A += M
b += M @ P

---

4. 直观理解

想象一下：

• 相机视线是 z \mathbf{z} z，
• 任何点沿着 z \mathbf{z} z 的方向都投影到同一个像素点。

所以我们只需要关心点在 垂直于 z \mathbf{z} z 的平面上 的位置（就是 M 的作用）。

• M @ X = 把候选点 X 投影到"相机看到的平面"；
• M @ P = 像素点对应的平面位置；
• 方程 (I - zz^T)X = (I - zz^T)P 表示：候选 3D 点和像素平面点在相机投影下必须一致。

---

5.完整代码

import numpy as np

def normalize(v):
    v = np.asarray(v, dtype=float)
    n = np.linalg.norm(v)
    return v / n if n > 1e-12 else v

def camera_axes(position, focal_point, view_up):
    """计算相机右、上、前方向"""
    pos = np.array(position, dtype=float)
    fpt = np.array(focal_point, dtype=float)
    up  = np.array(view_up, dtype=float)

    z = normalize(fpt - pos)   # viewing direction
    x = normalize(np.cross(z, up))
    y = np.cross(x, z)
    return x, y, z

def pixel_to_normalized(px, py, width, height):
    """像素归一化到 [-1,1]"""
    nx = (px - width/2) / (width/2)
    ny = (height/2 - py) / (height/2)  # 翻转 y
    return nx, ny

def pixel_to_world_on_plane(pos, focal_point, view_up,
                            width, height, parallel_scale,
                            px, py):
    """
    将像素点 (px,py) 转换到世界平面上的坐标
    以相机位置 pos 为参考中心
    """
    x_axis, y_axis, z_axis = camera_axes(pos, focal_point, view_up)
    nx, ny = pixel_to_normalized(px, py, width, height)

    dx = nx * parallel_scale * (width / height)
    dy = ny * parallel_scale

    P = np.array(pos) + dx * x_axis + dy * y_axis
    return P, z_axis

def reconstruct_point_from_ortho_with_position(cameras, pixels):
    """
    使用相机 position 作为平面中心，正交投影的多相机 3D 重建
    cameras: list of dict，每个包含
        'position','focal_point','view_up','parallel_scale','width','height'
    pixels: list of (px,py)，与 cameras 一一对应
    return: 重建的 3D 点
    """
    A = np.zeros((3,3))
    b = np.zeros(3)

    for cam, (px, py) in zip(cameras, pixels):
        pos = np.array(cam['position'], dtype=float)
        fpt = np.array(cam['focal_point'], dtype=float)
        vup = np.array(cam['view_up'], dtype=float)

        P, z = pixel_to_world_on_plane(pos, fpt, vup,
                                       cam['width'], cam['height'],
                                       cam['parallel_scale'],
                                       px, py)
        # 投影矩阵： I - z z^T
        I = np.eye(3)
        M = I - np.outer(z, z)

        A += M
        b += M @ P

    # 解最小二乘
    X = np.linalg.solve(A, b)
    return X


if __name__ == "__main__":
    cameras = [
        {
            'position': [0,0,10],
            'focal_point': [0,0,0],
            'view_up': [0,1,0],
            'parallel_scale': 5.0,
            'width': 1000,
            'height': 1000
        },
        {
            'position': [10,0,0],
            'focal_point': [0,0,0],
            'view_up': [0,1,0],
            'parallel_scale': 5.0,
            'width': 1000,
            'height': 1000
        }
    ]

    # 两张图像上都选中中心点
    pixels = [(500, 500), (500, 500)]

    X = reconstruct_point_from_ortho_with_position(cameras, pixels)
    print("Reconstructed 3D point:", X)