用節點表示相機和路標，用邊表示他們之間的觀測，使用自定義的點和邊
#include
#include
#include
#include
#include
#include
#include < iostream>
#include 「common.h」
#include 「sophus/se3.hpp」
using namespace sophus;
using namespace eigen;
using namespace std;
/// 姿態和內參的結構
struct poseandintrinsics
/////從給定的data位址來獲取相機r,t,和內參
explicit poseandintrinsics(double *data_addr)
/////將上面獲取的估計值放入記憶體
void set_to(double *data_addr)
//so3d rotation;
vector3d translation = vector3d::zero();
double focal = 0;
double k1 = 0, k2 = 0;
};///定義相機位姿加相機內參的頂點，9維，前三維為so3，接下去為t, f, k1, k2以及四個基本函式
class vertexposeandintrinsics : public g2o::basevertex<9, poseandintrinsics>
//virtual void settooriginimpl() override
//virtual void oplusimpl(const double *update) override
///// 根據估計值投影乙個世界點到相機歸一化座標系下並去除畸變
vector2d project(const vector3d &point)
//virtual bool read(istream &in) {}
//virtual bool write(ostream &out) const {}
};//定義路標點p頂點
class vertexpoint : public g2o::basevertex<3, vector3d>
//virtual void settooriginimpl() override
//virtual void oplusimpl(const double *update) override
//virtual bool read(istream &in) {}
//virtual bool write(ostream &out) const {}
};定義邊（觀測值z的維度u,v是2），邊連線的兩個頂點的型別
class edgeprojection :
public g2o::basebinaryedge<2, vector2d, vertexposeandintrinsics, vertexpoint>
//// use numeric derivatives
virtual bool read(istream &in) {}
//virtual bool write(ostream &out) const {}
void solveba(balproblem &bal_problem);
int main(int argc, char **ar**)
//balproblem bal_problem(ar**[1]);
bal_problem.normalize();
bal_problem.perturb(0.1, 0.5, 0.5);
bal_problem.writetoplyfile(「initial.ply」);
solveba(bal_problem);
bal_problem.writetoplyfile(「final.ply」);
//return 0;
}void solveba(balproblem &bal_problem)
for (int i = 0; i < bal_problem.num_points(); ++i)
//// edge
for (int i = 0; i < bal_problem.num_observations(); ++i)
optimizer.optimize(40);
////set to bal problem，由於最終problem生成.ply檔案所以要使用set_to函式
for (int i = 0; i < bal_problem.num_cameras(); ++i)
for (int i = 0; i < bal_problem.num_points(); ++i)
}
g2o學習 g2o整體框架
進來對g2o優化庫進行了學習，雖然才模仿著寫了兩個例程，但是對於整個g2o的理解和使用方面還是多了不少的感觸，特此寫下部落格，對這些天的學習進行記錄。說到整體的結構，不得不用一張比較概括的圖來說明 這張圖最好跟著畫一下，這樣能更好的理解和掌握，例如我第一次看的時候根本沒有注意說箭頭的型別等等的細節。...
g2o使用記錄
幾元邊就有幾個jacobian 例如g2o定義的二元邊就有 jacobianoplusxi和 jacobianoplusxj 原始碼中baseedge定義如下 d決定了誤差的維度 template int d,typename e class baseedge public optimizableg...
G2O使用要點總結
頂點的定義 繼承basevertex 定點byte數，頂點結構體 類 實現settoriginimpl 函式，裡面給 estimate成員函式賦初值 estimate的型別就是模板裡指定的頂點結構體 實現 oplusimpl const double update update 是指向跟新值得指標，...