Normalized graph cuts for visual SLAM John G. Rogers and Henrik I. Christensen Abstract— Simultaneous Localization and Mapping (SLAM) suffers from a quadratic space and time complexity per update step. Recent advancements have been made in approximating the posterior by forcing the information matrix to remain sparse as well as exact techniques for generating the posterior in the full SLAM solution to both the trajectory and the map. Current approximate techniques for maintaining an online estimate of the map for a robot to use while exploring make capacity- based decisions about when to split into sub-maps. This paper will describe an alternative partitioning strategy for online approximate real-time SLAM which makes use of normalized graph cuts to remove less information from the full map. I. INTRODUCTION The problem of Simultaneous Localization and Mapping (SLAM) has seen a flurry of publication over the past decade. Many of the core issues have been addressed in a research setting, yet a viable general-purpose implementation remains elusive. The absence of a general purpose reliable SLAM module is due to two main issues in moving from the research environment to the real world. The first main issue, which will be addressed in this paper, is the quadratic update complexity inherent in maintaining a fully dense covariance or information matrix. The second issue, which will be peripherally discussed in this paper, is the data association problem – if even a single feature is mistakenly confused for another, the entire map can be rendered inconsistent. Most modern approaches to solving the SLAM problem follow one of two main themes. One approach stems from advancements made on the Structure from Motion (SFM) problem in the computer vision community. This approach is to optimize the representation of the perceived environment in an offline manner. This has been applied in robotics as the Smoothing and Mapping (SAM) problem [12], which is also known as full SLAM because the entire robot trajectory is optimized along with the landmark poses. The original approach from the robotics community has been to use a recursive filter algorithm to marginalize past poses in the interest of real-time operation, but at the expense of correlating all map features. The original approach to the SLAM problem is to up- date a state vector and covariance matrix composed of the robot pose and the estimated landmark positions with the Extended Kalman Filter (EKF). This technique evolved out of the original successful application of the EKF for mobile robot localization with an a priori map in [7] and [5]. By This work was supported by the ARL CTA MAST project 104953 J. G. Rogers is a Ph.D. student at Georgia Tech College of Computing jgrogers@cc.gatech.edu H. I. Christensen is the KUKA Chair of Robotics at Georgia Tech College of Computing hic@cc.gatech.edu placing the estimated landmark locations within the state vector and updating them simultaneously, the first SLAM implementation was reported by [25]. The recent summary papers by Durrant-Whyte and Bailey [13], [1] serve as an excellent survey of the history of the SLAM problem, from its initial beginnings up to the state-of-the-art today. The full SLAM, or Smoothing and Mapping (SAM) problem allows for the use of a sparse representation for the covariance or information matrix by maintaining the entire robot trajectory within the state vector. Features are correlated when prior poses are marginalized out, in effect, the EKF does this with every prior pose and correlates all features resulting in a dense covariance matrix. Folkesson and Christensen developed GraphSLAM [15], which was able to close loops and avoided linearization error through the use of a nonlinear optimization engine. Loop closure was achieved by adding human-guided constraints between features and then reoptimizing. Dellaert et. al. [12] has developed the Square Root SAM algorithm which uses sparse Cholesky factorization to optimize a set of landmark measurements and the robot trajectory in an efficient manner. Further progress has been made on online solutions to the SAM problem which uses QR factorization for reordering the measurements to get optimal and online or incremental updates such as with incremental SAM (iSAM) [17], [18]. These algorithms represent excellent progress towards a large-scale solution to the SLAM problem; however, each of them requires an ever increasing amount of computation per update step. EKF updates suffer from quadratic complexity per update step, iSAM updates can be held to near linear complexity per update step with proper reordering. Sub- mapping strategies have been developed to approximate the solution to the SLAM problem while maintaining constant time update steps. This is accomplished by keeping the size of the sub-map at no larger than a fixed maximum number of features. Since the filter approximates the solution by assuming that features in other maps are independent, it can confine the update to the local map, which is smaller than a constant maximum size. Sub-mapping strategies such as Atlas [4] and Hierarchical SLAM [14] maintain constant sized local maps and offer loop closure updates which are linear in the size of the loop, in terms of local map frames traversed. Other algorithms postpone global updates until they are needed and focus on local updates such as the Compressed EKF [16]. Techniques within the SAM community such as Tectonic SAM [22] are able to optimize sub-maps to generate exactly the same results as full Square Root SAM but with the efficiency of a constant sized map. Since each local map starts with The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems October 11-15, 2009 St. Louis, USA 978-1-4244-3804-4/09/$25.00 ©2009 IEEE 918