如何用PnP算法高效求解最小二乘问题？

1. 引言 通过本系列前几篇文章（最小二乘问题详解：目录）的学习，我们对最小二乘问题有了较为系统的认识：它是一种广泛应用于科学与工程领域的参数估计与优化方法。在计算机视觉中，最小二乘思想贯穿于许多核心算法，尤其在运动恢复结构（Structure from Motion, SFM）这一经典框架中体现得尤为明显。 SFM 的完整流程通常包含五个关键子问题： 相机标定（Camera Calibration） PnP 问题（Perspective-n-Point） 三角化（Triangulation） 对极几何估计（Fundamental / Essential Matrix Estimation） 束平差（Bundle Adjustment, BA） 其中，PnP 问题是最基础、也最直观的一个环节。它负责在已知部分 3D 结构和对应 2D 观测的前提下，求解相机的位姿。虽然形式简单，但其求解质量直接影响后续三角化与全局优化的稳定性，是连接 2D 图像与 3D 场景的重要桥梁。 2. 问题模型 2.1 原理概述 PnP（Perspective-n-Point）问题的目标是：给定 \(n\) 个已知的世界坐标系下的 3D 点 \(\mathbf{X}_i = [X_i, Y_i, Z_i]^\top \in \mathbb{R}^3\) 及其在图像中对应的 2D 像素坐标 \(\mathbf{x}_i = [u_i, v_i]^\top \in \mathbb{R}^2\)（\(i = 1, \dots, n\)），求解相机相对于世界坐标系的位姿——即相机的位置和朝向。 这个问题建立在针孔相机成像模型之上。假设相机内参 \(\mathbf{K}\) 已通过标定获得（例如 \(\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\)），那么一个空间点 \(\mathbf{X}_i\) 经过刚体变换（旋转 + 平移）后，再经透视投影，最终落在图像平面上。这一过程可写为： \[s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{R} \mid \mathbf{t} \end{bmatrix} \begin{bmatrix} \mathbf{X}_i \\ 1 \end{bmatrix} \tag{1} \] 我们逐项解释这个公式： \(\mathbf{R} \in \mathbb{R}^{3 \times 3}\) 是一个旋转矩阵，描述相机坐标系相对于世界坐标系的朝向； \(\mathbf{t} \in \mathbb{R}^3\) 是一个平移向量，描述世界坐标系原点在相机坐标系下的位置（注意：这是从世界到相机的变换，所以点先旋转再平移：\(\mathbf{X}_i^c = \mathbf{R} \mathbf{X}_i + \mathbf{t}\)）； \([\mathbf{R} \mid \mathbf{t}] \in \mathbb{R}^{3 \times 4}\) 表示将 \(\mathbf{R}\) 和 \(\mathbf{t}\) 拼接成一个 \(3 \times 4\) 的矩阵，用于对齐次坐标 \([\mathbf{X}_i^\top, 1]^\top\) 进行线性变换； \(s_i\) 是一个未知的正实数尺度因子。它之所以出现，是因为等式左边是齐次坐标（homogeneous coordinates）。在齐次表示中，\([u, v, 1]^\top\) 和 \([s u, s v, s]^\top\) 代表同一个像素点。因此，\(s_i\) 不是我们要估计的参数，也不是已知量，而是一个由深度决定的中间变量（实际上 \(s_i = Z_i^c\)，即该点在相机坐标系下的 Z 坐标）。我们在优化时并不直接求 \(s_i\)，而是通过“重投影”来消除它。 为了进行数值优化，我们需要将 (1) 式转换为显式的像素坐标表达式。