SLAM代碼（三維重建）

三維重建的一般步驟
- 特徵點對計算基礎矩陣。
- 根據基礎矩陣計算相機矩陣
- 對於內個點對計算點在3D空間的位置。
The Fundamental Matrix Song

本文主要介紹三維重建的初始值的估計方法（三角測量triangulation）
這樣看來三維重建綜合了幾個問題（包括上述三角測量，以及其他的問題包括基礎矩陣估計，），如下圖所示

採用光束平差法對射影空間下的多個相機運動矩陣及目標的三維結構進行優化。光束平差法一般在各種重建算法的最後一步使用。這種優化方法的最大特點是可以處理數據丟失情況並提供真正的最大似然估計。但同時，其要求提供一個好的初始值，即首先確定射影空間下的相機運動矩陣和三維結構。採用三角測量法確定目標的射影空間的三維座標可以得到好的初始值。具體步驟見圖2。

其中需要介紹一些基礎知識

分層重建

• Projective reconstruction
• Affine reconstruction
• Metric reconstruction

如果從圖像能獲得在景物平面的某個仿射座標系下的二維射影變換H ,則這個變換的逆H−1 作用到圖像點 m 就可以得到它所對應的景物點在景物平面的仿射座標系下的座標 X=H−1m ,從而可以得到平面景物的仿射結構。類似地,如果從圖像能獲得在景物平面的歐氏座標系下的二維射影變換H ,如果這個歐氏座標系的度量單位與絕對 ( 或實際 ) 度量相差一個常數倍,則可以通過景物的圖像點和變換H 可以得到景物的相似結構;如果這個歐氏座標系的度量單位與絕對度量相等,則通過景物的圖像點和變換H 可以得到景物的絕對歐氏結構。因此,恢複平面景物的仿射結構,等價於某個仿射座標系下確定景物平面到圖像平面的射影變換;恢複平面景物的度量結構,等價於在某個歐氏座標系下確定景物平面到圖像平面的射影變換;恢複平面景物的絕對歐氏結構,等價於在某個絕對歐氏座標系下確定景物平面到圖像平面的射影變換。

Geometry	Reconstruction	content
Proejction Geometry	攝影重建
Affine Geometry	放射重建	infinity plane
Euclidean Geometry	歐式重建	absolute conics

歐式重建的中已知絕對二次曲線即可恢復歐式幾何的全部信息。

三角測量

三角原理的重構結果,是否爲空間景物的歐氏結構取決於攝像機所處的世界座標系。也就是說,如果世界座標系是歐氏的,則重構結果是空間景物的歐氏結構;否則如果世界座標系是仿射的(或射影的) ,則重構結果是空間景物的仿射結構(或射影結構) 。
在計算目標相對無人機的位置關係中，使用三角測量方法可以獲得相對相機座標系下目標的三維位置。具體原理如下。

相機在2個對應的位姿下得到個視圖的圖像。兩幅圖像在同一世界座標系下的攝像機矩陣P 和P′，m ↔ m′是兩幅圖像的一個點對應，即它們滿足極幾何約束m′TFm=0 ，現在要根據P和P′計算點對應m↔ m′的空間點X。m的反投射線 m(b) 與m′的反投射線 l(b)m′ 確定了通過兩攝像機光心的一張平面 πm ，即一張極平面，所以兩條反投影線l(b)m ，l(b)m′ 必相交於一個空間點。也就是說，對應點的反投射線與兩個攝像機的基線構成一個三角形，這個三角形的頂點是兩個攝像機的光心和兩條反投影線的交點，這個交點正是我們要確定的空間點，在前後的幾何關係中，假設相機矩陣分爲P和P′，因此有m=PX 以及m′=P′X 。因此有

p1TX−uP3TX=0p2TX−vP3TX=0p1TX−u′P3TX=0p2TX−v′P3TX=0

這裏
m=[u v ] ，m′=[u′v′] ，
P=[p1T p2T p3T ]
因此可以構造矩陣A，使得AX=0，這裏

A=⎡⎣⎢⎢⎢⎢p1T−uP3Tp2T−vP3Tp1T−u′P3Tp1T−v′P3T⎤⎦⎥⎥⎥⎥

通過求解這個線性方程可確定空間點X。這裏共有四個方程，未知數X爲一個4維的齊次座標，因此方程式超定的。上述方程是一個齊次方程，因此X的解爲矩陣A的最小奇異值對一個奇異向量，也就是矩陣ATA的最小特徵值對應的特徵向量。但是直接這種方法不具備達到射影不變性。另外三角測量法有多種，都是以上述內容作爲基礎，包括 Polynomial，Poly-Abs，Mid-point，Linear-Eigen，Linear-LS(least-squares)，Iterative-Eigen，Iterative-LS。其中Polynomial是一種非迭代多項式算法。它是仿射和射影不變的，適宜於兩幅圖像和對運算效率要求不高的情況。在高斯噪聲模型下，Polynomial 法是最優的。假設存在高斯噪聲，最小化重建誤差（損失函數）。損失函數的定義爲

C(m,m′)=d(m,m^)2+d(m′,m^′)2

使得m′TFm=0 ，這裏d(∗,∗) 是兩點之間的距離。