1.自己定義頂點類、邊類或者用已經有的。
1.1定義頂點
例子
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual 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 {}
};
自己能夠操作的，修改類名爲自己的類名。修改頂點的維度和類型，就是g2o:BaseVertex<>裏的兩個參數。
函數setToOriginImpl{}裏設置_estimate的初值。你可以如例子中_estimate<<0,0,0,也可以_estimate=SE3Quat(),也可以留空。
oplusImpl是設定估計的更新值的函數，一般裏面的變量都是更新值const double* update,至於這個update你可以設成update_什麼的。然後函數裏估計值會變成估計值和更新值的和或積，而更新值的類型必須修改成和頂點類型一致。也可以重寫這個函數，在括號後輸入override,而在{}定義一個新參數v,這個v可以和頂點類型不一致，v是由update得到或者由update和其他參數一起得到，然後是_estimate+=v,最後在設頂點估計值的時候，估計值要和v的類型一致。最重要的就是這個oplusImpl函數。
而存盤和讀盤函數，可以選擇留空，或者在{}裏輸入你想輸入的數，比如return false;等
也可以設置參數。
1.2定義邊
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
};
可操作的：邊類名，邊的頂點數，邊數據類型，和邊頂點類型。也就是g2o::BaseUnaryEdge<>中的內容。
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}這個是類賦值，可以有，也可以沒有，如果有，那麼要定義參數_x,因爲這裏是把x賦值給_x.
最重要的就是計算誤差函數computeError()，具體操作在{}，主要是要給出_error的具體形式。
一般賦值_vertices[i]給頂點，比如v或者v1,v2等。這裏v的類型就是之前我們定義的頂點類的指針。一般是這樣定義的
const 頂點類 v=static_cast<const 頂點類*>(_vertices[0])
要麼把v->estimate估計值傳給新的值，要麼直接用來計算誤差值。誤差值的是由測量值_measuremen減去預測值得到的，預測值是根據v->estimate得到的，具體怎麼得按實際情況來。
還有linearizeOplus函數，這個函數是求雅克比矩陣，也可以沒有這個函數。雅克比矩陣也就是誤差函數對頂點的求導值。不是範數啊。
過程：先把_vertices[0],_vertices[1]等賦值給頂點，跟computeError函數裏一模一樣，也是
const 頂點類* 頂點名=static_cast<const 頂點類*>(_vertices[0]);
是跟computeError函數裏重名的。
也會用到頂點的估計值來計算出雅克比矩陣的每一項的值。一般都是
_jacobianOplusXi(i,j)=;
2.定義圖模型
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每個誤差項優化變量維度爲3，誤差值維度爲1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDenseBlock::PoseMatrixType(); // 線性方程求解器
Block* solver_ptr = new Block( linearSolver ); // 矩陣塊求解器
// 梯度下降方法，從GN, LM, DogLeg 中選
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // 圖模型
optimizer.setAlgorithm( solver ); // 設置求解器
optimizer.setVerbose( true ); // 打開調試輸出
上面的基本就是套路了。
自己操作的是定義自己的誤差項的優化變量的維度，還有誤差值的維度，就是g2o::BlockSolverTraits<> 裏的兩個值。
線性方程求解器如果是提取特徵點之類的用Dense,稠密的，如果是要進行消元什麼的，用Cholmod.
優化算法一般用的都是列文伯格，也可以用高斯牛頓等。
過程就是由線性方程求解器linearSolver得到矩陣塊求解器solver_ptr,由矩陣塊求解器得到梯度下降方法solver,圖模型再設置算法爲solver.
2.1圖模型添加頂點
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
一般的模式就是先設置頂點，如果是多於一個頂點的話，以頂點數量爲最大值做一個for循環，在for循環裏，設置頂點，頂點估計值，頂點id,是否可以邊緣化，也就是之後要不要進行消元，圖模型添加邊。形式如下
頂點類* v=new 頂點類();要有一個括號，如果有輸入值的話，可以放在括號裏。
v->setId(i);
v->setEstimate();//估計值如果有初值，就添加上，如果沒有，就設成和頂點類型相同的，並且設爲0.如果頂點中update被重寫，那麼估計值類型和頂點類型不一致
v->setMarginlized(true);//默認false,需要設置的時候都是true
optimizer.addVertex(v)
注意頂點和邊都是指針，所以用->訪問。
2.2圖模型添加邊
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_sigmaw_sigma) ); // 信息矩陣：協方差矩陣之逆
optimizer.addEdge( edge );
}
根據邊的數量做一個for循環，先設置邊。然後邊要設置id,頂點，測量值，信息矩陣，圖模型添加邊。
形式如下，設邊名稱爲edge的話
邊類* edge=new 邊類();
edge->setId(j);
edge->setVertex(0,v);
edge->setMeasurement();測量值是必須得有的，視具體情況而定。
edge->setInformation();設置信息矩陣，信息矩陣是協方差矩陣的逆，所以也可以稱之爲設置協方差矩陣。
edge->setParameterId();//一般沒有這一項
optimizer->addEdge(edge);
2.3圖模型求解
optimizer.initializeOptimization();
optimizer.optimize(100);
直接就這兩句就可以了。
optimize()括號內的數值可以修改。