对g2o图优化的理解与c++实践
g2o
g2o是General（Hyper）Graph Optimization [1] 的缩写，是一个C ++框架(g2o在github的代码，注意有Python库和.Net库的g2o)。它把优化问题变成一个图。节点是要优化的变量，边是误差。它将许多典型的顶点和边缘实现为可以直接调用和使用的类，例如VertexSE3Expmap在SE3空间中表示机器人姿势，VertexSBAPointXYZ以表示3-D点，EdgeProjectXYZ2UV以表示相机图像平面中的3D点的观察。此外，实现了典型的优化求解器算法。使用g2o库，SLAM研究人员需要做的是定义问题中的节点和边缘，将它们添加到g2o提供的求解器中，它将执行所有优化的东西。g2o现在是SLAM研究人员中广泛使用的库，被许多着名的SLAM或VO作品采用，如: ORB_SLAM [2]和SVO [3]。
介绍
SLAM问题需要后端来优化地图和在其前端构建的姿势。后端通常是过滤框架（如EKF）或图形优化（即束调整）。如今，图形优化更受欢迎，并已成为最先进的方法。图优化的一般思想是将SLAM问题表示为图结构。如下图所示，图形包含两种类型的元素，节点（顶点）和约束（边缘）。
对于SLAM问题，机器人的关键帧姿势或地图中的地标位置表示为节点，而关键帧和关键帧，关键帧和地标，或地标和地标之间的观测和几何模型表示为连接某些的约束。节点。给定图形，图形优化旨在找到节点值的最佳估计，其最小化由约束确定的误差。因此，SLAM后端被转换为最小二乘最小化问题，可以通过以下等式来描述：
cmakelists.txt文件如下：
cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
# 寻找G2O
find_package( G2O REQUIRED )
include_directories(
${G2O_INCLUDE_DIRS}
"/usr/include/eigen3"
)
# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )
add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting
${OpenCV_LIBS}
g2o_core g2o_stuff
)
c++代码为：
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
if (DENSE) {
linearSolver= new g2o::LinearSolverDense<g2o
::BlockSolver_6_3::PoseMatrixType>();
} else {
linearSolver
= new g2o::LinearSolverCholmod<g2o
::BlockSolver_6_3::PoseMatrixType>();
}
g2o::BlockSolver_6_3 * solver_ptr
= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
double focal_length= 1000.;
Vector2d principal_point(320., 240.);
vector<g2o::SE3Quat,
aligned_allocator<g2o::SE3Quat> > true_poses;
g2o::CameraParameters * cam_params
= new g2o::CameraParameters (focal_length, principal_point, 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
int vertex_id = 0;
for (size_t i=0; i<15; ++i) {
Vector3d trans(i*0.04-1.,0,0);
Eigen:: Quaterniond q;
q.setIdentity();
g2o::SE3Quat pose(q,trans);
g2o::VertexSE3Expmap * v_se3
= new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
if (i<2){
v_se3->setFixed(true);
}
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);
true_poses.push_back(pose);
vertex_id++;
}
// Note: code has been simplified for demo convenience
for (size_t i=0; i<true_points.size(); ++i){
g2o::VertexSBAPointXYZ * v_p
= new g2o::VertexSBAPointXYZ();
v_p->setId(point_id);
v_p->setMarginalized(true);
v_p->setEstimate(true_points.at(i)
+ Vector3d(Sample::gaussian(1),
Sample::gaussian(1),
Sample::gaussian(1)));
optimizer.addVertex(v_p);
for (size_t j=0; j<true_poses.size(); ++j){
// Add edges. See the following passage.
}
++point_id;
}
for (size_t j=0; j<true_poses.size(); ++j){
Vector2d z
= cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
double sam = Sample::uniform();
z += Vector2d(Sample::gaussian(PIXEL_NOISE),
Sample::gaussian(PIXEL_NOISE));
g2o::EdgeProjectXYZ2UV * e
= new g2o::EdgeProjectXYZ2UV();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertices().find(j)->second));
e->setMeasurement(z);
e->information() = Matrix2d::Identity();
e->setParameterId(0, 0);
optimizer.addEdge(e);
}
optimizer.initializeOptimization();
cout << "Performing full BA:" << endl;
optimizer.optimize(10);
参考文献：
[1] Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard g2o: A General Framework for Graph Optimization IEEE International Conference on Robotics and Automation (ICRA), 2011
[2] ORB_SLAM
[3] SVO
[4] SLAM Implementation: Bundle Adjustment with g2o
