An Active Visual Loop Closure Detection and Validation System forTopological SLAM

Wen Lik Dennis Lui and Ray JarvisIntelligent Robotics Research Centre

Department of Electrical and Computer Systems EngineeringMonash University, Clayton, Victoria 3800, Australia

dwllui@gmail.com, ray.jarvis@monash.edu

Abstract

Loop closing is a vital component for mobilerobot navigation without a priori informationof the environment, since the mobile robot hasto explore, build and at the same time main-tain a globally consistent map. Moreover, itallows the mobile robot to recover from po-sitional drifts due to errors associated withsensor measurements once loop closing is per-formed. In this paper, an active loop closingdetection and validation system for a fully au-tonomous vision-based mobile robot perform-ing topological SLAM is presented. It inte-grates an appearance-based mapping and local-ization system with the path planner to achievean active loop closing system. This system doesnot only detect loop closures but actively val-idates them by taking advantage of the tightintegration with the path planner. Experimen-tal results conducted in a semi-outdoor envi-ronment validate the proposed system.

1 Introduction

Literally, loop closing in the context of mobile robot nav-igation can be loosely defined as the act of closing theloop by empowering the mobile robot with the abilityto recognize places that it is indeed revisiting in an ex-plored area in its map after traversing the environmentfor some time in such a manner that results in a loop-liketrajectory. For mobile robots without a priori informa-tion of the environment, traversing in a loop-like trajec-tory maximizes the area being explored and at the sametime directs it back to a previously explored location inthe map. In this manner, as the mobile robot returnsand performs loop closing, the accumulated errors in thepose of the mobile robot and detected landmarks in theenvironment, as a result of errors associated with sensormeasurements, can be partially rectified. This processis advantageous for both the localization of the mobile

robot and in maintaining the global consistency of themap. However, the problem of reliably detecting loopclosures is indeed difficult. Due to the vital importanceof this issue and its relevance in SLAM (SimultaneousLocalization and Mapping) systems, loop closure detec-tion has gained a tremendous amount of attention inrecent years. Lately, the term loop closing is not onlyused to describe the ability of the robot to perceive thatit is revisiting a location in its map after traversing ina loop-like trajectory but has been used to describe theability of the robot to recognize that it has returned toa previously explored location in general irrespective ofwhether it is navigating in a loop-like trajectory.

We propose that a complete loop closing systemshould be made up of three modules: (1) loop closuredetection, (2) loop closure validation and (3) systemrestoration. Loop closure detection can be performedin many ways depending on the type of sensors avail-able on the mobile robot (i.e. producing range data,visual data, etc). However, for a fully autonomous mo-bile robot, loop closure detection is more advantageousin many applications if the mobile robot actively seeksit. Once the mobile robot believes it is revisiting an ex-plored location, it should then validate this loop closureevent regardless of whether a single or multi-hypothesesframework is used. If validation fails, there should besome means of reversing the decision of loop closure tono loop closure in that particular time step, since mod-ifications are made to the map and the mobile robot’spose when it commits to a detected loop closure. Wecall this process system restoration.

This paper is an extension of the work described in[Lui and Jarvis, 2010b]. The main focus of this paperis to describe the strategy to achieve active loop clo-sure detection, how to actively validate the detected loopclosure and finally, decide whether system restorationshould be performed for an autonomous mobile robotperforming topological SLAM. Nevertheless, the ideaspresented here can also be easily adapted to other sys-tems building metric or hybrid maps. Having said that,

it is possible for systems to retain all information or tomaintain multiple hypotheses with the potential to ret-rospectively remove invalidated loop closures. For suchcases, some parts of the complete loop closing systemmight not be applicable for such SLAM systems. Forcompleteness, a brief overview of the loop closure detec-tion module and the entire system is provided.

2 Hardware

Figure 1 shows the main research platform used in thisproject. It is powered by a differential drive wheelchairmotor/gear set and is equipped with a Bumblebee stere-ovision camera, a Logitech web camera and a uniqueomnidirectional stereovision system with a variable base-line. Omnidirectional images are produced using acamera-mirror combination. A Canon Powershot S3 IScamera is mounted and oriented in such a way that itpoints vertically upwards, imaging the reflection on theequiangular mirror designed by [Chahl and Srinivasan,1997]. These camera-mirror systems are also known ascatadioptric vision systems and a pair of them are usedhere for stereovision. In addition, these Canon camerasare capable of producing still images with resolutions upto 6MP and a live video stream at 30Hz with a resolu-tion of 320 x 240. Lastly, the robot is equipped with twolaptops; a Dell XPS M1730 equipped with two NvidiaGraphics Processing Units (GPU - 8800M GTX) and an-other entry-level laptop. The GPUs are primarily usedby the omnidirectional stereovision system.

Figure 1: Mobile Robot Platform: The Eye-Full Tower

3 Related Work

Loop closure detection has recently gained tremendousattention from the mobile robotics research communityin recent years. In the literature, there are many ways todetect loop closures and they are largely dependent uponthe type of sensors available on the mobile robot. Thethree main sensors used for mapping and localization arelaser rangefinders, sonar systems and cameras. Laser

rangefinders and sonar systems normally provide rangedata, with those produced by laser rangefinders generallybeing a lot denser as compared to sonar systems. Sonarsystems are capable of providing accurate locations of asparse set of corner and edge features but they are onlysuitable for indoor applications (attenuation of ultra-sonic signals in air is so severe that ranging much beyond10m is unusual). Laser rangefinders, on the other hand,are less restrictive and have been successfully applied tooutdoor applications. Nevertheless, loop closure detec-tion is difficult when only range data is available, sinceit relies on the physical structure of the environment inorder to distinguish one place from another. This even-tually becomes an issue if the mobile robot is requiredto traverse urban man-made environments where it islikely that different places produce similar range data.Of course, the gravity of this issue reduces as the effec-tive measurable range of the laser rangefinder increases.However, there is a limitation with respect to how muchthis effective measurable range can be increased due tohealth and safety concerns since laser rangefinders areactive sensors. Despite so, current state-of-the-art laserscanners like the Riegl terrestrial scanners can range to>800m and yet still being eye safe (although it is quitecostly as well). In addition, laser rangefinders emit activeenergy which may result in possible interference and en-ergy detection in military situations that may sometimesbe a disadvantage. Scan matching [Lu and Milios, 1997;Diosi and Kleeman, 2005] is a popular technique for com-paring laser range data and in a more recent work by[Granstrom et al., 2009], rotation invariant features areextracted from the range data before scan matching isperformed in an attempt to improve the accuracy of loopclosure detection.

On the other hand, video cameras provide visual infor-mation that can be further processed to provide sparseor dense range data. However, range data derived fromvisual information, which satisfies the real time con-straint, are not as accurate and consistent as comparedto laser range data. As such, visual loop closure detec-tion techniques are normally appearance-based, match-ing the appearance model of previously seen locationswith the appearance model of the mobile robot’s cur-rent location. Appearance-based techniques are natu-rally suited to solving the kidnapped robot problem, per-form multi-map merging and particularly the loop clo-sure detection problem. In fact, the majority of recentwork in visual loop closure detection uses appearance-based techniques. For mobile robots without a prioriknowledge of the environment, the loop closure detec-tion module has to differentiate between a new or pre-viously seen location. For a previously seen location,it has to determine which location in its map that isbeing revisited. It is not until recently that a com-

plete probabilistic appearance-based loop closure de-tection framework was proposed [Angeli et al., 2008;Cummins and Newman, 2008]. These works are basedon the bag-of-visual words paradigm which uses a com-bination of visual cues extracted from the image andbuilds a visual dictionary. In addition, this approachhas been experimentally validated to be robust in alarge scale outdoor environment. Nevertheless, thereare other appearance-models such as the Fourier coef-ficients [Zhang and Kleeman, 2009], Haar wavelet co-efficients [Ho and Jarvis, 2008], principal componentsof an image [Krose et al., 1999] and image histograms[Ulrich and Nourbakhsh, 2000] which may be employedas the appearance model. Unfortunately, these sys-tems function more as image retrieval systems sincea priori knowledge in the form of a database of ap-pearance models for respective locations in the envi-ronment is made available. Similarly, visual loop clo-sure detection does suffer from perceptual aliasing andsevere lighting variations. This forms the motivationfor utilizing both structural and visual information inthe environment for reducing false positive detectionssuch as those described by [Ho and Newman, 2007;Tungadi et al., 2010]. Besides, there is a biologicallyinspired appearance-based SLAM system, known as Rat-SLAM [Milford and Wyeth, 2008], which has been shownto work robustly at real-time speed generating a coher-ent map of a complex suburban road network.

As discussed previously, a loop closure detection sys-tem without active loop closing on an autonomous mo-bile robot is similar to trying to perform loop closing bychance. This is due to the errors associated with sen-sor measurements which degrade the localization of themobile robot over time, affecting the reliability of thesystem to direct itself back to possible loop closing lo-cations. [Stachniss et al., 2004] and [Tungadi and Klee-man, 2009] have both incorporated active loop closinginto their hybrid map building systems (both topologi-cal and metric maps). However, the active loop closingstrategy proposed by [Stachniss et al., 2004] performsloop closing only when the uncertainty in the mobilerobot’s pose becomes too large and detects loop closurecandidates if the shortest length between the current andcandidate node in the topological map is large whereasthe shortest length between these node locations in themetric map is small. In this way, this is still not anybetter than trying to perform loop closing by chance.On the other hand, the strategy proposed by [Tungadiand Kleeman, 2009] better fits into our definition of ac-tive loop closing since they apply the Voronoi Graph forthe generation of loop-paths and actively selects possi-ble loop-paths for exploration. Finally, it detects loopclosure via laser scan matching.

Due to the ambiguities and inaccuracies of sensor mea-

surements and ambiguities present in the physical envi-ronment, it is difficult for a loop closure detection mod-ule to produce no false alarms without having to tradeoff with more false negatives or delay in convergence.This is where active loop closure validation comes intoplay. An active loop closure validation module does notonly validate the loop closure event based on informa-tion collected from the past to present but also attemptsto proactively gather more evidence in the future if re-quired. If sufficient new evidence leads the system to be-lieve that the loop closure event is indeed a false alarm,it should be able to reverse the effects of this decisionthrough a process we refer to as system restoration. Un-fortunately, systems such as [Angeli et al., 2008; Cum-mins and Newman, 2008] validate a loop closure eventonly based on the past and present information. Otherapproaches such as those which maintain multiple hy-potheses [Tomatis et al., 2002; Beevers and Huang, 2005;Tully et al., 2009] are able to gather more evidence be-fore committing to accepting a loop closure event butare lacking in an active loop closure validation strat-egy. The problem with a multiple hypotheses frameworkwithout an active loop closure validation strategy is thethe exponential increase in the total number of hypothe-ses it has to maintain as more loop closure events areyet to be validated. The approach presented in this pa-per is a complete loop closing system which performsactive loop closing, maintains a single hypothesis andvalidates it by proactively gathering more evidence inthe future if required and the decision for accepting theloop closure event if it fails the validation. Experimentsare conducted on a fully autonomous mobile robot in asemi-outdoor environment.

4 System Overview

This is actually an extension to the work presented in[Lui and Jarvis, 2010a; 2010b]. Nevertheless, a briefoverview of the core components of the system is pro-vided below for completeness. For more information,please refer to the abovementioned papers.

4.1 Visual Odometry

The visual odometry system developed for the mobilerobot can be classified as a direct image-based approachwhich combines ground plane optical flow tracking andan appearance-based panoramic visual compass tech-nique. It assumes that the mobile robot is restrictedto pure rotation or straight line motion at any point intime and provides a 3DoF motion estimate of the robot.The real-time constraint is satisfied and motion is es-timated by combining distance estimates calculated byusing a pseudo optical flow algorithm [Fernandez andPrice, 2004] with bearing estimates from the panoramic

visual compass [Labrosse, 2007; Scaramuzza and Sieg-wart, 2008] illustrated in Figure 2.

(a) Optical Flow (b) Vis. Compass

Figure 2: Visual Odometry

4.2 Reactive Obstacle AvoidanceThe disparity maps returned by the Bumblebee stereovi-sion system are used to create a reactive obstacle avoid-ance system on the mobile robot. It determines whetherthe mobile robot should be avoiding an obstacle shouldit be getting too close to one and ignores any preplannedpaths based on the compressed 1D disparity map, whereeach element of this 1D map is the maximum disparityvalue of each column in the original 2D disparity map.

4.3 Omnidirectional StereovisionThe GPU-based omnidirectional stereovision system al-lows the mobile robot to perceive its surroundings. Theoutput is a 3D voxelated environment illustrated in Fig-ure 3(a) which is eventually compressed into a 2D localgrid map where frontier headings [Yamauchi, 1997] areextracted to provide the system with traversable direc-tions.

(a) (b)

Figure 3: (a) 3D Voxelated Env. (b) 2D Local Grid Map

4.4 Path PlannerBased on the frontier headings extracted from the 2D lo-cal grid map (Figure 3(b)) and the location of the nodesin the topological map, the path planner decides thenext course of action. Paths are planned using DistanceTransform [Jarvis, 1993] from the location of the mobilerobot to the selected frontier heading [Yamauchi, 1997]in the 2D local grid map. If no frontier headings arevalid for exploration at the current location of the mo-bile robot, it searches for existing nodes in its map which

are registered with valid frontier headings that are yetto be explored. A nodal propagation technique [Jarvis,1992] is used to find the path back to the selected node.Since the path planner is now tightly integrated with theproposed loop closing system, this topic will be revisitedagain later in this paper.

4.5 Topological SLAMThe topological map built by this system relies on thevisual odometry information in order to establish thespatial relationship between nodes in the map. The mo-bile robot has no a priori information of the environmentand each node in the evolving topological map containsinformation of its global 2D location, global heading ofthe omnidirectional image registered to the node and anestimation variance (initialized with variance of previousnode plus 6% of distance travelled). The initial positionof the mobile robot is assumed to be at the origin of theglobal coordinate system with its current heading initial-ized to 0o and a variance of 0. Each node in the map isalso associated with an appearance model of the locationand a global relaxation algorithm [Duckett et al., 2000]is employed in order to maintain its global consistency.Relative metric information is recovered when it com-mits to a detected loop closure via a RANSAC procedurewhich determines the best transformation matrix. Thetransformation matrix parameters are calculated usinga closed form solution to the least squares estimation oftwo 3D point patterns [Umeyama, 1991] with the two 3Dpoint patterns generated using SURF correspondences[Bay et al., 2008] and 3D information from the omnidi-rectional stereovision system. The relative orientation isrecovered using the difference between locations of SURFcorrespondences to yield more reliable estimates.

5 The Loop Closing System

The proposed loop closing system consists of three mod-ules: loop closure detection, loop closure validation andsystem recovery. In this section, each module is thor-oughly described with the emphasis on how active loopclosing is achieved, validated and how to decide whethersystem restoration is necessary.

5.1 Loop Closure DetectionOverviewThe loop closure detection module uses image signaturescreated by decomposing unwarped omnidirectional im-ages using the standard Haar decomposition techniqueas the appearance model for each node in the topologicalmap. The Haar wavelet has been experimentally vali-dated in [Ho and Jarvis, 2008; Lui and Jarvis, 2010b] tobe robust for use in indoor, semi-outdoor and outdoorenvironments. The system described in [Lui and Jarvis,2010b] ranks the scores in ascending order and selects

the top S candidates which have a score less than thethreshold TS (a lower score represents a better match).Each loop closure hypothesis is subjected to a number ofvalidations using information available at present, suchas the number of SURF correspondences, and a checkthat the Euclidean distance between the location of thematching candidates with the current location of themobile robot to be less than its current variance value(measure of uncertainty in its position). Relative met-ric information between nodes is then recovered fromthose qualifying candidates using the method describedin Section 4.5 and the final candidate is chosen by select-ing the one with the minimum Euclidean distance. Thisfinal candidate is only accepted as a valid loop closureevent provided that it has a Euclidean distance of lessthan TD. We refer to this as the rank-based framework.

On the other hand, we have also demonstrated theintegration of the Haar wavelets in a probabilistic loopclosure detection framework in [Tungadi et al., 2010] fora multi map merging system. This modified probabilis-tic framework is based on the work of [Angeli et al.,2008] and its state transition model and likelihood vot-ing scheme are refined such that it can be easily adaptedto systems requiring different convergence speeds andallowing the developer to decide what is deemed as adiscriminative score (depending on the underlying ap-pearance model being used and its image similarity met-ric). The system described in [Tungadi et al., 2010] hasbeen tuned specifically for it and can be easily adaptedfor this work. If this probabilistic framework is used,it returns a single candidate if loop closure is detectedand the same validations as described for the rank-basedframework are applied to this matching candidate.

Once the final candidate passes these validations(based on past and present information), the systemcommits itself to the loop closure event. In this case,the system goes into the correction phase where the rel-ative metric information between nodes recovered is es-sentially used to correct the mobile robot’s pose and theglobal relaxation algorithm is iterated for 20 times in or-der to maintain the global consistency of the topologicalmap.

Active Loop Closing

Active loop closing is achieved through a tight integra-tion with the path planner. There are two factors to con-sider; (1) loop size and (2) target loop closing locationselection. Loop size is affected by (a) the type of mapbuilt by the system, (b) accuracy of the mobile robot’slocalization system, (c) the type of sensors used for loopclosure detection and of course, (d) physical restrictionsimposed by the structure of the environment.

For a metric mapping system which provides an op-tion to select any loop size (where the size of the loop

is measured by the total distance required to completethe loop), the loop size can be determined by balanc-ing two factors; (i) maximizing the area being exploredand (ii) minimizing the uncertainty in the mobile robot’sestimated pose for the navigation system to reliably di-rect the mobile robot to its intended target loop closinglocation. At times, these two factors conflict with oneanother depending on the effective range of the employedsensor since the amount of overlap between sensor mea-surements in the outgoing and returning paths of theloop should be minimized (assuming that this effectiverange is where uncertainty in sensor measurements islow). If the resulting loop size is too large, it directlyaffects the ability of the mobile robot to find its targetloop closing location. However, whether the area beingexplored is being maximized does not depend on the ef-fective range of the sensor alone but also the structureof the environment. As such, given the option to selectfrom a range of fixed loop sizes or the flexibility to pickany loop size, a higher priority is given for increasing thelikelihood of the mobile robot to return to the intendedloop closing location. For the active loop closing systemdescribed by [Tungadi and Kleeman, 2009], where theVoronoi Graph is used, loop-paths are generated (im-plicitly addressing the problem of target loop closing lo-cation selection) and executed in such an order so thatshorter loop-paths are executed first, if available, beforeattempting longer loop paths such that a more globallyconsistent map is built. Loop-paths that are too shortare ignored and at times, when only long loop-paths areavailable due to the structure of the environment, thesystem has no choice but to attempt these loop-paths.The definition of whether a loop-path is long or short islargely dependent on the system.

On the other hand, topological mapping systems arerequired to determine both the loop size and target loopclosure location selection. Similarly, loop size selectionis dependent upon whether the objective is to maximizethe area being explored or to reliably find the target loopclosing location. For appearance-based systems, the areabeing explored is maximized when the loop size is cho-sen based on the minimum separation between imageswhere the employed appearance model can still functionreliably as a place recognition system. Assuming thatthe minimum separation between images is small, theloop size can also be kept small such that it increases thelikelihood of the mobile robot finding its target loop clos-ing location. However, navigating in small loops drasti-cally increases the total operation time unless a densetopological map is desired. Nevertheless, this minimumseparation between images changes with respect to thestructure and texture available in the environment. Sim-ilar to metric mapping systems, our system selects a loopsize that is biased towards ensuring the reliability of the

mobile robot to find its target loop closing location. Thefollowing describes the refined path planner, how the tar-get loop closing node is selected and how it attempts tofind this target node.

There are three modes of operation in the refined pathplanner: (1) active loop closing validation which will bedescribed later, (2) pure exploration and lastly, (3) theactive loop closing and exploration modes. The system isalways initialized to start in the pure exploration mode.In this mode, the path planner selects any frontier head-ing extracted from the 2D local grid map as long as it isunlikely to bring the mobile robot too close to an exist-ing node in the evolving topological map. In addition, itis a lazy algorithm in the sense that it is always biasedtowards selecting a valid frontier heading which requiresthe least amount of effort (i.e. the amount of pure rota-tional motion required such that it minimizes the changein global heading). When the total distance travelledby the mobile robot exceeds DI , which is measured viathe relative distance between each node inserted into thetopological map so far, the path planner switches intoactive loop closing and exploration mode.

The objective of the active loop closing and explo-ration mode is to maximize the chances of exploring newareas in the map while it attempts to direct the mobilerobot towards a target loop closing node. This targetnode is selected based on the prospect of a more accu-rate relocalization of the mobile robot’s pose with re-spect to the target node’s global reference position andorientation. Ideally, it tries to select the node with theminimum positional variance. At the same time, it takesinto account whether the mobile robot is likely to reachthis target node based on the distance travelled so farprior to detecting a loop closure event, DF , and a roughestimate of how far the mobile robot is from this targetnode based on the Euclidean distance.

Specific to this work, the average positional drift ofthe visual odometry system used is reported as 5.64% ofthe total distance travelled in a semi outdoor environ-ment. Given that it is likely to detect loop closure if themobile robot is approximately 1.5m away from the ac-tual location of the target node in the environment, thetotal distance the mobile robot can travel prior to loopclosing, Dmax = 25m, given that the average drift is in-creased to 6%. DI is then set as 7m (0.25Dmax roundedup to the nearest integer). The positional variances ofall nodes are then stored into a list, L, which is sorted inascending order and the Euclidean distance, Di

E , fromthe current location of the mobile robot to each node iscomputed, where i is the index of the node in the list.If D0

E + DF < Dmax, the selected target loop closingnode is the node in index 0 of the list, L. Else, a subsetof nodes, with positional variances less than half of themobile robot’s current positional variance from list L, is

stored into a new list H with its associated Euclideandistances, Dj

E , where j is the index of the node in thelist H. Subsequently, if DF < Dmax, the first node inlist H (similarly sorted in ascending order according toits positional variance value) which satisfies the condi-tion DF + Dj

E < Dmax is chosen or else H is sorted inascending order according to Dj

E and the first node inindex 0 is selected.

Once the target loop closing node is selected, the pathplanner selects a frontier heading that avoids previouslyexplored locations in the map and at the same time di-recting it closer to this target node. Since the reactiveobstacle avoidance system might take over control of themobile robot when an obstacle is deemed too close tothe mobile robot, it has also been refined to intelligentlydirect the mobile robot to move in a direction which ismore likely to bring it closer to the target node whileavoiding obstacles before the path planner resumes con-trol of the system. DF is only reinitialized to 0 if a validloop closure event is found for the target loop closingnode or for any of of its immediate neighbours (neigboursto the target loop closing node prior to committing tothe detected loop closure).

5.2 Loop Closure ValidationAs described in Section 5.1, a number of validation testsare applied to possible loop closure candidates using pastand present information. However, a complete loop clos-ing system should proactively gather more evidence inthe future, to confirm and refute the earlier closure de-cision, if required. Our active loop closure validationstrategy can be illustrated using a simple example basedon the topological map in Figure 4.

Figure 4: Topological Map

Assume that at the current time, t, the mobile robotis at node 15 in the topological map. It then decidesto travel in an arbitrary direction to a location whichcreates an image that triggers the loop closure detectionmodule. The detected loop closure, LCt+1, is matchingthe current location of the mobile robot at time t + 1to node 8 in the topological map. Assuming that themobile robot is indeed at node 8, sequences of paths(i.e. 7-6-5, 7-3-4, 7-3-2, 9-7-6 and 9-7-3) can be gen-erated. LCt+1 can thus be validated as true if any of

these sequences can be executed in order with the loopclosure detection module robustly matching the mobilerobot to these nodes in the anticipated order. Each se-quence should have the same number of nodes with thefirst node being an immediate neighbour of the node inthe loop closure event LCt+1 (node 8 in this example).In addition, these immediate neigbours are limited tothose which are prior to committing to the loop closureevent. Increasing the number of nodes in each sequenceincreases the validity of the process but it also increasesthe total operation time.

For our system, a simplified version of this strategyhas been implemented instead (active loop closing vali-dation mode in the path planner). Our system stops andcaptures an image every time the visual odometry sys-tem finds the current location of the mobile robot to bemore than x meters away from the previous node in thetopological map. If the loop closure detection modulematches the current location of the mobile robot withnode 8 in the topological map at time t + 1, the pathplanner directs the mobile robot to the most convenientimmediate neighbour of node 8 (prior to committing tothe loop closure) which requires the least change in itsglobal heading. (1) If the matching node returned bythe loop closure detection module is located within adistance of 1.25x meters (based on its topological rela-tionship), this validates the loop closure event LCt+1.On the other hand, (2) if a match is returned and doesnot satisfy the specified condition, system restoration isperformed. Otherwise, (3) if no matches are found, thepath planner directs the mobile robot to the next mostconvenient node. This process is iterated for n number oftimes and the matching node has to lie within a distanceof 1.25nx given that n ∈ (1, 3) for LCt+1 to be validated.If no matches are found when n = 3, system restorationis also performed since it is less risky to assume that noloop closure has occurred when it cannot be validateddue to the possible damage it can inflict onto the map.

Referring to (1), as the matching node (a loop clo-sure event itself) validates the loop closure event LCt+1

(node 8 in this example), LCt+1 also validates the match-ing node making it unnecessary to further validate it.The proposed algorithm is actually recursive at timesif (2) occurs since the system would have to validatethe matching node. To determine whether a detectedloop closure requires further validation, some measuresneed to be in place to describe the degree of uncertaintyof the detected loop closure. For the rank-based frame-work, this can be measured via the number of SURF cor-respondences whereas for the probabilistic framework,a likelihood ratio test (min. Bayes risk [Radke et al.,2005]) can be conducted in order to gauge the degree ofrisk involved when committing to a loop closure as com-pared to no loop closure. This in fact describes how the

active loop closing validation mode in the path planneris triggered.

5.3 System RestorationThe purpose of having the system restoration module isto enable the system to revert to a previous state of thesystem which has committed to a detected loop closurewhen the decision of loop closure is subsequently over-turned. This module has strong associations with theloop closure validation module and functions analogousto system restorations available on operating systems.A restoration point is created whenever the loop closurevalidation module requires further evidence to validatethe detected loop closure event. This restoration pointcontains all the information deemed necessary (i.e. stateof the map, probability distributions). Assuming thata loop closure event which requires further validation isdetected at time t + 1, a restoration point, Rt, that con-tains all the necessary system information at time t iscreated. If the loop closure event is invalidated at timet+3, the system is firstly restored using Rt. This is thenfollowed by forcing the state of the system at time t + 1to change from loop closure detected status to no loopclosure status. Subsequent inputs to the system at timet + 2 and t + 3 are then reprocessed by the system againbut this time having the state at time t + 1 being noloop closure detected. As discussed previously in Sec-tion 5.2, this process becomes recursive at times if theloop closure event is invalidated by another loop closure,requiring the system to perform active loop closure vali-dation for this new loop closure event only if additionalevidence is required.

6 Results and Discussion

Experiments were conducted in a semi-outdoor environ-ment (Figure 5). The mobile robot was fully autonomousand stopped at intervals of 1m to capture digital still im-ages from the omnidirectional stereovision system; per-formed exploration, active loop closing and loop closuredetection and built a topological map associated to adatabase of image signatures. Active loop closing valida-tion is triggered when required and system restoration isperformed if necessary. As the robot executes its plannedpath, it performs real time visual odometry and turns onthe reactive obstacle avoidance system. The rank-basedframework (TD = 1m and TS = −200) is used insteadof the probabilistic framework since it is expected thatthe former produces more false detections. Loop closurevalidation is required when the total SURF correspon-dences range from 20-35. Nevertheless, it is not the mainfocus of this paper to compare the performance of theseframeworks. Instead, these experiments illustrate the ef-fectiveness of the proposed active loop closing strategy,loop closure validation and system restoration modules.

Figure 5: Semi Outdoor Environment

In the first experiment1 (Figure 6), the loop closuredetection module detects a loop closure event at t = 2,LCt=2, which matches the current location of the mobilerobot to node 0 in the topological map. Based on thetotal SURF correspondences, the system decides thatfurther evidence is required. Using the proposed activeloop closure validation strategy described in Section 5.2,it tries to validate LCt=2. However, LCt=2 was inval-idated by a loop closure event at t = 5, LCt=5, whichthen triggers it to perform system restoration. Figure6(e) shows the state of the map being restored to thestate at t = 1, which is the state before LCt=2 is de-tected. It then forces the state of the system to changefrom loop closure to no loop closure detected at t = 2.Finally, LCt=5, which invalidates LCt=2 can be seen inFigure 6(h). LCt=5 does not require further validationand the mobile robot continues navigation in the pureexploration mode and switches to the loop closing andexploration mode. Subsequently, it detects loop closuresat t = 14 (not shown here) and t = 15. Nevertheless,since the condition D0

E + DF < Dmax is still true, itattempts to perform loop closing at node 0 and reachesits target destination at t = 16.

In the second experiment2, the mobile robot is clearlyillustrated to be switching from pure exploration modeto loop closing and exploration mode in Figures 7(a) and7(b) and detecting its first loop closure at t = 13. Themobile robot detects multiple loop closures throughoutits entire course of navigation and a subset of these loopclosures is shown at t = 14 and t = 25. All loop clo-sures detected either do not require further validation orhad been validated. As such, system restoration is notrequired in this experiment.

Using the plan view of the experimental area createdusing stitched laser scans from a Riegl LMS Z420i terres-trial laser scanner, the ground truth for each node canbe recovered by going through the omnidirectional im-ages one by one and utilizing the grids on the floor tolocalize the mobile robot in the environment. Each cell’sdimension in Figure 8 is approximately 60 x 60 ± 5 cmin each dimension. The trajectory with white dashedlines is the ground truth trajectory. The ground truth

1Vid1: http://www.youtube.com/watch?v=JKWDhsLfgJs2Vid2: http://www.youtube.com/watch?v=4u5cBLzqitY

(a) t = 1 (b) t = 2

(c) t = 3 (d) t = 4

(e) t = 5 (Recovery) (f) Recovery

(g) Recovery (h) Recovery

(i) t = 15 (j) t = 16

Figure 6: Experiment 1

(a) t = 5 (b) t = 6

(c) t = 12 (d) t = 13

(e) t = 14 (f) t = 24

(g) t = 25 (h) t = 32

Figure 7: Experiment 2

trajectory has three different nodes; green node repre-senting its starting position, yellow node representing anormal node whereas an orange node represents that avalid loop closure event has taken place. The follow-ing are node pairs (node numbers based on the groundtruth trajectory) which raises a loop closure event forExperiment 1: 5-3, 14-10, 15-11 and 16-1 and Exper-iment 2: 14-1, 15-2, 16-11, 19-11, 26-21, 27-18, 29-17,30-11 and 32-12. The topological map built by the mo-bile robot (trajectory with black lines) is properly scaledand superimposed onto the plan view of the laser mapsin Figure 8.

(a) Experiment 1 (b) Experiment 2

Figure 8: Ground Truth Comparisons

The experimental results show why a complete loopclosing system should consist of the three modules; loopclosure detection with active loop closing, loop closure

validation and system recovery. Active loop closing en-ables the system to detect loop closures regularly in or-der to maintain a globally consistent map and recoverfrom errors in the mobile robot’s pose estimate accumu-lated over time. The proposed loop closure validationmodule, which consists of an active loop closure valida-tion strategy, has been demonstrated in the experimentsto be useful for validating uncertain loop closure eventssince inaccurate loop closures can severely damage themap. The system has also been shown to be capable ofproducing a good topological representation of the ac-tual trajectory travelled by mobile robot autonomously.This can be further improved by revisiting nodes in themap (since the relaxation algorithm determines the po-sition of the node based on where its neighbours thinkit is). Although the omnidirectional stereovision sys-tem can combine multiple stereo pairs together and isequipped with an automatic baseline selection system,the baseline is remained fix in these experiments and isleft as future work.

7 Conclusion

In this paper, we proposed that a complete loop closingsystem for an autonomous mobile robot should consistof three modules; loop closure detection, loop closurevalidation and system restoration. Problems specificto each module have been thoroughly discussed wheresome of the ideas presented can be easily adapted forother systems. Nevertheless, a specific solution for anappearance-based topological mapping system has beenproposed, discussed and successfully validated by exper-iments conducted in a semi-outdoor environment.

Acknowledgments

The authors would like to thank Nghia Ho for providingplan views of the stitched laser scans for use in this work.

References

[Angeli et al., 2008] A. Angeli, D. Filliat, S. Doncieuxand J.-A. Meyer. A Fast and Incremental Method forLoop-Closure Detection Using Bags of Visual Words.IEEE T–RO, 24(5):1027–1037, 2008.

[Bay et al., 2008] H. Bay, T. Tuytelaars and L. VanGool. SURF: Speeded Up Robust Features. In ECCV,2008.

[Beevers and Huang, 2005] K. Beevers and W. H.Huang. Loop Closing in Topological Maps. In IEEEICRA, pages 4368–4372, Barcelona, Spain, 2005.

[Chahl and Srinivasan, 1997] J. S. Chahl and M. V.Srinivasan. Reflective Surfaces for Panoramic Imag-ing. Applied Optics, 36(31):8275–8285, 1997.

[Cummins and Newman, 2008] M. Cummins and P.Newman. FAB-MAP: Probabilistic Localization andMapping in the Space of Appearance. IJRR,27(6):647–665, 2008.

[Diosi and Kleeman, 2005] A. Diosi and L. Kleeman.Laser Scan Matching in Polar Coordinates with Ap-plication to SLAM. In IEEE/RSJ IROS, Edmonton,Canada, 2005.

[Duckett et al., 2000] T. Duckett, S. Marsland and J.Shapiro. Learning Globally Consistent Maps by Re-laxation. In IEEE ICRA, volume 4, pages 3841–3846,San Francisco, CA USA, 2000.

[Fernandez and Price, 2004] D. Fernandez and A. Price.Visual Odometry for An Outdoor Mobile Robot. InIEEE RAM, pages 816–821, 2004.

[Granstrom et al., 2009] K. Granstrom, J. Callmer, F.Ramos and J. Nieto. Learning to Detect Loop Closurefrom Range Data. In IEEE ICRA, pages 15–22, Kobe,Japan, 2009.

[Ho and Newman, 2007] K. Leong Ho and P. Newman.Detecting Loop Closure with Scene Sequences. IJCV,74(3):261–286, 2007.

[Ho and Jarvis, 2008] N. Ho and R. Jarvis. Vision BasedGlobal Localisation Using a 3D Environmental ModelCreated by a Laser Range Scanner. In IEEE/RSJIROS, pages 2964–2969, Nice France, 2008.

[Jarvis, 1992] R. Jarvis. Optimal Pathways for Road Ve-hicle Navigation. In IEEE TENCON, volume 2, pages876–870, Melbourne, Australia, 1992.

[Jarvis, 1993] R. Jarvis. Distance Transform Based PathPlanning for Robot Navigation. Recent Trends in Mo-bile Robots, Chapter 1, Yuan F. Zhang, Ed. WorldScientific Publishing Co. Pty. Ltd., 1993.

[Krose et al., 1999] B. Krose, R. Bunschoten, N. Vlassisand Y. Motomura. Appearance Based Robot Local-ization. In IJCAI-99 Workshop, pages 53–58, 1999.

[Labrosse, 2007] Frederic Labrosse. The Visual Com-pass: Performance and Limitations of an Appearance-Based Method. JFR, 23(10):913–941, 2007.

[Lu and Milios, 1997] F. Lu and E. Milios. GloballyConsistent Range Scan Alignment for EnvironmentMapping. Autonomous Robots, 4:333–349, 1997.

[Lui and Jarvis, 2010a] W.L.D. Lui and R. Jarvis. Eye-Full Tower: A GPU-based Variable MultibaselineOmnidirectional Stereovision System with AutomaticBaseline Selection for Outdoor Mobile Robot Naviga-tion. Rob. and Aut. Systems, 58(6):747–761, 2010.

[Lui and Jarvis, 2010b] W.L.D. Lui and R. Jarvis. APure Vision-based Approach to Topological SLAM. In

IEEE/RSJ IROS, pages 3784–3791, Taipei, Taiwan,Oct. 2010.

[Milford and Wyeth, 2008] M. Milford and G. Wyeth.Mapping a Suburb With a Single Camera Using aBiologically Inspired SLAM System. IEEE T–RO,24(5):1038–1053, 2008.

[Radke et al., 2005] R.J. Radke, S. Andra, O. Al-Kohafiand B. Roysam. Image Change Detection Algorithms:A Systematic Survey. In IEEE Trans. on Img. Pro-cessing, 14(3):294–307, 2005.

[Scaramuzza and Siegwart, 2008] D. Scaramuzza and R.Siegwart. Appearance-Guided Monocular Omnidirec-tional Visual Odometry for Outdoor Ground Vehicles.IEEE T–RO, 24(5):1015-1026, 2008.

[Stachniss et al., 2004] C. Stachniss, D. Hahnel and W.Burgard. Exploration with Active Loop-Closing forFastSLAM. In IEEE/RSJ IROS, pages 1505–1510,Sendai, Japan, Sept.-Oct. 2004.

[Tomatis et al., 2002] N. Tomatis, I. Nourbakhsh and R.Siegwart. Hybrid Simultaneous Localization and MapBuilding: Closing the Loop with Multi-HypothesesTracking. In IEEE ICRA, pages 2749–2754, Wash-ington, DC USA, 2002.

[Tully et al., 2009] S. Tully, G. Kantor, H. Choset andF. Werner. A Mutli-Hypothesis Topological SLAMApproach for Loop Closing on Edge-Ordered Graphs.In IEEE/RSJ IROS, pages 11–15, St. Louis, 2009.

[Tungadi et al., 2010] F. Tungadi, W.L.D. Lui, L. Klee-man and R. Jarvis. Robust Online Map Merging Sys-tem using Laser Scan Matching and OmnidirectionalVision. In IEEE/RSJ IROS, pages 7–14, Taipei, Tai-wan, Oct. 2010.

[Tungadi and Kleeman, 2009] F. Tungadi and L. Klee-man. Loop Exploration for SLAM with Fusion of Ad-vanced Sonar Features and Laser Polar Scan Match-ing. In IEEE/RSJ IROS, pages 388–394, St Louis,MO USA, Oct. 2009.

[Ulrich and Nourbakhsh, 2000] I. Ulrich and I. Nour-bakhsh. Appearance-Based Place Recognition forTopological Localization. In IEEE ICRA, pages 1023–1029, San Francisco, CA USA, 2000.

[Umeyama, 1991] S. Umeyama. Least Squares Esti-mation of Transformation Parameters Between TwoPoint Patterns. IEEE TPAMI, 13(4):376–380, 1991.

[Yamauchi, 1997] B. Yamauchi. A Frontier-Based Ap-proach for Autonomous Exploration. In IEEE CIRA,pages 146–151, Monterey, CA USA, Jul. 1997.

[Zhang and Kleeman, 2009] A. M. Zhang and L. Klee-man. Robust Appearance Based Visual Route Fol-lowing for Navigation in Large-scale Outdoor Envi-ronments. IJRR, 28(3):331–356, 2009.