一.三角化
【1】三角化得到空間點的三維信息(深度值)
(1)三角化的提出
三角化最早由高斯提出，並應用於測量學中。簡單來講就是：在不同的位置觀測同一個三維點P(x, y, z)，已知在不同位置處觀察到的三維點的二維投影點X1(x1, y1), X2(x2, y2)，利用三角關系，恢復出三維點的深度信息z。
(2)三角化公式
按照對極幾何中的定義，設x1, x2為兩個特征點的歸一化坐標，則它們滿足：
s1x1 = s2Rx2 + t 公式(1)
=> s1x1 - s2Rx2 = t 公式(2)
對公式(2)左右兩側分別乘以x1T，得：
s1x1Tx1 - s2x1TRx2 = x1T t 公式(3)
對公式(2)左右兩側分別乘以(Rx2)T，得：
s1(Rx2)Tx1 - s2(Rx2)TRx2 = (Rx2)T t 公式(4)
由公式(3)和公式(4)可以聯立得到一個一元二次線性方程組，然后可以利用Cramer's法則（參見線性代數書）進行求解。
如下是對應的代碼（如果大家感覺不易讀懂，可以先跳過這段代碼，等看完理論部分再返回來看不遲）
1 // 方程 2 // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC 3 // => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t] 4 // [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t] 5 // 二階方程用克萊默法則求解並解之 6 Vector3d t = T_R_C.translation(); 7 Vector3d f2 = T_R_C.rotation_matrix() * f_curr; 8 Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) ); 9 double A[4]; 10 A[0] = f_ref.dot ( f_ref ); 11 A[2] = f_ref.dot ( f2 ); 12 A[1] = -A[2]; 13 A[3] = - f2.dot ( f2 ); 14 double d = A[0]*A[3]-A[1]*A[2]; 15 Vector2d lambdavec = 16 Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ), 17 -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d; 18 Vector3d xm = lambdavec ( 0,0 ) * f_ref; 19 Vector3d xn = t + lambdavec ( 1,0 ) * f2; 20 Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量 21 double depth_estimation = d_esti.norm(); // 深度值
(3)求解深度的另外兩種方法
a.利用叉乘進行消元進行求解
由
s1x1 = s2Rx2 + t 公式(1)
左右兩邊同時乘以x1的反對稱矩陣，可得：
s1x1^x1 = 0 = s2x1^Rx2 + x1^t 公式(2)
由上式可解得s2，將s2代入公式(1)，可求得s1
b.利用Mid Point Method進行求解(Hartley大名鼎鼎的《Multiple View Geometry》中有講解)
從此圖中我們可以知道，理想情況下O1P和O2P會相交於空間中的一點，但是由於圖像分辨率以及噪聲的存在，實際的情況更可能是上圖所描述的那樣：O1P和O2P在空間中沒有交點，這時我們需要找到一個O1P與O2P之間的公垂線，然后取其上的中點作為我們重建出的三維點，此即為Mid Point Method，具體的推導及公式請參看Hartley的《Multiple View Geometry》。【需要將上面的圖想象的立體一些】
附：
1.Cramer's 法則：
如果A的行列式不為0， Ax=b可以通過如下行列式進行求解：
矩陣Bj為A的第j列被b替換后得到的新的矩陣。
【2】三角化得到的三維信息中深度的誤差
(1)三角化中誤差分析
如上圖所示：
P為空間中的一個三維點，p1和p2分別為在兩個位置處，攝像機觀察到的投影的二維點坐標。
l2為p1在第二幅圖中所對應的極線（極線的概念請參考立體視覺中的對極幾何，這里不再贅述）。
現在，我們要探討的是：
如果我們在l2進行極線搜索時，所找到的p2'點與真實的p2點有一個像素的誤差，那么會給三角化后的三維點P的深度z帶來多大的誤差？
首先，根據上圖，我們可以得到向量之間的關系，以及三角化中的兩個夾角的定義：
a = p - t 公式(1)
α = arccos<p, t> 公式(2)
β = arccos<a, -t> 公式(3)
其中，a, p, t均為向量，α和β為圖中所示的兩個夾角。
如果此時，我們求取的p2'點與p2點有一個像素的偏差，同時，這一個像素的偏差又會給β帶來δβ的角度變化，我們利用β'來表示對β進行δβ擾動后的新的角度。
設相機的焦距為f，則：
公式(4):
公式(5):
公式(6):
至此，加入擾動后的所有新的角度我們都求出來了。
由正弦定理（a/sinA=b/sinB=c/sinC），我們可以得到：
公式(7):
則由第二個位置上的二維點的一個像素的誤差，可能導致的三角化后深度的誤差為：
δp = ||p|| - ||p'||
這里的δp其實也正是深度的一個均方差（不確定度σobs），這個不確定度是我們后面要介紹的深度濾波器的一個很重要的概念，深度濾波器的目的也正是要不斷減小這個不確定度，使得深度的不確定度最后能夠收斂到一個能夠接受的值。
(2)三角化中誤差的來源
上面分析了第二幅圖中的特征點p2的誤差是如何影響三角化后的深度值的。
下面，我們來指出三角化的誤差來源有哪幾方面：
a.圖像的分辨率：圖像的分辨率越高，一個像素所帶來的δβ就越小。
b.特征點求取時的精度：是否做到亞像素，在亞像素的基礎上，誤差有多大？
c.p1點的誤差：會引起極線l2的誤差，從而間接地影響p2點的精度。
d.相機兩次位置的平移向量t的大小：t的模的大小也代表了對極幾何中的基線長度，由公式(7)可以看出基線長度越大，三角化的誤差越小。
所以，這也體現出來了三角化的矛盾：若想提高三角化的精度，其一提高特征點的提取精度，即提高圖像的分辨率，但這會導致圖像的增大，增加計算成本；其二，使平移量增大，但這會導致圖像外觀的明顯變化，外觀變化會使得特征提取與匹配變得困難。總而言之，平移太大，會導致匹配失效；平移太小，三角化精度不夠。
(3)如何減小三角化所帶來的誤差
根據【(2)三角化中誤差的來源分析】中所分析的一些因素可知，要想減小三角化過程中引入的誤差，可以有如下幾個方法：
a.選取盡可能高分辨率的相機。
b.進行亞像素的優化（比如在極線搜索時對像素點坐標進行雙線性插值）
// 雙線性灰度插值
inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt )
{
uchar* d = & img.data[ int(pt(1,0))*img.step+int(pt(0,0)) ];
double xx = pt(0,0) - floor(pt(0,0));
double yy = pt(1,0) - floor(pt(1,0));
return (( 1-xx ) * ( 1-yy ) * double(d[0]) +
xx* ( 1-yy ) * double(d[1]) +
( 1-xx ) *yy* double(d[img.step]) +
xx*yy*double(d[img.step+1]))/255.0;
}
（關於雙線性插值，這篇文章做了比較清晰的講解：http://blog.163.com/guohuanhuan_cool@126/blog/static/167614238201161525538402/）
c.同樣使用亞像素級的圖像處理算法來處理p1點。
d.在不丟失特征點的情況下，讓平移量t盡量大。
由上面的公式推導我們可以看出，三角化中，必須要有平移量t，否則無法構成三角形，進行三角化。所以在有些單目的SLAM，AR/VR的場景中，有經驗的人都會有意識地將設備或者相機進行一定量的平移，而不會在原地進行純旋轉。
二.單目稠密重建
【1】立體視覺
在稠密重建中，我們需要知道每個像素點（或大部分像素點）的距離，對此大致有一下解決方案：
1.使用單目相機，通過移動相機之后進行三角化測量像素的距離。
2.使用雙目相機，利用左右目的視差計算像素的距離。
3.使用RGB-D相機直接獲得像素距離。
前兩種稱為立體視覺，相比於RGB-D直接測量的深度，單目和雙目對深度的獲取往往費力不討好，但是RGB-D有應用范圍和光照的限制，目前RGB-D還無法很好的應用於室外、大場景場合中，仍需通過立體視覺估計深度信息。
下面來介紹單目的稠密估計：
從最簡單的說起，我們給定的相機軌跡的基礎上，如何來估計某幅圖像的深度？
（回顧：對於特征點部分我們完成此過程的描述：我們對圖像提取特征，根據描述子計算了特征之間的匹配，即通過特征對某一空間點進行跟蹤，知道它在圖像的各個位置；由於一幅圖像無法確定特征點的空間位置，所以需要不同的視角下的觀測估計它的深度，即三角測量。）
而在稠密深度圖估計中，我們無法把每個像素都當作特征點計算描述子，所以匹配就顯得尤為重要，這里用到了極線搜索和塊匹配技術。但我們知道了某個像素在各個圖中的位置，我們就能像特征點那樣，用三角測量確定它們的深度。這里不同的是，我們需要很多次三角測量讓深度估計收斂到一個穩定值，這就是深度濾波器技術。
【2】極限搜索與塊匹配
左邊的相機觀測到了某個像素
，由於是個單目相機，我們無法知道其深度，所以假設深度可能在某個區域內，不妨說是某個最小值到無窮
，即該像素對應的空間點對應在本圖中的射線d。在右圖中，這條線段的投影也形成圖像平面上的一條線，也就是我們稱的極線。問題：極線上的哪一點才是我們對應的點呢？
在特征點中，我們通過特征匹配找到了
的位置，然而現在我們沒有描述子，所以在極線上搜索與長的相似的點，我們可能沿着右圖中的極線從一頭走到另一頭，逐個比較每個像素與的相似程度。從直接比較像素的角度來看，和直接法是異曲同工的。但在直接法中，我們發現比較單個像素點的亮度並不是穩定可靠的，萬一極線上有很多與相似的點，我們如何確定哪一個是真實的呢？所以，既然單個像素的亮度沒有區分性，我們來比較像素塊，在周圍取一個w*w的像小塊，然后在極線上也取很多同樣大小的小塊進行比較，在一定程度上提高區分性，這就是塊匹配。
我們把周圍的小塊記成
，在極線上的n個小塊 
。小塊與小塊之間的差異，用NNC（歸一化互相關）來計算，計算A與每一個
的相關性：
相關性0，表示圖像不相似；相關性1，表示圖像相似；
我們將得到一個沿極線的NCC分布，這個分布的形狀取決於圖像本身的樣子，如下圖所示。
圖13-3 匹配得分沿距離的分布
在搜索距離較長的情況下，我們通常會得到一個非凸函數：這個分布存在着很多峰值，然而真實的對應點必定一個只有。在這種情況下，我們傾向於使用概率分布描述深度值，而非用某一單一的數值描述深度。我們的問題轉為了在不斷對不同的圖像進行極限搜索時，我們估計的深度分布會有怎樣的變化---這就是所謂的深度濾波器。
【3】深度濾波器的原理及實現
介紹的是比較簡單的高斯分布假設下的深度濾波器。
高斯分布是自然界中最常見的一種分布形式，並且也符合絕大部分的自然情況。簡單起見，我們先假設三角化后恢復的深度值符合高斯分布。
對於像素點的深度值d，滿足：P(d) = N(μ，σ2)
每當新的數據過來，我們就要利用新的觀測數據更新原有的深度d的分布。
這里的數據融合的方式與經典的Kalman濾波方式大同小異。這里，我們利用觀測方程進行信息融合。
假設新計算出來的深度數據的分布為：P(dobs) = N(μobs，σobs2)
我們將新計算出來的深度數據乘在原來的分布上，進行信息融合，得到融合后的高斯分布：P(dfuse) = N(μfuse, σfuse2)
（兩個高斯分布的乘積還是高斯分布）。其中，
這里的μobs，σobs2該如何才能得到呢？
這里的μobs實際上就是每次我們新三角化出來的深度值，而對於σobs2，就是上面提到的 δp（不確定度σobs）。
那么原始的分布μ，σ2該如何得到呢？
這個很簡單，第一次三角化出來的μ，σ2就可以作為初始值，然后每次新三角化出一個三維點，就去更新深度值的分布。
至此，我們似乎得到了一個不錯的結果：既簡單又優美的公式。
實際上還會存在什么問題呢？
（1）實際的深度值分布是否真的符合高斯分布？
（2）如果我們中間過程有一次三角化的過程求錯了，並且還進行了信息融合，會有什么后果？
（3）我們如何避免第二個問題中所提出的情況？
流程圖
主函數： update：
1 bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov )
2 {
3 #pragma omp parallel for
4 for ( int x=boarder; x<width-boarder; x++ )
5 #pragma omp parallel for
6 for ( int y=boarder; y<height-boarder; y++ )
7 {
8 // 遍歷每個像素
9 if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov ) // 深度已收斂或發散
10 continue;
11 // 在極線上搜索 (x,y) 的匹配
12 Vector2d pt_curr;
13 bool ret = epipolarSearch (
14 ref,
15 curr,
16 T_C_R,
17 Vector2d(x,y),
18 depth.ptr<double>(y)[x],
19 sqrt(depth_cov.ptr<double>(y)[x]),
20 pt_curr
21 );
22
23 if ( ret == false ) // 匹配失敗
24 continue;
25
26 // 取消該注釋以顯示匹配
27 // showEpipolarMatch( ref, curr, Vector2d(x,y), pt_curr );
28
29 // 匹配成功，更新深度圖
30 updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov );
31 }
32 }
極線搜索：
1 // 極線搜索
2 // 方法見書 13.2 13.3 兩節
3 bool epipolarSearch(
4 const Mat& ref, const Mat& curr,
5 const SE3& T_C_R, const Vector2d& pt_ref,
6 const double& depth_mu, const double& depth_cov,
7 Vector2d& pt_curr )
8 {
9 Vector3d f_ref = px2cam( pt_ref );
10 f_ref.normalize();
11 Vector3d P_ref = f_ref*depth_mu; // 參考幀的 P 向量
12
13 Vector2d px_mean_curr = cam2px( T_C_R*P_ref ); // 按深度均值投影的像素
14 double d_min = depth_mu-3*depth_cov, d_max = depth_mu+3*depth_cov;
15 if ( d_min<0.1 ) d_min = 0.1;
16 Vector2d px_min_curr = cam2px( T_C_R*(f_ref*d_min) ); // 按最小深度投影的像素
17 Vector2d px_max_curr = cam2px( T_C_R*(f_ref*d_max) ); // 按最大深度投影的像素
18
19 Vector2d epipolar_line = px_max_curr - px_min_curr; // 極線（線段形式）
20 Vector2d epipolar_direction = epipolar_line; // 極線方向
21 epipolar_direction.normalize();
22 double half_length = 0.5*epipolar_line.norm(); // 極線線段的半長度
23 if ( half_length>100 ) half_length = 100; // 我們不希望搜索太多東西
24
25 // 取消此句注釋以顯示極線（線段）
26 // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );
27
28 // 在極線上搜索，以深度均值點為中心，左右各取半長度
29 double best_ncc = -1.0;
30 Vector2d best_px_curr;
31 for ( double l=-half_length; l<=half_length; l+=0.7 ) // l+=sqrt(2)
32 {
33 Vector2d px_curr = px_mean_curr + l*epipolar_direction; // 待匹配點
34 if ( !inside(px_curr) )
35 continue;
36 // 計算待匹配點與參考幀的 NCC
37 double ncc = NCC( ref, curr, pt_ref, px_curr );
38 if ( ncc>best_ncc )
39 {
40 best_ncc = ncc;
41 best_px_curr = px_curr;
42 }
43 }
44 if ( best_ncc < 0.85f ) // 只相信 NCC 很高的匹配
45 return false;
46 pt_curr = best_px_curr;
47 return true;
48 }
深度濾波器：
bool updateDepthFilter(
const Vector2d& pt_ref,
const Vector2d& pt_curr,
const SE3& T_C_R,
Mat& depth,
Mat& depth_cov
)
{
// 用三角化計算深度
SE3 T_R_C = T_C_R.inverse();
Vector3d f_ref = px2cam( pt_ref );
f_ref.normalize();
Vector3d f_curr = px2cam( pt_curr );
f_curr.normalize();
// 方程
// d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
// => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
// [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
// 二階方程用克萊默法則求解並解之
Vector3d t = T_R_C.translation();
Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
double A[4];
A[0] = f_ref.dot ( f_ref );
A[2] = f_ref.dot ( f2 );
A[1] = -A[2];
A[3] = - f2.dot ( f2 );
double d = A[0]*A[3]-A[1]*A[2];
Vector2d lambdavec =
Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
-A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
Vector3d xm = lambdavec ( 0,0 ) * f_ref;
Vector3d xn = t + lambdavec ( 1,0 ) * f2;
Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量
double depth_estimation = d_esti.norm(); // 深度值
// 計算不確定性（以一個像素為誤差）
Vector3d p = f_ref*depth_estimation;
Vector3d a = p - t;
double t_norm = t.norm();
double a_norm = a.norm();
double alpha = acos( f_ref.dot(t)/t_norm );
double beta = acos( -a.dot(t)/(a_norm*t_norm));
double beta_prime = beta + atan(1/fx);
double gamma = M_PI - alpha - beta_prime;
double p_prime = t_norm * sin(beta_prime) / sin(gamma);
double d_cov = p_prime - depth_estimation;
double d_cov2 = d_cov*d_cov;
// 高斯融合
double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2);
double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 );
depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse;
depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;
return true;
}