三角测量（Triangulation 三角化）与 SVD 求解

问题描述：
考虑某个路标点 y y y（上图的P）被若干关键帧 k = 1 , 2 … n \mathrm{k}=1,2 \ldots \mathrm{n} k=1,2…n（上图的O）看到。
其中， y ∈ R 4 y \in \mathbb{R}^{4} y∈R4取齐次坐标，即 y = [ x , y , z , 1 ] T y=[x, y, z, 1]^{T} y=[x,y,z,1]T。每次观测为 x k = [ u k , v k , 1 ] T x_{k}=\left[u_{k}, v_{k}, 1\right]^{T} xk=[uk,vk,1]T，取相机归一化平面坐标（这样可以忽略内参）。
记世界系到相机系的投影矩阵为 P k = [ R k , t k ] ∈ R 3 × 4 P_{k}=\left[R_{k}, t_{k}\right] \in \mathbb{R}^{3 \times 4} Pk=[Rk,tk]∈R3×4。
投影关系：
∀ k , λ k x k = P k y \forall \mathrm{k}, \lambda_{\mathrm{k}} \mathrm{x}_{\mathrm{k}}=\mathrm{P}_{\mathrm{k}} \mathrm{y} ∀k,λkxk=Pky
其中观测点深度值 λ k \lambda_{\mathrm{k}} λk未知
将投影关系写开：
{ λ k u k = p k , 1 y = [ R k , 1 ∣ t k , 1 ] [ x , y , z , 1 ] T λ k v k = p k , 2 y = [ R k , 2 ∣ t k , 2 ] [ x , y , z , 1 ] T λ k = p k , 3 y = [ R k , 3 ∣ t k , 3 ] [ x , y , z , 1 ] T \left\{\begin{array}{l}\lambda_{k} u_{k}=p_{k, 1} y=\left[R_{k, 1} \mid t_{k, 1}\right][x, y, z, 1]^{T} \\\lambda_{k} v_{k}=p_{k, 2} y=\left[R_{k, 2} \mid t_{k, 2}\right][x, y, z, 1]^{T} \\\lambda_{k}=p_{k, 3} y=\left[R_{k, 3} \mid t_{k, 3}\right][x, y, z, 1]^{T}\end{array}\right. ⎩⎨⎧λkuk=pk,1y=[Rk,1∣tk,1][x,y,z,1]Tλkvk=pk,2y=[Rk,2∣tk,2][x,y,z,1]Tλk=pk,3y=[Rk,3∣tk,3][x,y,z,1]T
其中为的第一行，容易发现第三式可以带入上两个式子，将深度值消元。
{ p k , 3 y u k = p k , 1 y p k , 3 y v k = p k , 2 y \left\{\begin{array}{l}p_{k, 3} y u_{k}=p_{k, 1} y \\p_{k, 3} y v_{k}=p_{k, 2} y\end{array}\right. {pk,3yuk=pk,1ypk,3yvk=pk,2y
化简得:
{ ( p k , 3 u k − p k , 1 ) y = 0 ( p k , 3 v k − p k , 2 ) y = 0 \left\{\begin{array}{l}\left(p_{k, 3} u_{k}-p_{k, 1}\right) y=0 \\\left(p_{k, 3} v_{k}-p_{k, 2}\right) y=0\end{array}\right. {(pk,3uk−pk,1)y=0(pk,3vk−pk,2)y=0
由于我们现在要求解 y = [ x , y , z , 1 ] T y=[x, y, z, 1]^{T} y=[x,y,z,1]T，有三个未知数，上面两个方程是不够的，至少得多一个观测(当然多几个观测结果是一样的)：
{ ( p 1 , 3 u k − p 1 , 1 ) y = 0 ( p 1 , 3 v k − p 1 , 2 ) y = 0 ( p 2 , 3 u k − p 2 , 1 ) y = 0 ( p 2 , 3 v k − p 2 , 2 ) y = 0 \left\{\begin{array}{l}\left(p_{1,3} u_{k}-p_{1,1}\right) y=0 \\\left(p_{1,3} v_{k}-p_{1,2}\right) y=0 \\\left(p_{2,3} u_{k}-p_{2,1}\right) y=0 \\\left(p_{2,3} v_{k}-p_{2,2}\right) y=0\end{array}\right. ⎩⎪⎪⎨⎪⎪⎧(p1,3uk−p1,1)y=0(p1,3vk−p1,2)y=0(p2,3uk−p2,1)y=0(p2,3vk−p2,2)y=0
整合得:
[ p 1 , 3 u k − p 1 , 1 p 1 , 3 v k − p 1 , 2 p 2 , 3 u k − p 2 , 1 p 2 , 3 v k − p 2 , 2 ] [ y ] = 0 ⇒ D y = 0 \left[\begin{array}{l}p_{1,3} u_{k}-p_{1,1} \\p_{1,3} v_{k}-p_{1,2} \\p_{2,3} u_{k}-p_{2,1} \\p_{2,3} v_{k}-p_{2,2}\end{array}\right][y]=0 \Rightarrow D y=0 ⎣⎢⎢⎡p1,3uk−p1,1p1,3vk−p1,2p2,3uk−p2,1p2,3vk−p2,2⎦⎥⎥⎤[y]=0⇒Dy=0
y为D零空间的一个非零元素。本来就是三个未知数，这时来了四个方程，系数矩阵必不满秩（在丁点噪声都没有的情况下,也就是上图红蓝线相交）。
若对D的SVD分解， D = U Σ V T \mathrm{D}=\mathrm{U} \Sigma \mathrm{V}^{\mathrm{T}} D=UΣVT， Σ \Sigma Σ为矩阵D特征值由大到小的组成的对角矩阵。挪一下位置，实际上是 D V = U Σ \mathrm{DV}=\mathrm{U} \Sigma DV=UΣ，如果将上述结果 D y = 0 \mathrm{Dy}=0 Dy=0，对应地放入SVD分解的式子中。
D [ v 1 … y ] = U [ σ 1 ⋯ 0 ⋮ ⋱ ⋮ 0 … 0 ] \mathrm{D}\left[\mathrm{v}_{1} \quad \ldots \quad \mathrm{y}\right]=\mathrm{U}\left[\begin{array}{ccc}\sigma_{1} & \cdots & 0 \\\vdots & \ddots & \vdots \\0 & \ldots & 0\end{array}\right] D[v1…y]=U⎣⎢⎡σ1⋮0⋯⋱…0⋮0⎦⎥⎤
显而易见，我们求解y只要对D做SVD分解，再取V矩阵的最后一维即可。But……实际操作上，我们会对做奇异值分解（这就成了特征值分解了阿），同样取V的最后一维。
D T D = ( U Σ V T ) T U Σ V T = V Σ T U T U Σ V T = V ( Σ T Σ ) V T D^{T} D=\left(U \Sigma V^{T}\right)^{T} U \Sigma V^{T}=V \Sigma^{T} U^{T} U \Sigma V^{T}=V\left(\Sigma^{T} \Sigma\right) V^{T} DTD=(UΣVT)TUΣVT=VΣTUTUΣVT=V(ΣTΣ)VT

一开始，我也很迷，为啥要算，分明算D就行了。认识太薄弱了，然后去b站学习了，MIT还有3b1b。
视频参考（MIT），这就明明白白：SVD分解
发现老师是这么讲的，我们用手算的过程也是这么算的，关键还是我代码这样运行是比较快的，哈哈哈。

另外，由于噪声的存在，D矩阵经常是满秩，这样我们原来假设零空间里的y就不存在了。因此在解SVD的时候，我们还要判断奇异值的 σ 3 > > σ 4 \sigma_{3}>>\sigma_{4} σ3>>σ4是否成立。若不成立，这意味着则三角测量是失败的，我们则要增加观测。
为什么要增加观测呢？我自己理解哈，噪声是服从零均值的高斯分布的（这是不是又太理想了，哈哈哈），只要你采样多了，这个“零”就会显现了，反之少的时候，一下子就把你带偏了；另外“多次实验得出普遍真理”嘛～

碎碎念：投影矩阵实际上就是相机的位姿变换。我们定第一帧坐标与世界坐标重合，根据上回说到的极对约束，我们根据匹配点，求解得第一帧与第二帧相机运动关系。同时这些匹配点，根据定义的第一帧与世界坐标重合，确定了点的世界坐标。在通过PNP等即可生成往后连续的位姿。这样说来，投影矩阵P所有是已知的。（我们用路标点求解位姿，又返回来用位姿去优化路标点的“深度”，形成一个“闭环”）

文为小记，但若有不当之处，还望赐教！
参考及盗图：深蓝学院vio，MIT线代，3b1b，十四讲

三角测量（Triangulation 三角化）与 SVD 求解相关推荐

Jonathan Richard Shewchuk的德罗奈三角化(Delaunay Triangulation) 崩溃(Crash)问题
Jonathan Richard Shewchuk的德罗奈三角化(Delaunay Triangulation) 崩溃(Crash)问题研究网格化的同学一定知道德罗奈三角化(Delaunay Tri ...
三角化公式推导手撕代码
前言三角测量是在已知相机参数和图像中匹配点的情况下,求解这些匹配点对应的空间点三维坐标的方法.针对单目与双目系统,三角测量的使用方法有所不同.双目视觉测距原理参见:双目视觉测距原理深度剖析:一个被忽 ...
《SLAM十四讲》7.6 三角测量 triangulation.cpp
三角测量是为了解决单目图像无法获取深度的问题,本博客是代码解释所以不解释三角测量的原理.先上全部代码: #include <iostream>int main ( int argc, ch ...
[转] 三角化求深度值（求三位坐标）
三角化得到深度值下面这个也要读读多视图几何总结--三角形法_基于l无穷范数极小化的多视图三角测量_Leo-Peng的博客-CSDN博客一 :三角化的提出三角化最早由高斯提出,并应用于测量学中. ...
视觉SLAM笔记（35）三角化求特征点的空间位置
视觉SLAM笔记(35) 三角化求特征点的空间位置 1. 特征点的空间位置 2. 三角测量函数 3. 求特征点的空间位置 1. 特征点的空间位置在视觉SLAM笔记(31) 特征提取和匹配已求出特 ...
SLAM基础 ICP PnP 三角化 F和H（ORB例子）
一.预备知识点 3D-3D ICP与SE3/Sim3 3D-2D PnP 2D-3D Triangulation 2D-2D 对极几何和单应变换作用: ICP:回环的时候用,检测两个帧足够相似的特征 ...
VSLAM中的特征点三角化
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:紫薯萝卜 https://zhuanlan.zhihu.com/p/103694374 本文仅做 ...
SLAM中双目三角化
双目三角化形式1:在等式左边同时乘x1x_1x1 和Rx2Rx_2Rx2 形式2:直接变形形式3:等式两边同时叉乘x1 形式4:Direct Linear Transform 参考资料在SL ...
今夜喜雨 | 直线线性三角化理论
雨夜,用了两个小时的时间终于推导了直线三角化的公式.直线三角化? 是的,即通过两帧的匹配直线得到空间直线的端点,如果端点严格匹配那么就是点的三角化,你可以从<SLAM十四讲>一书或者其他的 ...

三角测量（Triangulation 三角化）与 SVD 求解

三角测量（Triangulation 三角化）与 SVD 求解相关推荐

最新文章

热门文章