最小化重投影误差求解PnP

问题描述

已知n个空间点$P_i=[x_i,y_i,z_i{]}^T$，其投影的像素坐标$p_i=[u_i,v_i{]}^T$求相机的位姿R，T。

问题分析

根据相机模型，像素点和空间点的位置关系：
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]$$
即：
$$s_i{u}_i=KT{P}_i$$
由于存在噪声误差，因此以最小化误差平方和为目标构建最小二乘问题：
$$T^{\ast }=arg\underset{T}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert \begin{array}{c}{u}_i-\frac{1}{s_i}KT{P}_i\end{array}\Vert }_2^2$$
因为这个误差是将3D点的理论投影位置与观测到的实际投影位置之间的误差，因此称为重投影误差：
$$e_i=u_i-\frac{1}{s_i}KT{P}_i$$

按照之前讲过的高斯牛顿法进行求解：
旋转矩阵本身带有约束，即正交且行列式为1。而有约束的优化问题比无约束的优化问题复杂的多。因为李代数的特点，李代数表示的天然满足旋转矩阵的约束，因此通常使用李代数进行表示来求解。

问题求解

步骤概述

1. 初始化位姿：使用闭式解法（如EPnP、DLT）获取初始相机位姿 ( R ) 和 ( t )。
2. 构建重投影误差：将3D点投影到图像平面，计算与观测值的误差。
3. 计算雅可比矩阵：分析误差关于位姿参数的导数，指导优化方向。
4. 迭代优化：使用高斯-牛顿或Levenberg-Marquardt算法更新位姿，直至收敛。

详细推导与算法

1. 重投影误差定义

设第 $i$ 个3D点为 ${\mathbf{P}}_i=(X_i,Y_i,Z_i{)}^{\top }$，对应的图像观测为 ${\mathbf{p}}_i=(u_i,v_i{)}^{\top }$。相机位姿用李代数 $\mathbf{\xi }\in \mathfrak{se}(3)$ 表示，对应的变换矩阵为 $\mathbf{T}=\exp ({\mathbf{\xi }}^{\wedge })$。将 ${\mathbf{P}}_i$ 变换到相机坐标系：
$${\mathbf{P}}_i^{\prime }=\mathbf{R}{\mathbf{P}}_i+\mathbf{t}=(X_i^{\prime },Y_i^{\prime },Z_i^{\prime }{)}^{\top }.$$
投影到归一化平面：
$$Z_i^{\prime }{\hat{\mathbf{p}}}_i^{\prime }=\mathbf{K}{\mathbf{P}}_{\mathbf{i}}^{\prime }$$
即：
$$\left[\begin{array}{c}{Z}_i^{\prime }{u}_i\\ Z_i^{\prime }{v}_i\\ Z_i^{\prime }\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_i^{\prime }\\ Y_i^{\prime }\\ Z_i^{\prime }\end{array}\right].$$
解得：
$$\{\begin{array}{l}{u}_i=f_x\frac{X_i^{\prime }}{Z_i^{\prime }}+c_x\\ v_i=f_y\frac{Y_i^{\prime }}{Z_i^{\prime }}+c_y\end{array}$$
假设相机内参已知，重投影误差为：
$${\mathbf{e}}_i={\hat{\mathbf{p}}}_i^{\prime }-{\mathbf{p}}_i.$$

2. 误差函数与优化目标

最小化所有点的重投影误差平方和：
$$\underset{\mathbf{\xi }}{\min }\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{e}}_i{\Vert }^2.$$

3. 雅可比矩阵计算

误差关于李代数 $\mathbf{\xi }$ 的雅可比矩阵 ${\mathbf{J}}_i$ 分为两部分：
$${\mathbf{J}}_i=\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{\xi }}=\frac{\partial {\mathbf{e}}_i}{\partial {\mathbf{P}}_i^{\prime }}\cdot \frac{\partial {\mathbf{P}}_i^{\prime }}{\partial \mathbf{\xi }}.$$

• 误差对相机坐标系点的导数（2×3矩阵）：
  $$\frac{\partial e}{\partial P^{\prime }}=\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right].$$
• 相机坐标系点对位姿的导数（3×6矩阵，考虑李代数扰动）：
  $$\frac{\partial (TP)}{\partial \delta \xi }=\left[\begin{array}{cc}I& -P^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]$$
  由于所定义的P只有三维，因此取出结果的前三维，展开后为 2×6 矩阵：
  $${\mathbf{J}}_i=-\left[\begin{array}{cccccc}\frac{f_x}{Z_i^{\prime }}& 0& -\frac{f_x{X}_i^{\prime }}{(Z_i^{\prime }{)}^2}& -\frac{f_x{X}_i^{\prime }{Y}_i^{\prime }}{(Z_i^{\prime }{)}^2}& f_x\left(1+\frac{(X_i^{\prime }{)}^2}{(Z_i^{\prime }{)}^2}\right)& -\frac{f_x{Y}_i^{\prime }}{Z_i^{\prime }}\\ 0& \frac{f_y}{Z_i^{\prime }}& -\frac{f_y{Y}_i^{\prime }}{(Z_i^{\prime }{)}^2}& -f_y(1+\frac{(Y_i^{\prime }{)}^2}{(Z_i^{\prime }{)}^2})& \frac{f_y{X}_i^{\prime }{Y}_i^{\prime }}{(Z_i^{\prime }{)}^2}& \frac{f_y{X}_i^{\prime }}{Z_i^{\prime }}\end{array}\right].$$

4. 迭代优化过程

• 高斯-牛顿法：

  1. 计算误差和雅可比：对每个点计算 ${\mathbf{e}}_i$ 和 ${\mathbf{J}}_i$。
  2. 构建线性系统：堆叠所有 ${\mathbf{J}}_i$ 和 ${\mathbf{e}}_i$，得到 ${\mathbf{J}}^{\top }\mathbf{J}\Delta \mathbf{\xi }=-{\mathbf{J}}^{\top }\mathbf{e}$。
  3. 求解增量：解线性方程 $\Delta \mathbf{\xi }$。
  4. 更新位姿：$\mathbf{\xi }\leftarrow \mathbf{\xi }+\Delta \mathbf{\xi }$。
  5. 判断收敛：若 $\Delta \mathbf{\xi }$ 足够小或误差不再下降，停止迭代。

• Levenberg-Marquardt：通过引入阻尼因子 $\lambda$ 稳定求解：
  $$({\mathbf{J}}^{\top }\mathbf{J}+\lambda \mathbf{I})\Delta \mathbf{\xi }=-{\mathbf{J}}^{\top }\mathbf{e}.$$

关键点

• 李代数参数化：避免旋转矩阵的约束，简化优化过程。
• 雅可比推导：通过扰动模型或链式法则计算导数，确保优化方向正确。
• 鲁棒性：初始值影响收敛性，建议先用闭式解法初始化。