1、g2o: A General Framework for Graph OptimizationRainer Kummerle Giorgio Grisetti Hauke Strasdat Kurt Konolige Wolfram BurgardAbstract Many popular problems in robotics and computervision including various types of simultaneous localization andmapping (SLAM) or bundle adjustment (BA) can be phrasedas
2、least squares optimization of an error function that canbe represented by a graph. This paper describes the generalstructure of such problems and presents g2o, an open-sourceC+ framework for optimizing graph-based nonlinear errorfunctions. Our system has been designed to be easily extensibleto a wid
3、e range of problems and a new problem typically canbe specified in a few lines of code. The current implementationprovides solutions to several variants of SLAM and BA. Weprovide evaluations on a wide range of real-world and simulateddatasets. The results demonstrate that while being general g2ooffe
4、rs a performance comparable to implementations of state-of-the-art approaches for the specific problems.I. INTRODUCTIONA wide range of problems in robotics as well asin computer-vision involve the minimization of a non-linear error function that can be represented as a graph.Typical instances are si
5、multaneous localization and map-ping (SLAM) 19, 5, 22, 10, 16, 26 or bundleadjustment (BA) 27, 15, 18. The overall goal in theseproblems is to find the configuration of parameters or statevariables that maximally explain a set of measurementsaffected by Gaussian noise. For instance, in graph-basedSL
6、AM the state variables can be either the positions of therobot in the environment or the location of the landmarksin the map that can be observed with the robots sensors.Thereby, a measurement depends only on the relative loca-tion of two state variables, e.g., an odometry measurementbetween two con
7、secutive poses depends only on the con-nected poses. Similarly, in BA or landmark-based SLAM ameasurement of a 3D point or landmark depends only on thelocation of the observed point in the world and the positionof the sensor.All these problems can be represented as a graph. Whereaseach node of the g
8、raph represents a state variable to opti-mize, each edge between two variables represents a pairwiseobservation of the two nodes it connects. In the literature,many approaches have been proposed to address this classof problems. A naive implementation using standard meth-ods like Gauss-Newton, Leven
9、berg-Marquardt (LM), Gauss-Seidel relaxation, or variants of gradient descent typicallyprovides acceptable results for most applications. However,This work has partly been supported by the European Commission underFP7-231888-EUROPA and the European Research Council Starting Grant210346.R. Kummerle,
10、G. Grisetti, and W. Burgard are with the University ofFreiburg. H. Strasdat is with the Department of Computing, Imperial CollegeLondon. K. Konolige is with Willow Garage and a Consulting Professor atStanford University.TrajectoryLandmarksTrajectoryLandmarks(a)(b)Fig. 1. Real-world datasets processe
11、d with our system: The first row of(a) shows the Victoria Park dataset which consists of 2D odometry and 2Dlandmark measurements. The second row of (a) depicts a 3D pose graph ofa multi-level parking garage. While the left images shows the initial states,the right column depicts the respective resul
12、t of the optimization process.Full and zoomed view of the Venice bundle adjustment dataset after beingoptimized by our system (b). The dataset consists of 871 camera poses and2,838,740 projections.to achieve the maximum performance substantial efforts anddomain knowledge are required.In this paper,
13、we describe a general framework for per-forming the optimization of nonlinear least squares problemsthat can be represented as a graph. We call this frameworkg2o (for “general graph optimization”). Figure 1 gives anoverview of the variety of problems that can be solvedby using g2o as an optimization
14、 back-end. The proposedsystem achieves a performance that is comparable withimplementations of state-of-the-art algorithms, while beingable to accept general forms of nonlinear measurements. Weachieve efficiency by utilizing algorithms thatnull exploit the sparse connectivity of the graph,null take
15、advantage of the special structures of the graph thatoften occur in the problems mentioned above,null use advanced methods to solve sparse linear systems,null and utilize the features of modern processors like SIMDinstructions and optimize the cache usage.Despite its efficiency, g2o is highly genera
16、l and extensible:a 2D SLAM algorithm can be implemented in less than 30lines of C+ code. The user only has to specify the errorfunction and its parameters.In this paper, we apply g2o to different classes of leastsquares optimization problems and compare its performancewith different implementations
17、of problem-specific algo-rithms. We present evaluations carried out on a large set ofreal-world and simulated data-sets; in all experiments g2ooffered a performance comparable with the state-of-the-artapproaches and in several cases even outperformed them.The remainder of this paper is organized as
18、follows. Wefirst discuss the related work with a particular emphasis onsolutions to the problems of SLAM and bundle adjustment.Subsequently, in Section III we characterize the graph-embeddable optimization problems that are addressed byour system and discuss nonlinear least-squares via Gauss-Newton
19、or LM. In Section IV we then discuss the featuresprovided by our implementation. Finally, in Section V, wepresent an extensive experimental evaluation of g2o andcompare it to other state-of-the-art, problem-specific meth-ods.II. RELATED WORKIn the past, the graph optimization problems have beenstudi
20、ed intensively in the area of robotics and computervision. One seminal work is that of Lu and Milios 19where the relative motion between two scans was measuredby scan-matching and the resulting graph was optimized byiterative linearization. While at that time, optimization ofthe graph was regarded a
21、s too time-consuming for realtimeperformance, recent advancements in the development ofdirect linear solvers (e.g., 4), graph-based SLAM has re-gained popularity and a huge variety of different approachesto solve SLAM by graph optimization have been proposed.For example, Howard et al. 12 apply relax
22、ation to build amap. Duckett et al. 6 propose the usage of Gauss-Seidelrelaxation to minimize the error in the network of constraints.Frese et al. 8 introduced multi-level relaxation (MLR), avariant of Gauss-Seidel relaxation that applies the relaxationat different levels of resolution. Recently, Ol
23、son et al. 22suggested a gradient descent approach to optimize posegraphs. Later, Grisetti et al. 10 extended this approachby applying a tree-based parameterization that increasesthe convergence speed. Both approaches are robust to theinitial guess and rather easy to implement. However, theyassume t
24、hat the error is uniformly distributed and thus havedifficulties in optimizing pose-graphs where some constraintshave covariances with null spaces or substantial differencesin the eigenvalues.Graph optimization can be viewed as a nonlinear least-squares problem, which typically is solved by forming
25、alinear system around the current state, solving, and iterating.One promising technique for solving the linear system ispreconditioned conjugate gradient (PCG), which was usedby Konolige 17 as well as Montemerlo and Thrun 20 asan efficient solver for large sparse pose constraint systems.Because of i
26、ts high efficiency on certain problems, g2o.x1x2x3x4e12e31e23e24 F(x) = enull12 12 e12+enull23 23 e23+enull31 31 e31+enull24 24 e24+ . . .Fig. 2. This example illustrates how to represent an objective function bya graph.includes an implementation of a sparse PCG solver whichapplies a block-Jacobi pr
27、e-conditioner 13.More recently, Dellaert and colleagues suggested a systemcalled nullSAM 5 which they implement using sparse directlinear solvers 4. Kaess et al. 14 introduced a variant ofthis called iSAM that is able to update the linear matrixassociated with the nonlinear least-squares problem. Ko
28、no-lige et al. 16 showed how to construct the linear matrixefficiently by exploiting the typical sparse structure of thelinear system. However, the latter approach is restricted to2D pose graphs. In g2o we share similar ideas with thesesystems. Our system can be applied to both SLAM and BAoptimizati
29、on problems in all their variants, e.g., 2D SLAMwith landmarks, BA using a monocular camera, or BA usingstereo vision. However, g2o showed a substantially improvedperformance compared these systems on all the data we usedfor evaluation purposes.In computer vision, Sparse Bundle Adjustment 27 is anon
30、linear least-squares method that takes advantage of thesparsity of the Jacobian pattern between points and cameraposes. Very recently, there have been several systems 15,13 that advance similar concepts of sparse linear solversand efficient calculation of the Schur reduction (see SectionIII-D) for l
31、arge systems (null100M sparse matrix elements).There are also new systems based on nonlinear conjugategradient that never form the linear system explicitly 1, 2;these converge more slowly, but can work with extremelylarge datasets (null1000M matrix elements). In this paper wecompare g2o to the SSBA
32、system of 15, which is the best-performing publicly available system to date.III. NONLINEAR GRAPH OPTIMIZATION USINGLEAST-SQUARESMany problems in robotics or in computer vision can besolved by finding the minimum of a function of this form:F(x) =nullnulli,jnulle(xi,xj,zij)nullije(xi,xj,zij)null null
33、null nullFij(1)xnull = argminxF(x). (2)Here, x = (xnull1 , . ,xnulln )null is a vector of parameters,where each xi represents a generic parameter block, zijand ij represent respectively the mean and the informationmatrix of a constraint relating the parameters xj and xi, ande(xi,xj,zij) is a vector
34、error function that measures howwell the parameter blocks xi and xj satisfy the constraintzij. It is 0 when xi and xj perfectly match the constraint.For simplicity of notation, in the rest of this paper we willencode the measurement in the indices of the error function:e(xi,xj,zij) def.= eij(xi,xj)
35、def.= eij(x). (3)Note that each error function, each parameter block, andeach error function can span a different space. A problemin this form can be effectively represented by a directedgraph. A node i of the graph represents the parameter blockxi and an edge between the nodes i and j represents an
36、ordered constraint between the two parameter blocks xi andxj. Figure 2 shows an example of mapping between a graphand an objective function.A. Least Squares OptimizationIf a good initial guess x of the parameters is known, anumerical solution of Eq. (2) can be obtained by using thepopular Gauss-Newt
37、on or LM algorithms 23, null15.5. Theidea is to approximate the error function by its first orderTaylor expansion around the current initial guess xeij(xi +xi, xj +xj) = eij(x+x) (4)null eij +Jijx. (5)Here, Jij is the Jacobian of eij(x) computed in x and eij def.=eij(x). Substituting Eq. (5) in the
38、error terms Fij of Eq. (1),we obtainFij(x+x) (6)= eij(x+x)nullijeij(x+x) (7)null (eij +Jijx)nullij (eij +Jijx) (8)= enullijijeijnull nullnull nullcij+2enullijijJijnull nullnull nullbijx+xnullJnullijijJijnull nullnull nullHijx(9)= cij + 2bijx+xnullHijx (10)With this local approximation, we can rewrit
39、e the functionF(x) given in Eq. (1) asF(x+x) =nullnulli,jnullnullnullFij(x+x) (11)nullnullnulli,jnullnullnullcij + 2bijx+xnullHijx(12)= c + 2bnullx+xnullHx. (13)The quadratic form in Eq. (13) is obtained from Eq. (12)by setting c = nullcij, b = nullbij and H = nullHij. It canbe minimized in x by sol
40、ving the linear systemHxnull = nullb. (14)Here, H is the information matrix of the system. The solutionis obtained by adding the increments xnull to the initial guessxnull = x+xnull. (15)The popular Gauss-Newton algorithm iterates the lineariza-tion in Eq. (13), the solution in Eq. (14), and the upd
41、ate stepin Eq. (15). In every iteration, the previous solution is usedas the linearization point and the initial guess until a giventermination criterion is met.The LM algorithm introduces a damping factor and backupactions to Gauss-Newton to control the convergence. Insteadof solving Eq. 14, LM sol
42、ves a damped version(H+ I)xnull = nullb. (16)Here is a damping factor: the higher is the smaller thex are. This is useful to control the step size in case ofnon-linear surfaces. The idea behind the LM algorithm isto dynamically control the damping factor. At each iterationthe error of the new config
43、uration is monitored. If the newerror is lower than the previous one, is decreased for thenext iteration. Otherwise, the solution is reverted and lambdais increased. For a more detailed explanation of the LMalgorithm implemented in our framework we refer to 18.B. Alternative ParameterizationsThe pro
44、cedures described above are general approachesto multivariate function minimization. They assume that thespace of parameters x is Euclidean, which is not valid forseveral problems like SLAM or bundle adjustment. To dealwith state variables that span over non-Euclidean space, acommon approach is to e
45、xpress the increments xi in aspace different from the one of the parameters xi.For example, in the context of SLAM problem, eachparameter block xi consists of a translation vector ti anda rotational component i. The translation ti clearly formsa Euclidean space. In contrast to that, the rotational c
46、om-ponents i span over the non-Euclidean 2D or 3D rotationgroup SO(2) or SO(3). To avoid singularities, these spacesare usually described in an over-parameterized way, e.g., byrotation matrices or quaternions. Directly applying Eq. (15)to these over-parameterized representations breaks the con-strai
47、nts induced by the over-parameterization. To overcomethis problem, one can use a minimal representation for therotation (like Euler angles in 3D). This, however, is thensubject to singularities.An alternative idea is to compute a new error functionwhere the xi are perturbations around the current va
48、riablexi. xi uses a minimal representation for the rotations,while xi utilizes an over-parameterized one. Since the xiare usually small, they are far from the singularities. Thenew value of a variable xnulli after the optimization can beobtained by applying the increment through a nonlinearoperator
49、null : Dom(xi)nullDom(xi) null Dom(xi) as follows:xnulli = xi nullxnulli . (17)For instance, in case of 3D SLAM one can represent theincrements xi by the translation vector and the axis ofa normalized quaternion. The poses xi are represented asa translation vector and a full quaternion. The null operatorapplies the increment xi to xi by using the standardmotion composition operator null (see 25) after convertingthe increment to the same representation as
