g 2 o: A General Framework for Graph Optimization


 Elijah Mosley
 1 years ago
 Views:
Transcription
1 g 2 o: A General Framework for Graph Optimization Rainer Kümmerle Giorgio Grisetti Hauke Strasdat Kurt Konolige Wolfram Burgard Abstract Many popular problems in robotics and computer vision including various types of simultaneous localization and mapping (SLAM) or bundle adjustment (BA) can be phrased as least squares optimization of an error function that can be represented by a graph This paper describes the general structure of such problems and presents g 2 o, an opensource C++ framework for optimizing graphbased nonlinear error functions Our system has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code The current implementation provides solutions to several variants of SLAM and BA We provide evaluations on a wide range of realworld and simulated datasets The results demonstrate that while being general g 2 o offers a performance comparable to implementations of stateoftheart approaches for the specific problems Trajectory Landmarks (a) Trajectory Landmarks I INTRODUCTION A wide range of problems in robotics as well as in computervision involve the minimization of a nonlinear error function that can be represented as a graph Typical instances are simultaneous localization and mapping (SLAM) [19], [5], [22], [10], [16], [26] or bundle adjustment (BA) [27], [15], [18] The overall goal in these problems is to find the configuration of parameters or state variables that maximally explain a set of measurements affected by Gaussian noise For instance, in graphbased SLAM the state variables can be either the positions of the robot in the environment or the location of the landmarks in the map that can be observed with the robot s sensors Thereby, a measurement depends only on the relative location of two state variables, eg, an odometry measurement between two consecutive poses depends only on the connected poses Similarly, in BA or landmarkbased SLAM a measurement of a 3D point or landmark depends only on the location of the observed point in the world and the position of the sensor All these problems can be represented as a graph Whereas each node of the graph represents a state variable to optimize, each edge between two variables represents a pairwise observation of the two nodes it connects In the literature, many approaches have been proposed to address this class of problems A naive implementation using standard methods like GaussNewton, LevenbergMarquardt (LM), Gauss Seidel relaxation, or variants of gradient descent typically This work has partly been supported by the European Commission under FP EUROPA, FP RADHAR, and the European Research Council Starting Grant R Kümmerle, G Grisetti, and W Burgard are with the University of Freiburg G Grisetti is also with Sapienza, University of Rome H Strasdat is with the Department of Computing, Imperial College London K Konolige is with Willow Garage and a Consulting Professor at Stanford University (b) Fig 1 Realworld datasets processed with our system: The first row of (a) shows the Victoria Park dataset which consists of 2D odometry and 2D landmark measurements The second row of (a) depicts a 3D pose graph of a multilevel parking garage While the left images shows the initial states, the right column depicts the respective result of the optimization process Full and zoomed view of the Venice bundle adjustment dataset after being optimized by our system (b) The dataset consists of 871 camera poses and 2,838,740 projections provides acceptable results for most applications However, to achieve the maximum performance substantial efforts and domain knowledge are required In this paper, we describe a general framework for performing the optimization of nonlinear least squares problems that can be represented as a graph We call this framework g 2 o (for general graph optimization ) Figure 1 gives an overview of the variety of problems that can be solved by using g 2 o as an optimization backend The proposed system achieves a performance that is comparable with implementations of stateoftheart algorithms, while being able to accept general forms of nonlinear measurements We achieve efficiency by utilizing algorithms that exploit the sparse connectivity of the graph, take advantage of the special structures of the graph that often occur in the problems mentioned above, use advanced methods to solve sparse linear systems, and utilize the features of modern processors like SIMD instructions and optimize the cache usage Despite its efficiency, g 2 o is highly general and extensible:
2 a 2D SLAM algorithm can be implemented in less than 30 lines of C++ code The user only has to specify the error function and its parameters In this paper, we apply g 2 o to different classes of least squares optimization problems and compare its performance with different implementations of problemspecific algorithms We present evaluations carried out on a large set of realworld and simulated datasets; in all experiments g 2 o offered a performance comparable with the stateoftheart approaches and in several cases even outperformed them The remainder of this paper is organized as follows We first discuss the related work with a particular emphasis on solutions to the problems of SLAM and bundle adjustment Subsequently, in Section III we characterize the graphembeddable optimization problems that are addressed by our system and discuss nonlinear leastsquares via Gauss Newton or LM In Section IV we then discuss the features provided by our implementation Finally, in Section V, we present an extensive experimental evaluation of g 2 o and compare it to other stateoftheart, problemspecific methods II RELATED WORK In the past, the graph optimization problems have been studied intensively in the area of robotics and computer vision One seminal work is that of Lu and Milios [19] where the relative motion between two scans was measured by scanmatching and the resulting graph was optimized by iterative linearization While at that time, optimization of the graph was regarded as too timeconsuming for realtime performance, recent advancements in the development of direct linear solvers (eg, [4]), graphbased SLAM has regained popularity and a huge variety of different approaches to solve SLAM by graph optimization have been proposed For example, Howard et al [12] apply relaxation to build a map Duckett et al [6] propose the usage of GaussSeidel relaxation to minimize the error in the network of constraints Frese et al [8] introduced multilevel relaxation (MLR), a variant of GaussSeidel relaxation that applies the relaxation at different levels of resolution Recently, Olson et al [22] suggested a gradient descent approach to optimize pose graphs Later, Grisetti et al [10] extended this approach by applying a treebased parameterization that increases the convergence speed Both approaches are robust to the initial guess and rather easy to implement However, they assume that the covariance is roughly spherical and thus have difficulties in optimizing posegraphs where some constraints have covariances with null spaces or substantial differences in the eigenvalues Graph optimization can be viewed as a nonlinear leastsquares problem, which typically is solved by forming a linear system around the current state, solving, and iterating One promising technique for solving the linear system is preconditioned conjugate gradient (PCG), which was used by Konolige [17] as well as Montemerlo and Thrun [20] as an efficient solver for large sparse pose constraint systems Because of its high efficiency on certain problems, g 2 o x 4 x 1 e x F(x) = e 12 Ω 12 e e 23 Ω 23 e 23 e 12 e 23 +e 31 Ω 31 e 31 +e 24 Ω 24 e 24 e 31 x 3 + Fig 2 This example illustrates how to represent an objective function by a graph includes an implementation of a sparse PCG solver which applies a blockjacobi preconditioner [13] More recently, Dellaert and colleagues suggested a system called SAM [5] which they implement using sparse direct linear solvers [4] Kaess et al [14] introduced a variant of this called isam that is able to update the linear matrix associated with the nonlinear leastsquares problem Konolige et al [16] showed how to construct the linear matrix efficiently by exploiting the typical sparse structure of the linear system However, the latter approach is restricted to 2D pose graphs In g 2 o we share similar ideas with these systems Our system can be applied to both SLAM and BA optimization problems in all their variants, eg, 2D SLAM with landmarks, BA using a monocular camera, or BA using stereo vision However,g 2 o showed a substantially improved performance compared these systems on all the data we used for evaluation purposes In computer vision, Sparse Bundle Adjustment [27] is a nonlinear leastsquares method that takes advantage of the sparsity of the Jacobian pattern between points and camera poses Very recently, there have been several systems [15], [13] that advance similar concepts of sparse linear solvers and efficient calculation of the Schur reduction (see Section IIID) for large systems ( 100M sparse matrix elements) There are also new systems based on nonlinear conjugate gradient that never form the linear system explicitly [1], [2]; these converge more slowly, but can work with extremely large datasets ( 1000M matrix elements) In this paper we compare g 2 o to the SSBA system of [15], which is the bestperforming publicly available system to date III NONLINEAR GRAPH OPTIMIZATION USING LEASTSQUARES Many problems in robotics or in computer vision can be solved by finding the minimum of a function of this form: F(x) = e(x i,x j,z ij ) Ω ij e(x i,x j,z ij ) (1) F ij x = argminf(x) (2) x Here, x = (x 1,,x n) is a vector of parameters, where each x i represents a generic parameter block, z ij and Ω ij represent respectively the mean and the information matrix of a constraint relating the parameters x j and x i, and e(x i,x j,z ij ) is a vector error function that measures how well the parameter blocks x i and x j satisfy the constraint z ij It is 0 when x i and x j perfectly match the constraint
3 For simplicity of notation, in the rest of this paper we will encode the measurement in the indices of the error function: e(x i,x j,z ij ) def = e ij (x i,x j ) def = e ij (x) (3) Note that each error function, each parameter block, and each error function can span a different space A problem in this form can be effectively represented by a directed graph A node i of the graph represents the parameter block x i and an edge between the nodes i and j represents an ordered constraint between the two parameter blocks x i and x j Figure 2 shows an example of mapping between a graph and an objective function A Least Squares Optimization If a good initial guess x of the parameters is known, a numerical solution of Eq (2) can be obtained by using the popular GaussNewton or LM algorithms [23, 155] The idea is to approximate the error function by its first order Taylor expansion around the current initial guess x e ij ( x i + x i, x j + x j ) = e ij ( x+ x) (4) e ij +J ij x (5) Here, J ij is the Jacobian of e ij (x) computed in x and e ij def = e ij ( x) Substituting Eq (5) in the error terms F ij of Eq (1), we obtain F ij( x+ x) (6) = e ij( x+ x) Ω ije ij( x+ x) (7) (e ij +J ij x) Ω ij (e ij +J ij x) (8) = e ijω ije ij +2e c ij ijω ijj ij b ij x+ x J ijω ijj ij x (9) H ij = c ij +2b ij x+ x H ij x (10) With this local approximation, we can rewrite the function F(x) given in Eq (1) as F( x+ x) = F ij( x+ x) (11) c ij +2b ij x+ x H ij x (12) = c+2b x+ x H x (13) The quadratic form in Eq (13) is obtained from Eq (12) by setting c = c ij, b = b ij and H = H ij It can be minimized in x by solving the linear system H x = b (14) Here,His the information matrix of the system The solution is obtained by adding the increments x to the initial guess x = x+ x (15) The popular GaussNewton algorithm iterates the linearization in Eq (13), the solution in Eq (14), and the update step in Eq (15) In every iteration, the previous solution is used as the linearization point and the initial guess until a given termination criterion is met The LM algorithm introduces a damping factor and backup actions to GaussNewton to control the convergence Instead of solving Eq 14, LM solves a damped version (H+λI) x = b (16) Here λ is a damping factor: the higher λ is the smaller the x are This is useful to control the step size in case of nonlinear surfaces The idea behind the LM algorithm is to dynamically control the damping factor At each iteration the error of the new configuration is monitored If the new error is lower than the previous one, λ is decreased for the next iteration Otherwise, the solution is reverted and lambda is increased For a more detailed explanation of the LM algorithm implemented in our framework we refer to [18] B Alternative Parameterizations The procedures described above are general approaches to multivariate function minimization They assume that the space of parameters x is Euclidean, which is not valid for several problems like SLAM or bundle adjustment To deal with state variables that span over noneuclidean space, a common approach is to express the increments x i in a space different from the one of the parameters x i For example, in the context of SLAM problem, each parameter block x i consists of a translation vector t i and a rotational component α i The translation t i clearly forms a Euclidean space In contrast to that, the rotational components α i span over the noneuclidean 2D or 3D rotation group SO(2) or SO(3) To avoid singularities, these spaces are usually described in an overparameterized way, eg, by rotation matrices or quaternions Directly applying Eq (15) to these overparameterized representations breaks the constraints induced by the overparameterization To overcome this problem, one can use a minimal representation for the rotation (like Euler angles in 3D) This, however, is then subject to singularities An alternative idea is to compute a new error function where the x i are perturbations around the current variable x i x i uses a minimal representation for the rotations, while x i utilizes an overparameterized one Since the x i are usually small, they are far from the singularities The new value of a variable x i after the optimization can be obtained by applying the increment through a nonlinear operator : Dom(x i ) Dom( x i ) Dom(x i ) as follows: x i = x i x i (17) For instance, in case of 3D SLAM one can represent the increments x i by the translation vector and the axis of a normalized quaternion The poses x i are represented as a translation vector and a full quaternion The operator applies the increment x i to x i by using the standard motion composition operator (see [25]) after converting the increment to the same representation as the state variable: x i x i def = x i x i (18)
4 With this operator, a new error function can be defined as Error function e ij e ij ( x i, x j ) def = e ij ( x i x i, x j x j ) (19) = e ij ( x x) e ij +J ij x, (20) where x spans over the original overparameterized space The Jacobian J ij becomes J ij = e ij( x x) x (21) x=0 Since the increments x are computed in the local Euclidean surroundings of the initial guess x, they need to be remapped into the original redundant space by the operator Our framework allows for the easy definition of different spaces for the increments and the state variables and thus transparently supports arbitrary parameterizations within the same problem Regardless the choice of the parameterization, the structure of the Hessian H is in general preserved C Structure of the Linearized System According to Eq (13), the matrix H and the vector b are obtained by summing up a set of matrices and vectors, one for every constraint If we set b ij = J ij Ω ije ij and H ij = J ij Ω ijj ij we can rewrite b and H as b = H = H ij (22) b ij Every constraint will contribute to the system with an addend term The structure of this addend depends on the Jacobian of the error function Since the error function of a constraint depends only on the values of two nodes, the Jacobian in Eq (5) has the following form: J ij = 0 0 A ij 0 0 B ij 0 0 (23) }{{} }{{} i j Here A ij and B ij are the derivatives of the error function with respect to x i and x j From Eq (9) we obtain the following structure for the block matrix H ij : A ijω ija ij A ijω ijb ij H ij = b ij = B ijω ija ij B ijω ijb ij A ijω ije ij B ijω ije ij For simplicity of notation we omitted the zero blocks The reader might notice that the block structure of the matrix H is the adjacency matrix of the graph Thus, it has a number of nonzero blocks proportional to number of edges in the graph This typically results in sparse H matrices In g 2 o we take advantage of this characteristic of H by utilizing stateoftheart approaches to solve the linear system of Eq (14) Jacobian J ij (H + λi) x = b Linear Solver CSparse CHOLMOD Linear Structure Plain PCG Apply increments Schur Fig 3 Overview of our framework For addressing a new optimization problem, only the boxes in gray need to specified Furthermore, the framework allows to add different linear solvers D Systems Having Special Structure Certain problems (for instance, BA) result in an H matrix that has an even more characteristic structure Our system can take advantage of these special structures to improve the performance In BA there are in general two types of variables, namely the poses p of the camera and the poses l of the landmarks observed by the camera By reordering the variables in Eq (14) so that the camera poses have the lower indices we obtain the system ( ) ( Hpp H pl x ) ( ) p bp H pl H ll x = (24) l b l It can be shown that an equivalent reduced system is formed by taking the Schur complement of the H matrix [7] ( Hpp H pl H 1 ll H ) pl x p = b p +H pl H 1 ll b l (25) Note that calculating H 1 ll is easy, since H ll is a blockdiagonal matrix Solving Eq (25) yields the increments x p for the cameras and using this we can solve H ll x l = b l H pl x p, (26) which results in x l for adjusting the observed world features Typically the world features outnumber the camera poses, therefore Eq (25) can be solved faster than Eq (14) despite the additional time spent to calculate the lefthand side matrix in Eq (25) IV IMPLEMENTATION Our C++ implementation aims to be as fast as possible while remaining general We achieve this goal by implementing abstract base classes for vertices and edges in our graph Both base classes provide a set of virtual functions for easy user subclassing, while most of the internal operations are implemented using template arguments for efficiency We use the Eigen linear algebra package [11] which applies SSE instructions among other optimization techniques, such lazy evaluation and loop unrolling to achieve high performance Figure 3 depicts the design of our system Only the boxes in gray need to be defined to address a new optimization problem Using the provided base class, deriving a new type of node only requires defining the operator for applying the increments An edge connecting two nodes x i
5 Fig 4 3D posegraph datasets used for evaluating g 2 o: the left image shows a simulated sphere, the right image depicts a partial view of a realworld dataset of a multilevel parking garage andx j requires the definition of the error functione ij ( ) The JacobianJ ij is then evaluated numerically, or, for higher efficiency, the user can specify J ij explicitly by overwriting the virtual baseclass function Thus, implementing new types for addressing a new optimization problem or comparing different parameterizations is a matter of writing a few lines of code The computation of H in the general case uses matrices of a variable size If the dimension of the variables, ie, the dimension of x i, is known in advance, our framework allows fixedsize matrix computations Exploiting the apriori known dimensions enables compiletime optimizations such as loop unrolling to carry out matrix multiplications Special care has been taken in implementing matrix multiplications required for the Schur reduction in Eq (25) The sparse structure of the underlying graph is exploited to only multiply nonzero entries required to form the extra entries of H pp Additionally, we operate on the block structures of the underlying matrix (see [15]), which results in a cache efficient matrix multiplication compared to a scalar matrix multiplication Our framework is agnostic with respect to the embedded linear solver, so we can apply appropriate ones for different problems We have used two solvers for experiments Since H is positive semidefinite and symmetric, sparse Cholesky decomposition results in a an efficient solver [4], [3] Note that the nonzero pattern during the leastsquares iterations is constant We therefore are able to reuse a symbolic decomposition computed within the first iteration, which results in a reduced fillin and reduces the overall computation time in subsequent iterations Note that this Cholesky decomposition does not take advantage of the block structure of the parameters The second method is Preconditioned Conjugate Gradient (PCG) with a block Jacobi preconditioner [13], which takes advantage of block matrix operations throughout As PCG itself is an iterative method, solving a linear system requiresniterations for an n matrix Since carrying out n iterations of PCG is typically slower than Cholesky decomposition, we limit the number of iterations based on the relative decrease in the squared residual of PCG By this we are able to quantify the loss in the accuracy of the solution introduced by terminating PCG early In the experiments we will compare the different solvers V EXPERIMENTS In this section, we present experiments in which we compare g 2 o with other stateoftheart optimization approaches Fig 5 The BA real world datasets and the scaledrift dataset used for evaluating g 2 o: the left image shows the Venice dataset, whereas the middle image depicts the New College dataset [24] The pair of images on the right shows the Keble college dataset which was processed by monocular SLAM Here, scale drift occurs (top) which can be corrected when closing the loop using 7 DoF similarity transformations (bottom) TABLE I OVERVIEW OF THE TEST DATASETS Dataset # poses # landmarks # constraints Intel MIT Manhattan Victoria Grid Sphere Garage Venice New College Scale Drift using both realworld and synthetic datasets Figure 4 depicts the 3D posegraph data sets, in Figure 5 the BA datasets and the posegraph of the Keble college, which is used to perform scale driftaware SLAM using 7 DoF similarity constraints [26], are visualized, and Figure 6 shows the 2D datasets The number of variables and constraints is given in Table I for each of the datasets All experiments are executed on one core of an Intel Core i7930 running at 28 Ghz We compare g 2 o with other stateoftheart implementations: SAM [5] using the opensource implementation by M Kaess, SPA [16], ssba [15], and RobotVision [26] Note that these approaches are only targeting a subset of optimization problems while g 2 o is able to solve all of them, and also extends easily to new problems A Runtime Comparison In the following, we report the time needed by each approach to carry out one iteration We provided each approach with the full optimization problem and carried out 10 iterations, and measured the average time spent per iteration In this set of experiments g 2 o applies Cholesky decomposition to solve the linear system using CHOLMOD, which is also used by the approaches we compare to Therefore, the time required to solve the linear system is similar for all approaches and the difference reflects the Time per Iteration [s] SAM g 2 o SPA Fig 7 grid5k Victoria (bearing) Victoria Manhattan Killian Intel Time per Iteration [s] SAM g 2 o SPA / ssba RobotVision New College Venice Scale Drift Garage Sphere Time per iteration for each approach on each dataset
6 Trajectory Landmarks Fig 6 2D Datasets used for evaluating g 2 o From left to right: 2D posegraph of the Intel Research Lab; 2D posegraph of the Killian Court; Manhattan3500, a simulated posegraph; 2D dataset with landmarks of the Victoria Park; and Grid5000, a simulated landmark dataset Cumulative Time [s] g 2 o (Cholesky) every node g 2 o (PCG) every node isam every node HOGMan every node Number of nodes Cumulative Time [s] g 2 o (Cholesky) every 10 nodes isam every 10 nodes HOGMan every 10 nodes Number of nodes Fig 8 Online processing of the Manhattan3500 dataset The left image shows the runtime for optimizing after inserting a single node whereas the right image show the runtime for optimizing after inserting 10 nodes efficiency in constructing the linear system The results are summarized in Figure 7 Our system g 2 o is faster than the implementation of SAM on all the 2D and 3D datasets we tested While in principle they implement the same algorithm, g 2 o takes advantage of an efficient front end to generate the linearized problem On the 2D pose graph datasets the runtime of our framework is comparable to the highly optimized SPA implementation On the BA datasets g 2 o achieves a similar performance to ssba, which is slightly faster than our general framework Compared to RobotVision, g 2 o is on average two times faster Note that while g 2 o focuses on batch optimization, it can be used to process the data incrementally by optimizing after adding nodes to the graph The efficiency of g 2 o yields performance similar to approaches that are designed for incremental use, such as isam [14] or HOGMan [9] As visualized in Figure 8, by optimizing every 10 nodes or by relaxing the termination criterion of PCG for optimizing after inserting a single nodeg 2 o can achieve acceptable runtimes Furthermore, it can be used as an efficient building block of more complex online systems [21], [9] As mentioned, g 2 o can compute the Jacobian J ij numerically, which allows rapid prototyping of a new optimization problem or a new error function However, by specifying the Jacobians analytically one can achieve a substantial speedup For instance, the time required by one iteration of g 2 o on the Garage dataset drops from 80 ms to 40 ms when specifying the analytic Jacobian Despite the reduced efficiency we did not observe a decrease in the accuracy when using the numeric Jacobian B Testing different Parameterizations Since our framework only requires to implement the error function and the update step, we are able to easily compare different parameterizations To this end, we implemented two different parameterizations for representing poses in BA In the first parametrization, the increment x i is represented by a translation vector and the axis of a unit quaternion Whereas in the second one, the increments x i F(x) LIE ALGEBRA SE(3) UNIT QUATERNION Iteration F(x) LIE ALGEBRA SE(3) UNIT QUATERNION Iteration Fig 9 Evolution of F(x) using unit quaternions versus the Lie algebra se(3) on the New College dataset (left) and Venice (right) dataset TABLE II COMPARISON OF DIFFERENT LINEAR SOLVERS (TIME IN SECONDS) Dataset CHOLMOD CSparse PCG Intel ± MIT ± 0364 Manhattan ± Victoria ± 0683 Grid ± 1185 Sphere ± 0019 Garage ± 0016 New College ± 0201 Venice ± 0135 Scale Drift ± 001 are represented by members of the Lie algebra se(3) [26] We applied the different parameterizations to the New College and Venice datasets The evolution of the error is depicted in Figure 9 Both parameterizations converge to the same solution, but convergence occurs faster using se(3) C Comparison of Linear Solvers Our system allows different linear solvers to solve either Eq (14) or Eq (25) We currently have implemented two solvers based on Cholesky decomposition, namely CHOLMOD and CSparse [4] Additionally, we implemented PCG as an iterative method using a blockjacobi preconditioner Table II summarizes the time required for solving the linear system on several datasets PCG performs very well on the New College and Venice datasets, where it is around 7 times faster than CHOLMOD The PCG convergence depends on how close the initial guess is to the optimum We terminate PCG if the relative residual is below a given threshold (10 8 in the experiments) Therefore, PCG requires more time to converge, for example, on the MIT or Victoria datasets CHOLMOD is faster by up to a factor of 30 than CSparse on the larger datasets But surprisingly CSparse is the fastest solver on the smaller instances like the MIT dataset where it outperforms both CHOLMOD and PCG D Utilizing the Knowledge about the Structure As discussed in Section IIID, certain problems have a characteristic structure Using this structure may result in substantial improvements in the solution of the linear
7 TABLE III COMPARISON OF DIFFERENT LINEAR SOLVERS WE MEASURED THE AVERAGE TIME PER ITERATION OF g 2 o (IN SECONDS) Dataset direct solution Schur decomposition solve build / solve / total Victoria / 0121 / 0150 Grid / 016 / 028 New College / 707 / 1044 Venice / 178 / 1303 system Landmarkbased SLAM and BA have the same linear structure: the landmarks/points can be only connected with the robot poses/cameras, resulting in a block diagonal structure for the landmark part of the Hessian H ll In this experiment we evaluate the advantages of using this specific decomposition for landmark basedslam and BA Table III shows the timing for the different datasets where we enabled and disabled the decomposition From the table it is evident that preforming the decomposition results in a substantial speedup when the landmarks outnumber the poses, which is the typically the case in BA However, when the number of poses becomes dominant, performing the Schur marginalization leads to a highly connected system that is only slightly reduced in size, and requires more effort to be solved VI CONCLUSIONS In this paper we presented g 2 o, an extensible and efficient opensource framework for batch optimization of functions that can be embedded in a graph Relevant problems falling into this class are graphbased SLAM and bundle adjustment, two fundamental and highly related problems in robotics and computer vision To utilize g 2 o one simply has to define the error function and a procedure for applying a perturbation to the current solution Furthermore, one can easily embed in the system new linear solvers and thus verify the characteristics of the specific solver for a wide range of problems sharing this graph structure We showed the applicability of g 2 o to various variants of SLAM (2D, 3D, poseonly, and with landmarks) and to bundle adjustment Practical experiments carried out on extensive datasets demonstrate that g 2 o achieves performance comparable to implementations of problemspecific algorithms and often even outperforms them An opensource implementation of the entire system is freely available as part of ROS and on OpenSLAMorg ACKNOWLEDGMENTS We thank E Olson for the Manhattan3500 dataset, E Nebot and H DurrantWhyte for the Victoria Park dataset, N Snavely for the Venice dataset, and M Kaess for providing a highly efficient implementation of isam REFERENCES [1] S Agarwal, N Snavely, S M Seitz, and R Szeliski, Bundle adjustment in the large, in Proc of the European Conf on Computer Vision (ECCV), 2010 [2] M Byrod and K Astroem, Conjgate gradient bundle adjustment, in Proc of the European Conf on Computer Vision (ECCV), 2010 [3] Y Chen, T A Davis, W W Hager, and S Rajamanickam, Algorithm 887: Cholmod, supernodal sparse cholesky factorization and update/downdate, ACM Trans Math Softw, vol 35, no 3, pp 1 14, 2008 [4] T A Davis, Direct Methods for Sparse Linear Systems SIAM, 2006, part of the SIAM Book Series on the Fundamentals of Algorithms [5] F Dellaert and M Kaess, Square Root SAM: Simultaneous localization and mapping via square root information smoothing, Int Journal of Robotics Research, vol 25, no 12, pp , Dec 2006 [6] T Duckett, S Marsland, and J Shapiro, Fast, online learning of globally consistent maps, Autonomous Robots, vol 12, no 3, pp , 2002 [7] U Frese, A proof for the approximate sparsity of SLAM information matrices, in Proc of the IEEE Int Conf on Robotics & Automation (ICRA), 2005 [8] U Frese, P Larsson, and T Duckett, A multilevel relaxation algorithm for simultaneous localisation and mapping, IEEE Transactions on Robotics, vol 21, no 2, pp 1 12, 2005 [9] G Grisetti, R Kümmerle, C Stachniss, U Frese, and C Hertzberg, Hierarchical optimization on manifolds for online 2d and 3d mapping, in Proc of the IEEE Int Conf on Robotics & Automation (ICRA), 2010 [10] G Grisetti, C Stachniss, and W Burgard, Nonlinear constraint network optimization for efficient map learning, IEEE Trans on Intelligent Transportation Systems, 2009 [11] G Guennebaud, B Jacob, et al, Eigen v3, [12] A Howard, M Matarić, and G Sukhatme, Relaxation on a mesh: a formalism for generalized localization, in Proc of the IEEE/RSJ Int Conf on Intelligent Robots and Systems (IROS), 2001 [13] Y Jeong, D Nister, D Steedly, R Szeliski, and I Kweon, Pushing the envelope of modern methods for bundle adjustment, in Proc of the IEEE Conf on Comp Vision and Pattern Recognition (CVPR), 2010 [14] M Kaess, A Ranganathan, and F Dellaert, isam: Incremental smoothing and mapping, IEEE Trans on Robotics, vol 24, no 6, pp , Dec 2008 [15] K Konolige, Sparse sparse bundle adjustment, in Proc of the British Machine Vision Conference (BMVC), 2010 [16] K Konolige, G Grisetti, R Kümmerle, W Burgard, B Limketkai, and R Vincent, Sparse pose adjustment for 2d mapping, in Proc of the IEEE/RSJ Int Conf on Intelligent Robots and Systems (IROS), 2010 [17] K Konolige, Largescale mapmaking, in Proc of the National Conference on Artificial Intelligence (AAAI), 2004 [18] M A Lourakis and A Argyros, SBA: A Software Package for Generic Sparse Bundle Adjustment, ACM Trans Math Software, vol 36, no 1, pp 1 30, 2009 [19] F Lu and E Milios, Globally consistent range scan alignment for environment mapping, Autonomous Robots, vol 4, pp , 1997 [20] M Montemerlo and S Thrun, Largescale robotic 3d mapping of urban structures, in Proc of the Int Symposium on Experimental Robotics (ISER), 2004 [21] K Ni and F Dellaert, Multilevel submap based SLAM using nested dissection, in Proc of the IEEE/RSJ Int Conf on Intelligent Robots and Systems (IROS), 2010 [22] E Olson, J Leonard, and S Teller, Fast iterative optimization of pose graphs with poor initial estimates, in Proc of the IEEE Int Conf on Robotics & Automation (ICRA), 2006, pp [23] W Press, S Teukolsky, W Vetterling, and B Flannery, Numerical Recipes, 2nd Edition Cambridge Univ Press, 1992 [24] M Smith, I Baldwin, W Churchill, R Paul, and P Newman, The new college vision and laser data set, Int Journal of Robotics Research, vol 28, no 5, pp , May 2009 [25] R Smith, M Self, and P Cheeseman, Estimating uncertain spatial realtionships in robotics, in Autonomous Robot Vehicles, I Cox and G Wilfong, Eds Springer Verlag, 1990, pp [26] H Strasdat, J M M Montiel, and A Davison, Scale driftaware large scale monocular SLAM, in Proc of Robotics: Science and Systems (RSS), 2010 [27] B Triggs, P F McLauchlan, R I Hartley, and A W Fitzibbon, Bundle adjustment  a modern synthesis, in Vision Algorithms: Theory and Practice, ser LNCS Springer Verlag, 2000, pp
Towards Lineartime Incremental Structure from Motion
Towards Lineartime Incremental Structure from Motion Changchang Wu University of Washington Abstract The time complexity of incremental structure from motion (SfM) is often known as O(n 4 ) with respect
More informationEPnP: An Accurate O(n) Solution to the PnP Problem
DOI 10.1007/s1126300801526 EPnP: An Accurate O(n) Solution to the PnP Problem Vincent Lepetit Francesc MorenoNoguer Pascal Fua Received: 15 April 2008 / Accepted: 25 June 2008 Springer Science+Business
More informationIEEE TRANSACTIONS ON AUDIO, SPEECH, AND LANGUAGE PROCESSING, 2013. ACCEPTED FOR PUBLICATION 1
IEEE TRANSACTIONS ON AUDIO, SPEECH, AND LANGUAGE PROCESSING, 2013. ACCEPTED FOR PUBLICATION 1 ActiveSet Newton Algorithm for Overcomplete NonNegative Representations of Audio Tuomas Virtanen, Member,
More informationTHE PROBLEM OF finding localized energy solutions
600 IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 45, NO. 3, MARCH 1997 Sparse Signal Reconstruction from Limited Data Using FOCUSS: A Reweighted Minimum Norm Algorithm Irina F. Gorodnitsky, Member, IEEE,
More informationSimultaneous Localisation and Mapping (SLAM): Part II State of the Art
1 Simultaneous Localisation and Mapping (SLAM): Part II State of the Art Tim Bailey and Hugh DurrantWhyte Abstract This tutorial provides an introduction to the Simultaneous Localisation and Mapping (SLAM)
More informationFrom Few to Many: Illumination Cone Models for Face Recognition Under Variable Lighting and Pose. Abstract
To Appear in the IEEE Trans. on Pattern Analysis and Machine Intelligence From Few to Many: Illumination Cone Models for Face Recognition Under Variable Lighting and Pose Athinodoros S. Georghiades Peter
More informationTHE development of methods for automatic detection
Learning to Detect Objects in Images via a Sparse, PartBased Representation Shivani Agarwal, Aatif Awan and Dan Roth, Member, IEEE Computer Society 1 Abstract We study the problem of detecting objects
More informationGenerating Humanlike Motion for Robots
Generating Humanlike Motion for Robots Michael J. Gielniak, C. Karen Liu, and Andrea L. Thomaz Abstract Action prediction and fluidity are a key elements of humanrobot teamwork. If a robot s actions
More informationWMR Control Via Dynamic Feedback Linearization: Design, Implementation, and Experimental Validation
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 10, NO. 6, NOVEMBER 2002 835 WMR Control Via Dynamic Feedback Linearization: Design, Implementation, and Experimental Validation Giuseppe Oriolo, Member,
More informationChoosing Multiple Parameters for Support Vector Machines
Machine Learning, 46, 131 159, 2002 c 2002 Kluwer Academic Publishers. Manufactured in The Netherlands. Choosing Multiple Parameters for Support Vector Machines OLIVIER CHAPELLE LIP6, Paris, France olivier.chapelle@lip6.fr
More informationSubspace Pursuit for Compressive Sensing: Closing the Gap Between Performance and Complexity
Subspace Pursuit for Compressive Sensing: Closing the Gap Between Performance and Complexity Wei Dai and Olgica Milenkovic Department of Electrical and Computer Engineering University of Illinois at UrbanaChampaign
More informationRECENTLY, there has been a great deal of interest in
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 47, NO. 1, JANUARY 1999 187 An Affine Scaling Methodology for Best Basis Selection Bhaskar D. Rao, Senior Member, IEEE, Kenneth KreutzDelgado, Senior Member,
More informationOn SetBased Multiobjective Optimization
1 On SetBased Multiobjective Optimization Eckart Zitzler, Lothar Thiele, and Johannes Bader Abstract Assuming that evolutionary multiobjective optimization (EMO) mainly deals with set problems, one can
More informationScalable Collaborative Filtering with Jointly Derived Neighborhood Interpolation Weights
Seventh IEEE International Conference on Data Mining Scalable Collaborative Filtering with Jointly Derived Neighborhood Interpolation Weights Robert M. Bell and Yehuda Koren AT&T Labs Research 180 Park
More informationFast Solution of l 1 norm Minimization Problems When the Solution May be Sparse
Fast Solution of l 1 norm Minimization Problems When the Solution May be Sparse David L. Donoho and Yaakov Tsaig October 6 Abstract The minimum l 1 norm solution to an underdetermined system of linear
More informationHighDimensional Image Warping
Chapter 4 HighDimensional Image Warping John Ashburner & Karl J. Friston The Wellcome Dept. of Imaging Neuroscience, 12 Queen Square, London WC1N 3BG, UK. Contents 4.1 Introduction.................................
More informationHighRate Codes That Are Linear in Space and Time
1804 IEEE TRANSACTIONS ON INFORMATION THEORY, VOL 48, NO 7, JULY 2002 HighRate Codes That Are Linear in Space and Time Babak Hassibi and Bertrand M Hochwald Abstract Multipleantenna systems that operate
More informationParallel Tracking and Mapping for Small AR Workspaces
Parallel Tracking and Mapping for Small AR Workspaces Georg Klein David Murray Active Vision Laboratory Department of Engineering Science University of Oxford ABSTRACT This paper presents a method of estimating
More informationA Flexible New Technique for Camera Calibration
A Flexible New Technique for Camera Calibration Zhengyou Zhang December 2, 1998 (updated on December 14, 1998) (updated on March 25, 1999) (updated on Aug. 1, 22; a typo in Appendix B) (updated on Aug.
More information2 Basic Concepts and Techniques of Cluster Analysis
The Challenges of Clustering High Dimensional Data * Michael Steinbach, Levent Ertöz, and Vipin Kumar Abstract Cluster analysis divides data into groups (clusters) for the purposes of summarization or
More informationPictorial Structures Revisited: People Detection and Articulated Pose Estimation
Pictorial Structures Revisited: People Detection and Articulated Pose Estimation Mykhaylo Andriluka, Stefan Roth, and Bernt Schiele Department of Computer Science, TU Darmstadt Abstract Nonrigid object
More informationMesh Parameterization Methods and Their Applications
Foundations and Trends R in Computer Graphics and Vision Vol. 2, No 2 (2006) 105 171 c 2006 A. Sheffer, E. Praun and K. Rose DOI: 10.1561/0600000011 Mesh Parameterization Methods and Their Applications
More informationObject Detection with Discriminatively Trained Part Based Models
1 Object Detection with Discriminatively Trained Part Based Models Pedro F. Felzenszwalb, Ross B. Girshick, David McAllester and Deva Ramanan Abstract We describe an object detection system based on mixtures
More informationCOSAMP: ITERATIVE SIGNAL RECOVERY FROM INCOMPLETE AND INACCURATE SAMPLES
COSAMP: ITERATIVE SIGNAL RECOVERY FROM INCOMPLETE AND INACCURATE SAMPLES D NEEDELL AND J A TROPP Abstract Compressive sampling offers a new paradigm for acquiring signals that are compressible with respect
More informationDistinctive Image Features from ScaleInvariant Keypoints
Distinctive Image Features from ScaleInvariant Keypoints David G. Lowe Computer Science Department University of British Columbia Vancouver, B.C., Canada lowe@cs.ubc.ca January 5, 2004 Abstract This paper
More informationTop 10 algorithms in data mining
Knowl Inf Syst (2008) 14:1 37 DOI 10.1007/s1011500701142 SURVEY PAPER Top 10 algorithms in data mining Xindong Wu Vipin Kumar J. Ross Quinlan Joydeep Ghosh Qiang Yang Hiroshi Motoda Geoffrey J. McLachlan
More informationHow To Write Fast Numerical Code: A Small Introduction
How To Write Fast Numerical Code: A Small Introduction Srinivas Chellappa, Franz Franchetti, and Markus Püschel Electrical and Computer Engineering Carnegie Mellon University {schellap, franzf, pueschel}@ece.cmu.edu
More information(1) I. INTRODUCTION (2)
IEEE TRANSACTIONS ON IMAGE PROCESSING, VOL. 19, NO. 12, DECEMBER 2010 3243 Distance Regularized Level Set Evolution and Its Application to Image Segmentation Chunming Li, Chenyang Xu, Senior Member, IEEE,
More informationLearning Invariant Features through Topographic Filter Maps
Learning Invariant Features through Topographic Filter Maps Koray Kavukcuoglu Marc Aurelio Ranzato Rob Fergus Yann LeCun Courant Institute of Mathematical Sciences New York University {koray,ranzato,fergus,yann}@cs.nyu.edu
More informationFeature Sensitive Surface Extraction from Volume Data
Feature Sensitive Surface Extraction from Volume Data Leif P. Kobbelt Mario Botsch Ulrich Schwanecke HansPeter Seidel Computer Graphics Group, RWTHAachen Computer Graphics Group, MPI Saarbrücken Figure
More information