1、g2o_bal_class.h
1.1 projection.h
g2o還是用圖模型和邊，頂點就是相機和路標，邊就是觀測，就是像素坐標。只不過這里的相機是由
旋轉（3個參數，軸角形式，就是theta*nx,theta*ny,theta*ny),位移(3個參數），f,k1,k2.
就是之前BA模型的實現。
但是這里歸一化平面坐標取得是負值，而且最后沒有加cx,cy.
具體實現在projection.h中的CamProjectionWithDistortion函數實現的，而其中世界坐標轉成相機坐標是由rotation.h中的函數AngleAxisRotatePoint實現的。具體參見軸角和四元數和空間點.note
1.2頂點
自定義了相機頂點和路標頂點（名字自己定），繼承自g2o::BaseVertex.一個9維，Eigen::VectorXd，一個3維，Eigen::Vector3d.
跟之前一樣，不一樣的是有了覆蓋。就是對oplusImpl函數用override進行覆蓋。覆蓋形式為
virtual void oplusImpl(const double* update) override{}
里面會對update進行操作，變成v，之前都是 _estimate += Eigen::Vector3d(update);
而這里update不再是Eigen::Vector3d的形式，而是變成了地圖類型。
一個是Eigen::VectorXd::ConstMapType v(update,VertexCameraBAL::Dimension);
一個是Eigen::Vector3d::ConstMapType v(update);
然后_estimate+=v就可以了。
1.3邊
觀測值在當前圖片中可以看到這個路標，通過計算描述子可以在當前圖片中找到和路標匹配的特征點，這個匹配的特征點的像素坐標就是觀測值。
同樣還是把vertex(0)賦值給頂點camera,vertex(1)賦值給頂點point.
這里的計算誤差的函數被重寫了。
同樣virtual void computeError() override{ }
而計算誤差用了括號運算符()，在template<typename T>下。
過程是由CamProjectionWithDistortion(camera,point,predictions)得到預測值predictions,然后預測值與測量值measurement()的對應項相減得到residual[0],residual[2].
virtual void linearizeOplus() override{}，這里linearizeOplus也被重寫了。
同樣還是先賦值頂點。然后自定義BalAutoDiff自動求導函數。
typedef ceres::internal::AutoDiff<EdgeObservationBAL,double,VertexCameraBAL::Dimension,VertexPointBAL::Dimension> BalDutoDiff,
用了ceres的autodiff,里面放的四個參數分別是邊類，數據類型，兩個頂點類。
定義對Camera頂點求導后的矩陣，dError_dCamera,維度Dimension*頂點的維度，應該是9.
因為過往的雅克比矩陣是2*6形式。每個誤差都有一個雅可比矩陣，這里應該是每個雅可比矩陣的形式。
和對Point頂點求導后的矩陣dError_dPoint.
Eigen::Matrix<double,Dimension,VertexCameraBAL::Dimension,Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double,Dimension,VertexPointBAL::Dimension,Eigen::RowMajor> dError_dPoint.
parameters是由cam的估計值和point的估計值組成，指針形式。
jacobian是由camera的導數和point的導數組成。
使用BALAutoDiff中的Differentiate函數。放入邊，參數，維度Dimension,value(它的值就是Dimension),還有要得到雅可比矩陣jacobian.
返回值是一個bool值，表示的是差分狀態，也就是求導狀態。
如果它為true的話，_jacobianXi就為dError_dCamera,_jacobianXj就為dError_dPoint,如果返回false,那么，_jacobianXi,_jacobianXj就設為0.
問題就是Dimension這個值具體是什么，不知道。
2.g2o_bundle.cpp
2.1BuildProblem函數
作用：給圖模型添加頂點，添加邊
變量：bal_problem類，指針形式，圖模型optimizer,優化參數params
2.1.1添加頂點
從bal_problem中讀取路標數量，相機位姿數量，相機塊大小9或10（取決于旋轉用四元數還是用軸角表示），路標塊大小3。
造一個數raw_cameras,它是一個指針，由balProblem的cameras函數得出，這個函數的返回值是parameters,parameters為9*相機位姿的數量再加上3*路標點的數量。

做一個循環，閾值是相機位姿數量，接下來是要添加相機頂點。
類型為typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
值為(raw_cameras+9*i,9)
相機的估計值就是temVecCamera;估計值應該是跟相機頂點類型一樣的啊。應該也是Eigen::VectorXd的。
然后頂點的估計值就是temVecCamera,id就是i(一般在for循環里，頂點id都設成i.)圖模型添加相機頂點。頂點就是之前定義的類。一般指針形式，后面加new,是為了分配一個對象給它。
VertexCameraBAL* pCamera = new VertexCameraBAL();
添加路標頂點
也是造一個數指針raw_points,它的返回值是parameters+9*相機位姿數量，對每個路標點來說，
constVectorRef temVecPoint(raw_points+3*j,3)
同樣設置路標頂點為pPoint,pPoint的估計值就為temVecPoint.
這里要設置路標點可以進行邊緣化，也就是可以進行消元。但為什么相機沒有設這個值。
圖模型添加這個頂點pPoint.
這里設置路標點id為j+num_cameras,因為為了保證每個頂點都有一個對應的id.
2.1.2添加邊
定義常量觀測值的數量為num_observations。
定義指針observations對應的值為2*觀測值的數量。
利用觀測值造一個循環

設置魯棒核函數，之所以要設置魯棒核函數是為了平衡誤差，不讓二范數的誤差增加的過快。魯棒核函數里要自己設置delta值，這個delta值是，當誤差的絕對值小于等于它的時候，誤差函數不變。否則誤函數變為delta(|e|-1/2delta).
如果輸入的參數params的魯棒性檢驗通過if(params.robustify)
那么設置魯棒核函數為g2o里的huber loss函數。
g2o::RobustKernelHuber* rk=new g2o::RobustKernelHuber;
設置delta值為1.0.設置邊的魯棒核函數為rk..setDelta,.setRobustKernel().
設置邊的頂點0對應的相機頂點為optimizer->vertex(camera_id),指針camera_id為觀測值的數量[i]
設置邊的頂點1對應的路標頂點為optimizer->vertex(point_id),指針point_id為觀測值的數量[i]+相機數量.
設置其協方差矩陣也就是.setInformation，為2*2的單位矩陣。
設置其測量值為observations[2*i+0],observations[2*i+1].
圖模型添加邊。
設頂點，設估計值，設id，圖模型添加頂點。
設邊，設頂點，設協方差矩陣，設測量值，圖模型添加邊。

3.圖模型求解
還是之前一樣的過程，這里自定義類型名為BalBlockSolver,這里是造了一個函數SetSolverOptionsFromFlags,變量是bal_problem類，params參數，圖模型optimizer.
先把矩陣塊求解器solver_ptr,線性方程求解器linearSolver初值設為0
如果params.linear_solver的值為dense_schur,那么線性方程求解器用稠密的，也就是Dense,
如果params.linear_solver的值為sparse_schur，那么linearSolver用稀疏的，也就是Cholmod,而且設置linearSolver對矩陣排序保持稀疏性
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true); 這個只在稀疏的情況下使用。
由linearSolver得到矩陣塊求解器solver_ptr.
如果params.trust_region_strategy為列文伯格-馬夸而特，那么梯度下降方法solver就選為列文伯格-馬夸而特
如果為dogleg,那就用dogleg來做梯度下降方法solver.

g2o在做BA的優化時必須將其所有點云全部schur掉，否則會出錯。其原因在于我們使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>這個類型來指定linearsolver,其中模板參數當中的位姿矩陣類型在程序中為相機姿態參數的維度，于是BA當中schur消元后解得線性方程組必須是只含有相機姿態變量。
Ceres庫則沒有g20這樣的限制，Ceres給了開發者很大的空間去操作自己的優化策略，它在Schur消元操作時完全不需要把所有點云都消元掉，用戶可以自己編寫函數選擇消元哪些點云。接下來我們也會給出Ceres的BA。

?

轉載于:https://www.cnblogs.com/talugirl/p/7384115.html

總結

以上是生活随笔為你收集整理的g2o求解BA 第10章的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。