點“計算機視覺life”關注，置頂更快接收消息！
小白：師兄，g2o框架《從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼》，以及頂點《從零開始一起學習SLAM | 掌握g2o頂點編程套路》我都學完啦，今天給我講講g2o中的邊吧！是不是也有什么套路？
師兄：嗯，g2o的邊比頂點稍微復雜一些，不過前面你也了解了許多g2o的東西，有沒有發現g2o的編程基本都是固定的格式（套路）呢？
小白：是的，我現在按照師兄說的g2o框架和頂點設計方法，再去看g2o實現不同功能的代碼，發現都是一個模子出來的，只不過在某些地方稍微改改就行了啊
師兄：是這樣的。我們來看看g2o的邊到底是咋回事。
初步認識g2o的邊
師兄：在《g2o: A general Framework for (Hyper) Graph Optimization》這篇文檔里，我們找到那張經典的類結構圖，里面關於邊（edge）的部分是這樣的，重點是下圖中紅色框內。
上一次我們講頂點的時候，還專門去追根溯源查找頂點類之間的繼承關系，邊其實也是類似的，我們在g2o官方GitHub上這些
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h
頭文件下就能看到這些繼承關系了，我們就不像之前頂點那樣一個個去追根溯源了，如果有興趣你可以自己去試試看。我們主要關注一下上面紅框內的三種邊。
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分別表示一元邊，兩元邊，多元邊。
小白：他們有啥區別啊？
師兄：一元邊你可以理解為一條邊只連接一個頂點，兩元邊理解為一條邊連接兩個頂點，也就是我們常見的邊啦，多元邊理解為一條邊可以連接多個（3個以上）頂點
一個比較丑的示例
下面我們來看看他們的參數有什么區別？你看主要就是 幾個參數：D, E, VertexXi, VertexXj，他們的分別代表：
D 是 int 型，表示測量值的維度 （dimension）
E 表示測量值的數據類型
VertexXi，VertexXj 分別表示不同頂點的類型
比如我們用邊表示三維點投影到圖像平面的重投影誤差，就可以設置輸入參數如下：
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
你說說看 這個定義是什么意思？
小白：首先這個是個二元邊。第1個2是說測量值是2維的，也就是圖像像素坐標x,y的差值，對應測量值的類型是Vector2D，兩個頂點也就是優化變量分別是三維點 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap？
師兄：對的，就是這樣~當然除了輸入參數外，定義邊我們通常需要復寫一些重要的成員函數
小白：聽着和頂點類似哦，也是復寫成員函數，頂點里主要復寫了頂點更新函數oplusImpl和頂點重置函數setToOriginImpl，邊的話是不是也差不多？
師兄：邊和頂點的成員函數還是差別比較大的，邊主要有以下幾個重要的成員函數
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();
下面簡單解釋一下
read，write：分別是讀盤、存盤函數，一般情況下不需要進行讀/寫操作的話，僅僅聲明一下就可以
computeError函數：非常重要，是使用當前頂點的值計算的測量值與真實的測量值之間的誤差
linearizeOplus函數：非常重要，是在當前頂點的值下，該誤差對優化變量的偏導數，也就是我們說的Jacobian
除了上面幾個成員函數，還有幾個重要的成員變量和函數也一並解釋一下：
_measurement：存儲觀測值
_error：存儲computeError() 函數計算的誤差
_vertices[]：存儲頂點信息，比如二元邊的話，_vertices[] 的大小為2，存儲順序和調用setVertex(int, vertex) 是設定的int 有關（0 或1）
setId(int)：來定義邊的編號（決定了在H矩陣中的位置）
setMeasurement(type) 函數來定義觀測值
setVertex(int, vertex) 來定義頂點
setInformation() 來定義協方差矩陣的逆
后面我們寫代碼的時候回經常遇到他們的。
如何自定義g2o的邊？
小白：前面你介紹了g2o中邊的基本類型、重要的成員變量和成員函數，那么如果我們要定義邊的話，具體如何編程呢？
師兄：我這里正好有個模板給你看看，基本上定義g2o中的邊，就是如下套路：
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){}
virtual bool read(istream& in) {}
virtual bool write(ostream& out) const {}
virtual void computeError() override
{
// ...
_error = _measurement - Something;
}
virtual void linearizeOplus() override
{
_jacobianOplusXi(pos, pos) = something;
// ...
/*
_jocobianOplusXj(pos, pos) = something;
...
*/
}
private:
// data
}
我們可以發現，最重要的就是computeError()，linearizeOplus()兩個函數了
小白：嗯，看起來好像也不難啊
師兄：我們先來看一個簡單例子，地址在
https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp
這個是個一元邊，主要是定義誤差函數了，如下所示，你可以發現這個例子基本就是上面例子的一丟丟擴展，是不是感覺so easy？
// 誤差模型 模板參數：觀測值維度，類型，連接頂點類型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 計算曲線模型誤差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值為 _measurement
};
小白：嗯，這個能看懂
師兄：下面是一個復雜一點例子，3D-2D點的PnP 問題，也就是最小化重投影誤差問題，這個問題非常常見，使用最常見的二元邊，弄懂了這個基本跟邊相關的代碼也差不多都一通百通了
代碼在g2o的GitHub上這個地方可以看到
g2o/types/sba/types_six_dof_expmap.h
這里根據自己理解對代碼加了注釋，方便理解
//繼承了BaseBinaryEdge類，觀測值是2維，類型Vector2D,頂點分別是三維點、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//1. 默認初始化
EdgeProjectXYZ2UV();
//2. 計算誤差
void computeError() {
//李群相機位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 頂點v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
//相機參數
const CameraParameters * cam
= static_cast<const CameraParameters *>(parameter(0));
//誤差計算，測量值減去估計值，也就是重投影誤差obs-cam
//估計值計算方法是T*p,得到相機坐標系下坐標，然后在利用camera2pixel()函數得到像素坐標。
Vector2D obs(_measurement);
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
}
//3. 線性增量函數，也就是雅克比矩陣J的計算方法
virtual void linearizeOplus();
//4. 相機參數
CameraParameters * _cam;
bool read(std::istream& is);
bool write(std::ostream& os) const;
};
有一個地方比較難理解
_error = obs - cam->cam_map(v1->estimate().map(v2->estimate()));
小白：我確實看不懂這一句。。
師兄：其實就是：誤差 = 觀測 - 投影
下面我給你捋捋思路。我們先來看看cam_map 函數，它的定義在
g2o/types/sba/types_six_dof_expmap.cpp
cam_map 函數功能是把相機坐標系下三維點（輸入）用內參轉換為圖像坐標（輸出），具體代碼如下所示
Vector2 CameraParameters::cam_map(const Vector3 & trans_xyz) const {
Vector2 proj = project2d(trans_xyz);
Vector2 res;
res[0] = proj[0]*focal_length + principle_point[0];
res[1] = proj[1]*focal_length + principle_point[1];
return res;
}
然后看 .map函數，它的功能是把世界坐標系下三維點變換到相機坐標系，函數在
g2o/types/sim3/sim3.h
具體定義是
Vector3 map (const Vector3& xyz) const {
return s*(r*xyz) + t;
}
因此下面這個代碼
v1->estimate().map(v2->estimate())
就是用V1估計的pose把V2代表的三維點，變換到相機坐標系下。
小白：原來如此，以前我都忽視了這些東西了，沒想到里面是這樣的關聯的。
師兄：嗯，我們繼續，前面主要是對computeError() 的理解，還有一個很重要的函數就是linearizeOplus()，用來定義雅克比矩陣
我摘取了相關代碼（來自：g2o/g2o/types/sba/types_six_dof_expmap.cpp），並進行了標注，相信會更容易理解
十四講第169頁中的雅克比矩陣完全是按照書上 式子（7.45）、（7.47）來編程的，不難理解
小白：后面就是直接照抄書上就行，哈哈
如何向圖中添加邊？
師兄：前面我們講過如何往圖中增加頂點，可以說非常easy了，往圖中增加邊會稍微多一些內容，我們還是先從最簡單的 例子說起：一元邊的添加方法
下面代碼來自GitHub上，仍然是前面曲線擬合的例子
slambook/ch6/g2o_curve_fitting/main.cpp
// 往圖中增加邊
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 設置連接的頂點
edge->setMeasurement( y_data[i] ); // 觀測數值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協方差矩陣之逆
optimizer.addEdge( edge );
}
小白：setMeasurement 函數的輸入的觀測值具體是指什么？
師兄：對於這個曲線擬合，觀測值就是實際觀測到的數據點。對於視覺SLAM來說，通常就是我們我們觀測到的特征點坐標，下面就是一個例子。這個例子比剛才的復雜一點，因為它是二元邊，需要用邊連接兩個頂點
代碼來自GitHub上
slambook/ch7/pose_estimation_3d2d.cpp
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
小白：這里的setMeasurement函數里的p來自向量points_2d，也就是特征點的圖像坐標(x,y)了吧！
師兄：對，這正好呼應我剛才說的。另外，你看setVertex 有兩個一個是 0 和 VertexSBAPointXYZ 類型的頂點，一個是1 和pose。你覺得這里的0和1是什么意思？能否互換呢？
小白：0，1應該是分別指代哪個頂點吧，直覺告訴我不能互換，可能得去查查頂點定義部分的代碼
師兄：你的直覺沒錯！我幫你 查過啦，你看這個是setVertex在g2o官網的定義：
// set the ith vertex on the hyper-edge to the pointer supplied
void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}
這段代碼在
g2o/core/hyper_graph.h
里可以找到。你看 _vertices[i] 里的i就是我們這里的0和1，我們再去看看這里邊的類型： g2o::EdgeProjectXYZ2UV
的定義，前面我們也放出來了，就這兩句
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
.....
//李群相機位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 頂點v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
你看 _vertices[0] 對應的是 VertexSBAPointXYZ 類型的頂點，也就是三維點，_vertices[1] 對應的是VertexSE3Expmap 類型的頂點，也就是位姿pose。因此前面 1 對應的就應該是 pose，0對應的 應該就是三維點。
小白：原來如此，之前都沒注意這些，看來g2o不會幫我區分頂點的類型啊，以后這里編程要對應好，不然錯了都找不到原因呢！謝謝師兄，今天又是收獲滿滿的一天！
練習
題目：用直接法Bundle Adjustment 估計相機位姿。給定3張圖片，兩個txt文件，其中poses.txt中存儲3張圖片對應的相機初始位姿（Tcw），格式為：timestamp, tx, ty, tz, qx, qy, qz, qw ，分別對應時間戳、平移、旋轉（四元數），而points.txt中存儲的是3D點集合以及該點周圍 4x4 窗口的灰度值，記做 I(p)i，格式為：
x, y, z, 灰度1，灰度2...，灰度16
我們把每個3D點投影到對應圖像中，用投影后點周圍的灰度值與原始窗口的灰度值差異作為待優化誤差。
請使用g2o進行優化，並繪制結果（繪制函數已經寫好）。
代碼框架中需要你填寫頂點、邊的定義。如果正確，輸出結果如下圖所示：
參考：
高翔《視覺 SLAM十四講》
https://blog.csdn.net/try_again_later/article/details/81813639
代碼框架、數據、窗口值具體順序、優化目標函數、預期輸出結果已經為你准備好了，公眾號「計算機視覺life」后台回復：邊，即可獲得。
歡迎留言討論，更多學習視頻、文檔資料、參考答案等關注計算機視覺life公眾號，，菜單欄點擊“知識星球”查看「從零開始學習SLAM」星球介紹，快來和其他小伙伴一起學習交流~
推薦閱讀
從零開始一起學習SLAM | 為什么要學SLAM？
從零開始一起學習SLAM | 學習SLAM到底需要學什么？
從零開始一起學習SLAM | SLAM有什么用？
從零開始一起學習SLAM | C++新特性要不要學？
從零開始一起學習SLAM | 為什么要用齊次坐標？
從零開始一起學習SLAM | 三維空間剛體的旋轉
從零開始一起學習SLAM | 為啥需要李群與李代數？
從零開始一起學習SLAM | 相機成像模型
從零開始一起學習SLAM | 不推公式，如何真正理解對極約束?
從零開始一起學習SLAM | 神奇的單應矩陣
從零開始一起學習SLAM | 你好，點雲
從零開始一起學習SLAM | 給點雲加個濾網
從零開始一起學習SLAM | 點雲平滑法線估計
從零開始一起學習SLAM | 點雲到網格的進化
從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼
從零開始一起學習SLAM | 掌握g2o頂點編程套路
零基礎小白，如何入門計算機視覺？
SLAM領域牛人、牛實驗室、牛研究成果梳理
我用MATLAB擼了一個2D LiDAR SLAM
可視化理解四元數，願你不再掉頭發
最近一年語義SLAM有哪些代表性工作？
視覺SLAM技術綜述
匯總 | VIO、激光SLAM相關論文分類集錦
研究SLAM，對編程的要求有多高？
2018年SLAM、三維視覺方向求職經驗分享
深度學習遇到SLAM | 如何評價基於深度學習的DeepVO，VINet，VidLoc？