对极几何、三角测量、PnP、ICP问题描述_pnp essential matirx-优快云博客

本文链接：https://blog.youkuaiyun.com/JSWWSJSWWS/article/details/127177184

对极几何、三角测量、PnP、ICP问题描述

文章目录

- 对极几何、三角测量、PnP、ICP问题描述

前言

几周前草草看完了高翔老师的《视觉SLAM十四讲》第一版，对三种情形下的相机运动估计的求解流程有所了解，也在网上查资料学习了书中一些公式推导不清楚的地方，但是最近发现对这几个问题本质概念很模糊。所以这次再学习了一下书中第七讲的有关内容，本意是只想写一篇短文章希望记录我对2D-2D、3D-2D、3D-3D几种情况下的相机运动估计问题的认识，推导过程是副产物。

2D-2D：对极几何

在这里插入图片描述

2D-2D的对极几何：

已知：两张图像中一些匹配好的特征点的像素坐标；相机内参数矩阵 $K$
求解：相机的运动姿态 $R ， t$
方法：利用对极约束使用八点法或RANSAC求出基础矩阵（Fundamental Matrix） $F$ 或本质矩阵（Essential Matrix） $E$ ，根据 $E$ 或 $F$ 使用SVD求出 $R ， t$ 。
模型建立（《视觉SLAM十四讲》第一版和第二版的这部分我都没太看明白，参考文章[3]我把这部分推导了一遍）：
$s_1p_1=KP \\ s_2p_2=K(RP+t)$
取 $\begin{cases}x_1=K^{-1} p_1 \\ x_2=K^{-1}p_2 \end{cases}$ ，其中 $x_1$ ， $x_2$ 是两个像素点的归一化平面上的坐标，则有
$s_1x_1=P \\ s_2x_2=RP+t$
由式(2)可得
$s_2x_2=Rs_1x_1+t$
两边同时左乘 $t^{\land}$ ，即对两侧同时与 $t$ 做外积：
$s_2 t^{\land} x_2=t^{\land} Rs_1x_1$
然后，两侧同时左乘 $x_2^{T}$ ：
$s_2 x_2^{T} t^{\land} x_2 = -s_2 t^T x_2^{\land} x_2 = -s_2 \cdot t^T \cdot 0 = 0 = x_2^{T} t^{\land} Rs_1x_1$
于是就得到了一个式子：
$x_2^{T} t^{\land} Rx_1 = 0$
重新代入 $p_1$ ， $p_2$ ，有：
$p_2^{T} K^{-T} t^{\land} R K^{-1} p_1 = 0$
式(6)和式(7)称为对极约束。

在这里插入图片描述

书章节中出现的归一化平面坐标和归一化坐标指的是归一化相机平面坐标 $P_c$ 。

在这里插入图片描述

三角测量

三角化这部分文章[4]写的比较全面。

1、求解空间点深度

在这里插入图片描述

和对极几何一节相似，考虑图像 $I_1$ 和 $I_2$ ，以左图为参考，右图的变换矩阵为 $T$ 。相机光心为 $O_1$ 和 $O_2$ 。在 $I_1$ 中有特征点 $p_1$ ，对应 $I_2$ 中有特征点 $p_2$ 。理论上直线 $O_1p_1$ 与 $O_2p_2$ 在场景中会相交于一点 $P$ ，该点即两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。因此，可以通过最二小乘法求解。

按照对极几何中的定义，设 $x 1, x 2$ 为两个特征点的归一化坐标，那么它们满足（这里和《视觉SLAM十四讲》中不一样，我认为是勘误，欢迎指正）：
$s_2x_2=s_1Rx_1+t$
通过对极几何已经求得了 $R ， t$ ，现在求解两个特征点的深度 $s_1，s_2$ 。这里两个深度是可以分开求的。比如，如果要算 $s_2$ ，那么先对上式两侧左乘一个 $x_2^{\land}$ ，得：
$s_2 x_2^{\land} x_2 = 0 = s_1 x_2^{\land} Rx_1 + x_2^{\land}t$
即
$s_1 x_2^{\land} Rx_1 + x_2^{\land}t = 0$
根据上式即可求得 $s_2$ ，同样可求得 $s_1$ 。于是，就得到了两帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，估得的 $R, t$ 不一定精确使式(10)为零，所以更常见的做法是求最小二乘解而不是零解。

2、求解空间点坐标

转载文章[5]。

在这里插入图片描述

3D-2D：PnP

对于PnP之前看到有句话是“3D-2D是已知其中一张照片的深度和另一张照片的像素坐标求相机姿态 $R ， t$ 。”我认为这句话的准确描述应该是：已知地图点分别在两张图片上对应的像素坐标，以及其中一张照片的深度，求解相机的运动姿态 $R ， t$ 。

在这里插入图片描述

3D-2D的PnP：

已知：（以对极几何部分的图7-9为例）相机 $O_1$ 坐标系下的地图点 $P$ 的坐标，即世界坐标 $P_w$ ；地图点 $P$ 在图像2上的投影点的像素坐标；相机内参数矩阵 $K$
求解：相机的运动姿态 $R ， t$

1、直接线性变换（DLT）

对于某个空间点 $P$ ，坐标为 $P_w=(X,Y,Z)^T$ ，齐次坐标为 $P_w=(X,Y,Z,1)^T$ 。在图像 $I_1$ 中，投影到特征点 $x_1=(u_1,v_1,1)^T$ （以归一化平面齐次坐标表示）。根据相机与图像的坐标系转换关系，有
$sP_{uv}=K(RP_w+t)$
对式(8)稍作改变，定义增广矩阵 $[R, t]$ 为一个 $\times 4$ 的矩阵，并将 $P_w$ 改为齐次坐标，得
$K^{-1} P_{uv}=[R|t] P = [R|t] \left( {\begin{matrix} X \cr Y \cr Z \cr 1 \cr \end{matrix}} \right)$
为求解相机位姿 $R ， t$ ，将增广矩阵 $[R, t]$ 设为未知量：
$\left( {\begin{matrix} u_1 \cr v_1 \cr 1 \cr \end{matrix}} \right) = \left( {\begin{matrix} t_1 & t_2 & t_3 & t_4 \cr t_5 & t_6 & t_7 & t_8 \cr t_9 & t_{10} & t_{11} & t_{12} \cr \end{matrix}} \right) \left( {\begin{matrix} X \cr Y \cr Z \cr 1 \cr \end{matrix}} \right)$
在这里插入图片描述

在这里插入图片描述

2、P3P

在这里插入图片描述

3、BA

考虑 $n$ 个三维空间点 $P$ 及其投影 $p$ ，我们希望计算相机的位姿 $R ， t$ ，它的李代数表示为 $\xi$ 。假设某空间点坐标为 $P_i = [X_i , Y_i , Z_i ]^T$ ，其投影的像素坐标为 $u_i = [u_i , v_i ]^T$ 。像素位置与空间点位置的关系如下：
$s_{i}\left[\begin{array}{c} u_{i} \\ v_{i} \\ 1 \end{array}\right]={K} \exp \left({\xi}^{\wedge}\right)\left[\begin{array}{c} X_{i} \\ Y_{i} \\ Z_{i} \\ 1 \end{array}\right]$
写成矩阵形式为（其中隐含着齐次坐标到非齐次坐标的转换）：
$s_i u_i=K \exp \left({\xi}^{\wedge}\right) P_i$
由于相机位姿未知及观测点的噪声，该等式存在一个误差。为求解优化问题，把误差求和，寻找最好的相机位姿是误差和最小化：
$\boldsymbol{\xi}^{*}=\arg \min _{\boldsymbol{\xi}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{u}_{i}-\frac{1}{s_{i}} \boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}_{i}\right\|_{2}^{2}$

3D-3D：ICP

在这里插入图片描述

3D-3D的ICP：

已知：（以对极几何部分的图7-9为例）相机 $O_1$ 和 $O_2$ 坐标系下的地图点 $P$ 的坐标，即世界坐标 $P_w$ 与相机坐标 $P_c$ ；相机内参数矩阵 $K$
求解：相机的运动姿态 $R ， t$

1、SVD方法

先定义第 $i$ 对点的误差项：
$e_i=p_i-(Rp_i'+t)$
然后构建最小二乘问题，求使误差平方和达到极小的 $R ， t$ ：
$\min _{\boldsymbol{R}, \boldsymbol{t}} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}{ }^{\prime}+\boldsymbol{t}\right)\right)\right\|_{2}^{2}$
之后ICP可以分为以下三个步骤求解：

在这里插入图片描述

2、非线性优化方法

该方法和PnP非常相似，目标函数可以写成：
$\min _{\boldsymbol{\xi}}=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}_{i}^{\prime}\right)\right\|_{2}^{2}$