g2o是一個通用的圖優化框架。
本文結合論文g2o: A general framework fo graph optimizaiton說明g2o的理論，然後編寫一個示例程式，說明g2o的使用方法。

g2o理論：
許多問題可以建模為最小二乘優化的狀態估計問題。問題形式如圖

F(x)為目標函式，目的就是求解狀態x，使目標函式最小。x是一個向量，向量中的每個元素也可以為一個引數塊。zij是聯絡狀態變數i和狀態變數j的觀測，數學上表現為一個約束方程。由於有測量噪聲，採用誤差向量e()評價xi和xj滿足方程的程度。

論文中為了簡化符號，採用下圖描述誤差向量

這裡需要注意的是，狀態變數xi和xj（左側），已經變為了整個系統的狀態x（右側）。

這個誤差向量可以表示為一個有向圖，可以知道方程結構，便於視覺化分析

這個圖中，邊的個數就是所有的約束，也就是方程個數。一個節點為一個狀態變數，所有節點的結合組成了整個系統的狀態變數。不要忘記我們的目標就是為了狀態估計。

有了上述目標函式，我們就可以進行最小二乘優化。
論文中給出的是高斯牛頓法。
高斯牛頓法的思想是：對誤差函式e採用一階泰勒展開。然後構建一個新的誤差函式，求這個誤差函式的極小值，以極小值作為新的迭代點。重複迭代，直到滿足終止條件。

這裡需要注意的是，我們要求的是\delta x,
雅克比矩陣Jij為zij的方程對整個狀態變數x的雅克比,
方程14的H和b有關的引數為Jij.

也就是說，我們通過求解整個方程的雅克比，然後帶入到方程12中，組合為b和H，最後求解H\delta x = -b.

論文中還粗略介紹了LM方法，

線上性方程中加入了一個帶有阻尼係數的單位陣。
這裡不詳細介紹了。

最小二乘在SLAM中的應用
空間剛體有6個自由度，三個旋轉，3個平移。
平移位於歐式空間，但是旋轉位於非歐空間。

表示旋轉的方法有旋轉矩陣、尤拉角、四元數、旋轉向量。
尤拉角和旋轉向量是最小引數表示，但是有奇異性。
旋轉矩陣和四元數是過引數表示（3個自由度，但是旋轉矩陣9個引數，四元數4個引數），但是會有約束。

為了應用最小二乘，可以把\delta x最小引數表示，因為通常\delta x 比較小，遠離奇異點。
然後採用過引數表示當前的迭代點。
定義了一個田字格操作。

論文中說，可以採用歸一化的四元數表示增量，採用全四元數表示迭代點，這裡當前還沒弄明白。

田字格和準星是有關係的，這裡也就是兩個旋轉矩陣相乘，可以表示相機從當前位姿，運動\delta x，後的新的位姿

新的誤差函式形式如圖

重要的雅克比如圖，這玩意就和求導一樣的

新的迭代點為

不管引數咋樣變化，H矩陣的結構是不變的。

也就是說，每個方程都會有一個bij和一個Hij，所有方程的H和b組成了系統的H和b。

注意：bij與三個東西有關。
假設有n個變數，每個變數是個引數塊，假設只有1個引數，則Jij為1Xn的，\Omiga為1X1的，e為1X1，則bij為nx1的向量。

同理Hij = nX1X1X1X1Xn = nXn 的矩陣。

也就是在H的ij，ji，ii，jj和b的i和j位置非零，其他位置為0.

可以注意到H也是圖的鄰接矩陣，所以，H的非零塊正比於圖中邊的數量。這導致了H的稀疏性。
有些問題可以採用H的係數性，來求解線性系統。

在BA中，變數為相機姿態p和路標點l
重新排列變數順序，

由於通常相機變數個數少，路標點變數個數多，
採用H的舒爾補，可以化簡。

Hll的逆是塊對角陣，便於求解。並且方程個數少了，可以求解\delta x_p
然後可求\delta x_l

實現方法：

小結：

對每個觀測，計算誤差向量；
計算誤差向量的雅克比；
更新系統的H和b

然後求解線性系統

應用：
在使用g2o時，我們需要以圖優化的思想來編寫程式。

圖是由節點和邊組成的，我們首先以類的形式，定義節點和邊。

頂點可以繼承一個模板類BaseVertex，模板引數為頂點的最小引數個數和頂點的資料型別。
需要我們實現的是田操作，重寫 virtual void oplusImpl(const double* update) 函式。
估計值儲存在變數_estimate中；函式void setToOriginImpl() 將節點的值進行重置。定義節點時，利用setEstimate(type) 函式來設定初始值；setId(int) 定義節點編號。

邊也是繼承一個模板類，模板引數為誤差向量e的維度，觀測z的資料型別和邊連線的頂點型別。這裡需要注意的是，幾個頂點就是幾元邊。
邊也就是上文中的eij，我們需要為邊定義誤差函式void computeError()
定義雅克比Jij void linearizeOplus() ,觀測值儲存在_measurement 中，誤差儲存在_error 中，節點儲存在_vertices[] 。定義邊時，利用setId(int) 來定義邊的編號（決定了在H矩陣中的位置）；setMeasurement(type) 函式來定義觀測值；setVertex(int, vertex) 來定義節點；setInformation() 來定義協方差矩陣的逆。

定義完頂點和邊後，我們就可以構建這個圖，並對其優化。

定義一個圖優化的物件 g2o::SparseOptimizer optimizer;
設定一些圖優化相關的內容，
也就是有了線性系統H\deltax=-b後，我們還需要設定求解線性系統的方法。
線性系統可以是帶阻尼的LM方法，這就需要我們指定用哪個優化演算法
g2o::OptimizationAlgorithmLevenberg

  typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  MyBlockSolver;
  typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;
  
  MyLinearSolver* linearSolver = new MyLinearSolver();//定義一個線性求解器
  MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);//定義一個塊求解器
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);//定義優化方法
  optimizer.setAlgorithm(solver);//設定圖優化的求解器

這裡是自下而上的設定。
塊求解器這塊還沒看懂，

之後新增頂點和邊。

最後優化

 optimizer.initializeOptimization();
  optimizer.setVerbose(verbose);
  optimizer.optimize(maxIterations);