直接法
前言
直接法是視覺里程計另外一主要分支，它與特徵點法有很大不一樣。隨着SVO、LSD-SLAM等直接法SLAM方案的流行，直接法自己也獲得愈來愈多的關注。特徵點法與直接究竟誰更好一些，是近年視覺里程計研究領域一個很是有趣的問題。本講，咱們將介紹直接法的原理，並利用g2o實現基於直接法的視覺里程計。ios
1. 直接法的引出
儘管特徵點法在視覺里程計中佔據主流地位，研究者們認識它至少有如下幾個缺點：git
- 關鍵點的提取與描述子的計算很是耗時。實踐當中，SIFT目前在CPU上是沒法實時計算的，而ORB也須要近20毫秒的計算。若是整個SLAM以30毫秒/幀的速度運行，那麼一大半時間都花在計算特徵點上。
- 使用特徵點時，忽略了除特徵點之外的全部信息。一張圖像有幾十萬個像素，而特徵點只有幾百個。只使用特徵點丟棄了大部分可能有用的圖像信息。
- 相機有時會運動到特徵缺失的地方，每每這些地方都沒有什麼明顯的紋理信息。例如，有時咱們會面對一堵白牆，或者一個空蕩蕩的走廓。這些場景下特徵點數量會明顯減小，咱們可能找不到足夠的匹配點來計算相機運動。
咱們看到使用特徵點確實存在一些問題。針對這些缺點，也存在若干種可行的方法：github
- 只計算關鍵點，不計算描述子。同時，使用光流法（Optical Flow）來跟蹤特徵點的運動。這樣能夠迴避計算和匹配描述子帶來的時間，但光流自己的計算須要必定時間；
- 只計算關鍵點，不計算描述子。同時，使用直接法來計算特徵點在下一時刻圖像的位置。這一樣能夠跳過描述子的計算過程，並且直接法的計算更加簡單。
- 既不計算關鍵點、也不計算描述子——根據像素來直接計算相機運動。
第一種方法仍然使用特徵點，只是把匹配描述子替換成了光流跟蹤，估計相機運動時仍使用PnP或ICP算法。而在，後兩個方法中，咱們會根據圖像的像素信息來計算相機運動，它們稱爲直接法。
使用特徵點法估計相機運動時，咱們把特徵點看做固定在三維空間的不動點。根據它們在相機中的投影位置，經過最小化重投影偏差（Reprojection error）來優化相機運動。在這個過程當中，咱們須要精確地知道空間點在兩個相機中投影后的像素位置——這也就是咱們爲什麼要對特徵進行匹配或跟蹤的理由。而在直接法中，咱們最小化的再也不是重投影偏差，而是測量偏差（Phometric error）。
直接法是本講介紹的重點。它的存在就是爲了克服特徵點法的上述缺點（雖然它會引入另外一些問題）。直接法直接根據像素亮度信息，估計相機的運動，能夠徹底不用計算關鍵點和描述子。因而，直接法既避免了特徵的計算時間，也避免了特徵缺失的狀況。只要場景中存在明暗變化（能夠是漸變，不造成局部的圖像特徵），直接法就能工做。根據使用像素的數量，直接法分爲稀疏、稠密和半稠密三種，具備恢復稠密結構的能力。相比於特徵點法一般只能重構稀疏特徵點，直接法和稠密重建有更緊密的聯繫。
歷史上，雖然早期也有一些對直接法的使用，但直到RGB-D相機出現後，人們才發現直接法對RGB-D相機，進而對單目相機，都是行之有效的方法。隨着一些使用直接法的開源項目的出現（如SVO、LSD-SLAM等），它們逐漸地走上主流舞臺，成爲視覺里程計算法中重要的一部分。算法
2. 直接法數學推導
直接法和光流很是類似，它們都是基於灰度不變假設的：編程
灰度不變假設：同一個空間點的像素灰度，在各個圖像中是固定不變的。app
灰度不變假設是一個很強的假設，實際當中極可能不成立。事實上，因爲物體的材質不一樣，像素會出現高光和陰影部分；有時，相機會自動調整曝光參數，使得圖像總體變亮或變暗。這些時候灰度不變假設都是不成立的，所以直接法/光流的結果也不必定可靠。不過，暫且讓咱們認爲該假設成立，從而看看如何計算相機的運動。咱們先介紹直接法的原理，而後使用g2o實現直接法。ide
考慮某個空間點 $P$ 和兩個時刻的相機。$P$ 的世界座標爲 $[X,Y,Z]$，它在兩個相機上成像，其非齊次像素座標爲 $\mathbf{p}_1, \mathbf{p}_2$。咱們的目標是求第一個相機到第二個相機的相對位姿變換。設第一個相機旋轉、平移爲 $\mathbf{I}, \mathbf{0}$，第二個相機外參爲 $\mathbf{R}, \mathbf{t}$（李代數爲$\mathbf{\xi}$）。同時，兩相機的內參相同，記爲$\mathbf{K}$。爲清楚起見，咱們列寫完整的投影方程：函數
\[{\mathbf{p}_1} = {\left[ \begin{array}{l}
u\\
v
\end{array} \right]_1} = \mathbf{D} \frac{1}{Z_1} \mathbf{KP} \]
\[{\mathbf{p}_2} = {\left[ \begin{array}{l}
u\\
v
\end{array} \right]_2} = \mathbf{D}\frac{1}{Z_2} \mathbf{K}\left( {\mathbf{RP} +\mathbf{t}} \right) = \mathbf{D}\frac{1}{Z_2} \mathbf{K} \exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}\]oop
其中 $Z_1,Z_2$ 是 $P$ 在兩個相機座標系下的深度座標值。$\mathbf{D}$ 爲齊次座標到非齊次座標的轉換矩陣：
\[\begin{equation}
\mathbf{D} = \left[ {\begin{array}{*{20}{c}}
1&0&0\\
0&1&0
\end{array}} \right]
\end{equation}\]優化
在直接法中，咱們一樣是解一個優化問題，但這個優化最小化的不是重投影偏差，而是測量偏差（Photometric Error），也就是 $P$ 的兩個像的亮度偏差：
\[\begin{equation}
e = {\mathbf{I}_1}\left( {{\mathbf{p}_1}} \right) - {\mathbf{I}_2}\left( {{\mathbf{p}_2}} \right)
\end{equation}\]
咱們優化該偏差的二範數：
\[\begin{equation}
\mathop {\min }\limits_{\mathbf{\xi}} J\left( \mathbf{\xi} \right) = { e^T} e
\end{equation}\]
可以作這種優化的理由即灰度不變假設。在直接法中，咱們假設一個空間點在各個視角下，成像的灰度是不變的。一樣的，這依然是一個很強的假設，並且與光流法十分類似。實際當中，咱們有許多個（好比$N$個）空間點$P_i$，那麼，整個相機位姿估計問題變爲：
\[\begin{equation}\label{key}
\mathop {\min }\limits_{\mathbf{\xi}} J\left( \mathbf{\xi} \right) = \sum\limits_{i = 1}^N {e_i^T{e_i}}, \quad {e_i} = { \mathbf{I}_1}\left( {{\mathbf{p}_{1,i}}} \right) - {\mathbf{I}_2}\left( {{ \mathbf{p}_{2,i}}} \right)
\end{equation}\]
爲了求解這個優化問題，咱們關心偏差$e$是如何隨着相機位姿$\mathbf{\xi}$變化的，須要分析它們的導數關係。所以，使用李代數上的擾動模型。咱們給$\exp (\mathbf{\xi})$左乘一個小擾動$\exp( \delta \mathbf{\xi} )$，得：
\[\begin{equation}\begin{array}{ll}
e\left( { \mathbf{\xi} \oplus \delta \mathbf{\xi} } \right) &= { \mathbf{I} _1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP} } \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK}\exp \left( {\delta {\mathbf{\xi} ^ \wedge }} \right)\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}} \right)\\
& \approx {\mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK} \left( {1 + \delta {\mathbf{\xi} ^ \wedge }} \right)\exp \left( {{ \mathbf{\xi} ^ \wedge }} \right) \mathbf{P} } \right)\\
&= {\mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK}\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P} + \frac{1}{{{Z_2}}} \mathbf{DK} \delta { \mathbf{\xi} ^ \wedge }\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}} \right)
\end{array}\end{equation}\]
記
\[\begin{equation}\begin{array}{ll}
\mathbf{q} &= \delta \mathbf{\xi} ^\wedge \exp \left( {{ \mathbf{\xi} ^ \wedge }} \right) \mathbf{P} \\
\mathbf{u} &= \frac{1}{{{Z_2}}} \mathbf{DK} \mathbf{q}
\end{array}\end{equation}\]
$\mathbf{q}$ 即 $P$ 在第二個相機座標系下的座標，而$\mathbf{u}$爲它的像素座標。因而，利用一階泰勒展開，有：
\[\begin{equation}\begin{array}{ll}
e \left( { \mathbf{\xi} \oplus \delta \mathbf{\xi} } \right) &= {\mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK} \exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P} + \mathbf{u}} \right)\\
& \approx { \mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK}\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}} \right) - \frac{{\partial { \mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \mathbf{q}}}\frac{{\partial \mathbf{q}}}{{\partial \delta \mathbf{\xi} }}\delta \mathbf{\xi} \\
&= e\left( \mathbf{\xi} \right) - \frac{{\partial {\mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \mathbf{q}}}\frac{{\partial \mathbf{q}}}{{\partial \delta \mathbf{\xi} }}\delta \mathbf{\xi}
\end{array}\end{equation}\]
咱們看到，一階導數因爲鏈式法則分紅了三項，而這三項都是容易計算的：
- $ \partial \mathbf{I}_2 / \partial \mathbf{u} $ 爲$\mathbf{u}$處的像素梯度；
- $ \partial \mathbf{u} / \partial \mathbf{q} $ 爲投影方程關於相機座標系下的三維點的導數。咱們把投影方程展開，爲： \[\begin{equation}
u = \frac{{{f_x}X + {c_x}}}{Z},v = \frac{{{f_y}Y + {c_y}}}{Z}
\end{equation}\]因而導數爲：
\[\begin{equation}
\frac{{\partial \mathbf{u}}}{{\partial \mathbf{q}}} = \left[ {\begin{array}{*{20}{c}}
{\frac{{\partial u}}{{\partial X}}}&{\frac{{\partial u}}{{\partial Y}}}&{\frac{{\partial u}}{{\partial Z}}}\\
{\frac{{\partial v}}{{\partial X}}}&{\frac{{\partial v}}{{\partial Y}}}&{\frac{{\partial v}}{{\partial Z}}}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
{\frac{{{f_x}}}{{\rm{Z}}}}&0&{ - \frac{{{f_x}X}}{{{Z^2}}}}\\
0&{\frac{{{f_y}}}{Z}}&{ - \frac{{{f_y}Y}}{{{Z^2}}}}
\end{array}} \right]
\end{equation}\] - ${\partial \mathbf{q}}/{\partial \delta \mathbf{\xi} }$爲變換後的三維點對變換的導數，這在李代數章節已經介紹過了：
\[\begin{equation}
\frac{{\partial \mathbf{q}}}{{\partial \delta \mathbf{\xi} }} = \left[ { \mathbf{I}, - {\mathbf{q}^ \wedge }} \right]
\end{equation}\]
在實踐中，因爲後兩項只與三維點$\mathbf{q}$有關，而與圖像無關，咱們常常把它合併在一塊兒：
\[\begin{equation}
\frac{{\partial \mathbf{u}}}{{\partial \delta \mathbf{\xi} }} = \left[ {\begin{array}{*{20}{c}}
{\frac{{{f_x}}}{Z}}&0&{ - \frac{{{f_x}X}}{{{Z^2}}}}&{ - \frac{{{f_x}XY}}{{{Z^2}}}}&{{f_x} + \frac{{{f_x}{X^2}}}{{{Z^2}}}}&{ - \frac{{{f_x}Y}}{Z}}\\
0&{\frac{{{f_y}}}{Z}}&{ - \frac{{{f_y}Y}}{{{Z^2}}}}&{ - {f_y} - \frac{{{f_y}{Y^2}}}{{{Z^2}}}}&{\frac{{{f_y}XY}}{{{Z^2}}}}&{\frac{{{f_y}X}}{Z}}
\end{array}} \right]
\end{equation}\]
因而，如今咱們推導了偏差相對於李代數的雅可比矩陣：
\[ \begin{equation}
\label{eq:jacobianofDirect}
\mathbf{J} = - \frac{{\partial { \mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \delta \mathbf{\xi} }}
\end{equation}\]
對於$N$個點的問題，咱們能夠用這種方法計算優化問題的雅可比，而後使用G-N或L-M計算增量，迭代求解。例如，在G-N方法下，增量$\delta {\mathbf{\xi} ^*} $的計算方式爲：
\[\begin{equation}
\left( {\sum\limits_{i = 1}^N { \mathbf{J}_i^T \mathbf{J}_i} } \right)\delta {\mathbf{\xi} ^*} = - \sum\limits_{i = 1}^N {{\mathbf{J}_i^T}{e_i}}
\end{equation}\]
至此，咱們推導了直接法估計相機位姿的整個流程，下面咱們講講直接法是如何使用的。
3. 直接法的使用
在咱們上面的推導中，$P$是一個已知位置的空間點，它是怎麼來的呢？在RGB-D相機下，咱們能夠把任意像素反投影到三維空間，而後投影到下一個圖像中。若是在單目相機中，咱們也可使用已經估計好位置的特徵點（雖然是特徵點，但直接法裏是能夠避免計算描述子的）。根據$P$的來源，咱們能夠把直接法進行分類：
- $P$來自於稀疏特徵點，咱們稱之爲稀疏直接法。一般咱們使用數百個特徵點，而且會像L-K光流那樣，假設它周圍像素也是不變的。這種稀疏直接法速度沒必要計算描述子，而且只使用數百個像素，所以速度最快，但只能計算稀疏的重構。
- $P$來自部分像素。咱們看到式（\ref{eq:jacobianofDirect}）中，若是像素梯度爲零，整一項雅可比就爲零，不會對計算運動增量有任何貢獻。所以，能夠考慮只使用帶有梯度的像素點，捨棄像素梯度不明顯的地方。這稱之爲半稠密（Semi-Dense）的直接法，能夠重構一個半稠密結構。
- $P$爲全部像素，稱爲稠密直接法。稠密重構須要計算全部像素（通常幾十萬至幾百萬個），所以多數不能在現有的 CPU上實時計算，須要GPU的加速。
能夠看到，從稀疏到稠密重構，均可以用直接法來計算。它們的計算量是逐漸增加的。稀疏方法能夠快速地求解相機位姿，而稠密方法能夠創建完整地圖。具體使用哪一種方法，須要視機器人的應用環境而定。
4. 使用g2o實現直接法
如今，咱們來演示如何使用稀疏的直接法。因爲咱們不涉及GPU編程，稠密的直接法就省略掉了。同時，爲了保持程序簡單，咱們使用RGB-D數據而非單目數據，這樣能夠省略掉單目的深度恢復部分（這個很麻煩，咱們以後另講）。
求解直接法最後等價於求解一個優化問題，所以咱們可使用g2o這樣的通用優化庫幫助咱們求解。在使用g2o以前，須要把直接法抽象成一個圖優化問題。顯然，直接法是由如下頂點和邊組成的：
- 優化變量爲一個相機位姿，所以須要一個位姿頂點。因爲咱們在推導中使用了李代數，故程序中使用李代數表達的$SE(3)$位姿頂點，如g2o/types/types\_six\_dof\_expmap.h中的「VertexSE3Expmap」。
- 偏差項爲一個像素的測量偏差。因爲整個優化過程當中$\mathbf{I}_1(\mathbf{p}_1)$保持不變，咱們能夠把它當成一個固定的預設值，從而調整相機位姿，使$\mathbf{I}_2(\mathbf{p}_2)$接近這個值。因而，這種邊只鏈接一個頂點，爲 一元邊。因爲g2o中自己沒有計算測量偏差的邊，咱們須要本身定義一種新的邊。
整個直接法圖優化問題是由一個相機位姿頂點與許多條一元邊組成。若是使用稀疏的直接法，那咱們大約會有幾百至幾千條這樣的邊；稠密直接法則會有幾十萬條邊。優化問題對應的線性方程是計算李代數增量（6$\times$1），自己規模不大，因此主要的計算時間會花費在每條邊的偏差與雅可比的計算上。下面的實驗中，咱們先來定義一種專門用於計算直接法的邊，而後，使用該邊構建圖優化問題並求解之。
實驗工程位於：https://github.com/gaoxiang12/slambook 中的 ch8/directMethod中。
工程目錄下的g2o\_types.h中給出了自定義邊的聲明，g2o\_types.cpp中給出實現，咱們在CMakeLists.txt裏將它們編譯了成一個庫。
1 #ifndef G2O_TYPES_DIRECT_H_ 2 #define G2O_TYPES_DIRECT_H_ 3 4 // 定義應用於直接法的g2o邊 5 #include <Eigen/Geometry> 6 #include <g2o/core/base_unary_edge.h> 7 #include <g2o/types/sba/types_six_dof_expmap.h> 8 #include <opencv2/core/core.hpp> 9 namespace g2o 10 { 11 // project a 3d point into an image plane, the error is photometric error 12 // an unary edge with one vertex SE3Expmap (the pose of camera) 13 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 14 { 15 public: 16 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 18 EdgeSE3ProjectDirect() {} 19 20 EdgeSE3ProjectDirect( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image ) 21 : x_world_(point), fx_(fx), fy_(fy), cx_(cx), cy_(cy), image_(image) 22 {} 23 24 virtual void computeError(); 25 26 // plus in manifold 27 virtual void linearizeOplus( ); 28 29 // dummy read and write functions because we don't care... 30 virtual bool read( std::istream& in ); 31 virtual bool write( std::ostream& out ) const ; 32 33 protected: 34 float getPixelValue( float x, float y ); 35 public: 36 // 3d point in world frame 37 Eigen::Vector3d x_world_; 38 // Pinhole camera intrinsics 39 float cx_=0, cy_=0, fx_=0, fy_=0; 40 // the reference image 41 cv::Mat* image_=nullptr; 42 }; 43 } // namespace g2o 44 #endif // G@O_TYPES_DIRECT_H_
咱們的邊繼承自g2o::BaseUnaryEdge。在繼承時，須要在模板參數裏填入測量值的維度、類型，以及鏈接此邊的頂點，同時，咱們把空間點$P$、相機內參和圖像存儲在該邊的成員變量中。爲了讓g2o優化該邊對應的偏差，咱們須要覆寫兩個虛函數：用computeError()計算偏差值，用linearizeOplus()計算雅可比。其他的存儲和讀取函數，因爲本次實驗不關心，就略去了。下面咱們給出這兩個函數的實現：
1 #include "g2o_types.h" 2 #include <g2o/core/factory.h> 3 4 #include <iostream> 5 6 using namespace std; 7 8 namespace g2o 9 { 10 11 G2O_REGISTER_TYPE(EDGE_PROJECT_SE3_DIRECT, EdgeSE3ProjectDirect ); 12 13 // compute the photometric error 14 void EdgeSE3ProjectDirect::computeError() 15 { 16 const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] ); 17 Eigen::Vector3d x_local = v->estimate().map ( x_world_ ); 18 float x = x_local[0]*fx_/x_local[2] + cx_; 19 float y = x_local[1]*fy_/x_local[2] + cy_; 20 // check x,y is in the image 21 if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows ) 22 _error(0,0) = 999.0; 23 else 24 { 25 _error(0,0) = getPixelValue(x,y) - _measurement; 26 } 27 } 28 29 // the bilinear interpolated pixel value 30 float EdgeSE3ProjectDirect::getPixelValue ( float x, float y ) 31 { 32 uchar* data = & image_->data[ int(y) * image_->step + int(x) ]; 33 float xx = x - floor ( x ); 34 float yy = y - floor ( y ); 35 float v = ( 36 ( 1-xx ) * ( 1-yy ) * data[0] + 37 xx* ( 1-yy ) * data[1] + 38 ( 1-xx ) *yy*data[ image_->step ] + 39 xx*yy*data[image_->step+1] 40 ); 41 return v; 42 } 43 44 // plus in manifold 45 void EdgeSE3ProjectDirect::linearizeOplus() 46 { 47 VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] ); 48 49 Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); 50 51 double x = xyz_trans[0]; 52 double y = xyz_trans[1]; 53 double invz = 1.0/xyz_trans[2]; 54 double invz_2 = invz*invz; 55 56 float u = x*fx_*invz + cx_; 57 float v = y*fy_*invz + cy_; 58 59 // jacobian from se3 to u,v 60 // note that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation 61 Eigen::Matrix<double, 2, 6> jacobian_uv_ksai; 62 63 jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_; 64 jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_; 65 jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_; 66 jacobian_uv_ksai ( 0,3 ) = invz *fx_; 67 jacobian_uv_ksai ( 0,4 ) = 0; 68 jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_; 69 70 jacobian_uv_ksai ( 1,0 ) = -( 1+y*y*invz_2 ) *fy_; 71 jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_; 72 jacobian_uv_ksai ( 1,2 ) = x*invz *fy_; 73 jacobian_uv_ksai ( 1,3 ) = 0; 74 jacobian_uv_ksai ( 1,4 ) = invz *fy_; 75 jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_; 76 77 Eigen::Matrix<double, 1, 2> jacobian_pixel_uv; 78 79 jacobian_pixel_uv ( 0,0 ) = ( getPixelValue(u+1,v)-getPixelValue(u,v) ); 80 jacobian_pixel_uv ( 0,1 ) = ( getPixelValue(u,v+1)-getPixelValue(u,v) ); 81 82 _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai; 83 } 84 bool EdgeSE3ProjectDirect::read( std::istream& in ) 85 { 86 return true; 87 } 88 89 bool EdgeSE3ProjectDirect::write( std::ostream& out ) const 90 { 91 return true; 92 } 93 94 95 }// namespace g2o
能夠看到，這裏的雅可比計算與式（\ref{eq:jacobianofDirect}）是一致的。注意咱們在程序中的偏差計算裏，使用了$\mathbf{I}_2(\mathbf{p}_2) - \mathbf{I}_1(\mathbf{p}_1)$的形式，所以前面的負號能夠省去，只需把像素梯度乘以像素到李代數的梯度便可。
在程序中，相機位姿是用浮點數表示的，投影到像素座標也是浮點形式。爲了更精細地計算像素亮度，咱們要對圖像進行插值。咱們這裏採用了簡單的雙線性插值，您也可使用更復雜的插值方式，但計算代價可能會變高一些。
5. 使用直接法估計相機運動
定義了g2o邊後，咱們將節點和邊組合成圖，就能夠調用g2o進行優化了。實現代碼位於 slambook/ch8/directMethod/direct\_sparse.cpp中，請讀者閱讀該部分代碼並編譯它。咱們將代碼編譯的步驟留做習題吧（一臉賤笑中）。
1 #include <iostream> 2 #include <fstream> 3 #include <list> 4 #include <vector> 5 #include <chrono> 6 #include <ctime> 7 #include <climits> 8 9 #include <opencv2/core/core.hpp> 10 #include <opencv2/imgproc/imgproc.hpp> 11 #include <opencv2/highgui/highgui.hpp> 12 #include <opencv2/features2d/features2d.hpp> 13 14 #include "g2o_types.h" 15 #include <g2o/core/block_solver.h> 16 #include <g2o/core/optimization_algorithm_gauss_newton.h> 17 #include <g2o/solvers/dense/linear_solver_dense.h> 18 #include <g2o/core/robust_kernel.h> 19 #include <g2o/types/sba/types_six_dof_expmap.h> 20 21 using namespace std; 22 23 // 一次測量的值，包括一個世界座標系下三維點與一個灰度值 24 struct Measurement 25 { 26 Measurement( Eigen::Vector3d p, float g ): pos_world(p), grayscale(g) {} 27 Eigen::Vector3d pos_world; 28 float grayscale; 29 }; 30 31 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale ) 32 { 33 float zz = float ( d ) /scale; 34 float xx = zz* ( x-cx ) /fx; 35 float yy = zz* ( y-cy ) /fy; 36 return Eigen::Vector3d ( xx, yy, zz ); 37 } 38 39 inline Eigen::Vector2d project3Dto2D( float x, float y, float z, float fx, float fy, float cx, float cy ) 40 { 41 float u = fx*x/z+cx; 42 float v = fy*y/z+cy; 43 return Eigen::Vector2d(u,v); 44 } 45 46 // 直接法估計位姿 47 // 輸入：測量值（空間點的灰度），新的灰度圖，相機內參； 輸出：相機位姿 48 // 返回：true爲成功，false失敗 49 bool poseEstimationDirect( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw ); 50 51 int main ( int argc, char** argv ) 52 { 53 if ( argc != 2 ) 54 { 55 cout<<"usage: useLK path_to_dataset"<<endl; 56 return 1; 57 } 58 srand( (unsigned int) time(0) ); 59 string path_to_dataset = argv[1]; 60 string associate_file = path_to_dataset + "/associate.txt"; 61 62 ifstream fin ( associate_file ); 63 64 string rgb_file, depth_file, time_rgb, time_depth; 65 cv::Mat color, depth, gray; 66 vector<Measurement> measurements; 67 // 相機內參 68 float cx = 325.5; 69 float cy = 253.5; 70 float fx = 518.0; 71 float fy = 519.0; 72 float depth_scale = 1000.0; 73 Eigen::Matrix3f K; 74 K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f; 75 76 Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity(); 77 78 cv::Mat prev_color; 79 // 咱們演示兩個圖像間的直接法計算 80 for ( int index=0; index<2; index++ ) 81 { 82 cout<<"*********** loop "<<index<<" ************"<<endl; 83 fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 84 color = cv::imread ( path_to_dataset+"/"+rgb_file ); 85 depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 ); 86 cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY ); 87 if ( index ==0 ) 88 { 89 // 對第一幀提取FAST特徵點 90 vector<cv::KeyPoint> keypoints; 91 cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create(); 92 detector->detect ( color, keypoints ); 93 for ( auto kp:keypoints ) 94 { 95 // 去掉鄰近邊緣處的點 96 if ( kp.pt.x < 20 || kp.pt.y < 20 || (kp.pt.x+20)>color.cols || (kp.pt.y+20)>color.rows ) 97 continue; 98 ushort d = depth.ptr<ushort> ( int( kp.pt.y ) ) [ int(kp.pt.x) ]; 99 if ( d==0 ) 100 continue; 101 Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale ); 102 float grayscale = float ( gray.ptr<uchar> ( int(kp.pt.y) ) [ int(kp.pt.x) ] ); 103 measurements.push_back ( Measurement( p3d, grayscale ) ); 104 } 105 prev_color = color.clone(); 106 continue; 107 } 108 // 使用直接法計算相機運動及投影點 109 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 110 poseEstimationDirect( measurements, &gray, K, Tcw ); 111 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 112 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 ); 113 cout<<"direct method costs time: "<<time_used.count()<<" seconds."<<endl; 114 cout<<"Tcw="<<Tcw.matrix()<<endl; 115 116 // plot the feature points 117 cv::Mat img_show( color.rows, color.cols*2, CV_8UC3 ); 118 prev_color.copyTo( img_show( cv::Rect(0,0,color.cols, color.rows) ) ); 119 color.copyTo( img_show(cv::Rect(color.cols,0,color.cols, color.rows)) ); 120 for ( Measurement m:measurements ) 121 { 122 Eigen::Vector3d p = m.pos_world; 123 Eigen::Vector2d pixel_prev = project3Dto2D( p(0,0), p(1,0), p(2,0), fx, fy, cx, cy ); 124 Eigen::Vector3d p2 = Tcw*m.pos_world; 125 Eigen::Vector2d pixel_now = project3Dto2D( p2(0,0), p2(1,0), p2(2,0), fx, fy, cx, cy ); 126 127 float b = 255*float(rand())/RAND_MAX; 128 float g = 255*float(rand())/RAND_MAX; 129 float r = 255*float(rand())/RAND_MAX; 130 cv::circle(img_show, cv::Point2d(pixel_prev(0,0), pixel_prev(1,0)), 5, cv::Scalar(b,g,r),1 ); 131 cv::circle(img_show, cv::Point2d(pixel_now(0,0)+color.cols, pixel_now(1,0)), 5, cv::Scalar(b,g,r),1 ); 132 cv::line( img_show, cv::Point2d(pixel_prev(0,0), pixel_prev(1,0)), cv::Point2d(pixel_now(0,0)+color.cols, pixel_now(1,0)), cv::Scalar(b,g,r), 1 ); 133 } 134 cv::imshow( "result", img_show ); 135 cv::waitKey(0); 136 137 } 138 return 0; 139 } 140 141 bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw ) 142 { 143 // 初始化g2o 144 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的 145 DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > (); 146 DirectBlock* solver_ptr = new DirectBlock( linearSolver ); 147 g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); 148 g2o::SparseOptimizer optimizer; 149 optimizer.setAlgorithm( solver ); 150 optimizer.setVerbose( true ); // 打開調試輸出 151 152 g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); 153 pose->setEstimate( g2o::SE3Quat(Tcw.rotation(), Tcw.translation()) ); 154 pose->setId(0); 155 optimizer.addVertex( pose ); 156 157 // 添加邊 158 int id=1; 159 for( Measurement m: measurements ) 160 { 161 g2o::EdgeSE3ProjectDirect* edge = new g2o::EdgeSE3ProjectDirect( 162 m.pos_world, 163 K(0,0), K(1,1), K(0,2), K(1,2), gray 164 ); 165 edge->setVertex( 0, pose ); 166 edge->setMeasurement( m.grayscale ); 167 edge->setInformation( Eigen::Matrix<double,1,1>::Identity() ); 168 edge->setId( id++ ); 169 optimizer.addEdge(edge); 170 } 171 cout<<"edges in graph: "<<optimizer.edges().size()<<endl; 172 optimizer.initializeOptimization(); 173 optimizer.optimize(10); 174 Tcw = pose->estimate(); 175 }
在這個實驗中，咱們讀取TUM數據集的兩對RGB-D圖像。而後，對第一張圖像提取FAST關鍵點（不須要描述子），並使用直接法估計這些關鍵點在第二個圖像中的位置，以及第二個圖像的相機位姿。這就構成了一種簡單的稀疏直接法。最後，咱們畫出這些關鍵點在第二個圖像中的投影。運行
build/direct_sparse ~/dataset/rgbd_dataset_freiburg1_desk
程序會在做圖以後暫停，您能夠特徵點的位置關係，也能夠看到迭代偏差的降低過程。圖：稀疏直接法的實驗。左：偏差隨着迭代降低。右：參考幀與後1至3幀對比（選取部分關鍵點）。
6. 直接法的討論
相比於特徵點法，直接法徹底依靠像優化來求解相機位姿。從式（\ref{eq:jacobianofDirect}）中能夠看到，像素梯度引導着優化的方向。若是咱們想要獲得正確的優化結果，就必須保證大部分像素梯度可以把優化引導到正確的方向。
這是什麼意思呢？咱們但願更深刻地理解直接法的作法。如今，咱們來扮演一下優化算法。假設對於參考圖像，咱們測量到一個灰度值爲229的像素。而且，因爲咱們知道它的深度，能夠推斷出空間點$P$的位置。
此時咱們又獲得了一張新的圖像，須要估計它的相機位姿。這個位姿是由一個初值再上不斷地優化迭代獲得的。假設咱們的初值比較差，在這個初值下，空間點$P$投影后的像素灰度值是126。因而，這個像素的偏差爲$229-126=103$，咱們但願微調相機的位姿，使像素更亮一些。
怎麼知道往哪裏微調，像素會更亮呢？這就須要用到像素梯度。咱們在圖像中發現，沿$u$軸往前走一步，該處的灰度值變成了123，即減去了3。一樣地，沿$v$軸往前走一步，灰度值減18，變成108。在這個像素周圍，咱們看到梯度是$[-3,-18]$，爲了提升亮度，咱們會建議優化算法微調相機，使$P$的像往左上方移動。因爲這個梯度是在局部求解的，這個移動量不能太大。
可是，優化算法不能只聽這個像素的一面之詞，還須要聽取其餘像素的建議。綜合聽取了許多像素的意見以後，優化算法選擇了一個和咱們建議的方向偏離不遠的地方，計算出一個更新量$\exp ({\mathbf{\xi}^\wedge } )$。加上更新量後，圖像從$I_2$移動到了$I_2'$，像素的投影位置也變到了一個更亮的地方。咱們看到，經過此次更新，偏差變小了。在理想狀況下，咱們指望偏差會不斷降低，最後收斂。
可是實際是否是這樣呢？咱們是否真的只要沿着梯度方向走，就能走到一個最優值？注意到，直接法的梯度是直接由圖像梯度肯定的，所以咱們必須保證沿着圖像梯度走時，灰度偏差會不斷降低。然而，圖像一般是一個很強烈的非凸函數，以下圖所示。實際當中，若是咱們沿着圖像梯度前進，很容易因爲圖像自己的非凸性（或噪聲）落進一個局部極小值中，沒法繼續優化。只有當相機運動很小，圖像中的梯度不會有很強的非凸性時，直接法才能成立。
在例程中，咱們只計算了單個像素的差別，而且這個差別是由灰度直接相減獲得的。然而，單個像素沒有什麼區分性，周圍極可能有好多像素和它的亮度差很少。因此，咱們有時會使用小的圖像塊（patch），而且使用更復雜的差別度量方式，例如歸一化相關性（Normalized Cross Correlation，NCC）等。而例程爲了簡單起見，使用了偏差的平方和，以保持和推導的一致性。
7. 直接法的優缺點總結
最後，咱們總結一下直接法的優缺點。大致來講，它的優勢以下：
- 能夠省去計算特徵點、描述子的時間。
- 只要求有像素梯度便可，無須特徵點。所以，直接法能夠在特徵缺失的場合下使用。比較極端的例子是隻有漸變的一張圖像。它可能沒法提取角點類特徵，但能夠用直接法估計它的運動。
- 能夠構建半稠密乃至稠密的地圖，這是特徵點法沒法作到的。
另外一方面，它的缺點也很明顯：
- 非凸性——直接法徹底依靠梯度搜索，下降目標函數來計算相機位姿。其目標函數中須要取像素點的灰度值，而圖像是強烈非凸的函數。這使得優化算法容易進入極小，只在運動很小時直接法才能成功。
- 單個像素沒有區分度。找一個和他像的實在太多了！——因而咱們要麼計算圖像塊，要麼計算複雜的相關性。因爲每一個像素對改變相機運動的「意見」不一致。只能少數服從多數，以數量代替質量。
- 灰度值不變是很強的假設。若是相機是自動曝光的，當它調整曝光參數時，會使得圖像總體變亮或變暗。光照變化時亦會出現這種狀況。特徵點法對光照具備必定的容忍性，而直接法因爲計算灰度間的差別，總體灰度變化會破壞灰度不變假設，使算法失敗。
直接法就是這樣一種優缺點都很是明顯的方法，你有沒有愛上它呢？
- 1. 直接插入法
- 2. 直接插入排序法
- 3. 視覺SLAM--直接法
- 4. ALLEGRO 直接轉PADS方法
- 5. excel沒法直接發送
- 6. 反向直接點法
- 7. string的加法（直接拼接）
- 8. 視覺slam直接法，特徵點法
- 9. maven直接直接依賴問題。
- 10. 排序算法--------直接插入排序
- 更多相關文章...
- • PHP imagecharup - 垂直地畫一個字符 - PHP參考手冊
- • PHP 連接 MySQL - PHP教程
- • 算法總結-回溯法
- • 算法總結-廣度優先算法
-
每一个你不满意的现在，都有一个你没有努力的曾经。
- 1. 直接插入法
- 2. 直接插入排序法
- 3. 視覺SLAM--直接法
- 4. ALLEGRO 直接轉PADS方法
- 5. excel沒法直接發送
- 6. 反向直接點法
- 7. string的加法（直接拼接）
- 8. 視覺slam直接法，特徵點法
- 9. maven直接直接依賴問題。
- 10. 排序算法--------直接插入排序