对极几何、三角测量、PnP、ICP问题描述
对极几何、三角测量、PnP、ICP问题描述
文章目录
-
- 对极几何、三角测量、PnP、ICP问题描述
-
-
-
- 前言
- 2D-2D：对极几何
- 三角测量
-
- 1、求解空间点深度
- 2、求解空间点坐标
- 3D-2D：PnP
-
- 1、直接线性变换（DLT）
- 2、P3P
- 3、BA
- 3D-3D：ICP
-
- 1、SVD方法
- 2、非线性优化方法
- References
-
-
前言
几周前草草看完了高翔老师的《视觉SLAM十四讲》第一版，对三种情形下的相机运动估计的求解流程有所了解，也在网上查资料学习了书中一些公式推导不清楚的地方，但是最近发现对这几个问题本质概念很模糊。所以这次再学习了一下书中第七讲的有关内容，本意是只想写一篇短文章希望记录我对2D-2D、3D-2D、3D-3D几种情况下的相机运动估计问题的认识，推导过程是副产物。
2D-2D：对极几何
2D-2D的对极几何：
-
已知：两张图像中一些匹配好的特征点的像素坐标；相机内参数矩阵 K K K
-
求解：相机的运动姿态 R ， t R，t R，t
-
方法：利用对极约束使用八点法或RANSAC求出基础矩阵（Fundamental Matrix） F F F或本质矩阵（Essential Matrix） E E E，根据 E E E或 F F F使用SVD求出 R ， t R，t R，t。
-
模型建立（《视觉SLAM十四讲》第一版和第二版的这部分我都没太看明白，参考文章[3]我把这部分推导了一遍）：
s 1 p 1 = K P s 2 p 2 = K ( R P + t ) s_1p_1=KP \\ s_2p_2=K(RP+t) s1p1=KPs2p2=K(RP+t)
取 { x 1 = K − 1 p 1 x 2 = K − 1 p 2 \begin{cases}x_1=K^{-1} p_1 \\ x_2=K^{-1}p_2 \end{cases} {x1=K−1p1x2=K−1p2，其中 x 1 x_1 x1， x 2 x_2 x2是两个像素点的归一化平面上的坐标，则有
s 1 x 1 = P s 2 x 2 = R P + t s_1x_1=P \\ s_2x_2=RP+t s1x1=Ps2x2=RP+t
由式(2)可得
s 2 x 2 = R s 1 x 1 + t s_2x_2=Rs_1x_1+t s2x2=Rs1x1+t
两边同时左乘 t ∧ t^{\land} t∧，即对两侧同时与 t t t做外积：
s 2 t ∧ x 2 = t ∧ R s 1 x 1 s_2 t^{\land} x_2=t^{\land} Rs_1x_1 s2t∧x2=t∧Rs1x1
然后，两侧同时左乘 x 2 T x_2^{T} x2T：
s 2 x 2 T t ∧ x 2 = − s 2 t T x 2 ∧ x 2 = − s 2 ⋅ t T ⋅ 0 = 0 = x 2 T t ∧ R s 1 x 1 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 s2x2Tt∧x2=−s2tTx2∧x2=−s2⋅tT⋅0=0=x2Tt∧Rs1x1
于是就得到了一个式子：
x 2 T t ∧ R x 1 = 0 x_2^{T} t^{\land} Rx_1 = 0 x2Tt∧Rx1=0
重新代入 p 1 p_1 p1， p 2 p_2 p2，有：
p 2 T K − T t ∧ R K − 1 p 1 = 0 p_2^{T} K^{-T} t^{\land} R K^{-1} p_1 = 0 p2TK−Tt∧RK−1p1=0
式(6)和式(7)称为对极约束。
书章节中出现的归一化平面坐标和归一化坐标指的是归一化相机平面坐标 P c P_c Pc。
三角测量
三角化这部分文章[4]写的比较全面。
1、求解空间点深度
和对极几何一节相似，考虑图像 I 1 I_1 I1和 I 2 I_2 I2，以左图为参考，右图的变换矩阵为 T T T。相机光心为 O 1 O_1 O1和 O 2 O_2 O2。在 I 1 I_1 I1中有特征点 p 1 p_1 p1，对应 I 2 I_2 I2中有特征点 p 2 p_2 p2。理论上直线 O 1 p 1 O_1p_1 O1p1与 O 2 p 2 O_2p_2 O2p2在场景中会相交于一点 P P P，该点即两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。因此，可以通过最二小乘法求解。
按照对极几何中的定义，设 x 1 , x 2 x1, x2 x1,x2为两个特征点的归一化坐标，那么它们满足（这里和《视觉SLAM十四讲》中不一样，我认为是勘误，欢迎指正）：
s 2 x 2 = s 1 R x 1 + t s_2x_2=s_1Rx_1+t s2x2=s1Rx1+t
通过对极几何已经求得了 R ， t R，t R，t，现在求解两个特征点的深度 s 1 ， s 2 s_1，s_2 s1，s2。这里两个深度是可以分开求的。比如，如果要算 s 2 s_2 s2，那么先对上式两侧左乘一个 x 2 ∧ x_2^{\land} x2∧，得：
s 2 x 2 ∧ x 2 = 0 = s 1 x 2 ∧ R x 1 + x 2 ∧ t s_2 x_2^{\land} x_2 = 0 = s_1 x_2^{\land} Rx_1 + x_2^{\land}t s2x2∧x2=0=s1x2∧Rx1+x2∧t
即
s 1 x 2 ∧ R x 1 + x 2 ∧ t = 0 s_1 x_2^{\land} Rx_1 + x_2^{\land}t = 0 s1x2∧Rx1+x2∧t=0
根据上式即可求得 s 2 s_2 s2，同样可求得 s 1 s_1 s1。于是，就得到了两帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，估得的 R , t R, t R,t不一定精确使式(10)为零，所以更常见的做法是求最小二乘解而不是零解。
2、求解空间点坐标
转载文章[5]。
3D-2D：PnP
对于PnP之前看到有句话是“3D-2D是已知其中一张照片的深度和另一张照片的像素坐标求相机姿态 R ， t R，t R，t。”我认为这句话的准确描述应该是：已知地图点分别在两张图片上对应的像素坐标，以及其中一张照片的深度，求解相机的运动姿态 R ， t R，t R，t。
3D-2D的PnP：
- 已知：（以对极几何部分的图7-9为例）相机 O 1 O_1 O1坐标系下的地图点 P P P的坐标，即世界坐标 P w P_w Pw；地图点 P P P在图像2上的投影点的像素坐标；相机内参数矩阵 K K K
- 求解：相机的运动姿态 R ， t R，t R，t
1、直接线性变换（DLT）
对于某个空间点 P P P，坐标为 P w = ( X , Y , Z ) T P_w=(X,Y,Z)^T Pw=(X,Y,Z)T，齐次坐标为 P w = ( X , Y , Z , 1 ) T P_w=(X,Y,Z,1)^T Pw=(X,Y,Z,1)T。在图像 I 1 I_1 I1中，投影到特征点 x 1 = ( u 1 , v 1 , 1 ) T x_1=(u_1,v_1,1)^T x1=(u1,v1,1)T（以归一化平面齐次坐标表示）。根据相机与图像的坐标系转换关系，有
s P u v = K ( R P w + t ) sP_{uv}=K(RP_w+t) sPuv=K(RPw+t)
对式(8)稍作改变，定义增广矩阵 [ R , t ] [R,t] [R,t]为一个 3 × 4 3 \times 4 3×4的矩阵，并将 P w P_w Pw改为齐次坐标，得
s K − 1 P u v = [ R ∣ t ] P = [ R ∣ t ] ( X Y Z 1 ) s K^{-1} P_{uv}=[R|t] P = [R|t] \left( {\begin{matrix} X \cr Y \cr Z \cr 1 \cr \end{matrix}} \right) sK−1Puv=[R∣t]P=[R∣t]⎝ ⎛XYZ1⎠ ⎞
为求解相机位姿 R ， t R，t R，t，将增广矩阵 [ R , t ] [R,t] [R,t]设为未知量：
s ( u 1 v 1 1 ) = ( t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ) ( X Y Z 1 ) s \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) s⎝ ⎛u1v11⎠ ⎞=⎝ ⎛t1t5t9t2t6t10t3t7t11t4t8t12⎠ ⎞⎝ ⎛XYZ1⎠ ⎞
2、P3P
3、BA
考虑 n n n个三维空间点 P P P及其投影 p p p，我们希望计算相机的位姿 R ， t R，t R，t，它的李代数表示为 ξ \xi ξ。假设某空间点坐标为 P i = [ X i , Y i , Z i ] T P_i = [X_i , Y_i , Z_i ]^T Pi=[Xi,Yi,Zi]T，其投影的像素坐标为 u i = [ u i , v i ] T u_i = [u_i , v_i ]^T ui=[ui,vi]T。像素位置与空间点位置的关系如下：
s i [ u i v i 1 ] = K exp ( ξ ∧ ) [ X i Y i Z i 1 ] 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] si⎣ ⎡uivi1⎦ ⎤=Kexp(ξ∧)⎣ ⎡XiYiZi1⎦ ⎤
写成矩阵形式为（其中隐含着齐次坐标到非齐次坐标的转换）：
s i u i = K exp ( ξ ∧ ) P i s_i u_i=K \exp \left({\xi}^{\wedge}\right) P_i siui=Kexp(ξ∧)Pi
由于相机位姿未知及观测点的噪声，该等式存在一个误差。为求解优化问题，把误差求和，寻找最好的相机位姿是误差和最小化：
ξ ∗ = arg min ξ 1 2 ∑ i = 1 n ∥ u i − 1 s i K exp ( ξ ∧ ) P i ∥ 2 2 \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} ξ∗=argξmin21i=1∑n∥ ∥ui−si1Kexp(ξ∧)Pi∥ ∥22
3D-3D：ICP
3D-3D的ICP：
- 已知：（以对极几何部分的图7-9为例）相机 O 1 O_1 O1和 O 2 O_2 O2坐标系下的地图点 P P P的坐标，即世界坐标 P w P_w Pw与相机坐标 P c P_c Pc；相机内参数矩阵 K K K
- 求解：相机的运动姿态 R ， t R，t R，t
1、SVD方法
先定义第 i i i对点的误差项：
e i = p i − ( R p i ′ + t ) e_i=p_i-(Rp_i'+t) ei=pi−(Rpi′+t)
然后构建最小二乘问题，求使误差平方和达到极小的 R ， t R，t R，t：
min R , t J = 1 2 ∑ i = 1 n ∥ ( p i − ( R p i ′ + t ) ) ∥ 2 2 \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} R,tminJ=21i=1∑n∥(pi−(Rpi′+t))∥22
之后ICP可以分为以下三个步骤求解：
2、非线性优化方法
该方法和PnP非常相似，目标函数可以写成：
min ξ = 1 2 ∑ i = 1 n ∥ ( p i − exp ( ξ ∧ ) p i ′ ) ∥ 2 2 \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} ξmin=21i=1∑n∥ ∥(pi−exp(ξ∧)pi′)∥ ∥22
References
[1] 高翔, 张涛. 《视觉SLAM十四讲：从理论到实践》第一版. 电子工业出版社, 2017.
[2] 高翔, 等. 《视觉SLAM十四讲：从理论到实践》第二版. 电子工业出版社, 2019.
[3] https://www.cnblogs.com/clarenceliang/p/6704970.html
[4] 计算机视觉对极几何之Triangulate(三角化)_晨光ABC的博客-CSDN博客_视觉三角化
[5] [MVG] 三角化地图点 - 知乎 (zhihu.com)