by jie 2018.7
一. g2o的整體結(jié)構(gòu)
說到整體的結(jié)構(gòu)意蛀，不得不用一張比較概括的圖來說明：
這張圖最好跟著畫一下，這樣能更好的理解和掌握健芭，例如我第一次看的時候根本沒有注意說箭頭的類型等等的細節(jié)县钥。
先看上半部分。SparseOptimizer 是我們最終要維護的東東慈迈。它是一個Optimizable Graph若贮，從而也是一個Hyper Graph。一個 SparseOptimizer 含有很多個頂點 （都繼承自 Base Vertex）和很多個邊（繼承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）痒留。這些 Base Vertex 和 Base Edge 都是抽象的基類谴麦，而實際用的頂點和邊，都是它們的派生類伸头。我們用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一個圖中添加頂點和邊匾效，最后調(diào)用 SparseOptimizer.optimize 完成優(yōu)化。
在優(yōu)化之前恤磷，需要指定我們用的求解器和迭代算法面哼。從圖中下半部分可以看到，一個 SparseOptimizer 擁有一個 Optimization Algorithm扫步，繼承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我們常用的是GN或LM）精绎。同時，這個 Optimization Algorithm 擁有一個Solver锌妻，它含有兩個部分代乃。一個是 SparseBlockMatrix ，用于計算稀疏的雅可比和海塞仿粹； 一個是用于計算迭代過程中最關(guān)鍵的一步 見書p111
HΔx=?b
這就需要一個線性方程的求解器搁吓。而這個求解器，可以從 PCG, CSparse, Choldmod 三者選一吭历。
綜上所述堕仔，在g2o中選擇優(yōu)化方法一共需要三個步驟：
- 選擇一個線性方程求解器，從 PCG, CSparse, Choldmod中選晌区，實際則來自 g2o/solvers 文件夾中定義的東東摩骨。
- 選擇一個 BlockSolver 通贞。
- 選擇一個迭代策略，從GN, LM, Doglog中選恼五。
那么從圖中我們其實比較容易的就看出來整個庫里面較為重要的類之間的繼承以及包含關(guān)系昌罩，也可以看出整個框架里面最重要的東西就是SparseOptimizer這個類（或者說實例）。
頂點（Vertex）和邊（Edge）
順著圖往上看灾馒，可以看到我們所使用的優(yōu)化器最終是一個超圖（hyperGrahp）茎用，而這個超圖包含了許多頂點（Vertex）和邊（Edge）。這兩個類型是我們在看程序和寫程序中比較關(guān)注的東西了睬罗，g2o不像Ceres轨功，內(nèi)部很多東西其實作者都已經(jīng)寫好了，但是同時容达，我們也失去了一個比較完整的了解內(nèi)部關(guān)系的機會古涧，不過這個東西我們可以通過看內(nèi)部的實現(xiàn)補回來。
在圖優(yōu)化中花盐，頂點代表了要被優(yōu)化的變量羡滑，而邊則是連接被優(yōu)化變量的橋梁，因此卒暂，也就造成了說我們在程序中見得較多的就是這兩種類型的初始化和賦值啄栓。
在整個優(yōu)化過程中娄帖，頂點的值會越來越趨近于最優(yōu)值也祠，優(yōu)化完畢后則可以將頂點的優(yōu)化值作為最優(yōu)值進行使用；邊則是連接頂點的類型近速，在SLAM問題中诈嘿，一般是邊連接要被優(yōu)化的空間點（Point）和機器人的位姿（Pose），當然削葱，邊還可以連接一個頂點（類似與參數(shù)估計奖亚，邊的數(shù)量由量測的數(shù)量決定），也可以連接多個頂點（超圖）析砸，邊在圖優(yōu)化中的一個很大的作用就是計算誤差（視覺SLAM中計算的就是空間點的映射誤差）昔字，同時計算該誤差對于被優(yōu)化變量的jacobian矩陣，也是比較重要的存在首繁。
1.1 自定義頂點（Vertex）和邊（Edge）
我們在用g2o的時候作郭，不會一帆風順的就能適合自身機器人的實際情況，總會遇到自己獨特的頂點類型和邊類型弦疮，此時我們需要對頂點和邊進行重寫夹攒，那么重寫也比較簡單，這里簡單進行記錄胁塞。
在整體框架圖中咏尝，可以看到不管是頂點還是邊压语，都可以說是繼承自baseXXX這個類的，因此我們在自定義的時候编检，也可以仿照著繼承這兩個類胎食，當然也可以繼承自g2o中較為“成熟”的類，不管怎樣蒙谓，都要重寫下述的函數(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比較重要谤专，我們根據(jù)增量方程計算出增量之后躁锡，就是通過這個函數(shù)對估計值進行調(diào)整的，因此這個函數(shù)的內(nèi)容一定要寫對置侍，否則會造成一直優(yōu)化卻得不到好的優(yōu)化結(jié)果的現(xiàn)象映之。
自定義邊
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();
read和write函數(shù)同上，computeError函數(shù)是使用當前頂點的值計算的測量值與真實的測量值之間的誤差蜡坊，linearizeOplus函數(shù)是在當前頂點的值下杠输，該誤差對優(yōu)化變量的偏導數(shù)，即jacobian秕衙。
注意：如果沒有定義邊的linearizeOplus函數(shù)蠢甲，就會調(diào)用數(shù)值求導，運算比較慢据忘。
總結(jié)
不管是自定義邊還是頂點鹦牛，除了自己加入的一些變量，還都要對一些g2o框架要調(diào)用的函數(shù)進行覆寫勇吊，這些函數(shù)用戶可以聲明為實函數(shù)（即不加virtual）曼追，但是筆者還是建議聲明為虛函數(shù)。
1.2 優(yōu)化算法
順著整體結(jié)構(gòu)圖往下看汉规，可以看到這部分其實算是整個g2o里面比較隱晦的部分礼殊，設(shè)計到優(yōu)化的算法，塊求解器针史，線性求解器等等部分晶伦，在程序中，這部分通常位于g2o算法的開頭配置部分悟民，一般情況下我們可以隨著一個例程進行配置即可坝辫，這里對這部分進行了稍微淺顯的理解。
我們知道在求解增量方程HdeltaX=-b的時候射亏，通常情況下想到線性求解近忙，很簡單嘛竭业，deltaX=-H.inv*b，的確及舍，當H的維度較小的時候未辆，上述問題變得簡單，只需要矩陣的求逆就能解決問題锯玛，但是當H的維度較大時咐柜，問題變得復雜，此時我們就需要一些特殊的方法對矩陣進行求逆攘残，g2o中主要有圖中所示的三種方法拙友，PCG，CSparse和Cholmod方法歼郭。
注意遗契，這里說再多，線性求解器僅僅只是完成了一個求解的功能病曾，可以說是整個優(yōu)化中比較靠后的計算部分了牍蜂。
BlockSolver：塊求解器（參數(shù)塊求解器？）
塊求解器是包含線性求解器的存在泰涂，之所以是包含鲫竞，是因為塊求解器會構(gòu)建好線性求解器所需要的矩陣塊（也就是H和b），之后給線性求解器讓它進行運算逼蒙，邊的jacobian也就是在這個時候發(fā)揮了自己的光和熱袜茧。
這里再記錄下一個比較容易混淆的問題坛掠，也就是在初始化塊求解器的時候的參數(shù)問題讹躯。
大部分的例程在初始化塊求解器的時候都會使用如下的程序代碼：
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver =
g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
其中的BlockSolver_6_3有兩個參數(shù)凌唬，分別是6和3赁还，在定義的時候可以看到這是一個模板的重命名（模板類的重命名只能用using）
template<int p, int l>
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
其中p代表pose的維度妖泄，l表示landmark的維度，且這里都表示的是增量的維度（這里筆者也不是很確定艘策，但是從后續(xù)的程序中可以看出是增量的維度而并非是狀態(tài)變量的維度）蹈胡。
因此（后面的話以SLAM情況為例），對于僅僅優(yōu)化位姿的應用而言朋蔫，這里l的值是沒有太大影響的罚渐，因為H矩陣中并沒有Hll的塊，因此這里的維度也就沒有用武之地了驯妄。
優(yōu)化算法
從圖中可以看到主要有三種方法：GN荷并，LM，PSD青扔，不同的方法主要表現(xiàn)在最終的H矩陣構(gòu)造不同源织。
1.3 SLAM使用g2o過程
使用g2o來實現(xiàn)圖優(yōu)化還是比較容易的翩伪。它幫你把節(jié)點和邊的類型都定義好了，基本上只需使用它內(nèi)置的類型而不需自己重新定義谈息。要構(gòu)造一個圖缘屹，要做以下幾件事：
定義一個SparseOptimizer. 編寫方式參見tutorial_slam2d的聲明方式。你還要寫明它使用的算法侠仇。通常是Gauss-Newton或LM算法轻姿。個人覺得后者更好一些。
定義你要用到的邊逻炊、節(jié)點的類型互亮。例如我們實現(xiàn)一個3D SLAM。那么就要看它的g2o/types/slam3d下面的頭文件余素。節(jié)點頭文件都以vertex_開頭胳挎，而邊則以edge_開頭。在我們上面的模型中溺森，可以選擇vertex_se3作為節(jié)點慕爬，edge_se3作為邊。這兩個類型的節(jié)點和邊的數(shù)據(jù)都可以直接來自于Eigen::Isometry屏积，即上面講到過的變換矩陣T医窿。
編寫一個幀間匹配程序，通過兩張圖像算出變換矩陣炊林。這個用opencv, pcl都可以做姥卢。
把你得到的關(guān)鍵幀作為節(jié)點，變換矩陣作為邊渣聚，加入到optimizer中独榴。同時設(shè)定節(jié)點的估計值（如果沒有慣性測量就設(shè)成零）與邊的約束（變換矩陣）。此外奕枝，每條邊還需設(shè)定一個信息矩陣（協(xié)方差矩陣之逆）作為不確定性的度量棺榔。 例如你覺得幀間匹配精度在0.1m，那么把信息矩陣設(shè)成100的對角陣即可隘道。
在程序運行過程中不斷作幀間檢測症歇，維護你的圖。
程序結(jié)束時調(diào)用optimizer.optimize( steps )進行優(yōu)化谭梗。優(yōu)化完畢后讀取每個節(jié)點的估計值忘晤，此時就是優(yōu)化后的機器人軌跡。
參考：視覺SLAM漫談（二）:圖優(yōu)化理論與g2o的使用
二. 詳解g2o的頂點和邊
參考：g2o學習-再看頂點和邊
跟著g2o的slam2d_tutorial進行了學習激捏，發(fā)現(xiàn)自己對于頂點和邊的理解還是不太夠设塔，覺得有必要把頂點和邊的一些東西再給總結(jié)一下，主要參考的就是如下網(wǎng)站： 函數(shù)api
這個網(wǎng)站里面有較為全面的g2o的類以及函數(shù)的講解远舅，很方便闰蛔。
2.1 g2o的頂點vertex
首先我們來看一下頂點的繼承關(guān)系：
可以看到比較“成熟”的類型就是BaseVertex了竞思，由于我們一般在派生的時候就是繼承自這個類的，下面主要是對這個類進行分析钞护，其中個人認為還是有很多東西要注意的盖喷。
g2o::BaseVrtex< D, T >
int D, typename T
首先記錄一下定義模板的兩個參數(shù)D和T，兩個類型分別是int和typename的類型难咕，D表示的是維度课梳，g2o源碼里面是這個注釋的
static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
可以看到這個D并非是頂點（更確切的說是狀態(tài)變量）的維度，而是其在流形空間（manifold）的最小表示余佃，這里一定要區(qū)別開暮刃；之后是T，源碼里面也給出了T的作用
typedef T EstimateType;
EstimateType _estimate;
可以看到爆土，這里T就是頂點（狀態(tài)變量）的類型椭懊。
在頂點的繼承中，這兩個參數(shù)是直接面向我們的步势，所以務必要定義妥當氧猬。
hessian矩陣和b向量
增量方程很重要的就是H和b兩個參數(shù)，這里的_hessian和_b是增量方程中H和b的一部分坏瘩，更確切的說是對應于該頂點的部分盅抚，下面簡單的說一下這兩個參數(shù)的作用。
_hessian矩陣
它的類型是Eigen中的Map類型倔矾，也就是這個參數(shù)只是一個映射妄均，把一塊內(nèi)存區(qū)域映射為Eigen中的數(shù)據(jù)類型，具體這里就不贅述了哪自。它的作用也比較簡單丰包，就是拿到邊中算出的jacobian，之后根據(jù)公式H=JTWJ計算出整個大H中該處理的數(shù)據(jù)壤巷。
相關(guān)的程序代碼這里：
block_solver.hpp
_b矩陣
它的類型就是簡單的Eigen::Vector邑彪，這里不是用的映射關(guān)系，但是他的作用和上述的類似隙笆，只不過最后是通過拷貝把b=JTerror給整個的b锌蓄。
相關(guān)代碼如下：
virtual int copyB(number_t* b_) const {
memcpy(b_, _b.data(), Dimension * sizeof(number_t));
return Dimension;
}
2.2 g2o的邊（Edge）
首先我們也是先給出邊的繼承關(guān)系
g2o二元邊的繼承關(guān)系
這里截取的是二元邊的繼承圖升筏，下面的分析也是以該二元邊為例的撑柔。
g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >
int D, typename E
首先還是介紹這兩個參數(shù)，還是從源碼上來看
static const int Dimension = D;
typedef E Measurement;
typedef Eigen::Matrix<number_t, D, 1, Eigen::ColMajor> ErrorVector;
可以看到您访，D決定了誤差的維度铅忿，從映射的角度講，三維情況下就是2維的灵汪，二維的情況下是1維的檀训；然后E是measurement的類型柑潦，也就是測量值是什么類型的，這里E就是什么類型的（一般都是Eigen::VectorN表示的峻凫，N是自然數(shù)）渗鬼。和前面一樣，這兩個參數(shù)是直接面向我們的荧琼，一定要定義妥當譬胎。
typename VertexXi, typename VertexXj
這兩個參數(shù)就是邊連接的兩個頂點的類型，這里特別注意一下命锄，這兩個必須一定是頂點的類型堰乔，也就是繼承自BaseVertex等基礎(chǔ)類的類！不是頂點的數(shù)據(jù)類脐恩！例如必須是VertexSE3Expmap而不是VertexSE3Expmap的數(shù)據(jù)類型類SE3Quat镐侯。原因的話源碼里面也很清楚，因為后面會用到一系列頂點的維度等等的屬性驶冒，這些屬性是數(shù)據(jù)類型類里面沒有的苟翻。
總結(jié)
- 在定義自己的邊和頂點的時候，務必要弄明白D和E所代表的含義骗污，在定義邊的時候還要注意后面兩個參數(shù)一定是一個頂點的類而不是數(shù)據(jù)類型袜瞬；
- 在邊的linearizeOplus函數(shù)定義時，如果我們有更簡潔的數(shù)學表達身堡，那么可以轉(zhuǎn)化為編程語言進行求解邓尤，如果沒有，也可以使用父類的求解方法贴谎，但是這種方法由于使用了循環(huán)汞扎，甚至中間還求了多次映射誤差，因此較為耗時擅这；
- 千萬不要以為g2o會幫你區(qū)分什么是pose澈魄，什么是point，從而對號入座在Hpp和Hll中仲翎，這塊必須由用戶設(shè)定marginalization告訴g2o什么頂點歸在那一塊里面痹扇；
- 綜合看來，g2o幫助我們實現(xiàn)了很多內(nèi)部的算法溯香，但是在進行構(gòu)造的時候鲫构，也需要遵循一些規(guī)則。
三. 求解器
3.1 塊求解器（BlockSolver_P_L）
首先祭出他的源碼：
template<int p, int l>
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
接著往上走玫坛，我們會看到比較詳細的BlockSoverTraits類
template <int _PoseDim, int _LandmarkDim>
struct BlockSolverTraits
{
static const int PoseDim = _PoseDim;
static const int LandmarkDim = _LandmarkDim;
typedef Eigen::Matrix<number_t, PoseDim, PoseDim, Eigen::ColMajor> PoseMatrixType;
typedef Eigen::Matrix<number_t, LandmarkDim, LandmarkDim, Eigen::ColMajor> LandmarkMatrixType;
typedef Eigen::Matrix<number_t, PoseDim, LandmarkDim, Eigen::ColMajor> PoseLandmarkMatrixType;
typedef Eigen::Matrix<number_t, PoseDim, 1, Eigen::ColMajor> PoseVectorType;
typedef Eigen::Matrix<number_t, LandmarkDim, 1, Eigen::ColMajor> LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
/**
* \brief traits to summarize the properties of the dynamic size optimization problem
*/
template <>
struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
{
static const int PoseDim = Eigen::Dynamic;
static const int LandmarkDim = Eigen::Dynamic;
typedef MatrixX PoseMatrixType;
typedef MatrixX LandmarkMatrixType;
typedef MatrixX PoseLandmarkMatrixType;
typedef VectorX PoseVectorType;
typedef VectorX LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
接著是比較詳細的BlockSolver類
template <typename Traits>
class BlockSolver: public BlockSolverBase
{
public:
static const int PoseDim = Traits::PoseDim;
static const int LandmarkDim = Traits::LandmarkDim;
typedef typename Traits::PoseMatrixType PoseMatrixType;
typedef typename Traits::LandmarkMatrixType LandmarkMatrixType;
typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
typedef typename Traits::PoseVectorType PoseVectorType;
typedef typename Traits::LandmarkVectorType LandmarkVectorType;
typedef typename Traits::PoseHessianType PoseHessianType;
typedef typename Traits::LandmarkHessianType LandmarkHessianType;
typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
typedef typename Traits::LinearSolverType LinearSolverType;
public:
/**
* allocate a block solver ontop of the underlying linear solver.
* NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
* in its destructor.
*/
BlockSolver(std::unique_ptr<LinearSolverType> linearSolver);
~BlockSolver();
virtual bool init(SparseOptimizer* optmizer, bool online = false);
virtual bool buildStructure(bool zeroBlocks = false);
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
virtual bool buildSystem();
virtual bool solve();
virtual bool computeMarginals(SparseBlockMatrix<MatrixX>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
virtual bool setLambda(number_t lambda, bool backup = false);
virtual void restoreDiagonal();
virtual bool supportsSchur() {return true;}
virtual bool schur() { return _doSchur;}
virtual void setSchur(bool s) { _doSchur = s;}
LinearSolver<PoseMatrixType>& linearSolver() const { return *_linearSolver;}
virtual void setWriteDebug(bool writeDebug);
virtual bool writeDebug() const {return _linearSolver->writeDebug();}
virtual bool saveHessian(const std::string& fileName) const;
virtual void multiplyHessian(number_t* dest, const number_t* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
protected:
void resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
void deallocate();
std::unique_ptr<SparseBlockMatrix<PoseMatrixType>> _Hpp;
std::unique_ptr<SparseBlockMatrix<LandmarkMatrixType>> _Hll;
std::unique_ptr<SparseBlockMatrix<PoseLandmarkMatrixType>> _Hpl;
std::unique_ptr<SparseBlockMatrix<PoseMatrixType>> _Hschur;
std::unique_ptr<SparseBlockMatrixDiagonal<LandmarkMatrixType>> _DInvSchur;
std::unique_ptr<SparseBlockMatrixCCS<PoseLandmarkMatrixType>> _HplCCS;
std::unique_ptr<SparseBlockMatrixCCS<PoseMatrixType>> _HschurTransposedCCS;
std::unique_ptr<LinearSolverType> _linearSolver;
std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
# ifdef G2O_OPENMP
std::vector<OpenMPMutex> _coefficientsMutex;
# endif
bool _doSchur;
std::unique_ptr<number_t[], aligned_deleter<number_t>> _coefficients;
std::unique_ptr<number_t[], aligned_deleter<number_t>> _bschur;
int _numPoses, _numLandmarks;
int _sizePoses, _sizeLandmarks;
};
那么有了以上的信息结笨，整個BlockSolver_P_L的作用也就比較清楚了，還是那句老話：P表示的是Pose的維度（注意一定是流形manifold下的最小表示），L表示Landmark的維度（這里就不涉及流行什么事兒了）炕吸。這里思考一個問題伐憾，假如說在某個應用下，我們的P和L在程序開始并不能確定（例如程序中既有映射關(guān)系的邊赫模，同時還有位姿圖之間的邊树肃，這時候的Hpp矩陣將不是那么的稀疏），那么此時這個塊狀求解器如何定義呢瀑罗？其實也比較簡單扫外，g2o已經(jīng)幫我們定義了一個不定的BlockSolverTraits，所有的參數(shù)都在中間過程中被確定廓脆，那么為什么g2o還幫助我們定義了一些常用的類型呢筛谚？一種可能是出于節(jié)約初始化時間的角度出發(fā)的，但是個人估計不是很靠譜停忿，畢竟C++申請出一塊內(nèi)存應該還是很快的驾讲，沒有必要為了這點兒時間糾結(jié)；那么另一種也許就是作者想把常用的類型定義出來而已吧～
3.2 優(yōu)化器（optimizer）和初始化（initializeOptimization）
想必這句話大家應該也經(jīng)常用到
optimizer.initializeOptimization();
這個函數(shù)里面都干了什么：
- 把所有的頂點（Vertex）插入到vset的集合中（set不用擔心插入了相同的頂點）
- 遍歷vset集合席赂，取出每個頂點的邊（這里每個邊都有一個level的概念吮铭，默認情況下，g2o只處理level=0的邊颅停，在orbslam中谓晌，如果確定某個邊的重投影誤差過大，則把level設(shè)置為1癞揉，也就是舍棄這個邊對于整個優(yōu)化的影響）纸肉，并判斷邊所連接的頂點是否都是有效的（在vset中），如果是喊熟，則認為這是一個有效的邊和頂點柏肪，并分別加入到_activeEdges和_activeVertices中（媽媽在也不用擔心邊少頂點或者圖中沒有邊的頂點了）
- 對上述的_activeEdges和_activeVertices按照ID號進行排序，其中Vertex的ID號是自己設(shè)置的芥牌，而Edge的ID號是g2o內(nèi)部有個變量進行賦值的
- 對于上述的_activeVertices烦味，剔除掉固定點（fixed）之后，把所有的頂點按照不被margin 在前壁拉，被margin在后的順序排成vector類型谬俄，變量為_ivMap，這個變量很重要弃理，基本上后面的所有程序都是用這個變量進行遍歷的
以上就是初始化過程的全部溃论，具體的代碼如下：
bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
if (edges().size() == 0) {
cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
return false;
}
preIteration(-1);
bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level) {
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
if (vset.find(*vit) == vset.end()) {
allVerticesOK = false;
break;
}
}
if (allVerticesOK && !e->allVerticesFixed()) {
auxEdgeSet.insert(e);
levelEdges++;
}
}
}
if (levelEdges){
_activeVertices.push_back(v);
// test for NANs in the current estimate if we are debugging
# ifndef NDEBUG
int estimateDim = v->estimateDimension();
if (estimateDim > 0) {
VectorX estimateData(estimateDim);
if (v->getEstimateData(estimateData.data()) == true) {
int k;
bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
if (hasNan)
cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
}
}
# endif
}
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
_activeEdges.push_back(*it);
sortVectorContainers();
bool indexMappingStatus = buildIndexMapping(_activeVertices);
postIteration(-1);
return indexMappingStatus;
}
3.3 優(yōu)化器（Optimizer）的優(yōu)化（optimize）
初始化之后，我們基本上就可以調(diào)用optimize函數(shù)進行圖優(yōu)化了案铺，如果只看optimize函數(shù)的代碼蔬芥，十分簡單梆靖，首先判斷_ivMap是否為空控汉，之后直接調(diào)用_algorithm的solve函數(shù)笔诵，最后打印一些信息，之后循環(huán)一定的次數(shù)姑子，此時就大功告成乎婿。所以下面的內(nèi)容主要是圍繞著_algorithm的solve函數(shù)進行。
算法（algorithm）的solve函數(shù)
在第一次進行迭代的時候街佑，g2o要對整個矩陣塊進行初始化谢翎，主要依靠塊求解器的buildStructure()函數(shù)進行，那么想必大家也知道了沐旨，既然在塊求解器中森逮，那么必然就是對要用的矩陣塊進行初始化，確實如此磁携，我們稍微對這個函數(shù)進行展開：
（1） 對_ivMap進行遍歷褒侧，如果頂點的margin標志為false，則認為是Pose谊迄，否則闷供，認為是Landmark，所以在使用g2o的時候千萬要注意统诺，不要想當然的認為g2o會幫助我們區(qū)分Pose和Landmark
（2） 構(gòu)建期待已久的Hpp歪脏，Hll等矩陣，每個矩陣的類型都是SparseBlockMatrix類型的
（3） 開始對應環(huán)節(jié)粮呢，這個過程首先是遍歷_ivMap婿失，將Hpp和Hll中對應于該頂點的矩陣塊映射到頂點內(nèi)部的_hessian矩陣中，這步的作用在我的上個博客中已經(jīng)講過了啄寡，感興趣的可以看g2o學習——再看頂點和邊移怯，之后開始遍歷所有的邊，取出邊的兩個頂點这难，如果兩個頂點都不margin的話舟误，則在把Hpp中的兩個頂點相交的地方的內(nèi)存映射給邊的內(nèi)部_hessian矩陣；如果兩個中一個margin一個不margin的話姻乓，則把Hpl中相交的地方的內(nèi)存映射給邊的_hessian矩陣嵌溢；如果兩個都margin的話，則把Hll中相交的地方的內(nèi)存映射給邊的_hessian矩陣
（4） 映射完成之后蹋岩，如果我們使用schur消元的話赖草，還要構(gòu)建一個schur消元的中間矩陣，_Hschur剪个，這個地方比較復雜秧骑，這里不做展開，對照公式看源碼就可以，源碼如下：
template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
assert(_optimizer);
size_t sparseDim = 0;
_numPoses=0;
_numLandmarks=0;
_sizePoses=0;
_sizeLandmarks=0;
int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
blockPoseIndices[_numPoses]=_sizePoses;
++_numPoses;
} else {
v->setColInHessian(_sizeLandmarks);
_sizeLandmarks+=dim;
blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
++_numLandmarks;
}
sparseDim += dim;
}
resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
delete[] blockLandmarkIndices;
delete[] blockPoseIndices;
// allocate the diagonal on Hpp and Hll
int poseIdx = 0;
int landmarkIdx = 0;
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
if (! v->marginalized()){
//assert(poseIdx == v->hessianIndex());
PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++poseIdx;
} else {
LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++landmarkIdx;
}
}
assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
// temporary structures for building the pattern of the Schur complement
SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
if (_doSchur) {
schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
}
// here we assume that the landmark indices start after the pose ones
// create the structure in Hpp, Hll and in Hpl
for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
OptimizableGraph::Edge* e = *it;
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
if (ind1 == -1)
continue;
int indexV1Bak = ind1;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
std::swap(ind1, ind2);
}
if (! v1->marginalized() && !v2->marginalized()){
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
if (_Hschur) {// assume this is only needed in case we solve with the schur complement
schurMatrixLookup->addBlock(ind1, ind2);
}
} else if (v1->marginalized() && v2->marginalized()){
// RAINER hmm.... should we ever reach this here????
LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
} else {
if (v1->marginalized()){
PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
} else {
PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
}
}
}
}
}
if (! _doSchur) {
delete schurMatrixLookup;
return true;
}
_DInvSchur->diagonal().resize(landmarkIdx);
_Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
for (OptimizableGraph::Vertex* v : _optimizer->indexMapping()) {
if (v->marginalized()){
const HyperGraph::EdgeSet& vedges=v->edges();
for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
for (size_t i=0; i<(*it1)->vertices().size(); ++i)
{
OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
if (v1->hessianIndex()==-1 || v1==v)
continue;
for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
for (size_t j=0; j<(*it2)->vertices().size(); ++j)
{
OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
if (v2->hessianIndex()==-1 || v2==v)
continue;
int i1=v1->hessianIndex();
int i2=v2->hessianIndex();
if (i1<=i2) {
schurMatrixLookup->addBlock(i1, i2);
}
}
}
}
}
}
}
_Hschur->takePatternFromHash(*schurMatrixLookup);
delete schurMatrixLookup;
_Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
return true;
}
上述塊矩陣以及映射關(guān)系對應好以后乎折，接下來比較重要的函數(shù)就是solver的buildSystem函數(shù)绒疗，這個函數(shù)的主要功能就是將增量方程（HΔx=?b）中的H矩陣和b向量賦予該有的值，具體過程如下：
（1） 遍歷所有的邊骂澄，計算該邊誤差所產(chǎn)生的jacobian矩陣
（2）根據(jù)公式H=JTWJ計算每個頂點的_hessian矩陣吓蘑，b=JTWδ計算b向量，如果該邊是個二元邊坟冲，且兩個定點都沒有被fix的時候磨镶，還會根據(jù)H=JT1WJ計算相交部分的_hessian矩陣。
（3）由于頂點內(nèi)部類型b并不是映射健提，因此最后還要遍歷所有的頂點的b并將其真值拷貝到增量方程的b內(nèi)
這部分的代碼如下：
template <typename Traits>
bool BlockSolver<Traits>::buildSystem()
{
// clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
assert(v);
v->clearQuadraticForm();
}
_Hpp->clear();
if (_doSchur) {
_Hll->clear();
_Hpl->clear();
}
// resetting the terms for the pairwise constraints
// built up the current system by storing the Hessian blocks in the edges and vertices
# ifndef G2O_OPENMP
// no threading, we do not need to copy the workspace
JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
// if running with threads need to produce copies of the workspace for each thread
JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
# endif
for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
e->constructQuadraticForm();
# ifndef NDEBUG
for (size_t i = 0; i < e->vertices().size(); ++i) {
const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
if (! v->fixed()) {
bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
if (hasANan) {
std::cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << std::endl;
break;
}
}
}
# endif
}
// flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
int iBase = v->colInHessian();
if (v->marginalized())
iBase+=_sizePoses;
v->copyB(_b+iBase);
}
return 0;
}
上述完成整個矩陣塊的構(gòu)建之后琳猫，接下來就是激動人心的求解線性方程的時候了，主要依靠塊求解器BlockSolver的solve函數(shù)私痹，下面是過程：
（1） 判斷是否要做schur消元脐嫂，如果不做的話，則認為整個圖中沒有要margin的點侄榴，因此直接求解HppΔx=?b 就可以了雹锣；
（2） 如果要進行schur消元，則在后面構(gòu)建出schur需要的矩陣和向量癞蚕，由于這部分內(nèi)容比較大蕊爵，這里就不做展開，感興趣的可以看SLAM中的marginalization 和 Schur complement桦山，里面講的也比較清楚
該部分源碼如下：
template <typename Traits>
bool BlockSolver<Traits>::solve(){
//cerr << __PRETTY_FUNCTION__ << endl;
if (! _doSchur){
number_t t=get_monotonic_time();
bool ok = _linearSolver->solve(*_Hpp, _x, _b);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
}
return ok;
}
// schur thing
// backup the coefficient matrix
number_t t=get_monotonic_time();
// _Hschur = _Hpp, but keeping the pattern of _Hschur
_Hschur->clear();
_Hpp->add(*_Hschur);
//_DInvSchur->clear();
memset(_coefficients.get(), 0, _sizePoses*sizeof(number_t));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
// calculate inverse block for the landmark
const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
assert (D && D->rows()==D->cols() && "Error in landmark matrix");
LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
Dinv = D->inverse();
LandmarkVectorType db(D->rows());
for (int j=0; j<D->rows(); ++j) {
db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
}
db=Dinv*db;
assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
it_outer != landmarkColumn.end(); ++it_outer) {
int i1 = it_outer->row;
const PoseLandmarkMatrixType* Bi = it_outer->block;
assert(Bi);
PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
# ifdef G2O_OPENMP
ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
# endif
Bb.noalias() += (*Bi)*db;
assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
for (; it_inner != landmarkColumn.end(); ++it_inner) {
int i2 = it_inner->row;
const PoseLandmarkMatrixType* Bj = it_inner->block;
assert(Bj);
while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
++targetColumnIt;
assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
assert(Hi1i2);
(*Hi1i2).noalias() -= BDinv*Bj->transpose();
}
}
}
//cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
// _bschur = _b for calling solver, and not touching _b
memcpy(_bschur.get(), _b, _sizePoses * sizeof(number_t));
for (int i=0; i<_sizePoses; ++i){
_bschur[i]-=_coefficients[i];
}
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats){
globalStats->timeSchurComplement = get_monotonic_time() - t;
}
t=get_monotonic_time();
bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur.get());
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianPoseDimension = _Hpp->cols();
globalStats->hessianLandmarkDimension = _Hll->cols();
globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
}
//cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
if (! solvedPoses)
return false;
// _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
// solution;
number_t* xp = _x;
number_t* cp = _coefficients.get();
number_t* xl=_x+_sizePoses;
number_t* cl=_coefficients.get() + _sizePoses;
number_t* bl=_b+_sizePoses;
// cp = -xp
for (int i=0; i<_sizePoses; ++i)
cp[i]=-xp[i];
// cl = bl
memcpy(cl,bl,_sizeLandmarks*sizeof(number_t));
// cl = bl - Bt * xp
//Bt->multiply(cl, cp);
_HplCCS->rightMultiply(cl, cp);
// xl = Dinv * cl
memset(xl,0, _sizeLandmarks*sizeof(number_t));
_DInvSchur->multiply(xl,cl);
//_DInvSchur->rightMultiply(xl,cl);
//cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
return true;
}
最后就是把得到的Δx對應的調(diào)用頂點中的oplusImpl函數(shù)對狀態(tài)變量進行更新
總結(jié)
以上就是整個g2o在私底下為我們做的事情了攒射，當然其中很多細節(jié)這里沒有展現(xiàn)出來，本來看這部分源碼的意圖也是為了驗證過程是否和自己想象的一樣恒水，最后的感覺是從數(shù)學上來講大致上是相同的会放，但是要是從程序上覺得還是受益匪淺，作者的很多編程方式和方法還是很值得學習的钉凌。
四. 信息矩陣
H=JTWJ,b=JWe咧最，J是誤差對位姿等的雅克比，W是權(quán)重御雕。一般這個H矩陣也稱為信息矩陣矢沿，并且H矩陣是稀疏的.
信息矩陣 和信息向量，其實是另一組描述高斯分布的參數(shù)酸纲，叫做canonical parameterization.在GraphSLAM中捣鲸，我們需要通過優(yōu)化信息矩陣來求得合理的pose和map
總結(jié)：
信息矩陣等于協(xié)方差的逆，在圖優(yōu)化中作為不確定性的度量闽坡。這個矩陣主要是在構(gòu)成最小二乘優(yōu)化問題的時候用到的栽惶，其實理解成權(quán)重更好愁溜，協(xié)方差越大，說明距離真值的誤差越大外厂，那么在優(yōu)化的時候就要給予較小的權(quán)重冕象，不至于誤差較大的頂點或者邊帶偏了整個優(yōu)化。
