Part III · 第 11 章多视图几何 · 11.4Part III · Ch. 11 Multi-View Geometry · 11.4

PnP 位姿估计Perspective-n-Point

机器人常常"知道一些点在哪里，却不知道相机在哪里"：零件的 CAD 模型，贴在地面上的 AprilTag，SLAM 建好的地图。PnP（Perspective-n-Point）用这些已知的三维点和它们在图像里的位置，直接求出相机相对物体的位姿。本节从"一个像素给两个方程、位姿有六个未知数"的计数出发，看 P3P 的四个解；用 DLT 得到线性初值，再在 SE(3) 上用 Gauss–Newton / LM 精修（含解析雅可比）；用协方差回答"这个位姿有多可信"；看平面标签特有的翻转歧义；最后用 RANSAC 对付误匹配。 Robots often "know where some points are but not where the camera is": a part's CAD model, an AprilTag stuck on the floor, a SLAM map. Perspective-n-Point uses those known 3-D points and where they appear in the image to recover the camera's pose relative to the object, ${}^{c}T_{o}$. We start from the count "a pixel gives two equations, a pose has six unknowns" and meet the four solutions of P3P; get a linear first guess with the DLT and polish it on SE(3) with Gauss–Newton / LM and an analytic Jacobian; use the covariance to say how far the pose can be trusted; study the flip ambiguity peculiar to planar tags; and finally handle bad matches with RANSAC.

约 35 分钟~35 min 前置：1.12 SVD · 5.3 SE(3) · 5.6 exp/log · 5.7 扰动 · 6.6 Gauss–Newton/LM · 6.10 流形优化 · 10.3 内外参 · 11.3 三角化Prereqs: 1.12 SVD · 5.3 SE(3) · 5.6 exp/log · 5.7 Perturbations · 6.6 Gauss–Newton/LM · 6.10 Manifold optimization · 10.3 Intrinsics & extrinsics · 11.3 Triangulation 6 个交互6 interactives

1为什么机器人需要它Why robots need this

三角化（11.3 节）回答"相机已知，点在哪"。机器人更常遇到相反的问题：点已知，相机在哪？一旦几个点的三维位置是已知的（来自 CAD 模型、一张地图、一块标定板），一张图像就足以给出相机相对它们的完整六自由度位姿：

相对地图定位：视觉重定位和视觉里程计的跟踪线程，把当前帧的特征匹配到地图里的三维点，再用 PnP 求出机器人在地图中的位姿。
物体位姿：模型已知，网络预测出关键点的像素位置（PVNet、DOPE 一类），PnP 把它们变成物体相对相机的位姿，抓取规划拿去用。
标记与标定板：AprilTag、ArUco、棋盘格的四个角点或格点是已知平面上的点，一次 PnP 就给出标记位姿，用于机器人定位、视觉伺服和手眼标定（10.4 节）。

它与前后几节的关系可以一句话记住：三角化是"2D–2D ⇒ 3D"，PnP 是"3D–2D ⇒ 位姿"，光束法平差（11.6 节）则让点和位姿同时未知。本节的算法分四层：最小解（P3P，3 个点）、线性解（DLT，≥6 个点）、非线性精修（SE(3) 上的 Gauss–Newton / LM）、鲁棒外壳（RANSAC，11.5 节）。每一层都在页面里真实运行。

Triangulation (Lesson 11.3) answers "the cameras are known, where is the point?" Robots more often face the reverse: the points are known, where is the camera? Once the 3-D positions of a few points are known (from a CAD model, a map, a calibration board), a single image is enough to give the full six-degree-of-freedom camera pose relative to them:

Localizing against a map: relocalization and the tracking thread of visual odometry match the current frame's features to 3-D map points and run PnP to get the robot's pose in the map.
Object pose: the model is known, a network predicts keypoint pixels (PVNet, DOPE and relatives), and PnP turns them into the object's pose relative to the camera for the grasp planner.
Markers and boards: the four corners of an AprilTag or ArUco marker, or the grid points of a checkerboard, are known points on a plane; one PnP gives the marker pose for localization, visual servoing and hand–eye calibration (Lesson 10.4).

The relation to its neighbours fits in one line: triangulation is "2D–2D ⇒ 3D", PnP is "3D–2D ⇒ pose", and bundle adjustment (Lesson 11.6) makes points and poses unknown together. The algorithms here come in four layers: a minimal solver (P3P, 3 points), a linear solver (DLT, ≥ 6 points), nonlinear refinement (Gauss–Newton / LM on SE(3)) and a robust wrapper (RANSAC, Lesson 11.5). Every layer runs live in this page.

2直觉：像素给方向，模型给距离Intuition: pixels give directions, the model gives distances

先约定坐标系。已知点写在物体系 {o} 里（如果这些点是一张地图，{o} 就是世界系 {w}）；相机系 {c} 是 OpenCV 光学系：x 向右，y 向下，z 向前，像素 $(u,v)$ 的 $v$ 向下。PnP 求的是 ${}^{c}T_{o}=(R,\mathbf t)$，它把物体系里的点变到相机系：

$$ \mathbf x_c=R\,\mathbf x_o+\mathbf t,\qquad \lambda\,\tilde{\mathbf u}=K\,\mathbf x_c . $$

这正是 OpenCV 里的"外参" $[R\,|\,\mathbf t]$（10.3 节）。相机自己在物体系里的位姿是它的逆 ${}^{o}T_{c}=({}^{c}T_{o})^{-1}$，相机中心 $\mathbf C=-R^{\top}\mathbf t$。本节的 3D 图都画在物体系里，所以会动的是相机。

一个像素对应一条射线，只告诉你点在哪个方向，不告诉你有多远。所以每个对应点给 2 个方程，而位姿有 6 个未知数（3 个旋转、3 个平移）：一个点远远不够，两个点还留下 2 维的解族，三个点恰好 $2\times3=6$ 个方程对 6 个未知数。几何上是这样的：三个像素给出三条射线，模型告诉我们三个点两两之间的距离，于是问题变成"把一个已知形状的刚性三角形的三个顶点分别放在三条射线上，有哪些摆法？"这就是 P3P。三个距离约束是关于三个深度的三个二次方程（余弦定理，见"数学"一节），它们最多有 4 个深度全为正的公共解。

FIG 1 把这些解全部算出来。每一台"幽灵相机"都把选中的三个点精确投影到你给的像素上（误差不超过约 $10^{-8}$ 像素，通常小得多），它们只在其余的点上不同意。第四个点一出现，通常只剩一台相机对得上。

First the frames. The known points are written in the object frame {o} (if they form a map, {o} is the world frame {w}); the camera frame {c} is the OpenCV optical frame: x right, y down, z forward, and the pixel coordinate $v$ of $(u,v)$ points down. PnP solves for ${}^{c}T_{o}=(R,\mathbf t)$, which carries object-frame points into the camera frame:

$$ \mathbf x_c=R\,\mathbf x_o+\mathbf t,\qquad \lambda\,\tilde{\mathbf u}=K\,\mathbf x_c . $$

This is exactly OpenCV's "extrinsics" $[R\,|\,\mathbf t]$ (Lesson 10.3). The camera's own pose in the object frame is the inverse, ${}^{o}T_{c}=({}^{c}T_{o})^{-1}$, with camera centre $\mathbf C=-R^{\top}\mathbf t$. The 3-D figures here are drawn in the object frame, so it is the camera that moves.

A pixel corresponds to a ray: it tells you the direction of the point, not how far it is. So each correspondence gives 2 equations while a pose has 6 unknowns (3 rotation, 3 translation): one point is far from enough, two points still leave a 2-parameter family, and three points give exactly $2\times3=6$ equations for 6 unknowns. Geometrically: three pixels give three rays, the model gives the distances between the three points, and the question becomes "where can a rigid triangle of known shape be placed so that its vertices lie on the three rays?" That is P3P. The three distance constraints are three quadratic equations in the three depths (the law of cosines, see "The math"), and they have at most 4 common solutions with all depths positive.

FIG 1 computes all of them. Each "ghost camera" projects the three chosen points exactly onto the pixels you gave (to within about $10^{-8}$ px, usually far less); they only disagree about the other points. As soon as a fourth point appears, usually just one camera still fits.

FIG 1三个点够不够？P3P 的全部解Are three points enough? All solutions of P3P

物体系中的相机，拖动旋转CAMERAS IN THE OBJECT FRAME · drag to orbit

图像（640×480 像素，v 向下）IMAGE (640×480 px, v down)

相机始终看向盒子中心，滑块改变它在物体系里的方位角、俯仰角和距离。琥珀色的三个角点是 P3P 的输入（它们的像素加上盒子的尺寸 0.4×0.3×0.2 m），绿色相机是真值，其它颜色是 P3P 算出的另外的解，每个解的相机中心到三个点之间的细线都画了出来。右图是相机拍到的图像：每个解把整个盒子重投影成自己颜色的线框，在三个选中的点处所有线框完全重合。打开"第 4 个点"后，青色点是观测到的第 4 个角点，每个解预测它的位置与观测的距离写在读数里（红线是那段残差）。 The camera always looks at the box centre; the sliders change its azimuth, elevation and distance in the object frame. The three amber corners are the input to P3P (their pixels plus the box size 0.4×0.3×0.2 m), the green camera is the truth, and the other colours are the further solutions P3P finds; thin lines join each solution's camera centre to the three points. The right panel is the image: every solution re-projects the whole box as a wireframe in its own colour, and at the three chosen points all wireframes coincide exactly. With "4th point" on, the cyan dot is the observed fourth corner; each solution's predicted position is joined to it by a red residual, and the distances are in the readout.

试一试Try this

转动方位角（或按"找 4 解视角"），让 P3P 给出 4 个解。解的个数通常是 2 或 4；换一组三个点，会在不同的方位出现 4 个解。Turn the azimuth (or press "Find 4 solutions") until P3P returns 4 solutions. The count is usually 2 or 4; with a different triple the 4-solution views move to other azimuths.
在 3 个以上解的视角上打开"第 4 个点"：看读数，只有一个解把第 4 个点也投影到 0 像素误差，其余几个差了几十到几百像素。At a view with three or more solutions turn on "4th point": in the readout only one solution also hits the fourth pixel (error ≈ 0 px); the others miss by tens to hundreds of pixels.

核心洞见Key insight

PnP 的本质是"一幅图像 + 一个已知的刚体 = 一个位姿"。像素只提供方向，模型的几何提供距离，缺一不可。每个对应点是 2 个方程；6 个未知数意味着最少 3 个点（可能有最多 4 个解），4 个点起一般唯一，更多的点用来对抗噪声，所以实际用的是最小二乘。

PnP is essentially "one image + a known rigid body = one pose". Pixels supply directions, the model's geometry supplies distances, and you need both. Each correspondence is 2 equations; with 6 unknowns the minimum is 3 points (up to 4 solutions), from 4 points on the solution is generically unique, and extra points fight noise, which is why in practice we solve a least-squares problem.

3动手玩：先线性猜一个，再在流形上打磨Play: guess linearly, then polish on the manifold

线性初值：DLTA linear first guess: the DLT

有 $n$ 个点时最直接的办法是把问题变成线性的。把投影写成 $\lambda\tilde{\mathbf u}=P\tilde{\mathbf X}$，$P$ 是 $3\times4$ 矩阵。用叉积消掉 $\lambda$：$\tilde{\mathbf u}\times P\tilde{\mathbf X}=\mathbf 0$，每个点就给出关于 $P$ 的 12 个元素的 2 个线性方程。堆成 $A\mathbf p=\mathbf 0$，解是 $A$ 的最小奇异值对应的右奇异向量（1.12 节），这就是 DLT（与 10.5 节的单应 DLT 同一套路）。$P$ 有 11 个自由度，所以至少要 6 个点（"5.5 个"）。K 已知时，$K^{-1}P\approx\lambda[R\,|\,\mathbf t]$：用 SVD 把左边 $3\times3$ 块投影到最近的旋转矩阵 $R=UV^{\top}$，符号由 $\det R=+1$（等价于点在相机前方）定。

DLT 有两个典型毛病：它最小化的是代数误差而不是像素误差，噪声下不准；而且点共面时 $A$ 秩亏，答案不唯一。FIG 2 让你同时看到这两点，并用平面目标的专用解法（单应）补救。

With $n$ points the most direct route is to make the problem linear. Write the projection as $\lambda\tilde{\mathbf u}=P\tilde{\mathbf X}$ with a $3\times4$ matrix $P$. Eliminate $\lambda$ with a cross product, $\tilde{\mathbf u}\times P\tilde{\mathbf X}=\mathbf 0$, and each point gives 2 linear equations in the 12 entries of $P$. Stack them into $A\mathbf p=\mathbf 0$; the solution is the right singular vector of the smallest singular value of $A$ (Lesson 1.12). This is the DLT (the same trick as the homography DLT of Lesson 10.5). $P$ has 11 degrees of freedom, so at least 6 points are needed ("5½"). When K is known, $K^{-1}P\approx\lambda[R\,|\,\mathbf t]$: project the left $3\times3$ block to the nearest rotation, $R=UV^{\top}$ by SVD, and fix the sign by $\det R=+1$ (equivalently, points in front of the camera).

The DLT has two typical weaknesses: it minimizes an algebraic error rather than a pixel error, so it is inaccurate under noise; and when the points are coplanar, $A$ loses rank and the answer is not unique. FIG 2 shows both, and the cure for planar targets (the homography).

FIG 2线性 PnP：DLT 与平面目标Linear PnP: the DLT and planar targets

图像：观测 vs 估计的重投影IMAGE · observed vs re-projected

旋转误差 vs 点数（24 次噪声的中位数）ROTATION ERROR vs n (median of 24 noise draws)

DLT 矩阵 A 的 12 个奇异值（对数，相对最大值）THE 12 SINGULAR VALUES OF THE DLT MATRIX A (log, relative to the largest)

琥珀色点是带噪声的观测像素，空心绿圈是无噪声的真值，紫色叉是估计位姿的重投影，红线是残差。右上图是蒙特卡洛：在当前噪声下对每个点数重复 24 次，实线是 DLT（或单应）的旋转误差中位数，虚线是再用 8 步 LM 精修之后（见下一个图）。底部的条形图是 DLT 矩阵 $A$（$2n\times12$，已做 Hartley 归一化）的奇异值：一般位置的点只有最后一个接近 0，对应唯一的解；共面的点会多出 3 个恰好为 0 的奇异值（红色），无噪声时整个 4 维零空间里的任何 $P$ 都同样"完美"地拟合数据。真值位姿：旋转约 124°，距离 1.2 m，$f=600$ 像素。 Amber dots are the noisy observed pixels, hollow green rings the noise-free truth, purple crosses the re-projection of the estimated pose, and red lines the residuals. Top right is a Monte-Carlo run: at the current noise level, 24 repetitions per point count; the solid line is the median rotation error of the DLT (or homography), the dashed line after 8 LM steps of refinement (next figure). The bars at the bottom are the singular values of the DLT matrix $A$ ($2n\times12$, Hartley-normalized): for points in general position only the last one is near 0, giving a unique solution; coplanar points add 3 singular values that are exactly 0 (red), and for noise-free data every $P$ in the resulting 4-D null space fits equally "perfectly". True pose: rotation about 124°, distance 1.2 m, $f=600$ px.

试一试Try this

选"共面目标 + 3D DLT"：旋转误差跳到几十度甚至上百度，底部多出 3 根红色的奇异值条掉到底。点数再多也没用。为什么？Choose "Coplanar target + 3D DLT": the rotation error jumps to tens or even over a hundred degrees and 3 extra red singular-value bars fall to the floor. More points do not help. Why?
保持共面目标，把方法切到"单应"，噪声不小于 1 像素：误差回到几度以内，而且无论点多还是少都正常（4 个点就够）。Keep the coplanar target and switch the method to "Homography" with noise ≥ 1 px: the error returns to a few degrees, and it works with few points as well (4 are enough).

精修：在 SE(3) 上最小化重投影误差Refinement: minimizing reprojection error on SE(3)

DLT 只是个初值。我们真正想要的是使重投影误差 $E(T)=\tfrac12\sum_i\|\pi(K(R\mathbf X_i+\mathbf t))-\mathbf u_i\|^2$ 最小的位姿（像素噪声是高斯时，这就是最大似然，4.6 节）。位姿住在 SE(3) 上，所以每一步在流形上走（6.10 节）：求一个小扭转 $\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)$（角速度在前，和全课一致），然后 $T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T$。这是左扰动（5.7 节），它的雅可比特别干净：$\delta\mathbf x_c=\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v=[-[\mathbf x_c]_\times\ \ I]\,\delta\boldsymbol\xi$，再乘投影的 $2\times3$ 雅可比（3.3 节）即可。

FIG 3 从一个初值出发迭代，你可以拖动迭代滑块看每一步：相机位姿（3D）、重投影的残差（图像）、代价（曲线）。初值有两种：DLT 的结果，或者把真值转了一个很大的角度。Gauss–Newton（直接解 $J^{\top}J\,\delta=-J^{\top}\mathbf r$）从好初值几步就收敛；初值很差时它会一步跳进错误的盆地（停在约 180° 的局部极小）甚至发散，Levenberg–Marquardt 的阻尼项（6.6 节）让每一步都下降。

The DLT is only a starting point. What we actually want is the pose minimizing the reprojection error $E(T)=\tfrac12\sum_i\|\pi(K(R\mathbf X_i+\mathbf t))-\mathbf u_i\|^2$ (with Gaussian pixel noise this is the maximum-likelihood estimate, Lesson 4.6). The pose lives on SE(3), so every step moves on the manifold (Lesson 6.10): find a small twist $\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)$ (angular part first, as everywhere in the course) and set $T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T$. This is a left perturbation (Lesson 5.7) and its Jacobian is especially clean: $\delta\mathbf x_c=\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v=[-[\mathbf x_c]_\times\ \ I]\,\delta\boldsymbol\xi$, then multiply by the $2\times3$ Jacobian of the projection (Lesson 3.3).

FIG 3 iterates from a starting pose and lets you scrub through the steps: the camera pose (3-D), the residuals (image) and the cost (curve). The start is either the DLT result or the true pose rotated by a large angle. Gauss–Newton (solve $J^{\top}J\,\delta=-J^{\top}\mathbf r$ directly) converges in a few steps from a good start; from a bad one it can jump into the wrong basin (stalling at a local minimum about 180° off) or even diverge, while the damping of Levenberg–Marquardt (Lesson 6.6) makes every step go downhill.

FIG 3在 SE(3) 上精修：GN 与 LMRefinement on SE(3): GN vs LM

相机位姿轨迹（物体系），拖动旋转CAMERA POSE TRAIL (object frame) · drag to orbit

图像：重投影与残差IMAGE · re-projection & residuals

代价 E 随迭代（对数）COST E vs iteration (log)

3D 图里绿色相机是真值，琥珀色相机是第 $k$ 步的估计，小点和折线是此前各步的相机中心。图像里琥珀点是观测，紫色叉是第 $k$ 步的重投影，红线是残差 $\mathbf r_i$（悬停公式里的 $\mathbf r$ 会加粗）。曲线是两种方法的代价（虚线 GN，实线 LM），竖线是当前的 $k$。读数最后一行是核对：解析雅可比与对 $\operatorname{Exp}(\pm h\mathbf e_j)\,T$ 做中心差分得到的雅可比之差（相对 $\|J\|_{\max}$）。14 个点，噪声 1 像素，随机数固定。 In the 3-D panel the green camera is the truth and the amber camera the estimate at step $k$; dots and a polyline show the camera centres of earlier steps. In the image, amber dots are observations, purple crosses the re-projection at step $k$, and red lines the residuals $\mathbf r_i$ (hover $\mathbf r$ in the formulas to thicken them). The curves are the costs of the two methods (dashed GN, solid LM) and the vertical bar marks the current $k$. The last readout line is a check: the analytic Jacobian minus a central-difference Jacobian built from $\operatorname{Exp}(\pm h\mathbf e_j)\,T$, relative to $\|J\|_{\max}$. 14 points, 1 px noise, fixed random numbers.

试一试Try this

选"偏移初值"，把偏移角调到 130° 到 145° 之间，方法选 GN，把迭代滑到底：GN 的代价先乱跳，最后停在一个很大的值（旋转误差约 180° 的局部极小），回不到最优。Choose "Offset start", set the offset between 130° and 145°, method GN, and scrub to the end: the GN cost jumps around and finally stalls at a large value (a local minimum with about 180° of rotation error) and never returns to the optimum.
同样的偏移角（≥ 125°）切到 LM，滑到底：代价单调下降，最后旋转误差不到 1°。LM 多花了几步，但每一步都是安全的。With the same offset (≥ 125°) switch to LM and scrub to the end: the cost falls monotonically and the final rotation error is under 1°. LM takes more steps, but every step is safe.

4第二、第三视角：这个位姿有多可信？Second and third views: how far can this pose be trusted?

协方差：小而远的目标，位姿不确定Covariance: small, distant targets give uncertain poses

重投影误差降到 0.3 像素，并不说明位姿准到 0.3 像素对应的量。精修收敛后，估计的不确定性可以从同一个雅可比读出来（4.5 节的不确定性传播）：像素噪声为 $\sigma$，则扰动坐标 $\delta\boldsymbol\xi$ 的协方差是

$$ \Sigma_{\xi}\;\approx\;\big(J^{\top}\Sigma_u^{-1}J\big)^{-1}=\sigma^{2}\big(J^{\top}J\big)^{-1}\quad(\Sigma_u=\sigma^{2}I). $$

相机中心 $\mathbf C=-R^{\top}\mathbf t$，一阶下 $\delta\mathbf C=-R^{\top}\delta\mathbf v$，所以相机位置的协方差是 $R^{\top}\Sigma_{vv}R$（$\Sigma_{vv}$ 是 $\Sigma_\xi$ 右下 $3\times3$ 块）。FIG 4 把它画成以相机为中心的椭球。目标越小、越远、点越少，椭球越大，其尺度规律是 $\sigma_{\text{深度}}\propto \sigma Z^{2}/(f\,s\sqrt n)$（$Z$ 为距离，$s$ 为目标的半径，$n$ 为点数）。

A reprojection error of 0.3 px does not mean the pose is accurate to whatever 0.3 px corresponds to. Once the refinement has converged, the uncertainty of the estimate can be read from the same Jacobian (uncertainty propagation, Lesson 4.5): for pixel noise $\sigma$, the covariance of the perturbation coordinates $\delta\boldsymbol\xi$ is

$$ \Sigma_{\xi}\;\approx\;\big(J^{\top}\Sigma_u^{-1}J\big)^{-1}=\sigma^{2}\big(J^{\top}J\big)^{-1}\quad(\Sigma_u=\sigma^{2}I). $$

The camera centre is $\mathbf C=-R^{\top}\mathbf t$; to first order $\delta\mathbf C=-R^{\top}\delta\mathbf v$, so the camera-position covariance is $R^{\top}\Sigma_{vv}R$ ($\Sigma_{vv}$ is the lower-right $3\times3$ block of $\Sigma_\xi$). FIG 4 draws it as an ellipsoid around the camera. The smaller, farther and sparser the target, the larger the ellipsoid, following $\sigma_{\text{depth}}\propto \sigma Z^{2}/(f\,s\sqrt n)$ ($Z$ the distance, $s$ the target radius, $n$ the number of points).

FIG 4位姿协方差 $(J^{\top}J)^{-1}$Pose covariance $(J^{\top}J)^{-1}$

相机位置的 1σ 椭球（放大显示）1σ ELLIPSOID OF THE CAMERA POSITION (magnified)

左图：目标点（青色）在原点附近，相机（绿色）在 $Z$ 远处看它，青色线框是相机中心的 1σ 协方差椭球，已按图中标注的倍数放大，否则看不见；紫色小点是 Monte-Carlo：200 次重新加噪声并用 LM 精修后的相机中心（同样放大）。右图的条形：上面三条是姿态的 1σ（绕相机 x、y、z 轴，单位度），下面三条是相机中心的 1σ（沿相机 x、y、z 轴，z 是光轴即深度，单位毫米）；实心条是 $(J^{\top}J)^{-1}$ 的预测，竖线是 Monte-Carlo 的经验值，两者吻合。（三维里 1σ 椭球只装得下约 20% 的样本，所以紫点比椭球散得开。）注意对小目标，横向（相机绕目标"转圈"，等价于姿态误差）和深度的不确定度是同一量级，二者都随 $Z^{2}/(s\sqrt n)$ 增长。 Left: the target points (cyan) sit near the origin and the camera (green) views them from distance $Z$; the cyan wireframe is the 1σ covariance ellipsoid of the camera centre, magnified by the factor printed in the panel (otherwise it would be invisible); the purple dots are Monte-Carlo samples: the camera centres after 200 fresh noise draws and LM refinements (magnified equally). Right: the top three bars are the attitude 1σ (about the camera x, y, z axes, degrees), the bottom three the camera-centre 1σ (along camera x, y, z; z is the optical axis, i.e. depth; millimetres). Filled bars are the $(J^{\top}J)^{-1}$ prediction, the ticks are the Monte-Carlo values, and they agree. (In 3-D a 1σ ellipsoid holds only about 20% of the samples, so the purple dots spread beyond it.) For small targets the lateral uncertainty (the camera "orbiting" the target, equivalent to an attitude error) and the depth uncertainty are of the same order, and both grow like $Z^{2}/(s\sqrt n)$.

试一试Try this

把距离拉到 4 m 以上、目标半径保持很小：深度 1σ 超过 10 cm。再把点数加到 60、目标半径加大：椭球缩回毫米量级。Pull the distance beyond 4 m with a small target radius: the depth 1σ exceeds 10 cm. Then raise the points to 60 and enlarge the radius: the ellipsoid shrinks back to millimetres.

平面标签的翻转歧义The flip ambiguity of a planar tag

AprilTag 的四个角点是共面的，位姿只由 4 个点（8 个方程）决定。对一个小的、远的、斜着看的标签，投影近似于弱透视，而弱透视下，把平面沿视线"翻"到另一侧（法向量关于视线镜像）得到的图像几乎一样。透视效应只留下一点点差别，可能被噪声盖过。结果是重投影误差有两个局部极小值，噪声足够大时，错的那个甚至更低。FIG 5 中，先用平面单应得到初值并用 LM 精修，得到候选 1；再把它沿视线翻转作为另一个初值再精修，得到候选 2。IPPE（Collins & Bartoli）用闭式给出这两个解，AprilTag 库为两个候选各算一个误差，原因就在这里。底部曲线是"剖面"重投影误差：固定绕相机 y 轴的倾角 $s$，其余 5 个自由度都重新优化。

The four corners of an AprilTag are coplanar, and the pose is determined by only 4 points (8 equations). For a small, distant, obliquely viewed tag the projection is close to weak-perspective, and under weak perspective, flipping the plane about the line of sight (mirroring its normal across the viewing ray) produces almost the same image. The perspective effect leaves only a small difference, which noise can swamp. The result is two local minima of the reprojection error, and with enough noise the wrong one is even lower. In FIG 5 we get candidate 1 from the planar homography followed by LM, and candidate 2 by flipping it about the line of sight, using that as a second start, and refining again. IPPE (Collins & Bartoli) gives both solutions in closed form, and AprilTag libraries compute an error for each of the two candidates for exactly this reason. The bottom curve is the "profile" reprojection error: fix the tilt $s$ about the camera's y axis and re-optimize the other 5 degrees of freedom.

FIG 5平面标签的两个极小值Two minima for a planar tag

俯视（相机 x–z 平面，放大到标签）TOP VIEW (camera x–z plane, zoomed to the tag)

图像（放大到标签）IMAGE (zoomed to the tag)

剖面重投影误差 vs 倾角 sPROFILE REPROJECTION ERROR vs tilt s

俯视图里绿色是真实的标签（一条线段，因为从上面看它是侧立的），琥珀色和紫色是两个候选，虚线是视线。两个候选关于视线互为"镜像"。右图里两个候选的重投影几乎重合（差零点几像素），所以单看图像无法区分。底部曲线横轴是倾角 $s$，纵轴是 RMS 重投影误差（像素，每个坐标）：绿色竖线是真值倾角，圆点是两个候选。按"100 次噪声"会在当前设定下重复 100 次，统计两个候选同时存在以及"重投影误差较低的那个其实是错的"的比例。标签：正方形，倾角 $s$ 为绕相机 y 轴的倾斜。 In the top view green is the true tag (a line segment, since seen from above it stands edge-on), amber and purple are the two candidates, and the dashed line is the line of sight. The two candidates are mirror images about the line of sight. In the image the two candidates re-project almost identically (differences of a fraction of a pixel), so the image alone cannot tell them apart. The bottom curve has the tilt $s$ on the horizontal axis and the RMS reprojection error (pixels, per coordinate) on the vertical axis; the green vertical line is the true tilt and the dots are the two candidates. "100 noise draws" repeats the experiment 100 times at the current settings and reports how often two candidates exist and how often "the one with the lower reprojection error is actually the wrong one". The tag is a square; the tilt $s$ is a rotation about the camera's y axis.

试一试Try this

把标签调小、拉远（比如 5 cm、2 m，噪声 1 px），按"新噪声"几次，直到读数显示"误差较低的候选是错的"（约四成到一半的噪声样本会这样；再按"100 次噪声"核对）。再把标签变大或拉近：这种情况几乎消失。Make the tag small and far (say 5 cm at 2 m, 1 px noise) and press "New noise" a few times until the readout says "the candidate with the lower error is wrong" (about four to five in ten noise draws do this; check with "100 noise draws"). Then enlarge the tag or move it closer: the effect almost disappears.

同一个问题的三种说法One problem, three phrasings

代数：P3P 是三个二次方程，消元后是关于深度比的一个四次方程，最多 4 个正实根；平面翻转是其中的一对近似重根。几何：把刚性模型放在一束射线上；射线几乎平行（远、小）时，前后翻转几乎不改变图像。统计：最大似然估计，不确定性是 Fisher 信息 $J^{\top}\Sigma_u^{-1}J$ 的逆；翻转歧义是后验分布有两个峰，高斯近似 $(J^{\top}J)^{-1}$ 只描述其中一个峰的宽度，对"两个峰"的问题无能为力。

Algebra: P3P is three quadratic equations, which reduce to one quartic in the depth ratio with at most 4 positive real roots; the planar flip is a pair of nearly coincident roots. Geometry: place the rigid model on a bundle of rays; when the rays are nearly parallel (far and small), flipping front to back barely changes the image. Statistics: the maximum-likelihood estimate, with uncertainty given by the inverse Fisher information $J^{\top}\Sigma_u^{-1}J$; the flip ambiguity means the posterior has two peaks, and the Gaussian approximation $(J^{\top}J)^{-1}$ describes the width of one peak only, so it cannot represent a "two peaks" problem.

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 3 或 FIG 4 中对应的元素会被高亮。Tip: hover the highlighted symbols in the formulas; the matching element in FIG 3 or FIG 4 lights up.

模型、代价与计数Model, cost and counting

物体系中的已知点 $\mathbf X_i$ 及其观测像素 $\mathbf u_i$，$i=1..n$；相机内参 $K$（$f_x,f_y,c_x,c_y$）已知，且图像已去畸变（10.3 节）。令 $\mathbf x_{c,i}=R\mathbf X_i+\mathbf t$，重投影残差和代价是

Known object-frame points $\mathbf X_i$ and their observed pixels $\mathbf u_i$, $i=1..n$; the intrinsics $K$ ($f_x,f_y,c_x,c_y$) are known and the image is undistorted (Lesson 10.3). With $\mathbf x_{c,i}=R\mathbf X_i+\mathbf t$, the reprojection residual and cost are

$$ \hl{resid}{\mathbf r_i}(T)=\pi(\mathbf x_{c,i})-\mathbf u_i,\qquad \pi(\mathbf x)=\begin{bmatrix}f_x\,x/z+c_x\\ f_y\,y/z+c_y\end{bmatrix},\qquad E(T)=\tfrac12\sum_{i=1}^{n}\|\mathbf r_i(T)\|^{2},\qquad \hat T=\arg\min_{T\in\mathrm{SE}(3)}E(T). $$

像素噪声独立同分布 $\mathcal N(0,\sigma^2I)$ 时 $\hat T$ 是最大似然估计（4.6 节）。下表把 PnP 放进它的几个近亲里，计数都是"每个对应点的方程数 vs 未知数"：

With i.i.d. $\mathcal N(0,\sigma^2I)$ pixel noise $\hat T$ is the maximum-likelihood estimate (Lesson 4.6). The table places PnP among its relatives; the counting is always "equations per correspondence versus unknowns":

问题Problem	已知Known	未知（自由度）Unknown (DoF)	最少需要Minimum
三角化（11.3）Triangulation (11.3)	相机 $P_j$，像素 $\mathbf u_{ij}$cameras $P_j$, pixels $\mathbf u_{ij}$	$\mathbf X$ (3)	2 个视图（4 个方程）2 views (4 equations)
P3P / PnP	$K,\ \mathbf X_i,\ \mathbf u_i$	${}^{c}T_{o}$ (6)	3 个点（至多 4 个解）；4 个点起一般唯一3 points (≤ 4 solutions); unique generically from 4
DLT（K 未知）DLT (K unknown)	$\mathbf X_i,\ \mathbf u_i$	$P$ (11)	6 个点，且不共面6 points, not coplanar
平面目标 PnPPlanar-target PnP	$K$，平面上的点$K$, points on a plane	$H$ (8) → ${}^{c}T_{o}$ (6)	4 个点（有翻转歧义）4 points (flip ambiguity)
光束法平差（11.6）Bundle adjustment (11.6)	$K,\ \mathbf u_{ij}$	所有 $T_j$ 和 $\mathbf X_i$all $T_j$ and $\mathbf X_i$	多视图many views

DLT 与"投影到 SO(3)"The DLT and "projecting to SO(3)"

令 $\tilde{\mathbf X}_i=(X,Y,Z,1)^{\top}$，$\mathbf p\in\mathbb R^{12}$ 是 $P$ 的各行依次拼起来。$\tilde{\mathbf u}\times P\tilde{\mathbf X}=\mathbf 0$ 的前两个独立方程是

Let $\tilde{\mathbf X}_i=(X,Y,Z,1)^{\top}$ and let $\mathbf p\in\mathbb R^{12}$ stack the rows of $P$. The first two independent equations of $\tilde{\mathbf u}\times P\tilde{\mathbf X}=\mathbf 0$ are

$$ \underbrace{\begin{bmatrix}\mathbf 0^{\top} & -\tilde{\mathbf X}_i^{\top} & v_i\tilde{\mathbf X}_i^{\top}\\ \tilde{\mathbf X}_i^{\top} & \mathbf 0^{\top} & -u_i\tilde{\mathbf X}_i^{\top}\end{bmatrix}}_{2\times12}\mathbf p=\mathbf 0,\qquad \hat{\mathbf p}=\arg\min_{\|\mathbf p\|=1}\|A\mathbf p\|=\mathbf v_{12}\ \text{(the last right singular vector of }A). $$

点和像素都要先做 Hartley 归一化（平移到质心、缩放到平均距离 $\sqrt2$ 或 $\sqrt3$），否则条件数很差。共面退化：如果所有 $\tilde{\mathbf X}_i$ 满足 $\boldsymbol\pi^{\top}\tilde{\mathbf X}=0$，那么对任意 $\mathbf a\in\mathbb R^3$，$P'=P+\mathbf a\boldsymbol\pi^{\top}$ 对这些点给出同样的像，于是 $\mathbf e_k\boldsymbol\pi^{\top}$（$k=1,2,3$）总是 $A$ 的零向量，无论有无噪声；无噪声时 $P$ 本身也是，零空间就是 $\{P,\ \mathbf e_1\boldsymbol\pi^{\top},\ \mathbf e_2\boldsymbol\pi^{\top},\ \mathbf e_3\boldsymbol\pi^{\top}\}$ 张成的 4 维空间：FIG 2 底部掉到底的红色条。K 已知时：

Both the points and the pixels must first be Hartley-normalized (translate to the centroid, scale to mean distance $\sqrt2$ or $\sqrt3$), otherwise the conditioning is poor. The coplanar degeneracy: if every $\tilde{\mathbf X}_i$ satisfies $\boldsymbol\pi^{\top}\tilde{\mathbf X}=0$, then for any $\mathbf a\in\mathbb R^3$ the matrix $P'=P+\mathbf a\boldsymbol\pi^{\top}$ gives the same image of these points, so $\mathbf e_k\boldsymbol\pi^{\top}$ ($k=1,2,3$) are always null vectors of $A$, noise or not; for noise-free data $P$ itself is one too, and the null space is the 4-dimensional span of $\{P,\ \mathbf e_1\boldsymbol\pi^{\top},\ \mathbf e_2\boldsymbol\pi^{\top},\ \mathbf e_3\boldsymbol\pi^{\top}\}$: the red bars at the floor in FIG 2. With K known:

$$ K^{-1}\hat P=\big[\,M\mid\mathbf m\,\big]\approx\lambda\,[\,R\mid\mathbf t\,],\qquad M=U\Sigma V^{\top}\ \Rightarrow\ R=UV^{\top},\quad \lambda=\tfrac13\operatorname{tr}\Sigma,\quad \mathbf t=\mathbf m/\lambda . $$

$UV^{\top}$ 是 Frobenius 范数下离 $M$ 最近的正交矩阵（正交 Procrustes，12.1 节的 ICP/Kabsch 用的是同一个引理）；$\hat P$ 只确定到符号，要求 $\det R=+1$ 就固定了符号。

$UV^{\top}$ is the orthogonal matrix nearest to $M$ in Frobenius norm (orthogonal Procrustes; the Kabsch step of ICP in Lesson 12.1 uses the same lemma). $\hat P$ is only defined up to sign, and requiring $\det R=+1$ fixes it.

P3P：余弦定理与四次方程P3P: the law of cosines and a quartic

令 $\mathbf f_i$ 是像素 $\mathbf u_i$ 的单位视线（$K^{-1}\tilde{\mathbf u}_i$ 归一化），$\mathbf x_{c,i}=d_i\mathbf f_i$，$d_i>0$ 是未知的深度（沿视线的距离）。$a=\|\mathbf X_2-\mathbf X_3\|$，$b=\|\mathbf X_1-\mathbf X_3\|$，$c=\|\mathbf X_1-\mathbf X_2\|$ 在物体系里已知，$\cos\alpha=\mathbf f_2\!\cdot\!\mathbf f_3$，$\cos\beta=\mathbf f_1\!\cdot\!\mathbf f_3$，$\cos\gamma=\mathbf f_1\!\cdot\!\mathbf f_2$。三角形的三条边在相机系里必须等于模型里的边长：

Let $\mathbf f_i$ be the unit viewing ray of pixel $\mathbf u_i$ ($K^{-1}\tilde{\mathbf u}_i$, normalized), $\mathbf x_{c,i}=d_i\mathbf f_i$ with unknown depth $d_i>0$ (distance along the ray). The side lengths $a=\|\mathbf X_2-\mathbf X_3\|$, $b=\|\mathbf X_1-\mathbf X_3\|$, $c=\|\mathbf X_1-\mathbf X_2\|$ are known from the model, and $\cos\alpha=\mathbf f_2\!\cdot\!\mathbf f_3$, $\cos\beta=\mathbf f_1\!\cdot\!\mathbf f_3$, $\cos\gamma=\mathbf f_1\!\cdot\!\mathbf f_2$. The triangle's sides in the camera frame must equal the model's side lengths:

$$ d_2^{2}+d_3^{2}-2d_2d_3\cos\alpha=a^{2},\qquad d_1^{2}+d_3^{2}-2d_1d_3\cos\beta=b^{2},\qquad d_1^{2}+d_2^{2}-2d_1d_2\cos\gamma=c^{2}. $$

Grunert（1841）的消元：令 $x=d_2/d_1$，$y=d_3/d_1$，$Q(x)=1+x^2-2x\cos\gamma$，由第三式 $d_1^2=c^2/Q(x)$。把第一、二式用 $d_1^2$ 消去，再把两者相减消掉 $y^2$，得到 $y$ 是 $x$ 的有理函数：

Grunert's (1841) elimination: set $x=d_2/d_1$, $y=d_3/d_1$, $Q(x)=1+x^2-2x\cos\gamma$; the third equation gives $d_1^2=c^2/Q(x)$. Eliminate $d_1^2$ from the first two equations, then subtract them to cancel $y^2$; this makes $y$ a rational function of $x$:

$$ y=\frac{c^{2}(x^{2}-1)+(b^{2}-a^{2})\,Q(x)}{2c^{2}\,(x\cos\alpha-\cos\beta)},\qquad c^{2}y^{2}-2c^{2}\cos\beta\;y+c^{2}-b^{2}Q(x)=0\ \Longrightarrow\ \text{a quartic in }x. $$

每个满足 $x,y>0$ 的实根给出一组深度，随后 $(R,\mathbf t)$ 由两组三个点 $\{\mathbf X_i\}$ 与 $\{d_i\mathbf f_i\}$ 的刚体对齐得到（三点各定一个标架即可）。FIG 1 里每个解都用牛顿法在原三个方程上抛光，三点的重投影误差不超过约 $10^{-8}$ 像素（读数里通常是 $10^{-13}$ 左右）。现代 P3P（Gao 等 2003；Kneip 等 2011）数值更稳，思路相同。EPnP（Lepetit 等 2009）的想法：把每个点写成 4 个"控制点"的重心坐标加权和 $\mathbf X_i=\sum_{j=1}^4\alpha_{ij}\mathbf C_j$（权重在两个坐标系里相同），于是相机系里只有 12 个未知数（控制点的坐标），它们线性地进入投影方程，复杂度为 $O(n)$；零空间里的组合系数由控制点之间已知的距离定出，最后同样做刚体对齐。

Each real root with $x,y>0$ gives a set of depths, and $(R,\mathbf t)$ follows from the rigid alignment of the two point triples $\{\mathbf X_i\}$ and $\{d_i\mathbf f_i\}$ (build one orthonormal frame from each triple). In FIG 1 every solution is polished with Newton's method on the original three equations, which keeps the three-point reprojection error below about $10^{-8}$ px (typically about $10^{-13}$ in the readout). Modern P3P solvers (Gao et al. 2003; Kneip et al. 2011) are numerically better behaved but follow the same idea. EPnP (Lepetit et al. 2009) writes each point as a barycentric combination of 4 "control points", $\mathbf X_i=\sum_{j=1}^4\alpha_{ij}\mathbf C_j$ (the weights are the same in both frames), so only 12 unknowns remain in the camera frame (the control points' coordinates) and they enter the projection equations linearly, at cost $O(n)$; the combination in the null space is fixed by the known distances between control points, and the pose again follows from a rigid alignment.

SE(3) 上的 Gauss–Newton / LM 与解析雅可比Gauss–Newton / LM on SE(3) and the analytic Jacobian

左扰动 $T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T$，$\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)$（角在前，与 5.5 节的 $\boldsymbol\xi$ 同序）。一阶下 $\operatorname{Exp}(\delta\boldsymbol\xi)\,\mathbf x_c\approx\mathbf x_c+\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v$，而 $\delta\boldsymbol\omega\times\mathbf x_c=-[\mathbf x_c]_\times\delta\boldsymbol\omega$（1.8 节）。链式法则：

Left perturbation $T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T$ with $\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)$ (angular part first, the same order as $\boldsymbol\xi$ in Lesson 5.5). To first order $\operatorname{Exp}(\delta\boldsymbol\xi)\,\mathbf x_c\approx\mathbf x_c+\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v$, and $\delta\boldsymbol\omega\times\mathbf x_c=-[\mathbf x_c]_\times\delta\boldsymbol\omega$ (Lesson 1.8). The chain rule gives

$$ J_i=\frac{\partial\mathbf r_i}{\partial\,\delta\boldsymbol\xi}=\underbrace{\begin{bmatrix}f_x/z&0&-f_x x/z^{2}\\ 0&f_y/z&-f_y y/z^{2}\end{bmatrix}}_{\partial\pi/\partial\mathbf x_c}\begin{bmatrix}-[\mathbf x_c]_\times & I_3\end{bmatrix}\in\mathbb R^{2\times6},\qquad J=\begin{bmatrix}J_1\\ \vdots\\ J_n\end{bmatrix}\in\mathbb R^{2n\times6}. $$

$$ \text{GN:}\ \ J^{\top}J\,\delta\boldsymbol\xi=-J^{\top}\mathbf r,\qquad \text{LM:}\ \ \big(J^{\top}J+\hl{lm}{\mu}\operatorname{diag}(J^{\top}J)\big)\delta\boldsymbol\xi=-J^{\top}\mathbf r,\qquad T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)\,T . $$

LM 在代价上升时把 $\mu$ 乘大、下降时缩小（6.6 节）。如果改用右扰动 $T\leftarrow T\operatorname{Exp}(\delta\boldsymbol\xi)$，雅可比变成 $\partial\pi/\partial\mathbf x_c\,[-R[\mathbf X_o]_\times\ \ R]$，最小值点相同，但协方差表示在物体系而不是相机系，两者差一个伴随 $\mathrm{Ad}_T$（5.8 节）。FIG 3 读数里的雅可比核对就是对 $\operatorname{Exp}(\pm h\mathbf e_j)\,T$ 的中心差分，和上式相差约 $10^{-10}$（相对 $\|J\|_{\max}$）。

LM multiplies $\mu$ up when the cost rises and down when it falls (Lesson 6.6). With a right perturbation $T\leftarrow T\operatorname{Exp}(\delta\boldsymbol\xi)$ the Jacobian becomes $\partial\pi/\partial\mathbf x_c\,[-R[\mathbf X_o]_\times\ \ R]$; the minimizer is the same, but the covariance is expressed in the object frame instead of the camera frame, the two differing by the adjoint $\mathrm{Ad}_T$ (Lesson 5.8). The Jacobian check in FIG 3's readout is a central difference of $\operatorname{Exp}(\pm h\mathbf e_j)\,T$ and agrees with the formula to about $10^{-10}$ (relative to $\|J\|_{\max}$).

平面目标：从单应读出位姿Planar targets: reading the pose from a homography

平面取 $Z=0$，则 $\lambda\tilde{\mathbf u}=K[\mathbf r_1\ \mathbf r_2\ \mathbf r_3\ \mathbf t]\,(X,Y,0,1)^{\top}=K[\mathbf r_1\ \mathbf r_2\ \mathbf t]\,(X,Y,1)^{\top}$，即平面到图像的单应 $H\simeq K[\mathbf r_1\ \mathbf r_2\ \mathbf t]$（10.5 节；Zhang 标定里同一个式子）。

Take the plane as $Z=0$. Then $\lambda\tilde{\mathbf u}=K[\mathbf r_1\ \mathbf r_2\ \mathbf r_3\ \mathbf t]\,(X,Y,0,1)^{\top}=K[\mathbf r_1\ \mathbf r_2\ \mathbf t]\,(X,Y,1)^{\top}$, i.e. the plane-to-image homography $H\simeq K[\mathbf r_1\ \mathbf r_2\ \mathbf t]$ (Lesson 10.5; the same equation drives Zhang's calibration).

$$ K^{-1}H=[\mathbf h_1\ \mathbf h_2\ \mathbf h_3],\quad \lambda=\tfrac12(\|\mathbf h_1\|+\|\mathbf h_2\|),\quad \mathbf r_1=\tfrac{\mathbf h_1}{\lambda},\ \mathbf r_2=\tfrac{\mathbf h_2}{\lambda},\ \mathbf r_3=\mathbf r_1\times\mathbf r_2,\ \ \mathbf t=\tfrac{\mathbf h_3}{\lambda}\ (t_z>0),\qquad R=\operatorname{polar}\big([\mathbf r_1\ \mathbf r_2\ \mathbf r_3]\big) . $$

噪声使 $\mathbf r_1,\mathbf r_2$ 不严格正交，所以最后一步用 SVD 取最近的旋转（极分解）。这是 FIG 2 中"单应"方法，以及 OpenCV 里 $\textsf{SOLVEPNP\_ITERATIVE}$ 面对平面点时的初值。它没有解决翻转歧义：IPPE 把单应在标签中心处的局部一阶近似分解出两个旋转，对应上面的两个极小值；最终仍应以 LM 精修，再比较两者的重投影误差。

Noise makes $\mathbf r_1,\mathbf r_2$ not exactly orthonormal, so the last step takes the nearest rotation by SVD (the polar decomposition). This is the "Homography" method of FIG 2, and also the initial value OpenCV's $\textsf{SOLVEPNP\_ITERATIVE}$ uses for planar points. It does not resolve the flip ambiguity: IPPE decomposes the local first-order approximation of the homography at the tag centre into two rotations, which correspond to the two minima above; one should still refine both with LM and compare their reprojection errors.

不确定性与经验规律Uncertainty and rules of thumb

在最小值点，Fisher 信息是 $\mathcal I=\sigma^{-2}J^{\top}J$，Cramér–Rao 下界给出 $\operatorname{cov}(\hat{\boldsymbol\xi})\succeq\hl{cov}{\Sigma_\xi}=\sigma^{2}(J^{\top}J)^{-1}$，小噪声下 MLE 达到这个界（FIG 4 的 Monte-Carlo 和它吻合到 5% 左右）。设目标在距离 $Z$ 处，点相对质心的 RMS 横向半径为 $s$，共 $n$ 个点。FIG 4 的数值实验（$s$ 从 3 cm 到 30 cm，$Z\in[0.8,3.2]$ m，$n\in[6,40]$）给出沿光轴的相机位置不确定度

At the minimum the Fisher information is $\mathcal I=\sigma^{-2}J^{\top}J$, and the Cramér–Rao bound gives $\operatorname{cov}(\hat{\boldsymbol\xi})\succeq\hl{cov}{\Sigma_\xi}=\sigma^{2}(J^{\top}J)^{-1}$; for small noise the MLE attains it (FIG 4's Monte-Carlo agrees to about 5%). Let the target be at distance $Z$, with RMS lateral radius $s$ of the points about their centroid, and $n$ points. Numerical experiments in the FIG 4 setting ($s$ from 3 cm to 30 cm, $Z\in[0.8,3.2]$ m, $n\in[6,40]$) give, along the optical axis, a camera-position uncertainty

$$ \sigma_{\text{depth}}\;\approx\;0.7\,\frac{\sigma\,Z^{2}}{f\,s\,\sqrt n}, $$

常数 0.7 取决于点的布局，是数值拟合值，不是定理；它在目标比距离小（$s/Z\lesssim0.1$）时与 FIG 4 的精确值相差约 10%，目标更大（视场更宽）时公式偏大。$\sigma$, $Z^2$, $1/f$, $1/s$, $1/\sqrt n$ 这些依赖是稳固的：目标的像有 $fs/Z$ 像素宽，深度靠这个"尺度"测出，尺度的相对误差是 $\sigma/(fs/Z)/\sqrt n$，再乘 $Z$。相机的横向位置（绕目标"转圈"）同样按 $Z^{2}/(s\sqrt n)$ 增长，量级与深度相当，因为小目标的平移与小角度转动几乎不可区分。

The constant 0.7 depends on the layout of the points and is a numerical fit, not a theorem; it is within about 10% of FIG 4's exact value when the target is small compared with the distance ($s/Z\lesssim0.1$), and overestimates for larger targets (wider fields of view). The dependences on $\sigma$, $Z^2$, $1/f$, $1/s$, $1/\sqrt n$ are solid: the target's image is $fs/Z$ pixels across, depth is measured through that scale, whose relative error is $\sigma/(fs/Z)/\sqrt n$, and multiplying by $Z$ gives the formula. The camera's lateral position ("orbiting the target") grows the same way, as $Z^{2}/(s\sqrt n)$, at a comparable size, because for a small target a translation and a small rotation are nearly indistinguishable.

与邻居的联系Links to neighbouring topics

手眼标定（10.4）：每张标定板图像先做一次 PnP 得到 ${}^{c}T_{b}$，再解 $AX=XB$；PnP 的误差（FIG 4）直接传进标定精度，所以要用多个姿态、尽量大的板和靠近的距离。光束法平差（11.6）：把上式里的 $\mathbf X_i$ 也变成未知数，对多个相机同时优化；固定所有点只优化一个相机，就是本节的 PnP 精修（"仅位姿 BA"，motion-only BA）。视觉伺服（Part IV，只指路）：基于位置的视觉伺服每帧跑一次 PnP，把 ${}^{c}T_{o}$ 当作反馈，控制它收敛到期望位姿。

Hand–eye calibration (10.4): every board image first goes through PnP to give ${}^{c}T_{b}$, then $AX=XB$ is solved; PnP's error (FIG 4) passes straight into the calibration accuracy, hence use many poses, the largest board you can, and a short distance. Bundle adjustment (11.6): make the $\mathbf X_i$ in the cost above unknowns too and optimize many cameras together; fixing all points and optimizing a single camera is exactly the PnP refinement of this lesson ("motion-only BA"). Visual servoing (Part IV, a pointer only): position-based visual servoing runs PnP every frame, uses ${}^{c}T_{o}$ as feedback, and drives it to the desired pose.

6机器人中的它：带误匹配的定位In robotics: localizing with bad matches

真实的 2D–3D 匹配不是干净的：特征匹配会出错，神经网络会把关键点预测到别的物体上。一个错误的匹配就能毁掉最小二乘（FIG 6 的红色线框）。标准做法是 PnP + RANSAC（Fischler & Bolles 1981，11.5 节）：每次随机抽 3 个对应，P3P 给出至多 4 个候选位姿，按"重投影误差小于阈值的点数"给每个候选打分，保留得分最高的，最后只在内点上做 LM 精修。若内点比例为 $w$，每次抽 $s$ 个点，要以概率 $p$ 至少抽到一次全是内点的样本，需要迭代

$$ N\;\ge\;\frac{\log(1-p)}{\log(1-w^{s})} . $$

$w=0.7$、$s=3$、$p=0.99$ 时 $N\approx11$；最小样本越小，$N$ 越小，这是 P3P 在鲁棒估计里比 EPnP/DLT 更受欢迎的原因之一。FIG 6 里有 30 个匹配，你可以改变误匹配比例、迭代次数和阈值。

Real 2D–3D matches are not clean: feature matching makes mistakes, and a network may place a keypoint on another object. A single wrong match can wreck least squares (the red wireframe in FIG 6). The standard remedy is PnP + RANSAC (Fischler & Bolles 1981, Lesson 11.5): draw 3 random correspondences, let P3P give up to 4 candidate poses, score each candidate by the number of points with reprojection error below a threshold, keep the best, and finally run LM on the inliers only. If the inlier ratio is $w$ and each sample has $s$ points, to draw at least one all-inlier sample with probability $p$ we need

$$ N\;\ge\;\frac{\log(1-p)}{\log(1-w^{s})} . $$

With $w=0.7$, $s=3$, $p=0.99$ this is $N\approx11$; the smaller the minimal sample, the smaller $N$, which is one reason P3P is popular in robust estimation. FIG 6 has 30 matches, and you can change the outlier fraction, the iteration count and the threshold.

FIG 6PnP + RANSAC：30 个匹配，其中有误匹配PnP + RANSAC: 30 matches, some of them wrong

图像：匹配与三种位姿的盒子IMAGE · matches and the box under three poses

每次迭代的内点数INLIER COUNT PER ITERATION

每个点是一个 3D–2D 匹配的图像一端；绿色实心是 RANSAC 认定的内点，红色空心是被拒绝的点（其中真正的误匹配的像素是随机撒的，不在盒子上）。线框是同一个盒子在三个位姿下的投影：灰色是真值，绿色是 RANSAC + 内点上的 LM，红色虚线是对全部 30 个点直接做 DLT + LM 的结果。右图是 RANSAC 每次迭代得到的最大内点数（阶梯线）。随机数固定，"换一组"会重新生成匹配与误匹配。 Each dot is the image end of one 3-D–2-D match; filled green dots are the RANSAC inliers and hollow red dots are rejected points (a real outlier's pixel was scattered at random and is not on the box). The wireframes are the same box under three poses: grey is the truth, green is RANSAC plus LM on the inliers, and the red dashed one is plain DLT + LM on all 30 points. The right panel shows the best inlier count after each RANSAC iteration (a staircase). The random numbers are fixed; "Re-draw" generates new matches and outliers.

试一试Try this

误匹配 30%，迭代 ≥ 11 次：RANSAC 的旋转误差在 1.5° 以内，而对全部点做最小二乘的结果错得离谱（几十度）。再把迭代降到 2 到 3 次，多按几次"换一组"，看它偶尔失败。With 30% outliers and ≥ 11 iterations RANSAC's rotation error is within 1.5° while least squares on all points is wildly wrong (tens of degrees). Now drop the iterations to 2 or 3 and press "Re-draw" a few times to see it fail now and then.

真实系统里In real systems

标记：AprilTag 检测器找到标签的四个角点，再做平面 PnP；AprilTag 3 的 C 库用正交迭代（Lu、Hager & Mjolsness 2000）并返回两个候选位姿及各自的误差，让上层决定取舍。OpenCV 的 ArUco 模块也是对角点做 PnP。视觉重定位：hloc（Sarlin 等）和 COLMAP 的绝对位姿估计，用图像检索缩小范围，匹配局部特征，对 2D–3D 匹配做 P3P/EPnP + RANSAC，再做非线性精修。SLAM 跟踪：ORB-SLAM（Mur-Artal 等）跟踪线程把当前帧匹配到局部地图点，用"只优化位姿"的 BA 精修；丢失后用 EPnP + RANSAC 重定位。物体位姿：PVNet、DOPE 这类方法先预测物体关键点的像素位置，再用 PnP 求 ${}^{c}T_{o}$。标定：棋盘格和 ChArUco 板每张图一次 PnP，作为手眼标定（10.4 节）的输入。标记场定位：地面已知的标记场让机器人每看到一个标记就得到一次位姿测量，可以送进滤波器（Part IV）。

Markers: the AprilTag detector finds a tag's four corners and runs planar PnP; the AprilTag 3 C library uses orthogonal iteration (Lu, Hager & Mjolsness 2000) and returns two candidate poses with their errors, leaving the choice to the caller. OpenCV's ArUco module also runs PnP on the corners. Visual relocalization: hloc (Sarlin et al.) and COLMAP's absolute pose estimation narrow down candidates by image retrieval, match local features, run P3P/EPnP + RANSAC on the 2D–3D matches and refine non-linearly. SLAM tracking: the tracking thread of ORB-SLAM (Mur-Artal et al.) matches the current frame to local map points and refines the pose with a pose-only BA; after tracking is lost it relocalizes with EPnP + RANSAC. Object pose: PVNet, DOPE and related methods first predict the pixels of object keypoints and then use PnP to get ${}^{c}T_{o}$. Calibration: a checkerboard or ChArUco board gets one PnP per image, the input of hand–eye calibration (Lesson 10.4). Marker fields: with a known field of markers on the floor, a robot gets a pose measurement every time it sees one, which can go into a filter (Part IV).

OpenCV 的 solvePnP 通过 flags 暴露了本节几乎所有算法（具体要求以你所用版本的文档为准）：

OpenCV's solvePnP exposes almost every algorithm of this lesson through flags (check the documentation of your version for the exact requirements):

flag	思路Idea	何时用When to use
`SOLVEPNP_ITERATIVE`	DLT（共面点用单应）初始化 + LM 最小化重投影误差DLT (homography for coplanar points) start + LM on the reprojection error	默认；需要好初值，不鲁棒the default; needs a good start, not robust
`SOLVEPNP_P3P`, `SOLVEPNP_AP3P`	3 点最小解，用第 4 个点挑解3-point minimal solver, the 4th point picks the solution	恰好 4 个点；RANSAC 内部exactly 4 points; inside RANSAC
`SOLVEPNP_EPNP`	4 个控制点，$O(n)$4 control points, $O(n)$	点多、要快many points, speed
`SOLVEPNP_IPPE`, `SOLVEPNP_IPPE_SQUARE`	平面目标的闭式解，给出两个候选closed form for planar targets, two candidates	AprilTag/ArUco 标记，棋盘格AprilTag/ArUco markers, checkerboards
`SOLVEPNP_SQPNP`	全局最优解（Terzakis & Lourakis 2020）globally optimal (Terzakis & Lourakis 2020)	不想依赖初值时when you do not want to depend on a start
`solvePnPRansac`, `solvePnPGeneric`	RANSAC 外壳；返回全部解及其重投影误差RANSAC wrapper; returns all solutions with their reprojection errors	有误匹配；需要比较候选bad matches; comparing candidates

返回的 rvec 是 $R$ 的 Rodrigues（轴角）向量，tvec 是 $\mathbf t$，二者合起来就是 ${}^{c}T_{o}$，不是相机的位姿。

The returned rvec is the Rodrigues (axis–angle) vector of $R$ and tvec is $\mathbf t$; together they are ${}^{c}T_{o}$, not the camera's pose.

7常见误区Common pitfalls

误区 1：用 3D DLT 处理平面标定板Pitfall 1: using the 3-D DLT on a planar board

棋盘格、AprilTag 都是平面，所有点 $Z=0$。FIG 2 里这时 $A$ 至少有 3 维精确的零空间（无噪声时是 4 维），SVD 会毫不犹豫地返回其中任意一个，姿态可以错上百度，而重投影误差在这些点上仍是 0。"点上误差小"不等于"位姿对"：换一个点就暴露出来。平面用单应或 IPPE；近似共面（很薄的物体）同样危险，表现为倒数第二、第三小的奇异值也很小。

Checkerboards and AprilTags are planes, with every point at $Z=0$. In FIG 2 $A$ then has an exact null space of at least 3 dimensions (4 for noise-free data) and the SVD happily returns an arbitrary member; the attitude can be off by over a hundred degrees while the reprojection error on those points is still 0. "Small error at the points" does not mean "right pose": a different point would expose it. Use the homography or IPPE for planes; nearly coplanar (very thin) objects are just as dangerous, visible as the second- and third-smallest singular values also being tiny.

误区 2：重投影误差小的那个候选就是对的Pitfall 2: the candidate with the smaller reprojection error is the right one

FIG 5 里，5 cm 的标签在 $Z=2$ m（图像上只有 15 像素宽）、噪声 1 px 时，约四成到一半的噪声样本让错误的候选误差更低；10 cm 的标签在 3 m 远、倾斜 60° 时也接近一半。直觉"拟合得更好就更可能是真的"在这里失效，因为两个极小值之间的差别比噪声还小。对策：用更大的标签或更近的距离；用多个标签（共同的位姿约束）；用时间上的连续性或 IMU/里程计先验选择；至少别在两个候选的误差接近时把单帧结果当真。

In FIG 5 a 5 cm tag at $Z=2$ m (only 15 px wide in the image) with 1 px noise makes the wrong candidate the lower one in about four to five of ten noise draws, and a 10 cm tag 3 m away, tilted 60°, still does so in nearly half. The intuition "the better fit is more likely true" fails because the difference between the two minima is smaller than the noise. Countermeasures: a larger tag or a shorter distance; several tags (a joint pose constraint); temporal continuity or an IMU/odometry prior to choose; and at the very least do not trust a single frame when the two candidates' errors are close.

误区 3：像素误差小就是位姿准Pitfall 3: a small pixel error means an accurate pose

FIG 4：目标一远一小，同样 1 像素的噪声就变成几厘米到几十厘米的相机位置不确定度（按 $Z^{2}$），而重投影误差仍然只有 1 像素。点聚在一起（或近似共线）时 $J^{\top}J$ 接近奇异，某些方向几乎不被数据约束。检查协方差，而不是检查残差；把点分布得尽可能开；把"位姿测量"送进滤波器（Part IV）时带上这个协方差，不要用一个拍脑袋的常数。

FIG 4: for a small, far target the same 1 px of noise becomes centimetres to tens of centimetres of camera-position uncertainty (growing as $Z^{2}$), while the reprojection error is still just 1 px. When the points are clustered (or nearly collinear) $J^{\top}J$ is nearly singular and some directions are barely constrained by the data. Check the covariance, not the residual; spread the points as widely as you can; and when feeding the pose measurement into a filter (Part IV) pass this covariance instead of an arbitrary constant.

误区 4：搞混位姿的方向与约定Pitfall 4: mixing up the direction and convention of the pose

solvePnP 返回 ${}^{c}T_{o}$（物体在相机里），不是相机在物体里。要画相机、做地图定位，需要求逆：$R_{wc}=R^{\top}$，$\mathbf C=-R^{\top}\mathbf t$。把 tvec 当相机位置是错的：它是物体原点在相机系里的坐标，真正的相机位置 $-R^{\top}\mathbf t$ 与它差了一个旋转和一个符号。还有：像素的 $v$ 向下，相机系 y 向下（与 z-up 的世界系相反）；左扰动与右扰动的协方差不在同一个坐标系里；点在相机后面（$z\le0$）时 $\pi$ 仍给出一个"看起来合理"的像素，不会报错。

solvePnP returns ${}^{c}T_{o}$ (the object in the camera), not the camera in the object. To draw the camera or to localize in a map you must invert it: $R_{wc}=R^{\top}$, $\mathbf C=-R^{\top}\mathbf t$. Treating tvec as the camera position is wrong: it is the object's origin in camera coordinates, and the true camera position $-R^{\top}\mathbf t$ differs from it by a rotation and a sign. Also: the pixel $v$ points down and so does the camera y axis (opposite to the z-up world); left- and right-perturbation covariances live in different frames; and a point behind the camera ($z\le0$) still produces a plausible-looking pixel from $\pi$, without any error.

误区 5：忽略畸变、误匹配和初值Pitfall 5: ignoring distortion, outliers and the starting point

LM 只保证局部下降（FIG 3 里 GN 在大偏移下跳进错误的极小值，LM 虽然收敛，但在更差的初值或平面歧义下可能收敛到错误的极小值）：先用 P3P/EPnP/DLT/IPPE 取初值。畸变：边缘的像素若不去畸变，就等于带着系统性的几像素偏差做 PnP，尤其是广角镜头，要么先去畸变，要么在投影函数里带上畸变模型（10.3 节）。误匹配：一个就能毁掉最小二乘（FIG 6），必须有 RANSAC 或鲁棒核函数。噪声模型太乐观：角点检测的误差在模糊、小标签时远大于 0.3 px；用乐观的 $\sigma$ 算出的协方差也就过于自信。

LM only guarantees local descent (in FIG 3 GN lands in a wrong minimum from a large offset; LM converges there, but from an even worse start, or in a planar ambiguity, it can end in a wrong minimum): take the start from P3P/EPnP/DLT/IPPE. Distortion: pixels near the border, if not undistorted, mean running PnP with systematic errors of several pixels, particularly with wide-angle lenses; either undistort first or put the distortion model in the projection function (Lesson 10.3). Outliers: one is enough to ruin least squares (FIG 6), so use RANSAC or a robust kernel. Too optimistic a noise model: corner-detection errors are far above 0.3 px with blur or tiny tags, and a covariance computed with an optimistic $\sigma$ is overconfident accordingly.

8小练习Check yourself

1. 内参 $K$ 已知。至少需要几个 3D–2D 对应点，才能让相机位姿只有有限个解？这时最多有几个（深度全为正的）解？1. The intrinsics $K$ are known. What is the least number of 3D–2D correspondences that leaves only finitely many camera poses, and how many solutions (with all depths positive) can there be then?

每个点 2 个方程，位姿 6 个未知数，所以 3 个点是方程数等于未知数的最小情形。余弦定理给出的三个二次方程最多有 4 个深度全正的公共解（FIG 1 里 2 个或 4 个）。两个点只留下 2 维的解族；第四个点一般能挑出唯一的那个。6 个点是"不知道 K"的 DLT 的最小数。Each point gives 2 equations and a pose has 6 unknowns, so 3 points is the smallest case with as many equations as unknowns. The three quadratic equations from the law of cosines have at most 4 common solutions with positive depths (2 or 4 in FIG 1). Two points leave a 2-parameter family; a fourth point generally picks out the unique one. Six points is the minimum for the DLT, which does not know K.

2. 把 3D DLT 用在 12 个共面的点上（噪声为 0）。会发生什么？2. You run the 3-D DLT on 12 coplanar points (zero noise). What happens?

平面 $\boldsymbol\pi^{\top}\tilde{\mathbf X}=0$ 上的点满足 $(P+\mathbf a\boldsymbol\pi^{\top})\tilde{\mathbf X}=P\tilde{\mathbf X}$，所以 $P$、$\mathbf e_1\boldsymbol\pi^{\top}$、$\mathbf e_2\boldsymbol\pi^{\top}$、$\mathbf e_3\boldsymbol\pi^{\top}$ 都在零空间里（FIG 2 底部多出红色的、掉到底的条）。点数再多也没用，必须用单应（平面 PnP）。For points on the plane $\boldsymbol\pi^{\top}\tilde{\mathbf X}=0$, $(P+\mathbf a\boldsymbol\pi^{\top})\tilde{\mathbf X}=P\tilde{\mathbf X}$, so $P$, $\mathbf e_1\boldsymbol\pi^{\top}$, $\mathbf e_2\boldsymbol\pi^{\top}$, $\mathbf e_3\boldsymbol\pi^{\top}$ are all in the null space (the extra red bars at the floor in FIG 2). More points do not help; use the homography (planar PnP).

3. 用左扰动 $T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)T$，$\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)$。相机系中的点 $\mathbf x_c$ 对 $\delta\boldsymbol\xi$ 的雅可比是？3. With the left perturbation $T\leftarrow\operatorname{Exp}(\delta\boldsymbol\xi)T$, $\delta\boldsymbol\xi=(\delta\boldsymbol\omega,\delta\mathbf v)$, what is the Jacobian of the camera-frame point $\mathbf x_c$ with respect to $\delta\boldsymbol\xi$?

一阶：$\delta\mathbf x_c=\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v=-[\mathbf x_c]_\times\delta\boldsymbol\omega+\delta\mathbf v$，角速度在前，所以是 $[-[\mathbf x_c]_\times\ \ I]$。第一个选项把顺序写反了；第三个是右扰动 $T\operatorname{Exp}(\delta\boldsymbol\xi)$ 的雅可比；第四个符号错了（$\boldsymbol\omega\times\mathbf x=-\mathbf x\times\boldsymbol\omega$）。To first order $\delta\mathbf x_c=\delta\boldsymbol\omega\times\mathbf x_c+\delta\mathbf v=-[\mathbf x_c]_\times\delta\boldsymbol\omega+\delta\mathbf v$, with the angular part first, so it is $[-[\mathbf x_c]_\times\ \ I]$. The first option has the order reversed; the third is the Jacobian of the right perturbation $T\operatorname{Exp}(\delta\boldsymbol\xi)$; the fourth has the wrong sign ($\boldsymbol\omega\times\mathbf x=-\mathbf x\times\boldsymbol\omega$).

4. 一个 10 cm 的 AprilTag，在 3 m 远处斜着 60° 放置。检测器返回两个候选位姿，重投影误差分别是 0.8 px 和 0.9 px。最合理的做法是？4. A 10 cm AprilTag sits 3 m away, tilted 60°. The detector returns two candidate poses with reprojection errors of 0.8 px and 0.9 px. What is the most sensible reaction?

0.1 px 的差别远小于角点噪声（在 FIG 5 里把标签设成 10 cm、3 m、倾斜 60° 并按"100 次噪声"：接近一半的样本里，误差较低的候选是错的）。平均两个旋转没有意义（它们是镜像的），也不是标定问题，而是平面小目标的固有歧义。A 0.1 px difference is far below the corner noise (set FIG 5 to 10 cm, 3 m, tilt 60° and press "100 noise draws": in nearly half of the draws the lower-error candidate is the wrong one). Averaging two mirrored rotations is meaningless, and this is not a calibration problem but the inherent ambiguity of a small planar target.

5. 匹配里一半（$w=0.5$）是内点，P3P-RANSAC 每次抽 3 个点。要以 99% 的概率至少抽到一次全是内点的样本，大约需要多少次迭代？5. Half of the matches are inliers ($w=0.5$), and P3P-RANSAC draws 3 points per iteration. About how many iterations are needed to draw at least one all-inlier sample with 99% probability?

$N=\log(0.01)/\log(1-0.5^3)=4.605/0.1335\approx34.5$，取 35。对比 $w=0.7$（$N\approx11$）：内点比例降一点，迭代数涨很多；若最小样本是 4 个点，$N$ 还要翻倍（$w=0.5$、$s=4$ 时约 72）。$N=\log(0.01)/\log(1-0.5^3)=4.605/0.1335\approx34.5$, so 35. Compare $w=0.7$ ($N\approx11$): a small drop in the inlier ratio costs many more iterations, and with a 4-point minimal sample $N$ roughly doubles again (about 72 for $w=0.5$, $s=4$).

9延伸阅读Further reading

R. Hartley & A. Zisserman, Multiple View Geometry in Computer Vision (2nd ed., Cambridge, 2004), Ch. 7 — 相机矩阵 $P$ 的估计：DLT、归一化、代数误差与几何误差、共面退化Estimating the camera matrix $P$: the DLT, normalization, algebraic versus geometric error, the coplanar degeneracy
M. A. Fischler & R. C. Bolles, Random Sample Consensus, Communications of the ACM 24(6), 1981 — RANSAC 的原始论文，应用例子正是 P3P 位姿估计The original RANSAC paper, whose application example is P3P pose estimation
R. M. Haralick et al., Review and Analysis of Solutions of the Three Point Perspective Pose Estimation Problem, IJCV 13(3), 1994 — P3P 各种解法（含 Grunert）的比较A comparison of the P3P solutions including Grunert's
X.-S. Gao, X.-R. Hou, J. Tang & H.-F. Cheng, Complete Solution Classification for the Perspective-Three-Point Problem, IEEE TPAMI 25(8), 2003 — P3P 解的个数的完整分类A complete classification of the number of P3P solutions
V. Lepetit, F. Moreno-Noguer & P. Fua, EPnP: An Accurate O(n) Solution to the PnP Problem, IJCV 81(2), 2009 — 4 个控制点的 $O(n)$ 解The $O(n)$ solution with four control points
C.-P. Lu, G. D. Hager & E. Mjolsness, Fast and Globally Convergent Pose Estimation from Video Images, IEEE TPAMI 22(6), 2000 — 正交迭代，AprilTag 3 用的精修方法Orthogonal iteration, the refinement used by AprilTag 3
T. Collins & A. Bartoli, Infinitesimal Plane-Based Pose Estimation, IJCV 109(3), 2014 — IPPE：平面目标位姿的两个解的闭式解IPPE: closed-form two-solution pose from a planar target
G. Schweighofer & A. Pinz, Robust Pose Estimation from a Planar Target, IEEE TPAMI 28(12), 2006 — 平面目标位姿的双解歧义分析An analysis of the two-fold ambiguity of planar-target pose
G. Terzakis & M. Lourakis, A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem, ECCV 2020 — SQPnPSQPnP
E. Olson, AprilTag: A Robust and Flexible Visual Fiducial System, ICRA 2011 — AprilTag 检测系统The AprilTag detection system
R. Mur-Artal, J. M. M. Montiel & J. D. Tardós, ORB-SLAM: A Versatile and Accurate Monocular SLAM System, IEEE T-RO 31(5), 2015 — 跟踪、重定位与"仅位姿"优化的实际流程The practical pipeline of tracking, relocalization and pose-only optimization

问题Problem	已知Known	未知（自由度）Unknown (DoF)	最少需要Minimum
三角化（11.3）Triangulation (11.3)	相机 \(P_j\)，像素 \(\mathbf u_{ij}\)cameras \(P_j\), pixels \(\mathbf u_{ij}\)	\(\mathbf X\) (3)	2 个视图（4 个方程）2 views (4 equations)
P3P / PnP	\(K,\ \mathbf X_i,\ \mathbf u_i\)	\({}^{c}T_{o}\) (6)	3 个点（至多 4 个解）；4 个点起一般唯一3 points (≤ 4 solutions); unique generically from 4
DLT（K 未知）DLT (K unknown)	\(\mathbf X_i,\ \mathbf u_i\)	\(P\) (11)	6 个点，且不共面6 points, not coplanar
平面目标 PnPPlanar-target PnP	\(K\)，平面上的点\(K\), points on a plane	\(H\) (8) → \({}^{c}T_{o}\) (6)	4 个点（有翻转歧义）4 points (flip ambiguity)
光束法平差（11.6）Bundle adjustment (11.6)	\(K,\ \mathbf u_{ij}\)	所有 \(T_j\) 和 \(\mathbf X_i\)all \(T_j\) and \(\mathbf X_i\)	多视图many views