视觉SLAM十四讲 第7讲 (3) 相机运动估计 2D |
您所在的位置：网站首页 › 2d3d转换 › 视觉SLAM十四讲 第7讲 (3) 相机运动估计 2D |
|
相机运动估计 2D-2D/3D-2D/3D-3D
1. 2D-2D：对极约束2. 三角测量3. 3D-2D：PnP3.1 直接线性变换DLT3.2 P3P3.3 最小化投影误差求解PnP
4. 3D-3D：ICP4.1 SVD方法4.2 非线性优化方法
5. 总结
若已经有匹配好的点对，要根据点对估计相机的运动，可以分为以下三种情况：
2D-2D：即点对都是2D点，比如单目相机匹配到的点对。我们可以用对极几何来估计相机的运动。在估计完相机运动之后，我们还可以用三角测量（Triangulation）来估计特征点的空间位置（包括深度）。注意，是估计相机之间的运动。3D-2D：点对一组为3D，一组为2D，可以通过PnP求解。注意，是估计相机的位姿。3D-3D：通过双目或RGB-D或者某种方式得到了空间点的深度，即得到两组3D点，常用ICP解决。
1. 2D-2D：对极约束对极约束求解的是两个相机坐标系之间的运动。 设点在第一个相机坐标系下的坐标构建该坐标与像素1的关系构建该坐标经过相机运动之后即R,t与像素2的关系取两个像素对应的归一化平面坐标归一化平面坐标关系式代入上式关系式左乘 x 2 ⊤ x^\top_2 x2⊤，关系式一边为0此时尺度意义下相等的符号可以直接变为等号，对极约束成功推出 2. 三角测量之前使用了对极几何约束估计了相机运动，现在可以通过三角测量估计地图点的深度。 三角测量是指，通过不同位置对同一路标点进行观察，从观察到的位置推断路标点的距离。三角测量最早由高斯提出并应用于测量学中。 我们现在已知R，t，即两帧之间的运动，则知道 s 2 x 2 = s 1 R x 1 + t s_2x_2 = s_1Rx_1 + t s2x2=s1Rx1+t。如果我们相求s1s2，从几何上看，我们可以在射线O1P1或者O2P2上找，或者两条射线的中间找。如果我们要求s1，就左乘一个 x 2 ∧ x^\wedge_2 x2∧，得： s 2 x 2 ∧ x 2 = 0 = s 1 x 2 ∧ R x 1 + x 2 ∧ t s_2x^\wedge_2x_2 =0= s_1x^\wedge_2Rx_1 + x^\wedge_2t s2x2∧x2=0=s1x2∧Rx1+x2∧t 然后根据已知信息即可直接求得。但是由于Rt本身具有误差，所以更常见的做法是求最小二乘解而不是直接求解。 讨论： 三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈得上三角测量，因此，纯旋转是无法使用三角测量的，因为在平移为0时，对极约束一直为0增大平移，可能会导致失效，而平移太小，则三角化精度不够，这就是三角化的矛盾。我们把这个问题称为“视差”（parallax）在单目视觉中，由于单目图像没有深度信息，我们要等待特征点北追踪几帧之后，产生了足够的视角，再通过三角化来确定新特征点的深度值。这有时也被称为延迟三角化。本节只介绍了三角化的深度估计，但只要我们愿意，也能够定量地计算每个特征点的位置以及不确定性。所以，如果假设特征点服从高斯分布，并且不断地对它进行观测，在信息正确的情况下，我们就能够期望它的方差不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器（Depth Filter）。 3. 3D-2D：PnPPnP（Perspective-n-Point）是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。注意是相机的位姿，而不是相机两帧之间的运动。 如果两张图像中的一张的特征点的3D位置已知，则最少只需要3个点对（以及额外的一个用于验证结果）就可以估计相机运动（位姿）。在双目或RGBD的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，才能使用PnP。3D-2D的PnP不需要对极约束，又可以在很少的匹配点中获得较好的运动估计，是一种最重要的姿态估计方法。 PnP求解方法： 使用3对点估计位姿的P3P直接线性变换（DLT）EPnP、UPnP非线性优化方式，即构建最小二乘问题迭代求解，也就是万金油式的光束法平差（Bundle Adjustment，BA） 3.1 直接线性变换DLTDLT求解的是相机的位姿。直接看式子： s P u v = [ R ∣ t ] [ X Y Z 1 ] sP_{uv}=[R|t]\begin{bmatrix}X\\Y\\Z\\1\end{bmatrix} sPuv=[R∣t]⎣⎢⎢⎡XYZ1⎦⎥⎥⎤，然后用最后一行把s消掉，得到u1和v1的关系式。在使得关系式一侧为0，这样就构建类似于其次方程 A x = 0 Ax=0 Ax=0，然后再求解方程。这里的x是R|t展开后的12维向量，因此只需要6对匹配点便能求解。当匹配点大于6对时，也可以使用SVD方法对超定方程求最小二乘解。 在DLT求解中，我们直接将T矩阵看成了12个未知数，忽略了它们之间的联系。因为R属于特征正交群，用DLT求出的解不一定满足该约束。所以我们必须针对DLT估计的R的矩阵，寻找一个最好的旋转矩阵对它进行近似。这可以由QR分解完成。 3.2 P3PP3P利用给定的3个点的几何关系，即利用了三角形相似性质，进行PNP的求解。此外，P3P还需要使用一对验证点，以从可能的解中选出正确的那一个（类似于对极几何情形）。图像中的2D点a,b,c和世界坐标系的3D点A,B,C。P3P是求解3D点在相机坐标系下的坐标，一旦3D点在相机坐标系下的坐标能够算出，我们就得到了3D-3D的对应点，把PnP转换为了ICP问题。 理解P3P的求解流程： 利用三角形对应关系和余弦定理构造OAB和Oab的关系。余弦定理： c 2 = a 2 + b 2 − 2 a b c o s γ c^2=a^2+b^2-2abcos\gamma c2=a2+b2−2abcosγ。一共构造三条 O A 2 + O B 2 − 2 O A ⋅ O B ⋅ c o s < a , b > = A B 2 OA_2+OB^2-2OA\cdot OB\cdot cos=AB^2 OA2+OB2−2OA⋅OB⋅cos=AB2上面三条式子同时除以 O C 2 OC^2 OC2，记 x = O A / O C , y = O B / O C x = OA/OC,y=OB/OC x=OA/OC,y=OB/OC记 u = A B 2 / O C 2 , u v = B C 2 / O C 2 , w v = A C 2 / O C 2 u=AB^2/OC^2,uv=BC^2/OC^2,wv=AC^2/OC^2 u=AB2/OC2,uv=BC2/OC2,wv=AC2/OC2可以把第一个式子的v放到等式一边，然后带入其余两个式子得两个新式子然后就得到了要求解的式子。注意x,y是未知量，随着相机的移动会发生改变。 u,v可以通过ABC在世界坐标系下的坐标算出来，变换到相机坐标系下之后，这个比值并不改变。利用吴消元法，解出该方程，最多可能得到4个解，然后用验证点来计算最可能的解，最后得到了3个点的相机坐标系坐标，将P3P问题转化为ICP问题，然后再计算相机运动的R,t带有匹配信息的3D-3D位姿求解非常容易，但是P3P存在一些问题： P3P只利用了3个点的信息，当给定的点的配对点多于3组时，难以利用更多的信息。如果3D点或者2D点受噪声印象，或者存在误匹配，则算法失效。 3.3 最小化投影误差求解PnP最小化投影误差直接求解相机位姿和空间点。前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。这是一种非常通用的求解方式，我们可以用它对PnP或ICP给出的结果进行优化。这一类把相机和三维点放在一起进行最小化的问题，统称为Bundle Adjustment（光束法平差）。 最小化投影误差流程： 世界坐标与像素坐标的关系式 s i u i = K T P i s_iu_i=KTP_i siui=KTPi，这里的s就是深度构建误差项 u i − 1 s i K T P i u_i-\frac{1}{s_i}KTP_i ui−si1KTPi，用最小二乘法求解。也可使用李代数，构建无约束的优化问题，然后通过高斯牛顿法和列文伯格——马夸尔特方法求解。这时我们需要知道每个误差项关于优化变量的导数： e ( x + Δ x ) ≈ e ( x ) + J ⊤ Δ x e(x+\Delta x) \approx e(x) + J^\top \Delta x e(x+Δx)≈e(x)+J⊤Δx 类似于BCH， J ⊤ J^\top J⊤是一阶导数矩阵，是2*6的矩阵。知道空间点相机坐标系和世界坐标系的转换关系，变换矩阵T相机坐标系与uv的转换关系，K对T左乘扰动量，考虑e关于扰动量的导数，利用链式法则，求解e关于相机坐标系的导数（矩阵关于向量求导），再求解相机坐标系关于扰动的导数（其实就是SE(3)上的李代数求导）两项相乘，就得到了2*6的雅可比矩阵，即误差e关于扰动量的一阶导数。这个雅可比矩阵描述了重投影误差e关于相机位子T李代数的一阶变化关系以上是优化位姿的。我们还希望优化特征点的空间位置： 讨论e关于空间点P的导数链式法则: ∂ e ∂ P = ∂ e ∂ P ′ ∂ P ′ ∂ P \frac{\partial e}{\partial P} = \frac{\partial e}{\partial P'}\frac{\partial P'}{\partial P} ∂P∂e=∂P′∂e∂P∂P′，第一个是矩阵关于向量求导，第二个求导完只剩旋转矩阵R得到关于特征点的导数矩阵 4. 3D-3D：ICP迭代最近点（Iterative Closest Point，ICP），是根据一组配对好的3D点，比如两幅RGBD图像的匹配，求解两组点之间的变换关系，也就是相机的运动，跟相机模型没有关系K。和PNP类似，ICP的求解也分为两种方式：利用线性代数的求解（主要是SVD奇异值分界），以及利用非线性优化方式的求解（类似于BA） 4.1 SVD方法 构建误差项构建最小二乘问题，求使得误差平方和达到最小R,t找到两组点的质心利用质心在误差函数中做处理，简化误差函数。原理是所有点的和减去质心等于0得到只有两项的误差函数，第一项只与R有关，第二项与Rt都有关，所以先求第一项，再令第二项为0求解t求R的时候利用了SVD分解，其证明过程较为复杂，总之是定义了一个矩阵 W W W，对其进行奇异值分解 W = U ∑ V ⊤ W=U\sum V^\top W=U∑V⊤，当W满秩时， R = U V ⊤ R=UV^\top R=UV⊤ 4.2 非线性优化方法 对第二组点的相机坐标系进行左乘扰动做第一组点相机坐标系和第二组点扰动后的误差项再对误差项进行李代数扰动的求导于是，在非线性优化中只需要不断迭代，就能找到极小值。而且可以证明，ICP问题存在唯一解或无穷多解。在唯一解的情况下，只要能找到极小值，这个极小值就是全局最优解，因此不会遇到局部极小值的情况。这也意味着ICP求解可以任意选定初始值（相机运动Rt），这是已匹配点时求解ICP的一大好处。 缺点：前面讲的是匹配好的点的情况，ICP的研究者们往往更关心匹配未知的情况。 5. 总结在对极约束——PnP——ICP这个过程中，可以认为利用了越来越多的信息（没有深度——有一个图的深度——有两个图的深度），因此，在深度准确的情况下，得到的估计也将越来越准确。但是如果有噪声，深度图不准确，数据丢失，所以我们不得不丢弃一些没有深度数据的特征点。这可能导致ICP的估计不够准确，并且如果特征点丢弃的太多，可能由于特征点太少，无法进行运动估计的情况。 对极约束求的两个相机坐标系的运动Rt。三角测量求得是3D点的深度。（在对极几何之后）PnP DLT直接先行优化求解的是相机的位姿P3P求解3D点在相机坐标系下的坐标最小化投影误差通过BA优化相机位姿和3D点的相机坐标系坐标 ICP求解的是匹配好的两组点之间的运动，即相机运动 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |