直接法

 前言

　　直接法是視覺里程計另一主要分支，它與特征點法有很大不同。隨着SVO、LSD-SLAM等直接法SLAM方案的流行，直接法本身也得到越來越多的關注。特征點法與直接究竟誰更好一些，是近年視覺里程計研究領域一個非常有趣的問題。本講，我們將介紹直接法的原理，並利用g2o實現基於直接法的視覺里程計。

---

 1. 直接法的引出

　　盡管特征點法在視覺里程計中占據主流地位，研究者們認識它至少有以下幾個缺點：

1. 關鍵點的提取與描述子的計算非常耗時。實踐當中，SIFT目前在CPU上是無法實時計算的，而ORB也需要近20毫秒的計算。如果整個SLAM以30毫秒/幀的速度運行，那么一大半時間都花在計算特征點上。
2. 使用特征點時，忽略了除特征點以外的所有信息。一張圖像有幾十萬個像素，而特征點只有幾百個。只使用特征點丟棄了大部分可能有用的圖像信息。
3. 相機有時會運動到特征缺失的地方，往往這些地方都沒有什么明顯的紋理信息。例如，有時我們會面對一堵白牆，或者一個空盪盪的走廓。這些場景下特征點數量會明顯減少，我們可能找不到足夠的匹配點來計算相機運動。

　　我們看到使用特征點確實存在一些問題。針對這些缺點，也存在若干種可行的方法：

1. 只計算關鍵點，不計算描述子。同時，使用光流法（Optical Flow）來跟蹤特征點的運動。這樣可以回避計算和匹配描述子帶來的時間，但光流本身的計算需要一定時間；
2. 只計算關鍵點，不計算描述子。同時，使用直接法來計算特征點在下一時刻圖像的位置。這同樣可以跳過描述子的計算過程，而且直接法的計算更加簡單。
3. 既不計算關鍵點、也不計算描述子——根據像素來直接計算相機運動。

　　第一種方法仍然使用特征點，只是把匹配描述子替換成了光流跟蹤，估計相機運動時仍使用PnP或ICP算法。而在，后兩個方法中，我們會根據圖像的像素信息來計算相機運動，它們稱為直接法。
　　使用特征點法估計相機運動時，我們把特征點看作固定在三維空間的不動點。根據它們在相機中的投影位置，通過最小化重投影誤差（Reprojection error）來優化相機運動。在這個過程中，我們需要精確地知道空間點在兩個相機中投影后的像素位置——這也就是我們為何要對特征進行匹配或跟蹤的理由。而在直接法中，我們最小化的不再是重投影誤差，而是測量誤差（Phometric error）。
　　直接法是本講介紹的重點。它的存在就是為了克服特征點法的上述缺點（雖然它會引入另一些問題）。直接法直接根據像素亮度信息，估計相機的運動，可以完全不用計算關鍵點和描述子。於是，直接法既避免了特征的計算時間，也避免了特征缺失的情況。只要場景中存在明暗變化（可以是漸變，不形成局部的圖像特征），直接法就能工作。根據使用像素的數量，直接法分為稀疏、稠密和半稠密三種，具有恢復稠密結構的能力。相比於特征點法通常只能重構稀疏特征點，直接法和稠密重建有更緊密的聯系。
　　歷史上，雖然早期也有一些對直接法的使用，但直到RGB-D相機出現后，人們才發現直接法對RGB-D相機，進而對單目相機，都是行之有效的方法。隨着一些使用直接法的開源項目的出現（如SVO、LSD-SLAM等），它們逐漸地走上主流舞台，成為視覺里程計算法中重要的一部分。

---

 2. 直接法數學推導

　　直接法和光流非常相似，它們都是基於灰度不變假設的：

灰度不變假設：同一個空間點的像素灰度，在各個圖像中是固定不變的。

　　灰度不變假設是一個很強的假設，實際當中很可能不成立。事實上，由於物體的材質不同，像素會出現高光和陰影部分；有時，相機會自動調整曝光參數，使得圖像整體變亮或變暗。這些時候灰度不變假設都是不成立的，因此直接法/光流的結果也不一定可靠。不過，暫且讓我們認為該假設成立，從而看看如何計算相機的運動。我們先介紹直接法的原理，然后使用g2o實現直接法。

　　

　　考慮某個空間點 $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}\]

　　其中 $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}\]

　　我們看到，一階導數由於鏈式法則分成了三項，而這三項都是容易計算的：

1. $ \partial \mathbf{I}_2 / \partial \mathbf{u} $ 為$\mathbf{u}$處的像素梯度；
2.  $ \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}\]
3. ${\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$的來源，我們可以把直接法進行分類：

1. $P$來自於稀疏特征點，我們稱之為稀疏直接法。通常我們使用數百個特征點，並且會像L-K光流那樣，假設它周圍像素也是不變的。這種稀疏直接法速度不必計算描述子，並且只使用數百個像素，因此速度最快，但只能計算稀疏的重構。
2. $P$來自部分像素。我們看到式（\ref{eq:jacobianofDirect}）中，如果像素梯度為零，整一項雅可比就為零，不會對計算運動增量有任何貢獻。因此，可以考慮只使用帶有梯度的像素點，舍棄像素梯度不明顯的地方。這稱之為半稠密（Semi-Dense）的直接法，可以重構一個半稠密結構。
3. $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里將它們編譯了成一個庫。

#ifndef G2O_TYPES_DIRECT_H_
#define G2O_TYPES_DIRECT_H_

// 定義應用於直接法的g2o邊
#include <Eigen/Geometry>
#include <g2o/core/base_unary_edge.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <opencv2/core/core.hpp>
namespace g2o
{
// project a 3d point into an image plane, the error is photometric error 
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    EdgeSE3ProjectDirect() {}
    
    EdgeSE3ProjectDirect( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
    : x_world_(point), fx_(fx), fy_(fy), cx_(cx), cy_(cy), image_(image)
    {}
    
    virtual void computeError(); 
    
    // plus in manifold 
    virtual void linearizeOplus( ); 
    
    // dummy read and write functions because we don't care... 
    virtual bool read( std::istream& in );
    virtual bool write( std::ostream& out ) const ;
    
protected:
    float getPixelValue( float x, float y ); 
public:
    // 3d point in world frame 
    Eigen::Vector3d x_world_; 
    // Pinhole camera intrinsics 
    float cx_=0, cy_=0, fx_=0, fy_=0; 
    // the reference image 
    cv::Mat* image_=nullptr;
};
} // namespace g2o 
#endif // G@O_TYPES_DIRECT_H_

 　　我們的邊繼承自g2o::BaseUnaryEdge。在繼承時，需要在模板參數里填入測量值的維度、類型，以及連接此邊的頂點，同時，我們把空間點$P$、相機內參和圖像存儲在該邊的成員變量中。為了讓g2o優化該邊對應的誤差，我們需要覆寫兩個虛函數：用computeError()計算誤差值，用linearizeOplus()計算雅可比。其余的存儲和讀取函數，由於本次實驗不關心，就略去了。下面我們給出這兩個函數的實現：

#include "g2o_types.h"
#include <g2o/core/factory.h>

#include <iostream>

using namespace std; 

namespace g2o
{
    
G2O_REGISTER_TYPE(EDGE_PROJECT_SE3_DIRECT, EdgeSE3ProjectDirect );

// compute the photometric error
void EdgeSE3ProjectDirect::computeError()
{
    const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
    Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
    float x = x_local[0]*fx_/x_local[2] + cx_;
    float y = x_local[1]*fy_/x_local[2] + cy_;
    // check x,y is in the image
    if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        _error(0,0) = 999.0;
    else
    {
        _error(0,0) = getPixelValue(x,y) - _measurement;
    }
}

// the bilinear interpolated pixel value
float EdgeSE3ProjectDirect::getPixelValue ( float x, float y )
{
    uchar* data = & image_->data[ int(y) * image_->step + int(x) ];
    float xx = x - floor ( x );
    float yy = y - floor ( y );
    float v = (
                 ( 1-xx ) * ( 1-yy ) * data[0] +
                 xx* ( 1-yy ) * data[1] +
                 ( 1-xx ) *yy*data[ image_->step ] +
                 xx*yy*data[image_->step+1]
             );
    return v;
}

// plus in manifold
void EdgeSE3ProjectDirect::linearizeOplus()
{
    VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );

    Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double invz = 1.0/xyz_trans[2];
    double invz_2 = invz*invz;

    float u = x*fx_*invz + cx_;
    float v = y*fy_*invz + cy_;
    
    // jacobian from se3 to u,v
    // note that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
    Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

    jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
    jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
    jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
    jacobian_uv_ksai ( 0,3 ) = invz *fx_;
    jacobian_uv_ksai ( 0,4 ) = 0;
    jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

    jacobian_uv_ksai ( 1,0 ) = -( 1+y*y*invz_2 ) *fy_;
    jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
    jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
    jacobian_uv_ksai ( 1,3 ) = 0;
    jacobian_uv_ksai ( 1,4 ) = invz *fy_;
    jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

    Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
    
    jacobian_pixel_uv ( 0,0 ) = ( getPixelValue(u+1,v)-getPixelValue(u,v) );
    jacobian_pixel_uv ( 0,1 ) = ( getPixelValue(u,v+1)-getPixelValue(u,v) );
    
    _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}
bool EdgeSE3ProjectDirect::read( std::istream& in )
{
    return true;
}

bool EdgeSE3ProjectDirect::write( std::ostream& out ) const
{
    return true;
}


}// 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中，請讀者閱讀該部分代碼並編譯它。我們將代碼編譯的步驟留作習題吧（一臉賤笑中）。

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "g2o_types.h"
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;

// 一次測量的值，包括一個世界坐標系下三維點與一個灰度值
struct Measurement
{
    Measurement( Eigen::Vector3d p, float g ): pos_world(p), grayscale(g) {}
    Eigen::Vector3d pos_world;
    float grayscale;
};

inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
    float zz = float ( d ) /scale;
    float xx = zz* ( x-cx ) /fx;
    float yy = zz* ( y-cy ) /fy;
    return Eigen::Vector3d ( xx, yy, zz );
}

inline Eigen::Vector2d project3Dto2D( float x, float y, float z, float fx, float fy, float cx, float cy )
{
    float u = fx*x/z+cx;
    float v = fy*y/z+cy;
    return Eigen::Vector2d(u,v);
}

// 直接法估計位姿
// 輸入：測量值（空間點的灰度），新的灰度圖，相機內參； 輸出：相機位姿
// 返回：true為成功，false失敗
bool poseEstimationDirect( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    srand( (unsigned int) time(0) );
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin ( associate_file );

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, gray;
    vector<Measurement> measurements;
    // 相機內參
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity(); 

    cv::Mat prev_color;
    //  我們演示兩個圖像間的直接法計算
    for ( int index=0; index<2; index++ )
    {
        cout<<"*********** loop "<<index<<" ************"<<endl;
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
        color = cv::imread ( path_to_dataset+"/"+rgb_file );
        depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
        if ( index ==0 )
        {
            // 對第一幀提取FAST特征點
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect ( color, keypoints );
            for ( auto kp:keypoints )
            {
                // 去掉鄰近邊緣處的點
                if ( kp.pt.x < 20 || kp.pt.y < 20 || (kp.pt.x+20)>color.cols || (kp.pt.y+20)>color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( int( kp.pt.y ) ) [ int(kp.pt.x) ];
                if ( d==0 )
                    continue;
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                float grayscale = float ( gray.ptr<uchar> ( int(kp.pt.y) ) [ int(kp.pt.x) ] );
                measurements.push_back ( Measurement( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }
        // 使用直接法計算相機運動及投影點
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect( measurements, &gray, K, Tcw );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count()<<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix()<<endl;
        
        // plot the feature points 
        cv::Mat img_show( color.rows, color.cols*2, CV_8UC3 );
        prev_color.copyTo( img_show( cv::Rect(0,0,color.cols, color.rows) ) );
        color.copyTo( img_show(cv::Rect(color.cols,0,color.cols, color.rows)) );
        for ( Measurement m:measurements )
        {
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D( p(0,0), p(1,0), p(2,0), fx, fy, cx, cy );
            Eigen::Vector3d p2 = Tcw*m.pos_world;
            Eigen::Vector2d pixel_now = project3Dto2D( p2(0,0), p2(1,0), p2(2,0), fx, fy, cx, cy );
            
            float b = 255*float(rand())/RAND_MAX;
            float g = 255*float(rand())/RAND_MAX;
            float r = 255*float(rand())/RAND_MAX;
            cv::circle(img_show, cv::Point2d(pixel_prev(0,0), pixel_prev(1,0)), 5, cv::Scalar(b,g,r),1 );
            cv::circle(img_show, cv::Point2d(pixel_now(0,0)+color.cols, pixel_now(1,0)), 5, cv::Scalar(b,g,r),1 );
            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 );
        }
        cv::imshow( "result", img_show );
        cv::waitKey(0);
        
    }
    return 0;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock( linearSolver );
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    g2o::SparseOptimizer optimizer; 
    optimizer.setAlgorithm( solver );
    optimizer.setVerbose( true );       // 打開調試輸出
    
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); 
    pose->setEstimate( g2o::SE3Quat(Tcw.rotation(), Tcw.translation()) );
    pose->setId(0);
    optimizer.addVertex( pose );
    
    // 添加邊
    int id=1;
    for( Measurement m: measurements )
    {
        g2o::EdgeSE3ProjectDirect* edge = new g2o::EdgeSE3ProjectDirect(
            m.pos_world,
            K(0,0), K(1,1), K(0,2), K(1,2), gray
        );
        edge->setVertex( 0, pose );
        edge->setMeasurement( m.grayscale );
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId( id++ );
        optimizer.addEdge(edge);
    }
    cout<<"edges in graph: "<<optimizer.edges().size()<<endl;
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    Tcw = pose->estimate();
}

 　　在這個實驗中，我們讀取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. 直接法的優缺點總結

　　最后，我們總結一下直接法的優缺點。大體來說，它的優點如下：

• 可以省去計算特征點、描述子的時間。
• 只要求有像素梯度即可，無須特征點。因此，直接法可以在特征缺失的場合下使用。比較極端的例子是只有漸變的一張圖像。它可能無法提取角點類特征，但可以用直接法估計它的運動。
• 可以構建半稠密乃至稠密的地圖，這是特征點法無法做到的。

　　另一方面，它的缺點也很明顯：

• 非凸性——直接法完全依靠梯度搜索，降低目標函數來計算相機位姿。其目標函數中需要取像素點的灰度值，而圖像是強烈非凸的函數。這使得優化算法容易進入極小，只在運動很小時直接法才能成功。
• 單個像素沒有區分度。找一個和他像的實在太多了！——於是我們要么計算圖像塊，要么計算復雜的相關性。由於每個像素對改變相機運動的“意見”不一致。只能少數服從多數，以數量代替質量。
• 灰度值不變是很強的假設。如果相機是自動曝光的，當它調整曝光參數時，會使得圖像整體變亮或變暗。光照變化時亦會出現這種情況。特征點法對光照具有一定的容忍性，而直接法由於計算灰度間的差異，整體灰度變化會破壞灰度不變假設，使算法失敗。

　　直接法就是這樣一種優缺點都非常明顯的方法，你有沒有愛上它呢？