Part III · 第 10 章 相机与投影 · 10.4

相机标定与手眼标定

前面三节的所有公式都假设内参 \(K\)、畸变和外参是已知的，可没有哪台相机会告诉你这些数。标定就是用许多测量去估计这些未知数：拍一块已知尺寸的棋盘格，从每张图的单应里用 Zhang 方法线性地解出 \(K\)，再对整个重投影误差做 Levenberg–Marquardt 精化；相机装在机械臂上时，还要估计相机相对手的位姿 \(X\)，也就是解 \(AX=XB\)。本节把两个算法都在页面里真实地跑一遍，看它们何时精确、何时退化，以及怎样判断一次标定到底好不好。

约 35 分钟 前置：1.12 SVD · 1.13 最小二乘 · 4.6 最大似然 · 5.6 exp/log · 6.9 非线性最小二乘 · 7.3 正运动学 · 10.3 内参外参 5 个交互

    1为什么机器人需要它

    10.1 到 10.3 节的每个公式，都默认焦距、主点、畸变系数和外参是已知的。现实里没有哪台相机会把这些数告诉你：出厂说明书给的是同型号的典型值，换一次镜头、拧一下对焦环、磕一下支架、机身升温几十度，它们都会变。数错的后果是系统性的：像素到射线的映射整体歪掉，之后的三角化、PnP、点云全带着同一个偏差，滤波也平均不掉。

    机械臂上的相机还多一个问题。相机看到的杯子坐标在相机系里，而手臂要去的位置写在基座系里，中间隔着一个从来没人量过的刚体变换：相机相对手的位姿 \(X={}^{h}T_{c}\)。这个变换差几度、几毫米，抓取就差几厘米。

    • 内参与畸变标定（Zhang 方法）：看一块已知尺寸的平面标定板，估计 \(K\) 与畸变系数。
    • 手眼标定（\(AX=XB\)）：让手臂换几个姿态，每个姿态都拍到同一个固定目标，由此解出 \(X\)。
    • 判断好坏：重投影误差小就够了吗？为什么少数几张相似的图能给出"很小的误差"和"很错的相机"？

    这两个问题其实是同一个故事，也是本课程反复出现的套路：先用线性（闭式）方法得到一个粗糙的初值，再把真正的误差函数交给非线性最小二乘精化（6.9 节）。闭式解给你"落在正确的盆地里"，精化给你"统计上最优"。

    2直觉：一块棋盘，几个角度

    拿一块棋盘格，它的 54 个内角点在板上的坐标是已知的（9×6 个角点，方格 40 mm）。每拍一张图，就得到 54 对"板上坐标 ↔ 像素坐标"，由它们解出一个 3×3 的单应 \(H\)（10.5 节会深入讲；这里把它当作"平面的 DLT"：每对点给两个线性方程，用 SVD 解，和 10.2 节讲过的 DLT 是同一个方法）。

    单应里同时藏着两样东西：棋盘相对相机的位姿，以及相机自己的 \(K\)。靠什么把它们分开？靠"棋盘是个刚性平面"这一个事实：旋转矩阵的两列必须正交且等长。这对每张图给出关于 \(K\) 的 2 个线性方程；\(K\) 有 4 到 5 个未知数，所以几张不同角度的图就够了。角度不同是关键：棋盘正对着相机时，透视"近大远小"的信息几乎没有，焦距和距离分不开；把它斜过来，焦距才从距离里"露"出来。

    FIG 1 在页面里真实运行整套算法：左边是虚拟相机（青色视锥）和 \(n\) 块棋盘，右边是相机拍到的角点（带高斯噪声），下面是用前 \(k\) 张图估计的 \(\hat K\) 与真值的误差。拖动滑块改变视角的数量、倾斜度、噪声，也可以切到"全部正对相机"，看算法如何失败。

    FIG 1Zhang 方法，实时运行
    场景（拖动旋转）：相机在原点，看向 +y
    图像 (u 向右, v 向下)：角点检测
    用前 k 张图估计的误差 (px, 对数轴)
    左：每块棋盘用编号标出，琥珀色填充的是右图正在显示的那一张。右：琥珀色的点是当前视图的检测角点（连线只是帮助看出网格），灰点是其他视图的角点（覆盖了图像的哪些区域）；紫色 × 是真实主点，琥珀色圆圈是估计的主点。左下：琥珀线是 \(|\hat f_x-f|\)，青线是主点误差 \(\|\hat{\mathbf c}-\mathbf c\|\)，实线是当前这一组噪声，虚线是另外 9 组独立噪声的中位数。右下：估计的 \(\hat K\)，读数里的"约束健康度"是约束矩阵 \(V\) 第二小与最大奇异值之比：接近 0 表示有第二个零空间方向，即退化。方格尺寸滑块模拟"标定板量错了尺寸"。
    试一试
    • 把噪声 \(\sigma\) 拉到 0，视角数 \(\ge3\)，模式用"多轴倾斜"：\(\hat K\) 与真值只差 \(10^{-9}\) 量级。无噪声时 Zhang 的闭式解是精确的，不是近似。
    • 把视角数调到 2，并在右上角把"轴倾斜 skew"切到"自由"：只有 4 个方程却要解 5 个内参，算法拒绝给出答案。再切回"skew = 0"，2 张图就够了。
    • 选"全部正对相机"，视角数 \(\ge6\)：约束健康度掉到 \(10^{-2}\) 以下，\(\hat K\) 变成垃圾。棋盘在自己的平面内转动、前后平移都没用，只有改变平面的朝向才增加方程。
    • 把"方格尺寸误差"拉到 \(\pm10\%\) 以上：\(\hat K\) 完全不变，但估计的棋盘距离按同样比例错掉。尺度错误不会被重投影误差发现。
    核心洞见

    一张图 = 关于 \(K\) 的 2 个方程，与棋盘的位姿无关。\(K\) 有 \(5\) 个未知数（含 skew）时需要 \(\ge3\) 张朝向不同的图，\(4\) 个（skew = 0）时 2 张就够。"朝向不同"不是礼貌，是数学条件：朝向相同的图给出的是同一对方程。

    3动手玩：重投影误差与非线性精化

    闭式解有两个缺点。第一，它最小化的是"代数误差"（方程的残差），不是图像里真正量到的东西；第二，它根本不知道畸变，可镜头是弯的。下一步是回到物理：把全部参数 \((K,\ k_1,k_2,\ \{R_i,\mathbf t_i\})\) 放在一起，最小化所有角点的重投影误差，也就是"模型预测的像素位置"与"检测到的像素位置"的距离平方和。这正是高斯噪声下的最大似然估计（4.6 节），用 Levenberg–Marquardt（6.6、6.9 节）从闭式解出发迭代。

    FIG 2 里的虚拟镜头有桶形畸变。每次迭代都是一个真实的 LM 步（数值雅可比、Marquardt 阻尼 \(\lambda\,\mathrm{diag}(J^\top J)\)、接受或拒绝）。拖动"迭代"滑块可以倒带，右图的箭头是放大后的残差。读数中的"噪声下限"\(\sqrt2\,\sigma\sqrt{1-p/N}\)（二维距离的 RMS，见 5 节）是正确模型下 RMS 应当到达的值：它不会是 0，因为参数用掉了一部分噪声。

    FIG 2重投影误差与 Levenberg–Marquardt
    RMS 重投影误差 (px, 对数轴) 随迭代
    左：圆点是检测到的角点，琥珀色箭头从检测点指向模型在当前迭代预测的位置，长度放大了 ×m 倍（m 由第 0 次迭代的最大残差定下，之后不变，所以你能看到箭头缩短）。右：每次被接受的 LM 步之后的 RMS；虚线是正确模型的噪声下限。第 0 次迭代是闭式解（没有畸变）。RMS 取"每个角点二维距离的均方根"，所以纯噪声时约等于 \(\sqrt2\,\sigma\)；不同软件的约定不同，差一个 \(\sqrt2\)。读数里的 \(\pm\) 是协方差 \(\hat\sigma^2(J^\top J)^{-1}\) 给出的标准差。
    试一试
    • 把"迭代"滑块先拖到 0，再拖到最后：RMS 从闭式解的水平下降到噪声下限，箭头从有规律的径向模式变成无规律的短毛刺。
    • 用 \(k_1\le-0.2\)、\(\sigma\le0.5\)，先选"只有针孔"：RMS 停在噪声下限之上，残差箭头呈径向结构。再切到"针孔 + 径向畸变"，RMS 落回下限。残差有结构，就是模型错了。
    • 视角数调到 3，噪声 \(\sigma\ge1\)：RMS 仍贴着噪声下限（"看起来标定得很好"），但 \(f\) 的标准差超过 8 px。再加视角，标准差才收下来。小误差不等于好标定。
    同一件事的三个说法

    几何：让所有预测的角点尽量落在检测到的位置上。统计：噪声为各向同性高斯时，这是最大似然估计，协方差 \(\sigma^2(J^\top J)^{-1}\) 告诉你每个参数有多可信。优化：一个有 \(6+6n\) 个变量的非线性最小二乘，雅可比有分块稀疏结构（每个角点只依赖全局参数和它自己那张图的位姿），这正是 11.6 节光束法平差的雏形。

    4第二、第三视角：手眼标定 \(AX=XB\)

    相机在手上，目标固定在世界里

    现在把相机装到机械臂腕部（eye-in-hand，眼在手上）。有三个变换在起作用：机器人的正运动学给出手的位姿 \({}^{b}T_{h}(\mathbf q)\)（7.3 节，已知）；在棋盘上做 PnP（11.4 节）给出棋盘在相机系里的位姿 \(P={}^{c}T_{t}\)（测量值，有噪声）；未知的是相机在手里的位姿 \(X={}^{h}T_{c}\)，棋盘在基座系的位姿 \({}^{b}T_{t}\) 也不知道，但它固定不动。

    关键一步：对手臂的两个姿态 \(i,j\)，把同一个刚体运动分别从手和从相机的视角写出来。手走的是 \(A_{ij}={}^{h_i}T_{h_j}=({}^{b}T_{h,i})^{-1}\,{}^{b}T_{h,j}\)（机器人给出）；相机走的是 \(B_{ij}={}^{c_i}T_{c_j}=P_i\,P_j^{-1}\)（相机给出，棋盘在基座系的未知位姿在乘积里被消掉了）。手和相机是一个刚体，所以

    $$ \hl{A}{A_{ij}}\,\hl{X}{X}\;=\;X\,\hl{B}{B_{ij}},\qquad A_{ij}=({}^{b}T_{h,i})^{-1}\,{}^{b}T_{h,j},\quad B_{ij}={}^{c}T_{t,i}\,({}^{c}T_{t,j})^{-1}. $$

    FIG 3 是整个流程：一台 Robot.arm6，腕上有相机（紫色是真实的 \(X\)，琥珀色是算法估计的 \(\hat X\)），目标是固定在前方的棋盘。手臂的每个姿态都由逆运动学让相机"盯着"目标，围绕目标转动。你选三种运动：只绕一根轴、绕两根轴、或者任意。每个姿态的 \({}^{c}T_{t}\) 带有 PnP 噪声 \(\sigma_R\)（度）与 \(\sigma_t\)（毫米）。下面是两步法：先由旋转轴对齐求 \(R_X\)，再用线性最小二乘求 \(\mathbf t_X\)（5 节推导）。

    FIG 3手眼标定实验室：\(AX=XB\)
    机械臂 + 腕上相机（拖动旋转）
    左：灰色小点是 \(N\) 个姿态下的相机位置，完整的手臂是"显示姿态"那一个；紫色视锥是真实的相机，琥珀色视锥是估计的相机，两者重合时标定成功。运动只绕一根轴时，右下读数的"轴间最大夹角"为 0，估计出的 \(\hat X\) 是一族等价解里的任意一个，紫色淡影是这一族里另外几个解：它们对全部测量给出完全相同的 \(AX=XB\) 残差。右：每种运动下的误差中位数（9 组独立噪声）随姿态数 \(N\) 的变化，竖线标出当前的 \(N\)。\(\sigma_R\)、\(\sigma_t\) 是加在 \({}^{c}T_{t}\) 上的测量噪声（旋转用 \(\mathrm{Exp}\) 左乘，平移直接相加）。
    试一试
    • 选"一根轴"，\(N\ge6\)：轴间夹角为 0，旋转误差高达几十甚至上百度，而且无论加多少个姿态都不会变好。把运动换成"两根轴"，误差立刻降到几度以内。X 中有一个自由度在数据里根本不存在。
    • 用"任意"运动，\(N\ge8\)，把噪声调到 \(\sigma_R\le0.3^\circ\)、\(\sigma_t\le2\) mm，幅度 \(\ge25^\circ\)：旋转误差降到 \(1^\circ\) 以下，平移误差降到 8 mm 以下（换几组噪声看看它的波动）。

    试着把"运动幅度"调到 10° 以下再看右图：所有曲线整体上移。小转动使旋转轴 \(\beta_k\)（\(\|\beta_k\|\) 是转角）很短，同样大小的噪声在方向上被放大；此时需要更小的噪声或更多的姿态才能弥补。

    眼在手上，还是眼在手外？

    另一种常见的装法是eye-to-hand（眼在手外）：相机固定在工作台上，标志物（棋盘或 AprilTag）夹在手爪上。方程的形状完全一样，但每个字母的含义变了：未知的 \(X\) 变成标志物在手里的位姿 \({}^{h}T_{m}\)，\(B\) 是标志物的运动 \(({}^{c}T_{m,i})^{-1}\,{}^{c}T_{m,j}\)（注意顺序和 eye-in-hand 相反）。解出 \(X\) 之后，相机在基座系中的位姿由 \({}^{b}T_{c}={}^{b}T_{h,i}\,X\,({}^{c}T_{m,i})^{-1}\) 给出。还有一种不用相对运动的写法 \(AX=YB\)，把"机器人–世界"和"手–眼"两个未知变换同时解出来。FIG 4 并排画出这三种情形：实线是已知或测得的变换，虚线是要解的未知变换。

    FIG 4坐标系回路：谁已知，谁未知
    机器人给出（正运动学）相机测量（PnP）未知，要标定
    三种配置里闭合回路的含义都是"从基座到同一个物体的两条路径必须相等"。eye-in-hand：\({}^{b}T_{h}\,X\,{}^{c}T_{t}={}^{b}T_{t}\)；eye-to-hand：\({}^{b}T_{h}\,X={}^{b}T_{c}\,{}^{c}T_{m}\)。把回路在两个不同姿态下相除，固定的那个未知量（\({}^{b}T_{t}\) 或 \({}^{b}T_{c}\)）就消掉了，剩下 \(AX=XB\)；不相除，就是 \(AX=YB\)。
    试一试
    • 依次切换三种配置，各看一遍：每次找出哪条箭头是虚线（未知），并说出 \(A\)、\(B\)、\(X\)（\(Y\)）各对应哪条箭头。
    同一个方程的三个说法

    刚体运动：同一个运动，在手的坐标系里是 \(A\)，在相机的坐标系里是 \(B\)，\(X\) 是两个坐标系之间的"翻译词典"（共轭 \(B=X^{-1}AX\)）。旋转轴：共轭保持转角，只改变转轴的方向，所以 \(A\) 的转轴 = \(R_X\)（\(B\) 的转轴）；FIG 3 里就是把一堆 \(\beta_k\) 对齐到 \(\alpha_k\)。线性代数：旋转解出后，平移满足 \((R_A-I)\mathbf t_X=R_X\mathbf t_B-\mathbf t_A\)，是一个关于 \(\mathbf t_X\) 的线性方程，而 \(R_A-I\) 有一个沿 \(A\) 转轴的零空间。

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 或 FIG 3 中对应的元素会被高亮。

    Zhang 方法：从单应到 \(K\)

    棋盘上的点 \((X,Y,0)\)（板坐标系，\(Z=0\)）按 10.3 节的投影 \(\lambda\tilde{\mathbf x}=K[R\,|\,\mathbf t]\tilde{\mathbf X}\) 成像，这里 \([R\,|\,\mathbf t]\) 是外参 \({}^{c}T_{b}\)，\(R=[\mathbf r_1\ \mathbf r_2\ \mathbf r_3]\)。因为 \(Z=0\)，\(\mathbf r_3\) 这一列被乘零，剩下：

    $$ \lambda\begin{bmatrix}u\\v\\1\end{bmatrix}=K\,[\mathbf r_1\ \ \mathbf r_2\ \ \mathbf t]\begin{bmatrix}X\\Y\\1\end{bmatrix}\ \Longrightarrow\ \hl{H}{H}=[\mathbf h_1\ \mathbf h_2\ \mathbf h_3]=\mu\,K\,[\mathbf r_1\ \ \mathbf r_2\ \ \mathbf t], $$

    其中 \(\mu\) 是单应未知的整体尺度（DLT 只能确定到尺度）。\(H\) 由 54 对点用 DLT 得到（10.5 节）。把 \(\mathbf r_i=\mu^{-1}K^{-1}\mathbf h_i\) 代入旋转矩阵两列满足的 \(\mathbf r_1^\top\mathbf r_2=0\) 与 \(\|\mathbf r_1\|=\|\mathbf r_2\|\)，尺度 \(\mu\) 自己消掉：

    $$ \mathbf h_1^\top\,B\,\mathbf h_2=0,\qquad \mathbf h_1^\top B\,\mathbf h_1-\mathbf h_2^\top B\,\mathbf h_2=0,\qquad B=K^{-\top}K^{-1}=\begin{bmatrix}B_{11}&B_{12}&B_{13}\\ B_{12}&B_{22}&B_{23}\\ B_{13}&B_{23}&B_{33}\end{bmatrix}. $$

    \(B\) 对称，只有 6 个数 \(\mathbf b=(B_{11},B_{12},B_{22},B_{13},B_{23},B_{33})^\top\)，而 \(\mathbf h_i^\top B\,\mathbf h_j=\mathbf v_{ij}^\top\mathbf b\) 对 \(\mathbf b\) 是线性的，其中 \(\mathbf v_{ij}=(h_{1i}h_{1j},\ h_{1i}h_{2j}+h_{2i}h_{1j},\ h_{2i}h_{2j},\ h_{3i}h_{1j}+h_{1i}h_{3j},\ h_{3i}h_{2j}+h_{2i}h_{3j},\ h_{3i}h_{3j})^\top\)。\(n\) 张图叠成 \(2n\times6\) 矩阵：

    $$ V\,\mathbf b=\begin{bmatrix}\mathbf v_{12}^\top\\ (\mathbf v_{11}-\mathbf v_{22})^\top\\ \vdots\end{bmatrix}\mathbf b=\mathbf 0,\qquad \qquad \mathbf b=\mathbf v_{\min}(V)\ \ (\text{1.12}). $$

    然后 \(K\) 由 \(B=K^{-\top}K^{-1}\) 的 Cholesky 分解（或 Zhang 的显式公式）得到：

    $$ \begin{aligned} \hl{pp}{v_0}&=\frac{B_{12}B_{13}-B_{11}B_{23}}{B_{11}B_{22}-B_{12}^2},\qquad \lambda=B_{33}-\frac{B_{13}^2+v_0(B_{12}B_{13}-B_{11}B_{23})}{B_{11}},\\ f_x&=\sqrt{\tfrac{\lambda}{B_{11}}},\quad f_y=\sqrt{\tfrac{\lambda B_{11}}{B_{11}B_{22}-B_{12}^2}},\quad s=-\tfrac{B_{12}f_x^2f_y}{\lambda},\quad \hl{pp}{u_0}=\tfrac{s\,v_0}{f_y}-\tfrac{B_{13}f_x^2}{\lambda}. \end{aligned} $$

    （\(\mathbf b\) 只确定到符号和尺度；取 \(B_{11}>0\) 的那个符号。主点 \((u_0,v_0)=(c_x,c_y)\)。）有了 \(K\)，每张图的外参是 \(\mathbf r_1=\lambda K^{-1}\mathbf h_1\)、\(\mathbf r_2=\lambda K^{-1}\mathbf h_2\)、\(\mathbf r_3=\mathbf r_1\times\mathbf r_2\)、\(\mathbf t=\lambda K^{-1}\mathbf h_3\)，\(\lambda=1/\|K^{-1}\mathbf h_1\|\)，并取 \(t_z>0\)（板在相机前面）。噪声使得 \([\mathbf r_1\,\mathbf r_2\,\mathbf r_3]\) 不严格是旋转矩阵，用 SVD 取最近的旋转 \(UV^\top\)（1.12 节）。

    数方程：为什么 3 张，为什么要不同朝向

    \(\mathbf b\) 有 6 个分量但只确定到尺度，所以自由度是 5，恰好对应 \((f_x,f_y,s,c_x,c_y)\) 五个内参。每张图 2 个方程，\(n\) 张图 \(2n\) 个：\(2n\ge5\Rightarrow n\ge3\)。若假定 \(s=0\)（现代传感器几乎总成立，OpenCV 默认如此），\(B_{12}=0\)，只剩 4 个自由度，\(n\ge2\) 即可。再假定 \(f_x=f_y\)，且主点在图像中心，一张图就够求焦距。条件是方程独立：若所有棋盘与传感器平行（\(R=R_z\)），\(\mathbf h_1,\mathbf h_2\) 的第三个分量都是 0，\(V\) 的每一行只含 \(B_{11},B_{12},B_{22}\)，\(B_{13},B_{23},B_{33}\) 从来没有出现，主点与焦距不可恢复，FIG 1 里"全部正对相机"就是这样失败的。所有棋盘都绕同一根轴倾斜（FIG 1 的"单轴倾斜"）也是一种奇异配置：此时有一族近似等价的解，焦距会差出几百像素而主点仍然看似正常。这类退化配置的系统分析见 Sturm 与 Maybank (1999)。

    重投影误差就是最大似然

    设检测到的角点 \(\mathbf x_{ij}=\pi(K,\mathbf k;\,R_i\mathbf X_j+\mathbf t_i)+\boldsymbol\varepsilon_{ij}\)，\(\boldsymbol\varepsilon_{ij}\sim\mathcal N(\mathbf 0,\sigma^2I_2)\) 相互独立（\(\pi\) 是 10.3 节的带畸变投影）。负对数似然为 \(\frac1{2\sigma^2}\sum\|\boldsymbol\varepsilon_{ij}\|^2\) 加常数，所以最大似然就是

    $$ \begin{aligned}&\min_{K,\,\mathbf k,\,\{R_i,\mathbf t_i\}}\ \underbrace{\sum_{i=1}^{n}\sum_{j=1}^{54}\big\|\mathbf x_{ij}-\pi(K,\mathbf k;\,R_i\mathbf X_j+\mathbf t_i)\big\|^2}_{=\ \|\mathbf r(\mathbf p)\|^2},\\ &\mathbf p\in\mathbb R^{p},\ p=4+2+6n,\quad N=2\cdot54\,n. \end{aligned} $$

    LM 的一步解 \((J^\top J+\lambda\,\mathrm{diag}(J^\top J))\,\delta=-J^\top\mathbf r\)（6.6、6.9 节）；\(\lambda\) 大时像梯度下降（稳），小时像 Gauss–Newton（快），按"是否真的下降"自动调节。旋转用旋转向量参数化，更新是对旋转向量的加法，在小转角范围内没有问题（更严格的做法是 6.10 节在流形上更新）。

    两个检验值得牢记。噪声下限：模型正确时，残差平方和的期望是 \((N-p)\sigma^2\)，所以每个坐标上的 RMS 约为 \(\sigma\sqrt{1-p/N}\)（FIG 2 的虚线，二维距离的 RMS 再乘 \(\sqrt2\)）。参数不确定度：\(\mathrm{Cov}(\hat{\mathbf p})\approx\hat\sigma^2(J^\top J)^{-1}\)，\(\hat\sigma^2=\|\mathbf r\|^2/(N-p)\)（4.5 节的不确定性传播，也是 Cramér–Rao 下界）。RMS 只回答"拟合得多贴"，协方差才回答"参数有多可信"：参数之间可以高度相关（\(f\) 与到板的距离、\(f\) 与 \(k_1\)），RMS 对沿相关方向的偏移几乎不敏感，这就是 FIG 2 里"RMS 很好但 \(f\) 的标准差很大"的原因。

    手眼标定：推导

    符号含义来源
    \({}^{b}T_{h,i}\)姿态 \(i\) 的手（工具）在基座系的位姿正运动学 \(\mathrm{FK}(\mathbf q_i)\)（7.3 节）
    \(P_i={}^{c}T_{t,i}\)目标在相机系的位姿（"相机←目标"）PnP 测量（11.4 节），有噪声
    \(A_{ij}\)\(({}^{b}T_{h,i})^{-1}\,{}^{b}T_{h,j}\)手在其自身坐标系中的相对运动
    \(B_{ij}\)\(P_i\,P_j^{-1}\)相机在其自身坐标系中的相对运动
    \(X\)\({}^{h}T_{c}=(R_X,\mathbf t_X)\)未知：相机在手里的位姿

    推导：相机的位姿有两种写法，\({}^{b}T_{c,i}={}^{b}T_{h,i}X\)，也是 \({}^{b}T_{t}\,P_i^{-1}\)。于是 \(B_{ij}=({}^{b}T_{c,i})^{-1}\,{}^{b}T_{c,j}=P_i\,{}^{b}T_{t}^{-1}\,{}^{b}T_{t}\,P_j^{-1}=P_iP_j^{-1}\)，同时 \(B_{ij}=X^{-1}({}^{b}T_{h,i})^{-1}\,{}^{b}T_{h,j}X=X^{-1}A_{ij}X\)。按分块 \((R,\mathbf t)\) 展开 \(AX=XB\)：

    $$ \underbrace{R_AR_X=R_XR_B}_{\text{rotation}},\qquad \underbrace{(R_A-I)\,\mathbf t_X=R_X\mathbf t_B-\mathbf t_A}_{\text{translation, linear in }\mathbf t_X}. $$

    旋转部分：\(R_A=R_XR_BR_X^\top\) 是共轭，转角相同，转轴满足 \(\boldsymbol\alpha=R_X\boldsymbol\beta\)，其中 \(\boldsymbol\alpha=\mathrm{Log}(R_A)\)，\(\boldsymbol\beta=\mathrm{Log}(R_B)\)（5.6 节的旋转向量）。一个运动只给出一个向量对，\(R_X\) 还剩一个绕 \(\boldsymbol\beta\) 的自由度；两个转轴不平行的运动给出两个不共线的向量对，\(R_X\) 就唯一了。有 \(m\) 个运动（含噪声）时，最小二乘是 Kabsch 问题（不去中心化，因为 \(\boldsymbol\alpha=R_X\boldsymbol\beta\) 里没有平移）：

    $$ \hat R_X=\arg\min_{R\in\SO}\sum_{k}\|\boldsymbol\alpha_k-R\boldsymbol\beta_k\|^2=V\,\mathrm{diag}(1,1,\det(VU^\top))\,U^\top,\qquad \sum_k\boldsymbol\beta_k\boldsymbol\alpha_k^\top=U\Sigma V^\top. $$

    这与 Park 和 Martin (1994) 在欧氏群上的闭式解等价。平移部分：代入 \(\hat R_X\)，每个运动给 3 个关于 \(\mathbf t_X\) 的线性方程，叠起来是 \(3m\times3\) 的最小二乘（1.13 节）。矩阵 \(R_A-I\) 的零空间是 \(A\) 的转轴方向（\((R_A-I)\mathbf a=\mathbf 0\)），所以每个运动只约束了垂直于它转轴的 2 个分量；转轴不平行的运动叠在一起才满秩。

    不可观的方向

    当所有运动都是绕同一条直线 \(L\) 的转动（FIG 3 的"一根轴"）时，设 \(S\) 是相机系里绕 \(L\) 转 \(\varphi\)（并可沿 \(L\) 平移 \(d\)）的螺旋运动。\(S\) 与每一个 \(B_k\) 可交换，于是 \(A_k(XS)=X B_kS=(XS)B_k\)：\(X\) 和 \(XS\) 对全部数据同样成立，一个 2 参数的解族。这不是算法不够好，而是数据中没有信息：相机绕那条轴转一个角，或者沿它滑一段，全部测量一模一样（棋盘的未知位姿吸收了差别）。数值上它表现为 \(\sum\boldsymbol\beta_k\boldsymbol\alpha_k^\top\) 的秩为 1（FIG 3 读数的 \(\sigma_2/\sigma_1\approx0\)），以及 \(R_A-I\) 的叠加矩阵秩为 2。

    误差度量。标定好 \(\hat X\) 后，旋转误差用 \(\|\mathrm{Log}(\hat R_X^\top R_X)\|\)（度），平移误差用 \(\|\hat{\mathbf t}_X-\mathbf t_X\|\)（毫米）；真实标定时没有真值，只能看一致性：同一个固定目标，在手臂不同姿态下经 \({}^{b}T_{h,i}\hat X P_i\) 算出的基座系位置应当重合（FIG 5）。

    两步法的局限，与其他解法。上面的两步法先解旋转再解平移，旋转的误差会传到平移；而且它最小化的是运动方程的残差，不是图像里的误差。更好的做法是把它当作初值，再对图像重投影误差同时优化 \(X\) 与 \({}^{b}T_{t}\)（和 FIG 2 一样的 LM）。其他经典解法：Tsai 与 Lenz (1989)（先解旋转，再解平移的早期线性法），Daniilidis (1999)（对偶四元数，同时解旋转与平移），以及李群/Kronecker 积形式的 \(AX=YB\) 求解器（Shah, 2013）。

    6机器人中的它：相机引导的抓取到底准不准

    标定做得好不好，最终看机器人在做什么。典型流程：相机在图像里找到目标，PnP 或深度图给出目标在相机系里的位姿 \(P\)，机器人用 \({}^{b}T_{h}\,\hat X\,P\) 把它搬到基座系，再规划去抓。如果 \(\hat X\) 有误差，抓取点就会随着手臂姿态而系统性地飘。

    FIG 5 用 FIG 3 当前的估计 \(\hat X\) 做这件事。手臂访问 20 个新的随机姿态（与标定用的姿态无关，噪声也是新的），每个姿态下相机测到同一个固定目标，我们把它换算成基座系位置。如果标定与测量都完美，所有预测会落在同一个点；散开的云就是标定重复性。青色用真实的 \(X\)（只剩传感器噪声的底线），琥珀色用估计的 \(\hat X\)。琥珀云明显大于青云，或者中心偏离十字，说明标定误差在主导。回到 FIG 3 改善标定（更多姿态、更丰富的运动、更小的噪声），看琥珀云收缩到青云。

    FIG 5标定重复性：预测的目标位置
    基座系 x–y 平面内的偏差 (mm)
    每个点是一个测试姿态下 \({}^{b}T_{h,i}\hat X P_i\) 的位置减去目标的真实位置（十字），只画 x–y 分量。读数里的"散布"是 20 个预测相对其均值的均方根（三维，mm），"偏置"是均值到真值的距离。真实标定没有真值，只能看散布。散布小但偏置大是可能的（系统性错位，例如统一的尺度错误），所以它是必要的检验而不是充分的检验。
    试一试
    • 回到 FIG 3 选"一根轴"，看琥珀云散开到青色底线的 3 倍以上；再改用"任意"运动、足够大的幅度和足够多的姿态，让琥珀云的散布回到青色底线的 1.5 倍以内。
    真实系统里

    内参标定：OpenCV 的 findChessboardCorners + cornerSubPix + calibrateCamera 就是本节的流程（Zhang 初值 + LM 精化，支持径向与切向畸变，另有鱼眼模型 cv::fisheye）；ROS 的 camera_calibration 包在它外面套了一层交互界面。标定板：纯棋盘格要求整块板都可见，ChArUco（棋盘 + ArUco 标记）和 AprilTag 网格允许局部可见、自带编号，对遮挡和大角度更稳健。多传感器：Kalibr（ETH）标定多相机、相机–IMU，并估计时间偏移（Furgale 等，IROS 2013）。手眼：OpenCV 有 calibrateHandEye（Tsai、Park、Horaud、Andreff、Daniilidis 几种方法）与 calibrateRobotWorldHandEye（\(AX=YB\)）；ROS 里的 MoveIt 手眼标定插件与 easy_handeye 把采集姿态、检测标记、求解封装成了工作流。出厂与用户标定：RealSense、ZED 这类相机出厂时已逐台标定并把参数存在设备里，但温度漂移、镜头受力、更换光圈都可能让它偏离，高精度应用仍要现场重标。学习策略：以点云、体素或多视角相机为输入的 3D 感知策略（例如 PerAct 一类）把像素用相机内外参放进统一的三维空间，外参错 1 厘米，动作目标就错 1 厘米，所以在数据采集前必须先标定并记录。运动学标定是同一类问题的第三种：连杆长度、关节零位的小误差也会让 \(\mathrm{FK}(\mathbf q)\) 偏差，可用 7.3 节的正运动学对运动学参数求导（7.4 节的雅可比的"参数版"），同样用最小二乘估计；手眼标定的结果里不可避免地混进了这类误差。

    7常见误区

    误区 1：视角太少，或者太像

    20 张几乎正对相机、只是平移了一下的图，信息量接近 0（FIG 1 的"全部正对相机"）；20 张都绕同一根轴倾斜，也只约束了一半。好的标定集要覆盖多种倾斜方向、图像的四角（畸变在边缘最明显，中央的角点对 \(k_1\) 几乎没有约束）、以及几种距离。手眼同理：FIG 3 里"一根轴"的运动，加到 100 个姿态也不会变好，因为缺的不是样本数而是方向。

    误区 2：垃圾进，垃圾出：尺寸、平整度、模糊、同步

    方格尺寸输错，\(K\) 不变而所有平移（和手眼的 \(\mathbf t_X\)）按同一比例错，重投影误差一点也不会报警（FIG 1 的方格尺寸滑块）。常见来源是打印机"适应页面"悄悄缩放了标定板；用卡尺量几个方格才可靠。板子要平：贴在硬板上，不要用软纸或会翘的亚克力。运动模糊、卷帘快门会让角点位置偏移，所以采集时让机器人停稳再拍；机器人关节角和图像的时间戳要对得上，否则 \({}^{b}T_{h}\) 与 \(P\) 来自不同的时刻，手眼误差就是这一段位移。

    误区 3：在一种设置下标定，在另一种设置下使用

    焦距随对焦和变焦变化（自动对焦的"呼吸效应"可以让 \(f\) 变化百分之几），所以标定时要锁定对焦与光圈，使用时也一样。分辨率变了，\(K\) 也要跟着缩放：缩放因子 \(s\) 时 \(f\to sf\)，而主点在"像素中心为整数坐标"的约定下是 \(c\to s(c+0.5)-0.5\)，忘了 0.5 会引入半个像素的偏差。裁剪（crop）则只平移主点。相机在支架上被碰过、热了之后，外参也不再是标定时的值，最好定期用 FIG 5 的重复性检验来发现它。

    误区 4：搞错 \(AX=XB\) 里谁是 \(A\)、谁是 \(B\)

    教科书、论文和库在这里各不相同：\(A\) 用手的运动还是基座系里的运动，\(B\) 用 \(P_iP_j^{-1}\) 还是 \(P_i^{-1}P_j\)，\(P\) 是"目标在相机里"还是"相机在目标里"，eye-in-hand 与 eye-to-hand 的 \(X\) 又分别是什么。只要有一处求逆搞反，算法照样输出一个看起来像样的旋转矩阵和一个"合理"的平移，只是错的。防线有两条：一是写下每个变换的 \({}^{A}T_{B}\) 记号并检查下标能否像链条一样首尾相接；二是永远做 FIG 5 的一致性检验，用没参与标定的姿态预测固定目标的位置。

    误区 5：只看 RMS，或者用很小的转动做手眼

    FIG 2 里 3 张图就能把 RMS 压到噪声下限，但 \(f\) 的标准差有十几个像素：参数相关（\(f\) 与距离、\(f\) 与 \(k_1\)）让误差沿一个 RMS 几乎看不见的方向漂。看标准差、看相关系数、留一部分图当验证集，比只看 RMS 可靠。畸变模型也不是越多越好：用 \(k_3\)、切向项去拟合边缘很少被覆盖的区域，会产生在图像外面乱飞的"畸变曲线"。手眼标定则被小转动卡死：转角小，\(\boldsymbol\beta_k\) 很短，噪声使其方向严重失真；经验上，让相对转动的角度至少有十几度到几十度，并且转轴方向丰富（FIG 3 的"运动幅度"）。

    8小练习

    1. 用平面标定板估计包含 skew 的 5 个内参，每张图给出 2 个独立方程。至少需要几张朝向不同的图？
    \(2n\ge5\Rightarrow n\ge3\)。2 张图只有 4 个方程，欠定（FIG 1 把 skew 设为"自由"并取 2 张图时就拒绝求解）。若规定 skew = 0，只有 4 个未知数，2 张图就够。每张图的方程数是 2，不是 3：单应有 8 个自由度，其中 6 个被外参 \((R,\mathbf t)\) 占用，只剩 2 个留给 \(K\)。
    2. 手眼标定时，机器人所有的运动都是绕同一条固定直线（同一根轴）的转动。\(X\) 的哪一部分在数据里完全无法确定？
    转轴对齐 \(\boldsymbol\alpha=R_X\boldsymbol\beta\) 只固定了一个方向，\(R_X\) 还可以绕它任意转（秩 1）；平移方程里 \(R_A-I\) 沿转轴的方向是零空间，所以沿轴的平移也任意。这是一个二参数的解族（FIG 3 的紫色淡影），不是噪声问题：加样本无效，必须加入另一个方向的转动。
    3. 棋盘实际方格 44 mm，程序里却按 40 mm 输入。标定结果会怎样？
    棋盘更大但被当成更小，同样的像素需要它"更近"才能解释，所有平移乘以 \(40/44\approx1/1.1\)。\(K\) 来自 \(\mathbf r_1\perp\mathbf r_2\) 这类与尺度无关的条件，不受影响；重投影误差也完全不变（整个场景同比例缩放后成像相同），所以没有任何报警。手眼的 \(\mathbf t_X\) 同样会按这个比例错。
    4. eye-in-hand，\(P_i={}^{c}T_{t,i}\) 是 PnP 给出的"目标在相机系"的位姿，棋盘在基座系的位姿未知但固定。\(AX=XB\) 里的 \(B_{ij}\)（相机从姿态 \(i\) 到 \(j\) 的运动，写在相机自己的坐标系里）应取？
    相机位姿 \({}^{b}T_{c,i}={}^{b}T_{t}P_i^{-1}\)，于是 \(B_{ij}=({}^{b}T_{c,i})^{-1}\,{}^{b}T_{c,j}=P_i\,{}^{b}T_{t}^{-1}{}^{b}T_{t}\,P_j^{-1}=P_iP_j^{-1}\)，未知的 \({}^{b}T_{t}\) 刚好消掉。\(P_i^{-1}P_j\) 是 eye-to-hand 里标志物的运动。选错的后果见 FIG 5：预测的目标位置不再重合。
    5. 镜头有明显桶形畸变，你却只拟合针孔模型。FIG 2 的哪个现象最能说明"模型不对"？
    LM 照样收敛（到一个有偏差的最小值），协方差不会为负，主点也不一定跑掉。真正的信号是残差不像噪声：高于噪声下限，并且随图像位置有系统结构。这也是所有模型检验的通用做法：好模型的残差应当没有结构。

    9延伸阅读

    • Z. Zhang, A Flexible New Technique for Camera Calibration, IEEE TPAMI 22(11), 2000 — 本节第一部分的原始论文：单应、约束、闭式解与最大似然精化
    • R. Hartley & A. Zisserman, Multiple View Geometry in Computer Vision (2nd ed., Cambridge University Press, 2004), Ch. 7–8 — 相机矩阵的估计、相机标定与绝对二次曲线的几何
    • P. Sturm & S. Maybank, On Plane-Based Camera Calibration: A General Algorithm, Singularities, Applications, CVPR 1999 — 平面标定的奇异（退化）配置
    • R. Y. Tsai & R. K. Lenz, A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration, IEEE Trans. Robotics and Automation 5(3), 1989 — 手眼标定的经典早期方法
    • F. C. Park & B. J. Martin, Robot Sensor Calibration: Solving AX=XB on the Euclidean Group, IEEE Trans. Robotics and Automation 10(5), 1994 — 用李群的对数求解 \(AX=XB\)，本节的旋转步骤
    • K. Daniilidis, Hand-Eye Calibration Using Dual Quaternions, Int. J. Robotics Research 18(3), 1999 — 用对偶四元数同时求旋转与平移
    • M. Shah, Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product, J. Mechanisms and Robotics 5(3), 2013 — \(AX=YB\) 的求解
    • P. Furgale, J. Rehder & R. Siegwart, Unified Temporal and Spatial Calibration for Multi-Sensor Systems, IROS 2013 — Kalibr：多相机、相机–IMU 与时间偏移的标定
    • OpenCV, Camera Calibration and 3D Reconstruction (calib3d) documentation — 本节流程的工程实现