利用協方差約束單個方向的g2o寫法

由於匹配結果有時候只約束單個方向，比如法向，因此需要考慮只約束部分方向的誤差方程寫法，雖然可以使用自定義邊，但經過測試，自定義歐式距離邊加上位姿的求解通常不能收斂，因此這裡均採用位姿的誤差邊形式，借用協方差，來實現僅約束部分方向的實現。注意比較關鍵的是協方差方向是在區域性座標系下，而非世界座標系，原因是位姿相減，其平移向量殘差是在被減位姿所在座標系下的。

g2o::sparseoptimizer optimizer;

std::unique_ptrlinearsolver(new g2o::linearsolvercholmod());

std::unique_ptrblocksolver(new g2o::blocksolverx(std::move(linearsolver)));

g2o::optimizationalgorithmlevenberg* optimizationalgorithm = new g2o::optimizationalgorithmlevenberg(std::move(blocksolver));

optimizer.setalgorithm(optimizationalgorithm);

optimizer.setverbose(true);

g2o::parameterse3offset* cameraoffset = new g2o::parameterse3offset;

cameraoffset->setid(0);

optimizer.addparameter(cameraoffset);

eigen::matrixinf = eigen::matrix::zero();

inf(0, 0) = 1.0; inf(0, 1) = 0.0; inf(0, 2) = 0.0;

inf(1, 0) = 0.0; inf(1, 1) = 1.0; inf(1, 2) = 0.0;

inf(2, 0) = 0.0; inf(2, 1) = 0.0; inf(2, 2) = 1.0;

inf(3, 3) = 1e8; inf(3, 4) = 0.0; inf(3, 5) = 0.0;

inf(4, 3) = 0.0; inf(4, 4) = 1e8; inf(4, 5) = 0.0;

inf(5, 3) = 0.0; inf(5, 4) = 0.0; inf(5, 5) = 1e8;

eigen::isometry3d t;

double deg2rad = 3.14159265358979323846 / 180.0;

eigen::matrix3d rotrpy = g2o::internal::fromeuler(eigen::vector3d(-58.0118418734 * deg2rad, -4.2868610819 * deg2rad, 27.3365702914 * deg2rad));//which

t = rotrpy;

t.translation() = eigen::vector3d(-2848321.07828, 4660858.32744, 3282028.80025);

g2o::vertexse3* v = new g2o::vertexse3;

v->setid(0);

v->setestimate(t);

optimizer.addvertex(v);

size_t m_unaryedgenum = 0;

g2o::edgese3prior* e = new g2o::edgese3prior;

e->setvertex(0, v);

e->setid(m_unaryedgenum++);

e->setparameterid(0, 0);

e->setmeasurement(t);

e->setinformation(inf);

optimizer.addedge(e);

eigen::isometry3d t_new = t;

t_new.translation() = t.translation() - eigen::vector3d(10.0, 10.0, -5.0);

eigen::matrixinf_new = eigen::matrix::zero();

eigen::matrix3d pos_inf_new = eigen::matrix3d::identity();

pos_inf_new(0, 0) = 1e8;

pos_inf_new(1, 1) = 1e-8;

pos_inf_new(2, 2) = 1e8;

eigen::matrix3d pos_cov_new = pos_inf_new.inverse();

eigen::matrix3d rot = t.rotation();

eigen::matrix3d rot_t = rot.transpose();

eigen::matrix3d et = rot_t * pos_cov_new;

eigen::matrix3d pos_cov_trans = et* rot;

eigen::matrix3d pos_inf_trans = pos_cov_trans.inverse();

//eigen::matrix3d pos_inf_trans_llt = pos_cov_trans.llt().solve(eigen::matrix3d::identity());

eigen::matrix3d pos_inf_trans_llt = pos_cov_trans.ldlt().solve(eigen::matrix3d::identity());

std::cout << pos_inf_trans << std::endl;

std::cout << "llt:" << std::endl;

std::cout << pos_inf_trans_llt << std::endl;

bool issymmetric = pos_inf_trans_llt.transpose() == pos_inf_trans_llt;

std::cout << issymmetric << std::endl;

inf_new.block<3, 3>(0, 0) = pos_inf_trans_llt;

inf_new(3, 3) = 1e8; inf_new(3, 4) = 0.0; inf_new(3, 5) = 0.0;

inf_new(4, 3) = 0.0; inf_new(4, 4) = 1e8; inf_new(4, 5) = 0.0;

inf_new(5, 3) = 0.0; inf_new(5, 4) = 0.0; inf_new(5, 5) = 1e8;

g2o::edgese3prior* e_new_z = new g2o::edgese3prior;

e_new_z->setvertex(0, v);

e_new_z->setid(m_unaryedgenum++);

e_new_z->setparameterid(0, 0);

e_new_z->setmeasurement(t_new);

e_new_z->setinformation(inf_new);

optimizer.addedge(e_new_z);

std::cout << std::fixed << std::setprecision(8) << inf_new << std::endl;

bool psdflag = optimizer.verifyinformationmatrices(true);

if (!psdflag)

optimizer.initializeoptimization();

int iteration_count = optimizer.optimize(2000);

std::cout << "finish" << std::endl;

注意由於計算器捨入誤差，pos_inf_trans_llt通常不是對稱矩陣，因此導致g2o驗證正定失敗，雖然可以跳過驗證這一步，不影響優化結果，但是程式設計師不允許出現warning，因此通過把上三角賦值給下三角來解決：

pos_inf_trans_llt(1, 0) = pos_inf_trans_llt(0, 1);

pos_inf_trans_llt(2, 0) = pos_inf_trans_llt(0, 2);

pos_inf_trans_llt(2, 1) = pos_inf_trans_llt(1, 2);

利用協方差約束單個方向的g2o寫法

安裝g2o注意的問題找不到CHOLMOD

視覺SLAM十四講筆記 G2O庫的安裝

ubuntu下g2o的安裝編譯，以及遇到問題的解決

利用協方差約束單個方向的g2o寫法

安裝g2o注意的問題 找不到CHOLMOD

視覺SLAM十四講筆記 G2O庫的安裝

ubuntu下g2o的安裝編譯，以及遇到問題的解決

相關推薦

安裝g2o注意的問題找不到CHOLMOD