#include
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"
#include "ceres/autodiff.h"
#include "tools/rotation.h"
#include "common/projection.h"
//定義相機位姿頂點類,由於相機內參也作為優化變數,所以包含了:
//焦距f,畸變係數k1 k2, 3個引數的平移,3個引數的旋轉。一共九個量,9維,型別為eigen::vectorxd
class vertexcamerabal : public g2o::basevertex<9,eigen::vectorxd>
//這裡的讀寫功能函式就需要用了,引數分別是輸入輸出流型別例項的引用
virtual
bool read ( std::istream& /*is*/ )
virtual
bool write ( std::ostream& /*os*/ ) const
//設定頂點的初始值
virtual
void settooriginimpl() {}
//增量函式,增量為傳進的引數update,這裡是9個double值,所以就是double型別指標了(其實也就是陣列)
virtual
void oplusimpl ( const
double* update )
};//landmark型別頂點,維度3維,型別是eigen::vector3d
class vertexpointbal : public g2o::basevertex<3, eigen::vector3d>
virtual
bool read ( std::istream& /*is*/ )
virtual
bool write ( std::ostream& /*os*/ ) const
virtual
void settooriginimpl() {}
virtual
void oplusimpl ( const
double* update )
};//bal觀測邊,邊即誤差,繼承自基礎二元邊。這裡誤差應該是重投影的畫素誤差
// 引數為:誤差維度2維,誤差型別為eigen::vector2d,連線兩個頂點:vertexcamerabal和vertexpointbal(也就是說誤差和這兩個優化變數有關)
class edgeobservationbal : public g2o::basebinaryedge<2, eigen::vector2d, vertexcamerabal, vertexpointbal>
virtual
bool read ( std::istream& /*is*/ )
virtual
bool write ( std::ostream& /*os*/ ) const
//誤差計算函式
virtual
void computeerror() override // the virtual function comes from the edge base class. must define if you use edge.
//這裡即為過載的()函式,為模板函式,需要資料為相機位姿指標,空間點位置指標,用於承接輸出誤差的residuals。
// 上面呼叫時,用的_error.data()承接,完成誤差計算。
//這個模板類其實還是用的重投影誤差
template
bool
operator() ( const t* camera, const t* point, t* residuals ) const
//小總結一下,從computeerror()一直到這裡,搞得這一些就是為了計算乙個重投影誤差,
//誤差的計算被寫進了過載的()中,投影過程被寫進了camprojectionwithdistortion()中
//這裡重寫線性增量方程,也就是雅克比矩陣
virtual
void linearizeoplus() override
*///這裡來乙個typedef,將模板類簡化定義一下,定義成balautodiff
//看一下模板引數:
//edgeobservationbal,就是代價函式型別,這裡就是邊的型別了
//模板型別為double,不過具體這個double指什麼,還不大清楚,基本資料的型別?
//vertexcamerabal::dimension和vertexpointbal::dimension就是對應的兩個n0和n1,誤差函式引數的維度,這裡直接把維度取出來了(dimension即是取得維度),也可以直接輸入9和3
typedef ceres::internal::autodiffdouble, vertexcamerabal::dimension, vertexpointbal::dimension> balautodiff;
//這裡的dimension就是邊的維度(這裡還是在邊類定義中的linearizeoplus()函式定義)。定義如下,可知dimension為2維。
// static const int dimension = baseedge::dimension;
//定義乙個行優先的double型別矩陣,大小為dimension*vertexcamerabal::dimension,也就是2*9。這裡就是誤差對相機的導數
eigen::matrix derror_dcamera;
//定義乙個行優先的double型別矩陣,大小為dimension*vertexpointbal::dimension,也就是2*3。這裡就是誤差對空間點的導數
eigen::matrix derror_dpoint;
//double*型別的陣列,成員為double*,這裡裝了相機估計值陣列指標和空間點估計值陣列指標。
double * parameters = ;
//雅克比矩陣為兩塊導數拼合起來的,一塊是誤差對相機的導數,一塊是誤差對空間點的導數。也就是上方定義的2*9的derror_dcamera和2*3的derror_dpoint
double * jacobians = ;
//建立乙個double型別的value陣列,大小為dimension,2個元素。幹啥的??
double value[dimension];
//這裡就是一直所說的利用ceres的現行求導,這個differentiate()就是在autodiff結構體中定義的。
/*static bool differentiate(const functor& functor,
t const *const *parameters,
int num_outputs,
t *function_value,
t **jacobians) */
//看一下引數:
//const functor& functor,代價函式,這裡也就是這個邊類了,直接用*this
//t const *const *parameters,引數列表,就是上面定義的有兩個double指標的parameters陣列,這兩個指標乙個指向相機引數陣列,乙個指向空間點陣列
//int num_outputs,輸出的維度,這裡就是邊的維度dimension,也就是2維
//t *function_value,誤差函式functor的輸出值,用於承接functor的輸出,也就是*this計算出來的誤差。
//t **jacobians,這就是最終要求的雅克比矩陣了。用於承接。
bool diffstate = balautodiff::differentiate ( *this, parameters, dimension, value, jacobians );
//複製一下雅克比矩陣,將行優先轉換為列優先。為什麼?行列優先有啥區別
// copy over the jacobians (convert row-major -> column-major)
//雅克比矩陣到這裡就計算完成了,最後就是賦值給_jacobianoplusxi和_jacobianoplusxj了。
//防呆用的,判斷一下是夠計算成功
if ( diffstate )
else
}};
g2o學習 g2o整體框架
進來對g2o優化庫進行了學習,雖然才模仿著寫了兩個例程,但是對於整個g2o的理解和使用方面還是多了不少的感觸,特此寫下部落格,對這些天的學習進行記錄。說到整體的結構,不得不用一張比較概括的圖來說明 這張圖最好跟著畫一下,這樣能更好的理解和掌握,例如我第一次看的時候根本沒有注意說箭頭的型別等等的細節。...
g2o擬合曲線
cmakelists cmake minimum required version 3.7 project g2o curve fitting robin set cmake cxx standard 11 新增g2o的cmake modual path find package g2o requi...
g2o學習筆記
綜上所述,在g2o中選擇優化方法一共需要三個步驟 選擇乙個線性方程求解器,從pcg csparse choldmod三個選一 選擇乙個blocksolver 選擇乙個迭代器,從gn lm doglog中選 其中read和write函式可以不進行覆寫,僅僅宣告一下就可以了,settooriginimp...