G2O整理

整體內(nèi)容參考：

從零開始一起學習SLAM | 理解圖優(yōu)化，一步步帶你看懂g2o代碼

從零開始一起學習SLAM | 掌握g2o頂點編程套路

從零開始一起學習SLAM | 掌握g2o邊的代碼套路

什么是圖優(yōu)化

什么是圖？

圖優(yōu)化里的圖就是數(shù)據(jù)結(jié)構(gòu)里的圖，一個圖由若干個頂點（vertex），以及連接這些頂點的邊（edge）組成，給你舉個例子

比如一個機器人在房屋里移動，它在某個時刻 t 的位姿（pose）就是一個頂點，這個也是待優(yōu)化的變量。而位姿之間的關系就構(gòu)成了一個邊，比如時刻 t 和時刻 t+1 之間的相對位姿變換矩陣就是邊，邊通常表示誤差項。

在SLAM里，圖優(yōu)化一般分解為兩個任務：

1、構(gòu)建圖。機器人位姿作為頂點，位姿間關系作為邊。

2、優(yōu)化圖。調(diào)整機器人的位姿（頂點）來盡量滿足邊的約束，使得誤差最小。

PNP中圖優(yōu)化的例子

G2O整體結(jié)構(gòu)圖

1. 圖的核心

SparseOptimizer是整個圖的核心，注意右上角的 is-a 實心箭頭，這個SparseOptimizer它是一個Optimizable Graph，從而也是一個超圖（HyperGraph）

暫時只需要了解一下它們的名字，有些以后用不到，有些以后用到了再回看。

2. 頂點和邊

注意看 has-many 箭頭，你看這個超圖包含了許多頂點（HyperGraph::Vertex）和邊（HyperGraph::Edge）。而這些頂點繼承自 Base Vertex，也就是OptimizableGraph::Vertex，而邊可以繼承自 BaseUnaryEdge（單邊）, BaseBinaryEdge（雙邊）或BaseMultiEdge（多邊），它們都叫做OptimizableGraph::Edge

3. 配置SparseOptimizer的優(yōu)化算法和求解器

整個圖的核心SparseOptimizer 包含一個優(yōu)化算法（OptimizationAlgorithm）的對象。OptimizationAlgorithm是通過OptimizationWithHessian 來實現(xiàn)的。其中迭代策略可以從Gauss-Newton（高斯牛頓法，簡稱GN）, Levernberg-Marquardt（簡稱LM法）, Powell’s dogleg 三者中間選擇一個（我們常用的是GN和LM）

4. 求解

OptimizationWithHessian 內(nèi)部包含一個求解器（Solver），這個Solver實際是由一個BlockSolver組成的。這個BlockSolver有兩個部分，一個是SparseBlockMatrix ，用于計算稀疏的雅可比和Hessian矩陣；一個是線性方程的求解器（LinearSolver），它用于計算迭代過程中最關鍵的一步HΔx=?b，LinearSolver有幾種方法可以選擇：PCG, CSparse, Choldmod

編程順序

采用十四講中g(shù)2o求解曲線參數(shù)的例子來說明：

代碼

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每個誤差項優(yōu)化變量維度為3，誤差值維度為1// 第1步：創(chuàng)建一個線性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 第2步：創(chuàng)建BlockSolver。并用上面定義的線性求解器初始化
Block* solver_ptr = new Block( linearSolver );      // 第3步：創(chuàng)建總求解器solver。并從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// 第4步：創(chuàng)建終極大boss 稀疏優(yōu)化器（SparseOptimizer）
g2o::SparseOptimizer optimizer;     // 圖模型
optimizer.setAlgorithm( solver );   // 設置求解器
optimizer.setVerbose( true );       // 打開調(diào)試輸出// 第5步：定義圖的頂點和邊。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往圖中增加頂點
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
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] );      // 觀測數(shù)值edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協(xié)方差矩陣之逆optimizer.addEdge( edge );
}// 第6步：設置優(yōu)化參數(shù)，開始執(zhí)行優(yōu)化
optimizer.initializeOptimization();
optimizer.optimize(100);

求解步驟

1. 創(chuàng)建線性求解器LinearSolver

我們要求的增量方程的形式是：H△X=-b，通常情況下想到的方法就是直接求逆，也就是△X=-H.inv*b?？雌饋砗孟窈芎唵?#xff0c;但這有個前提，就是H的維度較小，此時只需要矩陣的求逆就能解決問題。但是當H的維度較大時，矩陣求逆變得很困難，求解問題也變得很復雜。

G2O上的求解方法總結(jié)：

• LinearSolverCholmod ：使用sparse cholesky分解法。繼承自LinearSolverCCS
• LinearSolverCSparse：使用CSparse法。繼承自LinearSolverCCS
• LinearSolverPCG ：使用preconditioned conjugate gradient 法，繼承自LinearSolver
• LinearSolverDense ：使用dense cholesky分解法。繼承自LinearSolver
• LinearSolverEigen： 依賴項只有eigen，使用eigen中sparse Cholesky 求解，因此編譯好后可以方便的在其他地方使用，性能和CSparse差不多。繼承自LinearSolver

2. 創(chuàng)建BlockSolver 并用上面定義的線性求解器初始化

BlockSolver 內(nèi)部包含 LinearSolver，用上面我們定義的線性求解器LinearSolver來初始化。

BlockSolver有兩種定義方式

一種是指定的固定變量的solver，我們來看一下定義

using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的維度（注意一定是流形manifold下的最小表示），l表示landmark的維度

另一種是可變尺寸的solver，定義如下

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

比較常用的幾種類型

• BlockSolver_6_3 ：表示pose 是6維，觀測點是3維。用于3D SLAM中的BA
• BlockSolver_7_3：在BlockSolver_6_3 的基礎上多了一個scale
• BlockSolver_3_2：表示pose 是3維，觀測點是2維

3. 創(chuàng)建總求解器solver。并從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化

Solver的優(yōu)化方法有三種：分別是高斯牛頓（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法

GN、 LM、 Doglet算法內(nèi)部，會發(fā)現(xiàn)他們都繼承自同一個類：OptimizationWithHessian

OptimizationAlgorithmWithHessian，發(fā)現(xiàn)它又繼承自OptimizationAlgorithm

總之，在該階段，我們可以選則三種方法：

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg 
g2o::OptimizationAlgorithmDogleg 

4. 創(chuàng)建終極大boss 稀疏優(yōu)化器（SparseOptimizer），并用已定義求解器作為求解方法。

創(chuàng)建稀疏優(yōu)化器

g2o::SparseOptimizer optimizer;

用前面定義好的求解器作為求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是設置優(yōu)化過程輸出信息用的

SparseOptimizer::setVerbose(bool verbose)

5. 定義圖的頂點和邊。 添加到SparseOptimizer中 （最重要的一步）

頂點（Vertex) 從哪里來的？

一步步來看吧。先來看看上圖中和vertex有關的第①個類： HyperGraph::Vertex，在g2o的GitHub上（https://github.com/RainerKuemmerle/g2o），它在這個路徑

g2o/core/hyper_graph.h

這個 HyperGraph::Vertex 是個abstract vertex，必須通過派生來使用。如下圖所示

然后我們看g2o 類結(jié)構(gòu)圖中第②個類，我們看到HyperGraph::Vertex 是通過類OptimizableGraph 來繼承的， 而OptimizableGraph的定義在

g2o/core/optimizable_graph.h

我們找到vertex定義，發(fā)現(xiàn)果然，OptimizableGraph 繼承自 HyperGraph，如下圖所示

不過，這個OptimizableGraph::Vertex 也非常底層，具體使用時一般都會進行擴展，因此g2o中提供了一個比較通用的適合大部分情況的模板。就是g2o 類結(jié)構(gòu)圖中 對應的第③個類：

BaseVertex

那么它在哪里呢？ 在這個路徑：

g2o/core/base_vertex.h

BaseVertex->OptimizableGraph->HyperGraph::Vertex

繼承關系是HyperGraph::Vertex繼承OptimizableGraph繼承BaseVertex

頂點（Vertex) 參數(shù)如何理解？

我們來看一下模板參數(shù) D 和 T，翻譯一下上圖紅框：

D是int 類型的，表示vertex的最小維度，比如3D空間中旋轉(zhuǎn)是3維的，那么這里 D = 3

T是待估計vertex的數(shù)據(jù)類型，比如用四元數(shù)表達三維旋轉(zhuǎn)的話，T就是Quaternion 類型

static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space

可以看到這個D并非是頂點（更確切的說是狀態(tài)變量）的維度，而是其在流形空間（manifold）的最小表示，這里一定要區(qū)別開，另外，源碼里面也給出了T的作用

typedef T EstimateType;
EstimateType _estimate;

可以看到，這里T就是頂點（狀態(tài)變量）的類型

如何自己定義頂點？

頂點的基本類型是 BaseVertex，那么下一步關心的就是如何使用了，因為在不同的應用場景（二維空間，三維空間），有不同的待優(yōu)化變量（位姿，空間點），還涉及不同的優(yōu)化類型（李代數(shù)位姿、李群位姿）

g2o本身內(nèi)部定義了一些常用的頂點類型

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) （注意，我們省略了四元數(shù)的w部分）
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
//SE3頂點內(nèi)部用變換矩陣參數(shù)化，外部用指數(shù)映射參數(shù)化
VertexSE3Expmap : public BaseVertex<6, SE3Quat>// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) （注意，我們省略了四元數(shù)的w部分）
//假設qw為正，否則qx、qy、qz作為旋轉(zhuǎn)存在歧義
VertexCam : public BaseVertex<6, SBACam>// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) 注意，我們省略了四元數(shù)的w部分
VertexSim3Expmap : public BaseVertex<7, Sim3>

當然我們可以直接用這些，但是有時候我們需要的頂點類型這里面沒有，就得自己定義了。

重新定義頂點一般需要考慮重寫如下函數(shù)：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

這幾個是主要要改的地方。我們來看一下他們都是什么意義：

read，write：分別是讀盤、存盤函數(shù)，一般情況下不需要進行讀/寫操作的話，僅僅聲明一下就可以

setToOriginImpl：頂點重置函數(shù)，設定被優(yōu)化變量的原始值。

oplusImpl：頂點更新函數(shù)。非常重要的一個函數(shù)，主要用于優(yōu)化過程中增量△x 的計算。我們根據(jù)增量方程計算出增量之后，就是通過這個函數(shù)對估計值進行調(diào)整的，因此這個函數(shù)的內(nèi)容一定要重視。

自己定義 頂點一般是下面的格式：
當我們使用Eigen時，在類中需要重載內(nèi)存分配的new delete函數(shù)時，加上這句話即可：EIGEN_MAKE_ALIGNED_OPERATOR_NEW ，比如：使用g2o優(yōu)化時，定義一個頂點，需要使用eigen中的一些底層函數(shù)

  class myVertex: public g2::BaseVertex<Dim, Type>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用時，加上此句，等于采用eigen中約定的方式重載了該類的new delete等內(nèi)存分配函數(shù)myVertex(){}virtual void read(std::istream& is) {}virtual void write(std::ostream& os) const {}virtual void setOriginImpl(){_estimate = Type();}virtual void oplusImpl(const double* update) override{_estimate += /*update*/;}}

先看一個簡單例子，來自十四講中的曲線擬合，來源如下

ch6/g2o_curve_fitting/main.cpp

// 曲線模型的頂點，模板參數(shù)：優(yōu)化變量維度和數(shù)據(jù)類型

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual void setToOriginImpl() // 重置{_estimate << 0,0,0;}virtual void oplusImpl( const double* update ) // 更新{_estimate += Eigen::Vector3d(update);}// 存盤和讀盤：留空virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
};

我們可以看到下面代碼中頂點初值設置為0，更新時也是直接把更新量 update 加上去的，知道為什么嗎？

小白：更新不就是 x + △x 嗎，這是定義吧

師兄：嗯，對于這個例子是可以直接加，因為頂點類型是Eigen::Vector3d，屬于向量，是可以通過加法來更新的。但是但是有些例子就不行，比如下面這個復雜點例子：李代數(shù)表示位姿VertexSE3Expmap

來自g2o官網(wǎng)，在這里

g2o/types/sba/types_six_dof_expmap.h

/**\* \brief SE3 Vertex parameterized internally with a transformation matrixand externally with its exponential map*/class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();bool read(std::istream& is);bool write(std::ostream& os) const;virtual void setToOriginImpl() {_estimate = SE3Quat();}virtual void oplusImpl(const number_t* update_) {Eigen::Map<const Vector6> update(update_);setEstimate(SE3Quat::exp(update)*estimate());    //更新方式}
};

第一個參數(shù)6 表示內(nèi)部存儲的優(yōu)化變量維度，這是個6維的李代數(shù)

第二個參數(shù)是優(yōu)化變量的類型，這里使用了g2o定義的相機位姿類型：SE3Quat。

在這里可以具體查看g2o/types/slam3d/se3quat.h

它內(nèi)部使用了四元數(shù)表達旋轉(zhuǎn)，然后加上位移來存儲位姿，同時支持李代數(shù)上的運算，比如對數(shù)映射（log函數(shù)）、李代數(shù)上增量（update函數(shù)）等操作

說完了，那我現(xiàn)在問你個問題，為啥這里更新時沒有像上面那樣直接加上去？

小白：這個表示位姿，好像是不能直接加的我記得，原因有點忘了

師兄：嗯，是不能直接加，原因是變換矩陣不滿足加法封閉。那我再問你，為什么相機位姿頂點類VertexSE3Expmap使用了李代數(shù)表示相機位姿，而不是使用旋轉(zhuǎn)矩陣和平移矩陣？

其實也是上述原因的拓展：這是因為旋轉(zhuǎn)矩陣是有約束的矩陣，它必須是正交矩陣且行列式為1。使用它作為優(yōu)化變量就會引入額外的約束條件，從而增大優(yōu)化的復雜度。而將旋轉(zhuǎn)矩陣通過李群-李代數(shù)之間的轉(zhuǎn)換關系轉(zhuǎn)換為李代數(shù)表示，就可以把位姿估計變成無約束的優(yōu)化問題，求解難度降低。

剛才是位姿的例子，下面是三維點的例子，空間點位置 VertexPointXYZ，維度為3，類型是Eigen的Vector3，比較簡單，就不解釋了

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW  VertexSBAPointXYZ();virtual bool read(std::istream& is);virtual bool write(std::ostream& os) const;virtual void setToOriginImpl() {_estimate.fill(0);}virtual void oplusImpl(const number_t* update){Eigen::Map<const Vector3> v(update);_estimate += v;}
};

如何向圖中添加頂點？

往圖中增加頂點比較簡單，我們還是先看看第一個曲線擬合的例子，setEstimate(type) 函數(shù)來設定初始值；setId(int) 定義節(jié)點編號

    // 往圖中增加頂點CurveFittingVertex* v = new CurveFittingVertex();v->setEstimate( Eigen::Vector3d(0,0,0) );v->setId(0);optimizer.addVertex( v );

這個是添加 VertexSBAPointXYZ 的例子，都很容易看懂

    int index = 1;for ( const Point3f p:points_3d )   // landmarks{g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();point->setId ( index++ );point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );point->setMarginalized ( true ); optimizer.addVertex ( point );}

+++

以上為設置頂點，以下為邊

+++

初步認識g2o的邊

上一次我們講頂點的時候，還專門去追根溯源查找頂點類之間的繼承關系，邊其實也是類似的，我們在g2o官方GitHub上這些
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

頭文件下就能看到這些繼承關系了

BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分別表示一元邊，兩元邊，多元邊。
一元邊你可以理解為一條邊只連接一個頂點，兩元邊理解為一條邊連接兩個頂點，也就是我們常見的邊啦，多元邊理解為一條邊可以連接多個（3個以上）頂點

下面我們來看看他們的參數(shù)有什么區(qū)別？你看主要就是 幾個參數(shù)：D, E, VertexXi, VertexXj，他們的分別代表：

D 是 int 型，表示測量值的維度 （dimension）
E 表示測量值的數(shù)據(jù)類型
VertexXi，VertexXj 分別表示不同頂點的類型

比如我們用邊表示三維點投影到圖像平面的重投影誤差，就可以設置輸入?yún)?shù)如下：

 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

首先這個是個二元邊。第1個2是說測量值是2維的，也就是圖像像素坐標x,y的差值，對應測量值的類型是Vector2D，兩個頂點也就是優(yōu)化變量分別是三維點 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap

除了輸入?yún)?shù)外，定義邊我們通常需要復寫一些重要的成員函數(shù)

頂點里主要復寫了頂點更新函數(shù)oplusImpl和頂點重置函數(shù)setToOriginImpl

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();

read，write：分別是讀盤、存盤函數(shù)，一般情況下不需要進行讀/寫操作的話，僅僅聲明一下就可以
computeError函數(shù)：非常重要，是使用當前頂點的值計算優(yōu)化的估計值與真實的測量值之間的誤差
linearizeOplus函數(shù)：非常重要，是在當前頂點的值下，該誤差對優(yōu)化變量的偏導數(shù)，也就是我們說的Jacobian矩陣

除了上面幾個成員函數(shù)，還有幾個重要的成員變量和函數(shù)也一并解釋一下：

• _measurement：存儲觀測值
• _error：存儲computeError() 函數(shù)計算的誤差
• __vertices[]：存儲頂點信息，比如二元邊的話，_vertices[] 的大小為2，存儲順序和調(diào)用setVertex(int, vertex) 是設定的int 有關（0 或1）
• setId(int)：來定義邊的編號（決定了在H矩陣中的位置）
• setMeasurement(type) 函數(shù)來定義觀測值
• setVertex(int, vertex) 來定義頂點
• setInformation() 來定義協(xié)方差矩陣的逆

如何自定義g2o的邊？

 class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用時，加上此句，等于采用eigen中約定的方式重載了該類的new delete等內(nèi)存分配函數(shù)      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}

先來看一個簡單例子，地址在
https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp
這個是個一元邊，主要是定義誤差函數(shù)了，如下所示，你可以發(fā)現(xiàn)這個例子基本就是上面例子的一丟丟擴展

// 誤差模型 模板參數(shù)：觀測值維度，類型，連接頂點類型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( 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
這里根據(jù)自己理解對代碼加了注釋，方便理解

//繼承了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()  {//李群相機位姿v1const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);// 頂點v2const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);//相機參數(shù)const CameraParameters * cam= static_cast<const CameraParameters *>(parameter(0));//誤差計算，測量值減去估計值，也就是重投影誤差obs-cam//估計值計算方法是T*p,得到相機坐標系下坐標，然后在利用camera2pixel()函數(shù)得到像素坐標。Vector2D obs(_measurement);_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));}//3. 線性增量函數(shù)，也就是雅克比矩陣J的計算方法virtual void linearizeOplus();//4. 相機參數(shù)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 函數(shù)，它的定義在
g2o/types/sba/types_six_dof_expmap.cpp
cam_map 函數(shù)功能是把相機坐標系下三維點（輸入）用內(nèi)參轉(zhuǎn)換為圖像坐標（輸出），具體代碼如下所示

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函數(shù)，它的功能是把世界坐標系下三維點變換到相機坐標系，函數(shù)在
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() 的理解，還有一個很重要的函數(shù)就是linearizeOplus()，用來定義雅克比矩陣
我摘取了相關代碼（來自：g2o/g2o/types/sba/types_six_dof_expmap.cpp），并進行了標注，相信會更容易理解

十四講第169頁中的雅克比矩陣完全是按照書上 式子（7.45）、（7.47）來編程的，不難理解
小白：后面就是直接照抄書上就行，哈哈

如何向圖中添加邊

一元邊的添加方法

下面代碼來自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] );      // 觀測數(shù)值edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協(xié)方差矩陣之逆optimizer.addEdge( edge );}

小白：setMeasurement 函數(shù)的輸入的觀測值具體是指什么？
師兄：對于這個曲線擬合，觀測值就是實際觀測到的數(shù)據(jù)點。對于視覺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函數(shù)里的p來自向量points_2d，也就是特征點的圖像坐標(x,y)了吧！
師兄：對，這正好呼應我剛才說的。另外，你看setVertex 有兩個，一個是 0 和 VertexSBAPointXYZ 類型的頂點，一個是1 和pose。你覺得這里的0和1是什么意思？能否互換呢？

小白：0，1應該是分別指代哪個頂點吧，直覺告訴我不能互換，可能得去查查頂點定義部分的代碼
師兄：你的直覺沒錯！我?guī)湍?查過啦，你看這個是setVertex在g2o官網(wǎng)的定義：

// 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對應的 應該就是三維點。

6. 設置優(yōu)化參數(shù)，開始執(zhí)行優(yōu)化

設置SparseOptimizer的初始化、迭代次數(shù)、保存結(jié)果等

初始化

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

設置迭代次數(shù)，開始執(zhí)行圖優(yōu)化

SparseOptimizer::optimize(int iterations, bool online)

• 練習題目：
  題目：給定一組世界坐標系下的3D點(p3d.txt)以及它在相機中對應的坐標(p2d.txt)，以及相機的內(nèi)參矩陣。使用bundle adjustment 方法（g2o庫實現(xiàn)）來估計相機的位姿T。初始位姿T為單位矩陣。

代碼框架：
鏈接：https://pan.baidu.com/s/1CiSU-8rDBWurk1ZpzMk_Nw
提取碼：ebcj
實現(xiàn)：https://zhuanlan.zhihu.com/p/64587232