2. 程式人生 > >三角化---深度濾波器

三角化---深度濾波器

阿新 • • 發佈：2017-10-25
code 顯示 -1 均值 原來 earch oar inline 特征匹配

一.三角化得到空間點的三維信息(深度值)

(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替換後得到的新的矩陣。

二.三角化得到的三維信息中深度的誤差

(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的場景中，有經驗的人都會有意識地將設備或者相機進行一定量的平移，而不會在原地進行純旋轉。

三.深度濾波器的原理及實現

介紹的是比較簡單的高斯分布假設下的深度濾波器。

高斯分布是自然界中最常見的一種分布形式，並且也符合絕大部分的自然情況。簡單起見，我們先假設三角化後恢復的深度值符合高斯分布。

對於像素點的深度值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）我們如何避免第二個問題中所提出的情況？

四.極限搜索與塊匹配

技術分享

左邊的相機觀測到了某個像素技術分享，由於是個單目相機，我們無法知道其深度，所以假設深度可能在某個區域內，不妨說是某個最小值到無窮技術分享，即該像素對應的空間點對應在本圖中的d射線。在右相機，這條線段的投影也形成圖像平面上的一條線，也就是我們稱的極線。問題：極線上的哪一點才是我們對應的技術分享點呢？

在特征點中，我們通過特征匹配找到了技術分享的位置，然而現在我們沒有描述子，所以在極線上搜索與技術分享長的相似的點，我們可能沿著右圖中的極線從一頭走到另一頭，逐個比較每個像素與技術分享的相似程度。從直接比較像素的角度來看，和直接法是異曲同工的。但在直接法中，我們發現比較單個像素點的亮度並不是穩定可靠的，萬一極線上有很多與技術分享相似的點，我們如何確定哪一個是真實的呢？所以，既然單個像素的亮度沒有區分性，我們來比較像素塊，在技術分享周圍取一個w*w的像小塊，然後在極線上也取很多同樣大小的小塊進行比較，在一定程度上提高區分性，這就是塊匹配。

我們把技術分享周圍的小塊記成技術分享 ，在極線上的n個小塊 技術分享。小塊與小塊之間的差異，用NNC（歸一化互相關）來計算，計算的是兩小塊之間的相關性：

技術分享

相關性0，表示圖像不相似；相關性1，表示圖像相似；

主函數： 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;
}

相信有了上面的流程圖，加上前面幾章的理論鋪墊，你能夠比較輕松地看懂這些代碼了。祝你閱讀代碼愉快！

三角化---深度濾波器

相關推薦

三角化---深度濾波器

code 顯示 -1 均值 原來 earch oar inline 特征匹配 一.三角化得到空間點的三維信息(深度值) (1)三角化的提出 三角化最早由高斯提出，並應用於測量學中。簡單來講就是：在不同的位置觀測同一個三維點P(x, y, z)，已知在不同位置處觀察到的三維點

[CGAL]帶島多邊形三角化

pty %d urn true height ado pop cat idt CGAL帶島多邊形三角化，並輸出(*.ply)格式的模型 模型輸出的關鍵是節點和索引 #include <CGAL/Triangulation_vertex_base_with_id_2

三維重建： 點雲三角化（二流型的）要點 很詳細 —— 乾貨

按照以下三維重建要點，你完全可以按步驟程式碼實現：   O.   初始化引數 一． 初始化一個三角面片：   1. 計算所有點的重心；   2. 找到離重心最近的一個點，

影象分類網路模組化,深度化,輕量化

Residual Network在深度網路變得越來越深的情況下，研究員發現的第一個問題就是退化現象，網路越深，對計算資源的要求就越高，並且當深度達到一定程度後，增加網路層數不但沒有準確性上的提升，反而出現了退化。解決或降低退化現象的第一款神器就是ResNet。ResNet引入了殘差網路結構(Residual

數字影象處理- 3.6 銳化空間濾波器

3.6 銳化空間濾波器 銳化處理的主要目的是突出影象中的突出灰度的過度部分。總的來說，微分運算元的響應強度與影象在該店(應用了運算元)的突變程度有關。這樣一來，影象微分增強了邊緣和其他突變(如噪聲)並削弱了灰度變化緩慢的區域。  為了說明簡單，主要集中討論一階微分的性質。我們最高興

點雲貪心三角化原理

目前點雲進行網格生成一般分為兩大類方法： 1、 插值法。顧名思義，也就是重建的曲面都是通過原始的資料點得到的 2、逼近法。用分片線性曲面或其他曲面來逼近原始資料點，得到的重建曲面是原始點集的一個逼近。 我們主要介紹一種比較簡單的貪心三角化法（對應的類名：pcl::GreedyProjec

基於核化相關濾波器的跟蹤－效果直逼Struck和TLD跟蹤器

原文：http://cvlab.hanyang.ac.kr/tracker_benchmark_v10.html 作者ECCV2012的文章就被我關注過，速度驚人的快。2015年新作出來了。發文以供同行參閱。 Kernelized Correlation Filters J

貪婪投影三角化演算法中的引數設定

本小節描述了怎樣使用貪婪投影三角化演算法對有向點雲進行三角化，具體方法是先將有向點雲投影到某一區域性二維座標平面內，再在座標平面內進行平面內的三角化，再根據平面內三位點的拓撲連線關係獲得一個三角網格曲面模型。 貪婪投影三角化演算法原理是處理一系列可以使網格“生長擴大”的點（邊緣點），延伸這些點直到所有符合

矩陣的擬上三角化（Hessenberg矩陣）

%實矩陣的擬上三角分解（Hessenberg分解） %用法:[Q,B]=hess2(A) %Q返回一個正交矩陣，B為相似於A的擬上三角矩陣 %注意：MATLAB自帶了Hessenberg分解的函式hess(A) %By Castor 2010-12-25 function

二、MyBatis Mapper Bean初始化深度解析

開發十年，就只剩下這套架構體系了！ >>>   

二、MyBatis Mapper Bean初始化深度解析5-5

開發十年，就只剩下這套架構體系了！ >>>   

二、MyBatis Mapper Bean初始化深度解析5-4

開發十年，就只剩下這套架構體系了！ >>>   

OSG初始化深度貼圖的程式碼

//原本的colorTexture是顏色取樣,沒法顯示深度,這裡換成深度貼圖 colorTexture = new Textu

說好不哭！資料視覺化深度乾貨，前端開發下一個漲薪點在這裡~

隨著網際網路在各行各業的影響不斷深入，資料規模越來越大，各企業也越來越重視資料的價值。作為一家專業的資料智慧公司，個推從訊息推送服

三角網格表面高斯曲率的計算與可視化

綠色 調試運行 即將 簡單 坐標 com 框架搭建 alt 建立 好久沒有寫代碼了，最近拿計算三角網格表面的高斯曲率練了練手，並實現了高斯曲率的可視化，復習了一點微分幾何的知識。感覺有時候還是要自己把代碼寫出來，調試運行，結合試驗結果，才能對相應的知識有更深的了解。 所謂曲

單目視覺SLAM:采用逆深度參數化的方法求取深度信息的過程

ive 魯棒性 圖片 lte 有著 研究 tla 一次 攝像機 一種實時的單目視覺SLAM改進算法 該算法采用一個攝像頭作為外部傳感器來提取機器人行進過程中周圍環境的特征信息，用實時性良好的FAST提取環境特征點，結合逆深度參數化進行特征點非延時初始化，用壓縮擴展卡爾曼濾波

吳恩達-深度學習-課程筆記-8: 超參數調試、Batch正則化和softmax( Week 3 )

erp 搜索 給定 via 深度 mode any .com sim 1 調試處理( tuning process ) 如下圖所示，ng認為學習速率α是需要調試的最重要的超參數。 其次重要的是momentum算法的β參數（一般設為0.9），隱藏單元數和mini-batch的

java二維數組定義初始化-楊輝三角

args pan string 數組定義 static highlight pri 位數 -- public class Array2Demo2_3 { public static void main(String[] args) {

【深度學習】批歸一化（Batch Normalization）

學習 src 試用 其中 put min 平移 深度 優化方法 BN是由Google於2015年提出，這是一個深度神經網絡訓練的技巧，它不僅可以加快了模型的收斂速度，而且更重要的是在一定程度緩解了深層網絡中“梯度彌散”的問題，從而使得訓練深層網絡模型更加容易和穩定。所以目前

深度剖析PHP序列化和反序列化

私有 class 文件 對象序列化 bpa php應用 OS 個人博客 def 序列化 序列化格式 在PHP中，序列化用於存儲或傳遞 PHP 的值的過程中，同時不丟失其類型和結構。 序列化函數原型如下： string serialize ( mixed $value ) 先