肉棒插比比,久,这里只有精品

CreateNewMapPoints()共視程度較高的關(guān)鍵幀通過(guò)三角化恢復(fù)一些MapPoints

在當(dāng)前關(guān)鍵幀的共視關(guān)鍵幀中找到共視程度最高的nn幀相鄰幀vpNeighKFs
遍歷相鄰關(guān)鍵幀
- 這個(gè)過(guò)程很耗時(shí)，如果有新的關(guān)鍵幀需要處理就return
- 根據(jù)兩個(gè)關(guān)鍵幀位姿計(jì)算他們之間的基本矩陣
  -SearchForTriangulation通過(guò)極線約束限制匹配時(shí)的搜索范圍，進(jìn)行特征點(diǎn)匹配
- 通過(guò)三角化生成3D點(diǎn)，生成之后還為該點(diǎn)添加屬性：觀測(cè)到該店的關(guān)鍵幀，描述子，平均觀測(cè)方向和深度范圍

SearchForTriangulation通過(guò)極線約束限制匹配時(shí)的搜索范圍，進(jìn)行特征點(diǎn)匹配

將屬于同一節(jié)點(diǎn)的ORB特征進(jìn)行匹配
這里和SearchbyBow差不多，如果同屬于同意節(jié)點(diǎn)則遍歷第一幀該節(jié)點(diǎn)中的特征點(diǎn)，每個(gè)特征點(diǎn)和該節(jié)點(diǎn)中第二幀特征點(diǎn)的描述子計(jì)算距離進(jìn)行匹配。
和其他匹配不同的是，這里還利用基礎(chǔ)矩陣計(jì)算特征點(diǎn)kp2到kp1極線（kp1對(duì)應(yīng)pKF2的一條極線）的距離是否小于閾值
匹配過(guò)程還對(duì)旋轉(zhuǎn)進(jìn)行直方圖統(tǒng)計(jì)，去除直方圖中值最大的三個(gè)index，剔除除這三個(gè)最大index的特征點(diǎn)。因?yàn)閮蓭瑘D像之間的匹配點(diǎn)旋轉(zhuǎn)應(yīng)該是相同的

三角法恢復(fù)特征點(diǎn)

已知兩幀圖像相機(jī)位姿和兩幀圖像對(duì)應(yīng)匹配點(diǎn) $x$ 和 $x'$ ，恢復(fù)出世界點(diǎn)在三維世界的坐標(biāo)。
已知點(diǎn) $x$ 和 $x'$ 和兩個(gè)圖像的投影矩陣 $P$ 和 $P'$ ,根據(jù)相機(jī)投影模型，對(duì)應(yīng)3D點(diǎn) $X$ 滿足:
$x=PX\ \ \ \ \ \ \ x'=P'X$
三角法使用的是直接線性變化（DLT）對(duì)點(diǎn)的世界坐標(biāo)進(jìn)行求解。使用DLT需要改成 $AX=0$ 的形式。利用外積的性質(zhì)：
$x×(PX)=0\ \ \ \ \ \ x'×(P'X)=0$
在按照十四講中方法外積可以改寫成
$\begin{equation} \begin{bmatrix} 0 & -1 & y\\ 1 & 0 & -x\\ -y & x & 0 \end{bmatrix} \begin{bmatrix} P^{1T}X\\P^{2T}X\\P^{3T}X \end{bmatrix} = 0 \end{equation}$
即： $\begin{equation} \begin{split} x(P^{3T}X)-(P^{1T}X)=0\\ y(P^{3T}X)-(P^{2T}X)=0\\ x(P^{2T}X)-y(P^{1T}X)=0\\ \end{split} \end{equation}$
可見(jiàn)第三個(gè)式子可以由上兩個(gè)式子線性表示，所以只需要取前連個(gè)式子即可，從而有形如 $AX=0$ 的方程，其中:
$\begin{equation} A =\begin{bmatrix} xP^{3T}-P^{1T}\\ yP^{3T}-P^{2T}\\ x'P'^{3T}-P'^{1T}\\ y'P'^{3T}-P'^{2T}\\ \end{bmatrix} \end{equation}$
對(duì)A進(jìn)行SVD分解，方程的解為A的最小奇異值對(duì)應(yīng)的單位奇異矢量，解得X=(x,y,z,w)，則最后令X縮放使得的最后一項(xiàng)為1即可得到我們所求的3D點(diǎn)X的坐標(biāo)。