Contents
Introduction
SLAM problems require a back-end to refine the map and poses constructed in its front-end. The back-end is usually either a filtering framework (like EKF) or graph optimization (i.e. bundle adjustment). Nowadays, graph optimization is much more popular, and has become a state-of-art method.
The general idea of graph optimization is to express the SLAM problem as a graph structure. As the figure below shows, a graph contains two types of elements, nodes (vertices) and constraints (edges). For SLAM problems, a keyframe pose of the robot or a landmark position in the map is denoted as a node, while the observations and geometric model between keyframe and keyframe, keyframe and landmark, or landmark and landmark are denoted as the constraints that connected certain nodes.
Given a graph, graph optimization aims to find an optimal estimation of the nodes values which minimize the errors that determined by the constraints. Therefore, SLAM back-end is transformed to be a least squares minimization problem, which can be described by the following equation:
g2o
g2o, short for General (Hyper) Graph Optimization [1], is a C++ framework for performing the optimization of nonlinear least squares problems that can be embedded as a graph or in a hyper-graph. It implements many typical vertices and edges as classes that can be directly called and used, like VertexSE3Expmap to represent robot poses in SE3 space, VertexSBAPointXYZ to represent 3-D points, EdgeProjectXYZ2UV to represent observations of 3D points in camera image plane. Also, typical optimization solver algorithms are implemented. With g2o library, what SLAM researcher need to do is defining the nodes and edges in their problems, adding them to the solver provided by g2o, and it will execute all the optimization stuff. g2o is now is a widely used library among SLAM researchers, adopted in many famous SLAM or VO works like ORB_SLAM [2] and SVO [3].
Bundle Adjustment Demo
Code Review
We use one of the example code from g2o to do this bundle adjustment demo. The demo first generalizes some simulated 3D points and keyframe poses, as well the observations of the 3D points in the keyframe camera plane with Gaussian noises, then optimize the graph made up by them. Let’s briefly examine the code.
First initialize the optimizer. As the optimization structure may be complicated, we need to assign many solver types mannually:
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);
In this problem, camera intrinsic parameter is also required as part of the measurement constraints, which can be added to the optimizer like:
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);
}
Then we add all poses as vertices:
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++;
}
And map points as well:
// 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;
}
During the process of adding map points vertices, the edges connecting map point vertex and corresponding keyframe vertex are also added:
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);
}
Please note that for each edge, the two vertices it connects should be specified. Also, an information matrix should be given. In its physical meaning, information matrix represents how reliable this measurement is. Therefore, the more precisely the measurement is made or the more you trust in this measurement, the larger values in the information matrix you can set.
Finally, we can do the optimization:
optimizer.initializeOptimization();
cout << "Performing full BA:" << endl;
optimizer.optimize(10);
Build the Demo
To build this project, we must install g2o and include and link to it. The installation routine of g2o is given in its project homepage. To include and link to g2o, we use cmake here, with a CMakeLists.txt like:
project(ba_demo)
cmake_minimum_required(VERSION 2.8)
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules/)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -Wall")
FIND_PACKAGE(Eigen REQUIRED)
FIND_PACKAGE(CSparse REQUIRED)
FIND_PACKAGE(Cholmod REQUIRED)
FIND_PACKAGE(G2O REQUIRED)
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}
${EIGEN_INCLUDE_DIRS}
${CSPARSE_INCLUDE_DIR}
${Cholmod_INCLUDE_DIR}
${G2O_INCLUDE_DIR}
/usr/include/suitesparse
)
LIST(APPEND G2O_LIBS
cxsparse
cholmod
g2o_cli g2o_simulator
g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d
g2o_core g2o_solver_csparse g2o_solver_structure_only
g2o_types_sba g2o_types_slam3d g2o_csparse_extension
g2o_solver_dense g2o_stuff
g2o_types_sclam2d g2o_parser g2o_solver_pcg
g2o_types_data g2o_types_sim3
)
aux_source_directory(. DIR_SRCS)
add_executable(ba_demo ${DIR_SRCS})
target_link_libraries(ba_demo
${G2O_LIBS}
)
Do not forget to put the required .cmake files in a sub-directory of the project like:
Run the Demo
Run the program, and we can get:
PIXEL_NOISE: 1
OUTLIER_RATIO: 0
ROBUST_KERNEL: 0
STRUCTURE_ONLY: 0
DENSE: 0
Performing full BA:
iteration= 0 chi2= 132333131.273130 time= 0.128667 cumTime= 0.128667 edges= 4881 schur= 1 lambda= 2949.576169 levenbergIter= 1
iteration= 1 chi2= 5627495.269566 time= 0.108231 cumTime= 0.236898 edges= 4881 schur= 1 lambda= 983.192056 levenbergIter= 1
iteration= 2 chi2= 216111.580556 time= 0.106446 cumTime= 0.343344 edges= 4881 schur= 1 lambda= 327.730685 levenbergIter= 1
iteration= 3 chi2= 41147.168515 time= 0.106624 cumTime= 0.449968 edges= 4881 schur= 1 lambda= 109.243562 levenbergIter= 1
iteration= 4 chi2= 15799.044991 time= 0.106301 cumTime= 0.556268 edges= 4881 schur= 1 lambda= 36.414521 levenbergIter= 1
iteration= 5 chi2= 10624.163303 time= 0.106286 cumTime= 0.662555 edges= 4881 schur= 1 lambda= 12.138174 levenbergIter= 1
iteration= 6 chi2= 9855.835927 time= 0.106119 cumTime= 0.768674 edges= 4881 schur= 1 lambda= 8.092116 levenbergIter= 1
iteration= 7 chi2= 9601.932859 time= 0.10637 cumTime= 0.875044 edges= 4881 schur= 1 lambda= 5.394744 levenbergIter= 1
iteration= 8 chi2= 9172.279560 time= 0.200304 cumTime= 1.07535 edges= 4881 schur= 1 lambda= 7.192992 levenbergIter= 2
iteration= 9 chi2= 8892.451941 time= 0.200096 cumTime= 1.27544 edges= 4881 schur= 1 lambda= 9.590656 levenbergIter= 2
Point error before optimisation (inliers only): 1.74126
Point error after optimisation (inliers only): 0.423081
It can be seen from the output that g2o did reduce the point errors.
References
[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 [PDF]
[2] ORB_SLAM
[3] SVO