特征法前端（二）

PnP

PnP (Perspective-n-Point) 是求解3D - 2D 点对运动的一种方法。和摄影测量中的后方交会类似。它描述了当已知n 个3维空间点及其在相片上的2D 投影点时，如何估计相机的位姿。

在特征点的三维位置已知的情况下，那么最少只需要3个点对就可以估计相机的运动（还需要额外一个点对来验证结果）。特征点的3D 位置可以由RGB-D 相机获得，或者由之前的三角化获得。PnP 的求解方法有很多，下面主要介绍直接线性变换 (DLR)，P3P 和非线性优化。

直接线性变换

考虑某个空间点P = (X, Y, Z, 1)T。在对应的图像I1 中，对应特征点的归一化平面齐次坐标为x1 = (u1, v1, 1)T。I1 对应的相机位姿R, t 未知。设 T = [R | t]，则有

$s \begin{pmatrix} u_1 \\ v_1 \\ 1 \end{pmatrix} = \begin{pmatrix} t_1 & t_2 & t_3 & t_4 \\ t_5 & t_6 & t_7 & t_8 \\ t_9 & t_{10} & t_{11} & t_{12} \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix}$

消去s 后可得：

$\begin{align} u_1 & = \frac{t_1 X + t_2 Y + t_3 Z + t_4}{t_9 X + t_{10} Y + t_{11} Z + t_{12}} \\ v_1 & = \frac{t_5 X + t_6 Y + t_7 Z + t_8}{t_9 X + t_{10} Y + t_{11} Z + t_{12}} \\ \end{align}$

定义矩阵T 的行向量为：

$\mathbf{T} = \begin{pmatrix} \mathbf{t}^T_1 \\ \mathbf{t}^T_2 \\ \mathbf{t}^T_3 \end{pmatrix}$

于是有

$\begin{align} \mathbf{t}^T_1 \mathbf{P} - \mathbf{t}^T_3 \mathbf{P} u_1 & = 0 \\ \mathbf{t}^T_2 \mathbf{P} - \mathbf{t}^T_3 \mathbf{P} v_1 & = 0 \end{align}$

可以看到，每个特征点提供了两个关于T 的线形约束。则有

$\begin{pmatrix} \mathbf{P}^T_1 & 0 & -u_1 \mathbf{P}^T_1 \\ 0 & \mathbf{P}^T_1 & -v_1 \mathbf{P}^T_1 \\ \vdots & \vdots & \vdots \\ \mathbf{P}^T_N & 0 & -u_N \mathbf{P}^T_N \\ 0 & \mathbf{P}^T_N & -v_N \mathbf{P}^T_n \end{pmatrix} \begin{pmatrix} \mathbf{t}_1 \\ \mathbf{t}_2 \\ \mathbf{t}_3 \\ \end{pmatrix} = 0$

由于T 一共有12 个未知量，DLT 最少需要6 对匹配点来求解T。当匹配点对的个数大于6时，也可使用SVD 等方法对超定方程求最小二乘解。

在DLT 中，我们直接将T 的12个元素当作12 个未知量来求解，而忽略了它们之间的内在联系。对于旋转矩阵R，求得的结果可能并不满足SO3 群的要求。可以利用QR 分解，寻找一个最合适的旋转矩阵R 来近似T 左边3 * 3 的矩阵块。这相当于把计算所得的T 重新投影到SE3 的流形上，转换成旋转和平移两部分。

P3P

求解PnP 的另一种方法是P3P，该算法仅仅使用3个3D - 2D 匹配点对（和一对验证点对），对数据要求较少。十四讲中的推导主要借鉴了这篇

欢迎回来app.yinxiang.com/shard/s2/nl/4424600/5e9a66e9-a14d-445f-bb76-c174738dbc4d/

（原文无法访问了，这里是我保存在印象笔记中的副本）。更加详细的介绍和推导也可以参看这篇

论文www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf

这里仅仅做简单介绍。

设3对匹配点对中，物方点为A, B, C，它们在世界坐标系中的坐标已知。对应的2D 点为a, b, c（图中分别为u, v, w）。验证点对则为D - d。相机光心为O。

显然，图中的三角形存在对应关系：

$\Delta Oab (Ouv) - \Delta OAB,\ \ \Delta Obc (Ovw) - \Delta OBC,\ \ \Delta Oac (Ouw) - \Delta OAC$

先来查看第一对三角形Oab 和OAB，利用余弦定理，有：

$OA^2 + OB^2 - 2OA \cdot OB \cdot \cos(a,b) = AB^2$

类似的，对于其他两对三角形也有类似的性质。合起来就有：

$\begin{align} OA^2 + OB^2 - 2OA \cdot OB \cdot \cos(a,b) & = AB^2 \\ OB^2 + OC^2 - 2OB \cdot OC \cdot \cos(b,c) & = BC^2 \\ OA^2 + OC^2 - 2OA \cdot OC \cdot \cos(a,c) & = AC^2 \end{align}$

上面三个式子统一除以 $OC^2$ ，并记x = OA / OC, y = OB / OC，得：

$\begin{align} x^2 + y^2 - 2xy \cos (a, b) & = \frac{AB^2}{OC^2} \\ y^2 + 1 - 2y \cos(b, c) & = \frac{BC^2}{OC^2} \\ x^2 + 1 - 2x \cos(a, c) & = \frac{AC^2}{OC^2} \end{align}$

记 $v = \frac{AB^2}{OC^2}, uv = \frac{BC^2}{OC^2}, wv = \frac{AC^2}{OC^2}$ ，有

$\begin{align} x^2 + y^2 -2xy \cos(a,b) - v & = 0\\ y^2 +1 - 2y \cos (b, c) - uv & = 0 \\ x^2 + 1 - 2x \cos(a, c) -wv & = 0 \end{align}$

将第一个等式中的v 移到右边，并代入后两式可得

$\begin{align} (1 - u) y^2 - u x^2 - 2 y \cos (b, c) + 2 u x y \cos (a, b) + 1 & = 0 \\ (1 - w) x^2 - w y^2 - 2 x \cos (a, c) + 2 w x y \cos (a, b) + 1 & = 0 \end{align}$

由于我们知道2D 点的图像坐标，所以三个余弦角cos(a, b), cos(b, c) 和cos(b, c) 可以计算出来；u 和w 由于是比例关系，可以通过A, B, C 三点的世界坐标算出，变换到相机坐标系下之后，比值不变。所以，以上两个等式中只有x 和y 是未知的，会随着相机位姿的变化而变化。所以该方程组是一个二元二次方程组。

求解该方程组比较复杂，详细的理论可以参考以上的两个链接。利用吴消元法，最多可以得到4 组可能解。此时，就可以利用验证点对D - d 来寻找最可能的解。这样就得到了A, B, C 三点在相机坐标系下的三维坐标。就可以利用ICP 求解R, t。

除了P3P，还有许多其他方法，如EPnP, UPnP 等等。

Bundle Adjustment

除了线性方法之外，如果使用李代数来表示相机位姿，就可以把PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。前面所介绍的各种方法，往往是将相机位姿和空间点位置分开求解，而非线性优化则是把它们都看作是优化变量一起求解。在PnP 中，光束法平差的核心就是最小化重投影误差。

假设我们有n 个三维空间点P 及其对应投影p，空间点在世界坐标系下的坐标为P = [X, Y, Z] 而对应投影p 的像素坐标为[u, v]。未知量则是相机的位姿R, t。相应地有：

$s_i \begin{bmatrix}u_i \\ v_i \\1 \end{bmatrix} = \mathbf{K} \exp(\xi^{\wedge }) \begin{bmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{bmatrix}$

矩阵形式为

$s_i \mathbf{u}_i = \mathbf{K} \exp(\xi^{\wedge}) \mathbf{P}_i$

实践中，由于噪声的原因，上式并不严格成立，即等式存在误差。而我们的目标就是最小化误差的平方和。

$\xi ^* =\arg\min_{\xi} \frac{1}{2} \sum_{i=1}^n ||\mathbf{u}_i - \frac{1}{s_i} \mathbf{K} \exp(\xi^{\wedge}) \mathbf{P}_i ||_2^2$

可以看到，这里的误差项，是观测所得的像素坐标和对应空间点的三维位置根据当前估计的位姿投影到像片上的位置之间的差异，因此称为重投影误差。忽略齐次坐标的最后一维，这个误差项是一个二维向量。此外，我们最小化的是所有点对重投影误差的平方和，所以最后得到的是一个总体误差最小的结果，而单个点的误差并不会精确为零。

可以看到上式是非线性的，根据之前的介绍，我们要对误差项进行线性化处理。

$\mathbf{e}(\mathbf{x} + \Delta \mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + \mathbf{J} \Delta \mathbf{x}$

这里的J 是雅可比矩阵。这里，e 是像素坐标之差，显然是二维的；状态量x 则包含两部分：相机位姿（6维）和物方坐标（3维）。所以J 也包含了这两部分的雅可比矩阵，一个是2 * 6 大小，一个是2 * 3 大小。

先考虑误差对于相机位姿的雅可比矩阵。这里需要用到链式法则，考虑e 是如何计算的（参考式(48)）。

$e = u - u^{\prime} = u - \mathbf{K} \mathbf{P}^{\prime} = u - \mathbf{K}\mathbf{T}\mathbf{P}$

其中，P‘ = [X', Y', Z', 1] 是P 变换到相机坐标系下的空间点坐标。对相机位姿T 施加一个扰动，并让e 对其求导可得

$\frac{\partial e}{\partial \delta \xi} = \frac{\partial e}{\partial u^{\prime}} \cdot \frac{\partial u^{\prime}}{\partial \mathbf{P}^{\prime}} \cdot \frac{\partial (\mathbf{TP})}{\partial \delta \xi}$

由前可知，

$\begin{align} u^{\prime} & = f_x \frac{X^{\prime}}{Z^{\prime}} + c_x \\ v^{\prime} & = f_y \frac{Y^{\prime}}{Z^{\prime}} + c_y \end{align}$

可得

$\frac{\partial u^{\prime}}{\partial \mathbf{P}^{\prime}} = \begin{bmatrix} \frac{\partial u^{\prime}}{\partial X^{\prime}} & \frac{\partial u^{\prime}}{\partial Y^{\prime}} & \frac{\partial u^{\prime}}{\partial Z^{\prime}} \\ \frac{\partial v^{\prime}}{\partial X^{\prime}} & \frac{\partial v^{\prime}}{\partial Y^{\prime}} & \frac{\partial v^{\prime}}{\partial Z^{\prime}} \end{bmatrix} = \begin{bmatrix} \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^{\prime2}} \end{bmatrix}$

而

$\frac{\partial (\mathbf{TP})}{\partial \delta \xi} = (\mathbf{TP})^{\bigodot} = \begin{bmatrix} \mathbf{I} & -\mathbf{P}^{\prime \wedge} \\ 0^T & 0^T \end{bmatrix}$

对于齐次坐标点P‘，我们只关心其前三维，将之取出，可得

$\frac{\partial (\mathbf{P}^{\prime})}{\partial \delta \xi} = \begin{bmatrix} \mathbf{I} & -\mathbf{P}^{\prime \wedge} \\ \end{bmatrix}$

将各个求导结果相乘，即可得

$\frac{\partial e}{\partial \delta \xi} = - \begin{bmatrix} \frac{f_x}{Z^{\prime}} & 0 & - \frac{f_x X^{\prime}}{Z^{\prime 2}} & - \frac{f_x X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_x + \frac{f_x X^2}{Z^{\prime 2}} & - \frac{f_x Y^{\prime}}{Z^{\prime}} \\ 0 & \frac{f_y}{Z^{\prime}} & -\frac{f_y Y^{\prime}}{Z^{\prime 2}} & -f_y - \frac{f_y Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_y X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_y X^{\prime}}{Z^{\prime}} \end{bmatrix}$

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。注意到前面半部分是e 关于平移的导数，而后半部分是e 关于旋转的导数。

为了优化空间点的位置，我们还需要e 关于空间点P 的导数。同样，根据链式法则有：

$\frac{\partial e}{\partial \mathbf{P}} = \frac{\partial e}{\partial u^{\prime}} \cdot \frac{\partial u^{\prime}}{\partial \mathbf{P}^{\prime}} \cdot \frac{\partial \mathbf{P}^{\prime}}{\partial \mathbf{P}}$

前面两项已经算过了，而 $\mathbf{P}^{\prime} = \mathbf{R} \mathbf{P} + \mathbf{t}$ 。所以

$\frac{\partial \mathbf{P}^{\prime}}{\partial \mathbf{P}} = \mathbf{R}$

最后，

$\frac{\partial e}{\partial \mathbf{P}} = - \begin{bmatrix} \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^{\prime2}} \end{bmatrix} \mathbf{R}$

至此，我们就求得了重投影误差关于相机位姿和空间点的导数，可以通过高斯牛顿法或列文伯格-马夸尔特法来进行优化。

ICP

如果对两幅RGB - D 图像进行了特征匹配，由于通过RGB - D 图像可以较为轻松地获得特征点在对应相机坐标系下的三维坐标，我们就得到了一组匹配好的3维点：

$\mathbf{P} = \{\mathbf{p}_1, \ldots, \mathbf{p}_n \},\ \mathbf{P}^{\prime} = \{ \mathbf{p}^{\prime}_1, \ldots, \mathbf{p}^{\prime} _n\}$

我们的目标是找到一个欧式变换R, t，使得

$\forall i, \mathbf{p}_i = \mathbf{R} \mathbf{p}^{\prime}_i + \mathbf{t}$

这个问题可是使用迭代最近点 (ICP) 来求解。而ICP 的求解也分两种方式：

利用线性代数进行求解：主要是SVD；
利用非线性优化的方式求解。

SVD

定义第i 对匹配点的误差项：

$\mathbf{e}_i = \mathbf{p}_i - (\mathbf{R} \mathbf{p}^{\prime}_i + \mathbf{t})$

构建一个最小二乘问题，优化目标是求R, t 使得总体误差平方和最小：

$\min_{\mathbf{R,t}} J = \frac{1}{2} \sum_{i=1}^n ||\mathbf{p}_i - (\mathbf{R} \mathbf{p}^{\prime}_i + \mathbf{t})||_2^2$

下面就来推导该最小二乘问题的线性解法。

定义这两组点的质心分别为：

$\mathbf{p} = \frac{1}{n} \sum_{i=1}^n \mathbf{p}_i,\ \ \mathbf{p}^{\prime} = \frac{1}{n} \sum_{i=1}^n \mathbf{p}_i ^{\prime}$

对于误差函数，则有

$\begin{align} \frac{1}{2} \sum_{i=1}^n ||\mathbf{p}_i - (\mathbf{R} \mathbf{p}^{\prime}_i + \mathbf{t})||_2^2 & = \frac{1}{2} \sum_{i=1}^n ||\mathbf{p}_i - \mathbf{R} \mathbf{p}^{\prime}_i - \mathbf{t} - \mathbf{p} + \mathbf{R} \mathbf{p}^{\prime} + \mathbf{p} - \mathbf{R} \mathbf{p}^{\prime}||^2_2 \\ & = \frac{1}{2} \sum_{i=1}^n || (\mathbf{p}_i - \mathbf{p} - \mathbf{R}(\mathbf{p}_i^{\prime} - \mathbf{p}^{\prime}) ) + (\mathbf{p} - \mathbf{R} \mathbf{p}^{\prime} - \mathbf{t}) ||^2 \\ & = \frac{1}{2} \sum_{i=1}^n \{||\mathbf{p}_i - \mathbf{p} - \mathbf{R}(\mathbf{p}_i^{\prime} - \mathbf{p}^{\prime})||^2 +||\mathbf{p} - \mathbf{R} \mathbf{p}^{\prime} - \mathbf{t}||^2 \\ &+ 2(\mathbf{p}_i - \mathbf{p} - \mathbf{R}(\mathbf{p}_i^{\prime} - \mathbf{p}^{\prime} ) )^T (\mathbf{p} - \mathbf{R} \mathbf{p}^{\prime} - \mathbf{t})\} \end{align}$

注意到展开后的交叉项中 $(\mathbf{p}_i - \mathbf{p} - \mathbf{R}(\mathbf{p}_i^{\prime} - \mathbf{p}^{\prime} ) )$ 在求和之后为0，所以误差函数可以简化为

$\min_{\mathbf{R, t}} J = \frac{1}{2} \sum_{i=1}^n ( ||\mathbf{p}_i - \mathbf{p} - \mathbf{R}(\mathbf{p}_i^{\prime} - \mathbf{p}^{\prime})||^2 +||\mathbf{p} - \mathbf{R} \mathbf{p}^{\prime} - \mathbf{t}||^2)$

上式之中，求和符号里的第一项只和旋转矩阵R 相关，而第二项则和R, t 都相关。所以求解的思路变为通过最小化第一项获得旋转矩阵R，再令第二项为0就得到平移项t。

此时，ICP 就由以下三步求解：

计算两组点的质心p, p'，然后计算各个匹配点的去质心坐标： $\mathbf{q}_i = \mathbf{p}_i - \mathbf{p},\ \mathbf{q}_i^{\prime} = \mathbf{p}_i^{\prime} - \mathbf{p}^{\prime}$ .
求解旋转矩阵R： $\mathbf{R}^* = \arg\max\limits_{\mathbf{R}} \frac{1}{2}\sum_{i=1}^n || \mathbf{q}_i - \mathbf{R} \mathbf{q}_i^{\prime} ||^2$ .
根据求得的旋转矩阵计算t： $\mathbf{t}^* = \mathbf{p} - \mathbf{R} \mathbf{p}^{\prime}$

此时，优化问题就转变成了如何求解旋转矩阵R。展开关于R 的误差项：

$\frac{1}{2}\sum_{i=1}^n || \mathbf{q}_i - \mathbf{R} \mathbf{q}_i^{\prime} ||^2 = \frac{1}{2} \sum_{i=1}^n \mathbf{q}_i^T \mathbf{q}_i + \mathbf{q}^{\prime T}_i \mathbf{R}^T \mathbf{R} \mathbf{q}_i^{\prime} - 2\mathbf{q}_i^T \mathbf{R} \mathbf{q}^{\prime}_i$

看到上式中由于 $\mathbf{R}^T \mathbf{R}= \mathbf{I}$ ，前两项和R 都无关。目标函数简化为

$\sum_{i=1}^n -\mathbf{q}_i^T \mathbf{R} \mathbf{q}_i^{\prime} = \sum_{i=1}^n -tr(\mathbf{R} \mathbf{q}_i^{\prime} \mathbf{q}_i^T) = -tr(\mathbf{R} \sum_{i=1}^n \mathbf{q}^{\prime}_i \mathbf{q}^T_i)$

为了求解这个优化问题，定义矩阵：

$\mathbf{W} = \sum_{i=1}^n \mathbf{q}^{\prime}_i \mathbf{q}^T_i$

W 是一个3 * 3 大小的矩阵，对其进行SVD 分解可得

$\mathbf{W} = \mathbf{U} \Sigma \mathbf{V}^T$

其中，是由奇异值组成的对角矩阵，对角线元素从大到小排列；而U 和V 为对角阵。当W 满秩时，

$\mathbf{R} = \mathbf{U} \mathbf{V}^T$

解得R 后，即可求解t。

非线性优化

求解ICP 的另一个方法是使用非线性优化，用迭代的方式去寻找最优解。和SVD 解法一样，优化目标函数可以

表达为

$\min_{\xi} = \frac{1}{2} \sum_{i = 1}^n || (\mathbf{p}_i - \exp(\xi ^{\wedge}) \mathbf{p}^{\prime}_i) ||_2^2$

待求解的变量为相机位姿T，而单个匹配点对之间的误差项关于位姿的导数可以推导为：

$\frac{\partial e}{\partial \delta \xi} = - (\exp(\xi^{\wedge}) \mathbf{p}^{\prime}_i)^{\bigodot} = - (\mathbf{T} \mathbf{p}^{\prime}_i) ^{\bigodot}$

利用非线性优化，即使用高斯牛顿法或列文伯格-马夸尔特法不断迭代，即可求得最优解。

最后，十四讲在Mac 系统下的随书代码练习在

DavidLee999/SLAMbookgithub.com/DavidLee999/SLAMbook

特征法前端（二）

David LEE

PnP

直接线性变换

P3P

Bundle Adjustment

ICP

SVD

非线性优化

为你推荐

地图构建 -- occupancy grid mapping

一阶RC低通滤波器的数学模型及算法实现

【自动驾驶】运动规划丨轨迹规划丨算法综述分享

OTSU算法（大津法—最大类间方差法）原理及实现

四旋翼无人机避障飞行

运动学求解器 IKFast + UR5

关于作者

David LEE

18

0

1

1

李群和李代数（三）：优化基础

常见的几种SLAM方案

非线性优化（二）：徒手实现LM算法

相关推荐

倒立摆状态反馈控制——分析、建模与仿真(matlab)

Webots+Matlab：使用Simulink搭建RHex控制器（2）

控制算法手记-认识你的被控系统

经典控制理论-序言

PyBullet（七）在PyBullet中使用VR

三维gazebo模型库3dgems介绍

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

特征法前端（二）

David LEE

PnP

直接线性变换

P3P

Bundle Adjustment

ICP

SVD

非线性优化

为你推荐

地图构建 -- occupancy grid mapping

一阶RC低通滤波器的数学模型及算法实现

【自动驾驶】运动规划丨轨迹规划丨算法综述分享

OTSU算法（大津法—最大类间方差法）原理及实现

四旋翼无人机避障飞行

运动学求解器 IKFast + UR5

评论（0）

关于作者

David LEE

18

0

1

1

李群和李代数（三）：优化基础

常见的几种SLAM方案

非线性优化（二）：徒手实现LM算法

相关推荐

倒立摆状态反馈控制——分析、建模与仿真(matlab)

Webots+Matlab：使用Simulink搭建RHex控制器（2）

控制算法手记-认识你的被控系统

经典控制理论-序言

PyBullet（七）在PyBullet中使用VR

三维gazebo模型库3dgems介绍

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板